How to calculate w variable for quaternion on Thingy Gyro?

[ndarkness]

Hello,

I was wondering how is calcualted  or read on the Thingy firmware the variable w for the quaternion used in the Gyroscope readings.

Looking into m_motion.c in the handler drv_motion_evt_handler() I see the snippet below, where `data.w = p_quat[0]` happens, but I cannot track down where the p_quat[0] is calculated.

Could you give me some pointers?

Thanks!

   case DRV_MOTION_EVT_QUAT:
        {
            APP_ERROR_CHECK_BOOL(size == sizeof(int32_t) * 4);

            ble_tms_quat_t data;
            int32_t      * p_quat = (int32_t *)p_data;

            data.w = p_quat[0];
            data.x = p_quat[1];
            data.y = p_quat[2];
            data.z = p_quat[3];

            #if NRF_LOG_ENABLED
                static const uint8_t QUAT_ELEMENTS = 4;
                double f_buf;
                char buffer[QUAT_ELEMENTS][7];

                for (uint8_t i = 0; i < QUAT_ELEMENTS; i++)
                {
                    f_buf = (double)p_quat[i];
                    f_buf = f_buf/(1<<30);
                    sprintf(buffer[i], "% 1.3f", f_buf);
                }

                NRF_LOG_DEBUG("DRV_MOTION_EVT_QUAT: w:%s x:%s y:%s z:%s\r\n", nrf_log_push(buffer[0]),
                                                                              nrf_log_push(buffer[1]),
                                                                              nrf_log_push(buffer[2]),
                                                                              nrf_log_push(buffer[3]));
            #endif

            (void)ble_tms_quat_set(&m_tms, &data);
        }
        break;

[Bjørn Kvaale]

Hi,

It looks like the quaternion output can be found in this function in eMPL_outputs.c:

/**
 *  @brief      Body-to-world frame quaternion.
 *  The elements are output in the following order: W, X, Y, Z.
 *  @param[out] data        Quaternion, q30 fixed point.
 *  @param[out] accuracy    Accuracy of the measurement from 0 (least accurate)
 *                          to 3 (most accurate).
 *  @param[out] timestamp   The time in milliseconds when this sensor was read.
 *  @return     1 if data was updated. 
 */
int inv_get_sensor_type_quat(long *data, int8_t *accuracy, inv_time_t *timestamp)
{
    memcpy(data, eMPL_out.quat, sizeof(eMPL_out.quat));
    accuracy[0] = eMPL_out.quat_accuracy;
    timestamp[0] = eMPL_out.nine_axis_timestamp;
    return eMPL_out.nine_axis_status;
}

This function is called in drv_motion.c:

    if (m_motion.features & DRV_MOTION_FEATURE_MASK_QUAT)
    {
        if (inv_get_sensor_type_quat((long *)data, &accuracy, &timestamp))
        {
            evt = DRV_MOTION_EVT_QUAT;
            // W, X, Y, and Z
            m_motion.evt_handler(&evt, data, sizeof(long) * 4);
        }
    }

After the function call, the DRV_MOTION_EVT_QUAT event gets called. This event is picked up by the drv_motion_evt_handler() in m_motion.c, which then leads you to the code that you mention above. So I believe the quaternion calculations are done inside the chip itself & the inv_get_sensor_type_quat() function only gets the quaternion data, but does not calculate this data.

Kind Regards,

Bjørn Kvaale