Roy
259 lines
7.8 KiB
C
259 lines
7.8 KiB
C
#include <stdbool.h>
|
|
#include <stdint.h>
|
|
|
|
#ifdef __cplusplus
|
|
extern "C"
|
|
{
|
|
#endif
|
|
|
|
#include "app_config.h"
|
|
#include "apply_old_config.h"
|
|
#include "sdk_config.h"
|
|
|
|
#include "nrf_log.h"
|
|
#include "nrf_log_ctrl.h"
|
|
#include "nrf_log_default_backends.h"
|
|
|
|
#include "nrf_sdh.h"
|
|
#include "nrf_sdh_ble.h"
|
|
#include "nrf_sdh_freertos.h"
|
|
|
|
#include "ble_srv_common.h"
|
|
|
|
#include "FreeRTOS.h"
|
|
#include "queue.h"
|
|
#include "semphr.h"
|
|
#include "task.h"
|
|
|
|
#ifdef __cplusplus
|
|
}
|
|
#endif
|
|
static QueueHandle_t regular_data_q = NULL;
|
|
static SemaphoreHandle_t regular_data_sem = NULL;
|
|
|
|
typedef struct __PACKED
|
|
{
|
|
uint32_t timestamp;
|
|
uint8_t id;
|
|
uint8_t type : 4;
|
|
uint8_t : 4;
|
|
union
|
|
{
|
|
double double_val;
|
|
float flaot_val;
|
|
uint64_t u64_val;
|
|
uint32_t u32_val;
|
|
uint16_t u16_val;
|
|
uint8_t u8_val;
|
|
int64_t i64_val;
|
|
int32_t i32_val;
|
|
int16_t i16_val;
|
|
int8_t i8_val;
|
|
} content;
|
|
uint8_t padding[2];
|
|
} channel_data_t;
|
|
|
|
#define REGULAR_I8_TYPE 0
|
|
#define REGULAR_I16_TYPE 1
|
|
#define REGULAR_I32_TYPE 2
|
|
#define REGULAR_I64_TYPE 3
|
|
#define REGULAR_U8_TYPE 4
|
|
#define REGULAR_U16_TYPE 5
|
|
#define REGULAR_U32_TYPE 6
|
|
#define REGULAR_U64_TYPE 7
|
|
#define REGULAR_FLOAT_TYPE 8
|
|
#define REGULAR_DOUBLE_TYPE 9
|
|
#define REGULAR_MAX_TYPE 10
|
|
|
|
const uint32_t channel_size_table[REGULAR_MAX_TYPE] = {
|
|
[REGULAR_I8_TYPE] = offsetof(channel_data_t, content) + sizeof(int8_t),
|
|
[REGULAR_I16_TYPE] = offsetof(channel_data_t, content) + sizeof(int16_t),
|
|
[REGULAR_I32_TYPE] = offsetof(channel_data_t, content) + sizeof(int32_t),
|
|
[REGULAR_I64_TYPE] = offsetof(channel_data_t, content) + sizeof(int64_t),
|
|
[REGULAR_U8_TYPE] = offsetof(channel_data_t, content) + sizeof(uint8_t),
|
|
[REGULAR_U16_TYPE] = offsetof(channel_data_t, content) + sizeof(uint16_t),
|
|
[REGULAR_U32_TYPE] = offsetof(channel_data_t, content) + sizeof(uint32_t),
|
|
[REGULAR_U64_TYPE] = offsetof(channel_data_t, content) + sizeof(uint64_t),
|
|
[REGULAR_FLOAT_TYPE] = offsetof(channel_data_t, content) + sizeof(float),
|
|
[REGULAR_DOUBLE_TYPE] = offsetof(channel_data_t, content) + sizeof(double),
|
|
};
|
|
|
|
void pseudo_sensorAA_read(channel_data_t *p_channel_data)
|
|
{
|
|
static uint32_t val = 0;
|
|
p_channel_data->timestamp = xTaskGetTickCount();
|
|
p_channel_data->id = 0xAA;
|
|
p_channel_data->type = REGULAR_U32_TYPE;
|
|
p_channel_data->content.u32_val = val++;
|
|
}
|
|
|
|
void pseudo_sensorBB_read(channel_data_t *p_channel_data)
|
|
{
|
|
static uint32_t val = 0;
|
|
p_channel_data->timestamp = xTaskGetTickCount();
|
|
p_channel_data->id = 0xBB;
|
|
p_channel_data->type = REGULAR_U32_TYPE;
|
|
p_channel_data->content.u32_val = val--;
|
|
}
|
|
|
|
static void pseudo_data_task(void *pArg)
|
|
{
|
|
void (*pfCallback)(channel_data_t *p_channel_data) = pArg;
|
|
for (;;)
|
|
{
|
|
vTaskDelay(pdMS_TO_TICKS(1));
|
|
channel_data_t ch_data;
|
|
memset(&ch_data, 0x00, sizeof(ch_data));
|
|
for (int i = 0; i < 1; i++)
|
|
{
|
|
pfCallback(&ch_data);
|
|
if (xQueueSend(regular_data_q, &ch_data, pdMS_TO_TICKS(100)) == pdFAIL)
|
|
{
|
|
__BKPT(255);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
static union
|
|
{
|
|
uint8_t raw[256];
|
|
struct __PACKED
|
|
{
|
|
uint8_t len;
|
|
uint8_t seq;
|
|
uint16_t : 16;
|
|
uint32_t time;
|
|
uint8_t channel_len; /* channel len => group length */
|
|
} header;
|
|
} payload;
|
|
|
|
static void regular_data_msg_task(void *pArg)
|
|
{
|
|
|
|
for (;;)
|
|
{
|
|
channel_data_t ch_data;
|
|
|
|
uint32_t offset = sizeof(payload.header);
|
|
/*
|
|
Receive a message on the regular_data_q queue.
|
|
Block for forever if a message is not immediately available.
|
|
*/
|
|
if (xQueueReceive(regular_data_q, &ch_data, portMAX_DELAY) == pdTRUE)
|
|
{
|
|
uint32_t channel_size = channel_size_table[ch_data.type % REGULAR_MAX_TYPE];
|
|
memcpy(&payload.raw[offset], &ch_data, channel_size);
|
|
offset += channel_size;
|
|
/* set channel len = 1 */
|
|
payload.header.channel_len = 1;
|
|
}
|
|
else
|
|
{
|
|
continue;
|
|
}
|
|
|
|
extern uint16_t le_gatt_mtu(void);
|
|
uint16_t att_mtu = le_gatt_mtu() - 3;
|
|
for (;;)
|
|
{
|
|
/*
|
|
Receive a message on the regular_data_q queue.
|
|
Block for 50ms if a message is not immediately available.
|
|
if there is no new message after 50ms, breaks out of the loop.
|
|
*/
|
|
if (xQueueReceive(regular_data_q, &ch_data, pdMS_TO_TICKS(50)) == pdTRUE)
|
|
{
|
|
/* set channel data total length. */
|
|
uint32_t channel_size = channel_size_table[ch_data.type % REGULAR_MAX_TYPE];
|
|
/* copy channel data to payload buffer. */
|
|
memcpy(&payload.raw[offset], &ch_data, channel_size);
|
|
offset += channel_size;
|
|
/* increase channel len */
|
|
payload.header.channel_len++;
|
|
/* if payload length is almost full, breaks out of the loop. */
|
|
if (offset > (att_mtu - (sizeof(ch_data) - 2) - 1))
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
|
|
extern bool le_regular_data_notify_is_enable(void);
|
|
if (le_regular_data_notify_is_enable())
|
|
{
|
|
extern ret_code_t le_regular_data_notify(uint8_t * p_value, uint16_t len);
|
|
/* update payload length & sequence number */
|
|
payload.header.len = offset + 1;
|
|
payload.header.seq++;
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
/* update payload notify time */
|
|
payload.header.time = xTaskGetTickCount();
|
|
/* calculate checksum */
|
|
uint8_t chksum = 0;
|
|
for (uint32_t i = 0; i < offset; i++)
|
|
{
|
|
chksum += payload.raw[i];
|
|
}
|
|
/* append checksum */
|
|
payload.raw[offset] = chksum;
|
|
/* try to send notify */
|
|
ret_code_t err_code = le_regular_data_notify(payload.raw, offset + 1);
|
|
/* if notify success, breaks out of the loop. */
|
|
if (err_code == NRF_SUCCESS)
|
|
{
|
|
break;
|
|
}
|
|
/* if timeout, breaks out of the loop. */
|
|
if (xSemaphoreTake(regular_data_sem, pdMS_TO_TICKS(50)) == false)
|
|
{
|
|
NRF_LOG_INFO("regulat data underrun.");
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
static void edc_regular_data_handler(ble_evt_t const *p_ble_evt, void *p_context)
|
|
{
|
|
switch (p_ble_evt->header.evt_id)
|
|
{
|
|
case BLE_GAP_EVT_CONNECTED:
|
|
payload.header.seq = 0xFF;
|
|
break;
|
|
case BLE_GATTS_EVT_HVN_TX_COMPLETE:
|
|
xSemaphoreGive(regular_data_sem);
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
void edc_regular_data_init(void)
|
|
{
|
|
regular_data_q = xQueueCreate(512, sizeof(channel_data_t));
|
|
if (regular_data_q == NULL)
|
|
{
|
|
// Will not get here unless there is insufficient RAM.
|
|
__BKPT(255);
|
|
}
|
|
|
|
regular_data_sem = xSemaphoreCreateBinary();
|
|
if (regular_data_sem == NULL)
|
|
{
|
|
// Will not get here unless there is insufficient RAM.
|
|
__BKPT(255);
|
|
}
|
|
|
|
xTaskCreate(regular_data_msg_task, "regular_msg", 192, NULL, 5, NULL);
|
|
xTaskCreate(pseudo_data_task, "pseudo_AA", 192, pseudo_sensorAA_read, 2, NULL);
|
|
xTaskCreate(pseudo_data_task, "pseudo_BB", 192, pseudo_sensorBB_read, 2, NULL);
|
|
NRF_SDH_BLE_OBSERVER(m_edc_regular_data_observer, 3, edc_regular_data_handler, NULL);
|
|
}
|