Adding timestamps to ble tms service's raw characteristic.

Question

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.