This post is older than 2 years and might not be relevant anymore
More Info: Consider searching for newer posts

How to calculate w variable for quaternion on Thingy Gyro?

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;

  • 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

Related