www.pudn.com > MPL.zip > eMPL_outputs.c, change:2016-04-15,size:11204b


/* 
 $License: 
    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. 
    See included License.txt for License information. 
 $ 
 */ 
 
/** 
 *   @defgroup  HAL_Outputs hal_outputs 
 *   @brief     Motion Library - HAL Outputs 
 *              Sets up common outputs for HAL 
 * 
 *   @{ 
 *       @file eMPL_outputs.c 
 *       @brief Embedded MPL outputs. 
 */ 
#include "eMPL_outputs.h" 
#include "ml_math_func.h" 
#include "mlmath.h" 
#include "start_manager.h" 
#include "data_builder.h" 
#include "results_holder.h" 
 
struct eMPL_output_s { 
    long quat[4]; 
    int quat_accuracy; 
    int gyro_status; 
    int accel_status; 
    int compass_status; 
    int nine_axis_status; 
    inv_time_t nine_axis_timestamp; 
}; 
 
static struct eMPL_output_s eMPL_out; 
 
/** 
 *  @brief      Acceleration (g's) in body frame. 
 *  Embedded MPL defines gravity as positive acceleration pointing away from 
 *  the Earth. 
 *  @param[out] data        Acceleration in g's, q16 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_accel(long *data, int8_t *accuracy, inv_time_t *timestamp) 
{ 
    inv_get_accel_set(data, accuracy, timestamp); 
    if (eMPL_out.accel_status & INV_NEW_DATA) 
        return 1; 
    else 
        return 0; 
} 
 
/** 
 *  @brief      Angular velocity (degrees per second) in body frame. 
 *  @param[out] data        Angular velocity in dps, q16 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_gyro(long *data, int8_t *accuracy, inv_time_t *timestamp) 
{ 
    inv_get_gyro_set(data, accuracy, timestamp); 
    if (eMPL_out.gyro_status & INV_NEW_DATA) 
        return 1; 
    else 
        return 0; 
} 
 
/** 
 *  @brief      Magnetic field strength in body frame. 
 *  @param[out] data        Field strength in microteslas, q16 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_compass(long *data, int8_t *accuracy, inv_time_t *timestamp) 
{ 
    inv_get_compass_set(data, accuracy, timestamp); 
    if (eMPL_out.compass_status & INV_NEW_DATA) 
        return 1; 
    else 
        return 0; 
} 
 
/** 
 *  @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; 
} 
 
/** 
 *  @brief      Quaternion-derived heading. 
 *  @param[out] data        Heading in degrees, q16 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_heading(long *data, int8_t *accuracy, inv_time_t *timestamp) 
{ 
    long t1, t2, q00, q03, q12, q22; 
    float fdata; 
 
    q00 = inv_q29_mult(eMPL_out.quat[0], eMPL_out.quat[0]); 
    q03 = inv_q29_mult(eMPL_out.quat[0], eMPL_out.quat[3]); 
    q12 = inv_q29_mult(eMPL_out.quat[1], eMPL_out.quat[2]); 
    q22 = inv_q29_mult(eMPL_out.quat[2], eMPL_out.quat[2]); 
 
    /* X component of the Ybody axis in World frame */ 
    t1 = q12 - q03; 
 
    /* Y component of the Ybody axis in World frame */ 
    t2 = q22 + q00 - (1L << 30); 
    fdata = atan2f((float) t1, (float) t2) * 180.f / (float) M_PI; 
    if (fdata < 0.f) 
        fdata += 360.f; 
    data[0] = (long)(fdata * 65536.f); 
 
    accuracy[0] = eMPL_out.quat_accuracy; 
    timestamp[0] = eMPL_out.nine_axis_timestamp; 
    return eMPL_out.nine_axis_status; 
} 
 
/** 
 *  @brief      Body-to-world frame euler angles. 
 *  The euler angles are output with the following convention: 
 *  Pitch: -180 to 180 
 *  Roll: -90 to 90 
 *  Yaw: -180 to 180 
 *  @param[out] data        Euler angles in degrees, q16 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_euler(long *data, int8_t *accuracy, inv_time_t *timestamp) 
{ 
    long t1, t2, t3; 
    long q00, q01, q02, q03, q11, q12, q13, q22, q23, q33; 
    float values[3]; 
 
    q00 = inv_q29_mult(eMPL_out.quat[0], eMPL_out.quat[0]); 
    q01 = inv_q29_mult(eMPL_out.quat[0], eMPL_out.quat[1]); 
    q02 = inv_q29_mult(eMPL_out.quat[0], eMPL_out.quat[2]); 
    q03 = inv_q29_mult(eMPL_out.quat[0], eMPL_out.quat[3]); 
    q11 = inv_q29_mult(eMPL_out.quat[1], eMPL_out.quat[1]); 
    q12 = inv_q29_mult(eMPL_out.quat[1], eMPL_out.quat[2]); 
    q13 = inv_q29_mult(eMPL_out.quat[1], eMPL_out.quat[3]); 
    q22 = inv_q29_mult(eMPL_out.quat[2], eMPL_out.quat[2]); 
    q23 = inv_q29_mult(eMPL_out.quat[2], eMPL_out.quat[3]); 
    q33 = inv_q29_mult(eMPL_out.quat[3], eMPL_out.quat[3]); 
 
    /* X component of the Ybody axis in World frame */ 
    t1 = q12 - q03; 
 
    /* Y component of the Ybody axis in World frame */ 
    t2 = q22 + q00 - (1L << 30); 
    values[2] = -atan2f((float) t1, (float) t2) * 180.f / (float) M_PI; 
 
    /* Z component of the Ybody axis in World frame */ 
    t3 = q23 + q01; 
    values[0] = 
        atan2f((float) t3, 
                sqrtf((float) t1 * t1 + 
                      (float) t2 * t2)) * 180.f / (float) M_PI; 
    /* Z component of the Zbody axis in World frame */ 
    t2 = q33 + q00 - (1L << 30); 
    if (t2 < 0) { 
        if (values[0] >= 0) 
            values[0] = 180.f - values[0]; 
        else 
            values[0] = -180.f - values[0]; 
    } 
 
    /* X component of the Xbody axis in World frame */ 
    t1 = q11 + q00 - (1L << 30); 
    /* Y component of the Xbody axis in World frame */ 
    t2 = q12 + q03; 
    /* Z component of the Xbody axis in World frame */ 
    t3 = q13 - q02; 
 
    values[1] = 
        (atan2f((float)(q33 + q00 - (1L << 30)), (float)(q13 - q02)) * 
          180.f / (float) M_PI - 90); 
    if (values[1] >= 90) 
        values[1] = 180 - values[1]; 
 
    if (values[1] < -90) 
        values[1] = -180 - values[1]; 
    data[0] = (long)(values[0] * 65536.f); 
    data[1] = (long)(values[1] * 65536.f); 
    data[2] = (long)(values[2] * 65536.f); 
 
    accuracy[0] = eMPL_out.quat_accuracy; 
    timestamp[0] = eMPL_out.nine_axis_timestamp; 
    return eMPL_out.nine_axis_status; 
} 
 
/** 
 *  @brief      Body-to-world frame rotation matrix. 
 *  @param[out] data        Rotation matrix, 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_rot_mat(long *data, int8_t *accuracy, inv_time_t *timestamp) 
{ 
    inv_quaternion_to_rotation(eMPL_out.quat, data); 
    accuracy[0] = eMPL_out.quat_accuracy; 
    timestamp[0] = eMPL_out.nine_axis_timestamp; 
    return eMPL_out.nine_axis_status; 
} 
 
static inv_error_t inv_generate_eMPL_outputs 
    (struct inv_sensor_cal_t *sensor_cal) 
{ 
    int use_sensor; 
    long sr = 1000; 
    inv_get_quaternion_set(eMPL_out.quat, &eMPL_out.quat_accuracy, &eMPL_out.nine_axis_timestamp); 
    eMPL_out.gyro_status = sensor_cal->gyro.status; 
    eMPL_out.accel_status = sensor_cal->accel.status; 
    eMPL_out.compass_status = sensor_cal->compass.status; 
     
    /* Find the highest sample rate and tie sensor fusion timestamps to that one. */ 
    if (sensor_cal->gyro.status & INV_SENSOR_ON) { 
        sr = sensor_cal->gyro.sample_rate_ms; 
        use_sensor = 0; 
    } 
    if ((sensor_cal->accel.status & INV_SENSOR_ON) && (sr > sensor_cal->accel.sample_rate_ms)) { 
        sr = sensor_cal->accel.sample_rate_ms; 
        use_sensor = 1; 
    } 
    if ((sensor_cal->compass.status & INV_SENSOR_ON) && (sr > sensor_cal->compass.sample_rate_ms)) { 
        sr = sensor_cal->compass.sample_rate_ms; 
        use_sensor = 2; 
    } 
    if ((sensor_cal->quat.status & INV_SENSOR_ON) && (sr > sensor_cal->quat.sample_rate_ms)) { 
        sr = sensor_cal->quat.sample_rate_ms; 
        use_sensor = 3; 
    } 
 
    switch (use_sensor) { 
    default: 
    case 0: 
        eMPL_out.nine_axis_status = (sensor_cal->gyro.status & INV_NEW_DATA) ? 1 : 0; 
        eMPL_out.nine_axis_timestamp = sensor_cal->gyro.timestamp; 
        break; 
    case 1: 
        eMPL_out.nine_axis_status = (sensor_cal->accel.status & INV_NEW_DATA) ? 1 : 0; 
        eMPL_out.nine_axis_timestamp = sensor_cal->accel.timestamp; 
        break; 
    case 2: 
        eMPL_out.nine_axis_status = (sensor_cal->compass.status & INV_NEW_DATA) ? 1 : 0; 
        eMPL_out.nine_axis_timestamp = sensor_cal->compass.timestamp; 
        break; 
    case 3: 
        eMPL_out.nine_axis_status = (sensor_cal->quat.status & INV_NEW_DATA) ? 1 : 0; 
        eMPL_out.nine_axis_timestamp = sensor_cal->quat.timestamp; 
        break; 
    } 
     
     
    return INV_SUCCESS; 
} 
 
static inv_error_t inv_start_eMPL_outputs(void) 
{ 
    return inv_register_data_cb(inv_generate_eMPL_outputs, 
        INV_PRIORITY_HAL_OUTPUTS, INV_GYRO_NEW | INV_ACCEL_NEW | INV_MAG_NEW); 
} 
 
static inv_error_t inv_stop_eMPL_outputs(void) 
{ 
    return inv_unregister_data_cb(inv_generate_eMPL_outputs); 
} 
 
static inv_error_t inv_init_eMPL_outputs(void) 
{ 
    memset(&eMPL_out, 0, sizeof(eMPL_out)); 
    return INV_SUCCESS; 
} 
 
/** 
 *  @brief  Turns on creation and storage of HAL type results. 
 */ 
inv_error_t inv_enable_eMPL_outputs(void) 
{ 
    inv_error_t result; 
    result = inv_init_eMPL_outputs(); 
    if (result) 
        return result; 
    return inv_register_mpl_start_notification(inv_start_eMPL_outputs); 
} 
 
/** 
 *  @brief  Turns off creation and storage of HAL type results. 
 */ 
inv_error_t inv_disable_eMPL_outputs(void) 
{ 
    inv_stop_eMPL_outputs(); 
    return inv_unregister_mpl_start_notification(inv_start_eMPL_outputs); 
} 
 
/** 
 * @} 
 */