| /** |
| * @defgroup HAL_Outputs |
| * @brief Motion Library - HAL Outputs |
| * Sets up common outputs for HAL |
| * |
| * @{ |
| * @file datalogger_outputs.c |
| * @brief Windows 8 HAL outputs. |
| */ |
| |
| #include <string.h> |
| |
| #include "datalogger_outputs.h" |
| #include "ml_math_func.h" |
| #include "mlmath.h" |
| #include "start_manager.h" |
| #include "data_builder.h" |
| #include "results_holder.h" |
| |
| /* |
| Defines |
| */ |
| #define ACCEL_CONVERSION (0.000149637603759766f) |
| |
| /* |
| Types |
| */ |
| struct datalogger_output_s { |
| int quat_accuracy; |
| inv_time_t quat_timestamp; |
| long quat[4]; |
| struct inv_sensor_cal_t sc; |
| }; |
| |
| /* |
| Globals and Statics |
| */ |
| static struct datalogger_output_s dl_out; |
| |
| /* |
| Functions |
| */ |
| |
| /** |
| * Raw (uncompensated) angular velocity (LSB) in chip frame. |
| * @param[out] values raw angular velocity in LSB. |
| * @param[out] timestamp Time when sensor was sampled. |
| */ |
| void inv_get_sensor_type_gyro_raw_short(short *values, inv_time_t *timestamp) |
| { |
| struct inv_single_sensor_t *pg = &dl_out.sc.gyro; |
| |
| if (values) |
| memcpy(values, &pg->raw, sizeof(short) * 3); |
| if (timestamp) |
| *timestamp = pg->timestamp; |
| } |
| |
| /** |
| * Raw (uncompensated) angular velocity (degrees per second) in body frame. |
| * @param[out] values raw angular velocity in dps. |
| * @param[out] timestamp Time when sensor was sampled. |
| */ |
| void inv_get_sensor_type_gyro_raw_body_float(float *values, |
| inv_time_t *timestamp) |
| { |
| struct inv_single_sensor_t *pg = &dl_out.sc.gyro; |
| long raw[3]; |
| long raw_body[3]; |
| |
| raw[0] = (long) pg->raw[0] * (1L << 16); |
| raw[1] = (long) pg->raw[1] * (1L << 16); |
| raw[2] = (long) pg->raw[2] * (1L << 16); |
| inv_convert_to_body_with_scale(pg->orientation, pg->sensitivity, |
| raw, raw_body); |
| if (values) { |
| values[0] = inv_q16_to_float(raw_body[0]); |
| values[1] = inv_q16_to_float(raw_body[1]); |
| values[2] = inv_q16_to_float(raw_body[2]); |
| } |
| if (timestamp) |
| *timestamp = pg->timestamp; |
| } |
| |
| /** |
| * Angular velocity (degrees per second) in body frame. |
| * @param[out] values Angular velocity in dps. |
| * @param[out] accuracy 0 (uncalibrated) to 3 (most accurate). |
| * @param[out] timestamp Time when sensor was sampled. |
| */ |
| void inv_get_sensor_type_gyro_float(float *values, int8_t *accuracy, |
| inv_time_t *timestamp) |
| { |
| long gyro[3]; |
| inv_get_gyro_set(gyro, accuracy, timestamp); |
| |
| values[0] = (float)gyro[0] / 65536.f; |
| values[1] = (float)gyro[1] / 65536.f; |
| values[2] = (float)gyro[2] / 65536.f; |
| } |
| |
| /** |
| * Raw (uncompensated) acceleration (LSB) in chip frame. |
| * @param[out] values raw acceleration in LSB. |
| * @param[out] timestamp Time when sensor was sampled. |
| */ |
| void inv_get_sensor_type_accel_raw_short(short *values, inv_time_t *timestamp) |
| { |
| struct inv_single_sensor_t *pa = &dl_out.sc.accel; |
| |
| if (values) |
| memcpy(values, &pa->raw, sizeof(short) * 3); |
| if (timestamp) |
| *timestamp = pa->timestamp; |
| } |
| |
| /** |
| * Acceleration (g's) in body frame. |
| * Microsoft defines gravity as positive acceleration pointing towards the |
| * Earth. |
| * @param[out] values Acceleration in g's. |
| * @param[out] accuracy 0 (uncalibrated) to 3 (most accurate). |
| * @param[out] timestamp Time when sensor was sampled. |
| */ |
| void inv_get_sensor_type_accel_float(float *values, int8_t *accuracy, |
| inv_time_t *timestamp) |
| { |
| long accel[3]; |
| inv_get_accel_set(accel, accuracy, timestamp); |
| |
| values[0] = (float) -accel[0] / 65536.f; |
| values[1] = (float) -accel[1] / 65536.f; |
| values[2] = (float) -accel[2] / 65536.f; |
| } |
| |
| /** |
| * Raw (uncompensated) compass magnetic field (LSB) in chip frame. |
| * @param[out] values raw magnetic field in LSB. |
| * @param[out] timestamp Time when sensor was sampled. |
| */ |
| void inv_get_sensor_type_compass_raw_short(short *values, inv_time_t *timestamp) |
| { |
| struct inv_single_sensor_t *pc = &dl_out.sc.compass; |
| |
| if (values) |
| memcpy(values, &pc->raw, sizeof(short) * 3); |
| if (timestamp) |
| *timestamp = pc->timestamp; |
| } |
| |
| /** |
| * Magnetic heading/field strength in body frame. |
| * TODO: No difference between mag_north and true_north yet. |
| * @param[out] mag_north Heading relative to magnetic north in degrees. |
| * @param[out] true_north Heading relative to true north in degrees. |
| * @param[out] values Field strength in milligauss. |
| * @param[out] accuracy 0 (uncalibrated) to 3 (most accurate). |
| * @param[out] timestamp Time when sensor was sampled. |
| */ |
| void inv_get_sensor_type_compass_float(float *mag_north, float *true_north, |
| float *values, int8_t *accuracy, inv_time_t *timestamp) |
| { |
| long compass[3]; |
| long q00, q12, q22, q03, t1, t2; |
| |
| /* 1 uT = 10 milligauss. */ |
| #define COMPASS_CONVERSION (10 / 65536.f) |
| inv_get_compass_set(compass, accuracy, timestamp); |
| if (values) { |
| values[0] = (float)compass[0]*COMPASS_CONVERSION; |
| values[1] = (float)compass[1]*COMPASS_CONVERSION; |
| values[2] = (float)compass[2]*COMPASS_CONVERSION; |
| } |
| |
| /* TODO: Stolen from euler angle computation. Calculate this only once per |
| * callback. |
| */ |
| q00 = inv_q29_mult(dl_out.quat[0], dl_out.quat[0]); |
| q12 = inv_q29_mult(dl_out.quat[1], dl_out.quat[2]); |
| q22 = inv_q29_mult(dl_out.quat[2], dl_out.quat[2]); |
| q03 = inv_q29_mult(dl_out.quat[0], dl_out.quat[3]); |
| t1 = q12 - q03; |
| t2 = q22 + q00 - (1L << 30); |
| if (mag_north) { |
| *mag_north = atan2f((float) t1, (float) t2) * 180.f / (float) M_PI; |
| if (*mag_north < 0) |
| *mag_north += 360; |
| } |
| if (true_north) { |
| if (!mag_north) { |
| *true_north = atan2f((float) t1, (float) t2) * 180.f / (float) M_PI; |
| if (*true_north < 0) |
| *true_north += 360; |
| } else { |
| *true_north = *mag_north; |
| } |
| } |
| } |
| |
| #if 0 |
| // put it back when we handle raw temperature |
| /** |
| * Raw temperature (LSB). |
| * @param[out] value raw temperature in LSB (1 element). |
| * @param[out] timestamp Time when sensor was sampled. |
| */ |
| void inv_get_sensor_type_temp_raw_short(short *value, inv_time_t *timestamp) |
| { |
| struct inv_single_sensor_t *pt = &dl_out.sc.temp; |
| if (value) { |
| /* no raw temperature, temperature is only handled calibrated |
| *value = pt->raw[0]; |
| */ |
| *value = pt->calibrated[0]; |
| } |
| if (timestamp) |
| *timestamp = pt->timestamp; |
| } |
| #endif |
| |
| /** |
| * Temperature (degree C). |
| * @param[out] values Temperature in degrees C. |
| * @param[out] timestamp Time when sensor was sampled. |
| */ |
| void inv_get_sensor_type_temperature_float(float *value, inv_time_t *timestamp) |
| { |
| struct inv_single_sensor_t *pt = &dl_out.sc.temp; |
| long ltemp; |
| if (timestamp) |
| *timestamp = pt->timestamp; |
| if (value) { |
| /* no raw temperature, temperature is only handled calibrated |
| ltemp = pt->raw[0]; |
| */ |
| ltemp = pt->calibrated[0]; |
| *value = (float) ltemp / (1L << 16); |
| } |
| } |
| |
| /** |
| * Quaternion in body frame. |
| * @e inv_get_sensor_type_quaternion_float outputs the data in the following |
| * order: X, Y, Z, W. |
| * TODO: Windows expects a discontinuity at 180 degree rotations. Will our |
| * convention be ok? |
| * @param[out] values Quaternion normalized to one. |
| * @param[out] accuracy 0 (uncalibrated) to 3 (most accurate). |
| * @param[out] timestamp Time when sensor was sampled. |
| */ |
| void inv_get_sensor_type_quat_float(float *values, int *accuracy, |
| inv_time_t *timestamp) |
| { |
| values[0] = dl_out.quat[0] / 1073741824.f; |
| values[1] = dl_out.quat[1] / 1073741824.f; |
| values[2] = dl_out.quat[2] / 1073741824.f; |
| values[3] = dl_out.quat[3] / 1073741824.f; |
| accuracy[0] = dl_out.quat_accuracy; |
| timestamp[0] = dl_out.quat_timestamp; |
| } |
| |
| /** Gravity vector (gee) in body frame. |
| * @param[out] values Gravity vector in body frame, length 3, (gee) |
| * @param[out] accuracy Accuracy of the measurment, 0 is least accurate, |
| while 3 is most accurate. |
| * @param[out] timestamp The timestamp for this sensor. Derived from the |
| timestamp sent to inv_build_accel(). |
| */ |
| void inv_get_sensor_type_gravity_float(float *values, int *accuracy, |
| inv_time_t * timestamp) |
| { |
| struct inv_single_sensor_t *pa = &dl_out.sc.accel; |
| |
| if (values) { |
| long lgravity[3]; |
| (void)inv_get_gravity(lgravity); |
| values[0] = (float) lgravity[0] / (1L << 16); |
| values[1] = (float) lgravity[1] / (1L << 16); |
| values[2] = (float) lgravity[2] / (1L << 16); |
| } |
| if (accuracy) |
| *accuracy = pa->accuracy; |
| if (timestamp) |
| *timestamp = pa->timestamp; |
| } |
| |
| /** |
| * This corresponds to Sensor.TYPE_ROTATION_VECTOR. |
| * The rotation vector represents the orientation of the device as a combination |
| * of an angle and an axis, in which the device has rotated through an angle @f$\theta@f$ |
| * around an axis {x, y, z}. <br> |
| * The three elements of the rotation vector are |
| * {x*sin(@f$\theta@f$/2), y*sin(@f$\theta@f$/2), z*sin(@f$\theta@f$/2)}, such that the magnitude of the rotation |
| * vector is equal to sin(@f$\theta@f$/2), and the direction of the rotation vector is |
| * equal to the direction of the axis of rotation. |
| * |
| * The three elements of the rotation vector are equal to the last three components of a unit quaternion |
| * {x*sin(@f$\theta@f$/2), y*sin(@f$\theta@f$/2), z*sin(@f$\theta@f$/2)>. |
| * |
| * Elements of the rotation vector are unitless. The x,y and z axis are defined in the same way as the acceleration sensor. |
| * The reference coordinate system is defined as a direct orthonormal basis, where: |
| |
| -X is defined as the vector product Y.Z (It is tangential to the ground at the device's current location and roughly points East). |
| -Y is tangential to the ground at the device's current location and points towards the magnetic North Pole. |
| -Z points towards the sky and is perpendicular to the ground. |
| * @param[out] values |
| * @param[out] accuracy Accuracy 0 to 3, 3 = most accurate |
| * @param[out] timestamp Timestamp. In (ns) for Android. |
| */ |
| void inv_get_sensor_type_rotation_vector_float(float *values, int *accuracy, |
| inv_time_t * timestamp) |
| { |
| if (accuracy) |
| *accuracy = dl_out.quat_accuracy; |
| if (timestamp) |
| *timestamp = dl_out.quat_timestamp; |
| if (values) { |
| if (dl_out.quat[0] >= 0) { |
| values[0] = dl_out.quat[1] * INV_TWO_POWER_NEG_30; |
| values[1] = dl_out.quat[2] * INV_TWO_POWER_NEG_30; |
| values[2] = dl_out.quat[3] * INV_TWO_POWER_NEG_30; |
| } else { |
| values[0] = -dl_out.quat[1] * INV_TWO_POWER_NEG_30; |
| values[1] = -dl_out.quat[2] * INV_TWO_POWER_NEG_30; |
| values[2] = -dl_out.quat[3] * INV_TWO_POWER_NEG_30; |
| } |
| } |
| } |
| |
| /** Main callback to generate HAL outputs. Typically not called by library users. */ |
| inv_error_t inv_generate_datalogger_outputs(struct inv_sensor_cal_t *sensor_cal) |
| { |
| memcpy(&dl_out.sc, sensor_cal, sizeof(struct inv_sensor_cal_t)); |
| inv_get_quaternion_set(dl_out.quat, &dl_out.quat_accuracy, |
| &dl_out.quat_timestamp); |
| return INV_SUCCESS; |
| } |
| |
| /** Turns off generation of HAL outputs. */ |
| inv_error_t inv_stop_datalogger_outputs(void) |
| { |
| return inv_unregister_data_cb(inv_generate_datalogger_outputs); |
| } |
| |
| /** Turns on generation of HAL outputs. This should be called after inv_stop_dl_outputs() |
| * to turn generation of HAL outputs back on. It is automatically called by inv_enable_dl_outputs().*/ |
| inv_error_t inv_start_datalogger_outputs(void) |
| { |
| return inv_register_data_cb(inv_generate_datalogger_outputs, |
| INV_PRIORITY_HAL_OUTPUTS, INV_GYRO_NEW | INV_ACCEL_NEW | INV_MAG_NEW); |
| } |
| |
| /** Initializes hal outputs class. This is called automatically by the |
| * enable function. It may be called any time the feature is enabled, but |
| * is typically not needed to be called by outside callers. |
| */ |
| inv_error_t inv_init_datalogger_outputs(void) |
| { |
| memset(&dl_out, 0, sizeof(dl_out)); |
| return INV_SUCCESS; |
| } |
| |
| /** Turns on creation and storage of HAL type results. |
| */ |
| inv_error_t inv_enable_datalogger_outputs(void) |
| { |
| inv_error_t result; |
| result = inv_init_datalogger_outputs(); |
| if (result) |
| return result; |
| return inv_register_mpl_start_notification(inv_start_datalogger_outputs); |
| } |
| |
| /** Turns off creation and storage of HAL type results. |
| */ |
| inv_error_t inv_disable_datalogger_outputs(void) |
| { |
| inv_stop_datalogger_outputs(); |
| return inv_unregister_mpl_start_notification(inv_start_datalogger_outputs); |
| } |
| |
| /** |
| * @} |
| */ |
