Hello,
I'm implementing some ble service that send node id, timestamps, accel values, gyro values.
So I'm trying to modify ble tms service at Nordic-Thingy53-FW .
I want to add node id and timestamp at raw characteristic, so gatt client can request for notification of that characteristic.
But now, I have some problem adding node id and timestamps.
I add two attribute
uint32_t time_ms
uint8_t id
to ble_tms_raw_t type at ble_tms.h file.
And also change m_motion.c file's drv_motion_evt_handler function to assign value to node id and timestamps.
static void drv_motion_evt_handler(drv_motion_evt_t const * p_evt, void * p_data, uint32_t size)
{
switch (*p_evt)
{
case DRV_MOTION_EVT_RAW:
{
APP_ERROR_CHECK_BOOL(size == sizeof(int32_t) * RAW_PARAM_NUM);
ble_tms_raw_t data;
int32_t * p_raw = (int32_t *)p_data;
/* p_raw is in 16Q16 format. This is compressed for BLE transfer. */
// Set upper and lower overflow limits.
static const int16_t overflow_limit_upper[RAW_PARAM_NUM] = {
(1 << (RAW_Q_FORMAT_ACC_INTEGER_BITS - 1)) - 1,
(1 << (RAW_Q_FORMAT_ACC_INTEGER_BITS - 1)) - 1,
(1 << (RAW_Q_FORMAT_ACC_INTEGER_BITS - 1)) - 1,
(1 << (RAW_Q_FORMAT_GYR_INTEGER_BITS - 1)) - 1,
(1 << (RAW_Q_FORMAT_GYR_INTEGER_BITS - 1)) - 1,
(1 << (RAW_Q_FORMAT_GYR_INTEGER_BITS - 1)) - 1,
(1 << (RAW_Q_FORMAT_CMP_INTEGER_BITS - 1)) - 1,
(1 << (RAW_Q_FORMAT_CMP_INTEGER_BITS - 1)) - 1,
(1 << (RAW_Q_FORMAT_CMP_INTEGER_BITS - 1)) - 1};
static const int16_t overflow_limit_lower[RAW_PARAM_NUM] = {
-(1 << (RAW_Q_FORMAT_ACC_INTEGER_BITS - 1)),
-(1 << (RAW_Q_FORMAT_ACC_INTEGER_BITS - 1)),
-(1 << (RAW_Q_FORMAT_ACC_INTEGER_BITS - 1)),
-(1 << (RAW_Q_FORMAT_GYR_INTEGER_BITS - 1)),
-(1 << (RAW_Q_FORMAT_GYR_INTEGER_BITS - 1)),
-(1 << (RAW_Q_FORMAT_GYR_INTEGER_BITS - 1)),
-(1 << (RAW_Q_FORMAT_CMP_INTEGER_BITS - 1)),
-(1 << (RAW_Q_FORMAT_CMP_INTEGER_BITS - 1)),
-(1 << (RAW_Q_FORMAT_CMP_INTEGER_BITS - 1))};
int16_t overflow_check;
for (uint8_t i = 0; i < RAW_PARAM_NUM; i++)
{
overflow_check = p_raw[i] >> 16; // Right shift 16 to remove decimal part.
if (overflow_check >= overflow_limit_upper[i])
{
NRF_LOG_WARNING("p_raw[%d] over limit. Val: %d limit: %d \r\n", i, overflow_check, overflow_limit_upper[i]);
p_raw[i] = overflow_limit_upper[i] << 16;
}
else if (overflow_check < overflow_limit_lower[i])
{
NRF_LOG_WARNING("p_raw[%d] below limit. Val: %d limit: %d \r\n", i, overflow_check, overflow_limit_lower[i]);
p_raw[i] = overflow_limit_lower[i] << 16;
}
else
{
// No overflow has occured.
}
}
data.id = 1;
data.time_ms = (((uint32_t)app_timer_cnt_get()) * (APP_TIMER_PRESCALER + 1) * 1000) / APP_TIMER_CLOCK_FREQ;
data.accel.x = (int16_t)(p_raw[0] >> RAW_Q_FORMAT_ACC_INTEGER_BITS);
data.accel.y = (int16_t)(p_raw[1] >> RAW_Q_FORMAT_ACC_INTEGER_BITS);
data.accel.z = (int16_t)(p_raw[2] >> RAW_Q_FORMAT_ACC_INTEGER_BITS);
data.gyro.x = (int16_t)(p_raw[3] >> RAW_Q_FORMAT_GYR_INTEGER_BITS);
data.gyro.y = (int16_t)(p_raw[4] >> RAW_Q_FORMAT_GYR_INTEGER_BITS);
data.gyro.z = (int16_t)(p_raw[5] >> RAW_Q_FORMAT_GYR_INTEGER_BITS);
data.compass.y = -(int16_t)(p_raw[6] >> RAW_Q_FORMAT_CMP_INTEGER_BITS); // Changed axes and inverted. Corrected for rotation of axes.
data.compass.x = (int16_t)(p_raw[7] >> RAW_Q_FORMAT_CMP_INTEGER_BITS); // Changed axes. Corrected for rotation of axes.
data.compass.z = (int16_t)(p_raw[8] >> RAW_Q_FORMAT_CMP_INTEGER_BITS);
(void)ble_tms_raw_set(&m_tms, &data);
}
break;
/*.......*/
default:
NRF_LOG_WARNING("drv_motion_evt_handler: Unknown data!\r\n");
break;
}
}
But, when GATT client receive the notification of this characteristic, data is not correct.
they have to receive data in <node_id, timestamps, accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z> form.
I set node id as 1 but they receive 0 for node id, and timestamps are also not correct.
I think accel and gyro raw data is correct, but node_id and timestamps are not.
I'm using s132 v4.0.2 softdevice, and Nordic Thingy:52 SDK.
