1/** 2 * @defgroup HAL_Outputs 3 * @brief Motion Library - HAL Outputs 4 * Sets up common outputs for HAL 5 * 6 * @{ 7 * @file datalogger_outputs.c 8 * @brief Windows 8 HAL outputs. 9 */ 10 11#include <string.h> 12 13#include "datalogger_outputs.h" 14#include "ml_math_func.h" 15#include "mlmath.h" 16#include "start_manager.h" 17#include "data_builder.h" 18#include "results_holder.h" 19 20/* 21 Defines 22*/ 23#define ACCEL_CONVERSION (0.000149637603759766f) 24 25/* 26 Types 27*/ 28struct datalogger_output_s { 29 int quat_accuracy; 30 inv_time_t quat_timestamp; 31 long quat[4]; 32 struct inv_sensor_cal_t sc; 33}; 34 35/* 36 Globals and Statics 37*/ 38static struct datalogger_output_s dl_out; 39 40/* 41 Functions 42*/ 43 44/** 45 * Raw (uncompensated) angular velocity (LSB) in chip frame. 46 * @param[out] values raw angular velocity in LSB. 47 * @param[out] timestamp Time when sensor was sampled. 48 */ 49void inv_get_sensor_type_gyro_raw_short(short *values, inv_time_t *timestamp) 50{ 51 struct inv_single_sensor_t *pg = &dl_out.sc.gyro; 52 53 if (values) 54 memcpy(values, &pg->raw, sizeof(short) * 3); 55 if (timestamp) 56 *timestamp = pg->timestamp; 57} 58 59/** 60 * Raw (uncompensated) angular velocity (degrees per second) in body frame. 61 * @param[out] values raw angular velocity in dps. 62 * @param[out] timestamp Time when sensor was sampled. 63 */ 64void inv_get_sensor_type_gyro_raw_body_float(float *values, 65 inv_time_t *timestamp) 66{ 67 struct inv_single_sensor_t *pg = &dl_out.sc.gyro; 68 long raw[3]; 69 long raw_body[3]; 70 71 raw[0] = (long) pg->raw[0] * (1L << 16); 72 raw[1] = (long) pg->raw[1] * (1L << 16); 73 raw[2] = (long) pg->raw[2] * (1L << 16); 74 inv_convert_to_body_with_scale(pg->orientation, pg->sensitivity, 75 raw, raw_body); 76 if (values) { 77 values[0] = inv_q16_to_float(raw_body[0]); 78 values[1] = inv_q16_to_float(raw_body[1]); 79 values[2] = inv_q16_to_float(raw_body[2]); 80 } 81 if (timestamp) 82 *timestamp = pg->timestamp; 83} 84 85/** 86 * Angular velocity (degrees per second) in body frame. 87 * @param[out] values Angular velocity in dps. 88 * @param[out] accuracy 0 (uncalibrated) to 3 (most accurate). 89 * @param[out] timestamp Time when sensor was sampled. 90 */ 91void inv_get_sensor_type_gyro_float(float *values, int8_t *accuracy, 92 inv_time_t *timestamp) 93{ 94 long gyro[3]; 95 inv_get_gyro_set(gyro, accuracy, timestamp); 96 97 values[0] = (float)gyro[0] / 65536.f; 98 values[1] = (float)gyro[1] / 65536.f; 99 values[2] = (float)gyro[2] / 65536.f; 100} 101 102/** 103 * Raw (uncompensated) acceleration (LSB) in chip frame. 104 * @param[out] values raw acceleration in LSB. 105 * @param[out] timestamp Time when sensor was sampled. 106 */ 107void inv_get_sensor_type_accel_raw_short(short *values, inv_time_t *timestamp) 108{ 109 struct inv_single_sensor_t *pa = &dl_out.sc.accel; 110 111 if (values) 112 memcpy(values, &pa->raw, sizeof(short) * 3); 113 if (timestamp) 114 *timestamp = pa->timestamp; 115} 116 117/** 118 * Acceleration (g's) in body frame. 119 * Microsoft defines gravity as positive acceleration pointing towards the 120 * Earth. 121 * @param[out] values Acceleration in g's. 122 * @param[out] accuracy 0 (uncalibrated) to 3 (most accurate). 123 * @param[out] timestamp Time when sensor was sampled. 124 */ 125void inv_get_sensor_type_accel_float(float *values, int8_t *accuracy, 126 inv_time_t *timestamp) 127{ 128 long accel[3]; 129 inv_get_accel_set(accel, accuracy, timestamp); 130 131 values[0] = (float) -accel[0] / 65536.f; 132 values[1] = (float) -accel[1] / 65536.f; 133 values[2] = (float) -accel[2] / 65536.f; 134} 135 136/** 137 * Raw (uncompensated) compass magnetic field (LSB) in chip frame. 138 * @param[out] values raw magnetic field in LSB. 139 * @param[out] timestamp Time when sensor was sampled. 140 */ 141void inv_get_sensor_type_compass_raw_short(short *values, inv_time_t *timestamp) 142{ 143 struct inv_single_sensor_t *pc = &dl_out.sc.compass; 144 145 if (values) 146 memcpy(values, &pc->raw, sizeof(short) * 3); 147 if (timestamp) 148 *timestamp = pc->timestamp; 149} 150 151/** 152 * Magnetic heading/field strength in body frame. 153 * TODO: No difference between mag_north and true_north yet. 154 * @param[out] mag_north Heading relative to magnetic north in degrees. 155 * @param[out] true_north Heading relative to true north in degrees. 156 * @param[out] values Field strength in milligauss. 157 * @param[out] accuracy 0 (uncalibrated) to 3 (most accurate). 158 * @param[out] timestamp Time when sensor was sampled. 159 */ 160void inv_get_sensor_type_compass_float(float *mag_north, float *true_north, 161 float *values, int8_t *accuracy, inv_time_t *timestamp) 162{ 163 long compass[3]; 164 long q00, q12, q22, q03, t1, t2; 165 166 /* 1 uT = 10 milligauss. */ 167#define COMPASS_CONVERSION (10 / 65536.f) 168 inv_get_compass_set(compass, accuracy, timestamp); 169 if (values) { 170 values[0] = (float)compass[0]*COMPASS_CONVERSION; 171 values[1] = (float)compass[1]*COMPASS_CONVERSION; 172 values[2] = (float)compass[2]*COMPASS_CONVERSION; 173 } 174 175 /* TODO: Stolen from euler angle computation. Calculate this only once per 176 * callback. 177 */ 178 q00 = inv_q29_mult(dl_out.quat[0], dl_out.quat[0]); 179 q12 = inv_q29_mult(dl_out.quat[1], dl_out.quat[2]); 180 q22 = inv_q29_mult(dl_out.quat[2], dl_out.quat[2]); 181 q03 = inv_q29_mult(dl_out.quat[0], dl_out.quat[3]); 182 t1 = q12 - q03; 183 t2 = q22 + q00 - (1L << 30); 184 if (mag_north) { 185 *mag_north = atan2f((float) t1, (float) t2) * 180.f / (float) M_PI; 186 if (*mag_north < 0) 187 *mag_north += 360; 188 } 189 if (true_north) { 190 if (!mag_north) { 191 *true_north = atan2f((float) t1, (float) t2) * 180.f / (float) M_PI; 192 if (*true_north < 0) 193 *true_north += 360; 194 } else { 195 *true_north = *mag_north; 196 } 197 } 198} 199 200#if 0 201// put it back when we handle raw temperature 202/** 203 * Raw temperature (LSB). 204 * @param[out] value raw temperature in LSB (1 element). 205 * @param[out] timestamp Time when sensor was sampled. 206 */ 207void inv_get_sensor_type_temp_raw_short(short *value, inv_time_t *timestamp) 208{ 209 struct inv_single_sensor_t *pt = &dl_out.sc.temp; 210 if (value) { 211 /* no raw temperature, temperature is only handled calibrated 212 *value = pt->raw[0]; 213 */ 214 *value = pt->calibrated[0]; 215 } 216 if (timestamp) 217 *timestamp = pt->timestamp; 218} 219#endif 220 221/** 222 * Temperature (degree C). 223 * @param[out] values Temperature in degrees C. 224 * @param[out] timestamp Time when sensor was sampled. 225 */ 226void inv_get_sensor_type_temperature_float(float *value, inv_time_t *timestamp) 227{ 228 struct inv_single_sensor_t *pt = &dl_out.sc.temp; 229 long ltemp; 230 if (timestamp) 231 *timestamp = pt->timestamp; 232 if (value) { 233 /* no raw temperature, temperature is only handled calibrated 234 ltemp = pt->raw[0]; 235 */ 236 ltemp = pt->calibrated[0]; 237 *value = (float) ltemp / (1L << 16); 238 } 239} 240 241/** 242 * Quaternion in body frame. 243 * @e inv_get_sensor_type_quaternion_float outputs the data in the following 244 * order: X, Y, Z, W. 245 * TODO: Windows expects a discontinuity at 180 degree rotations. Will our 246 * convention be ok? 247 * @param[out] values Quaternion normalized to one. 248 * @param[out] accuracy 0 (uncalibrated) to 3 (most accurate). 249 * @param[out] timestamp Time when sensor was sampled. 250 */ 251void inv_get_sensor_type_quat_float(float *values, int *accuracy, 252 inv_time_t *timestamp) 253{ 254 values[0] = dl_out.quat[0] / 1073741824.f; 255 values[1] = dl_out.quat[1] / 1073741824.f; 256 values[2] = dl_out.quat[2] / 1073741824.f; 257 values[3] = dl_out.quat[3] / 1073741824.f; 258 accuracy[0] = dl_out.quat_accuracy; 259 timestamp[0] = dl_out.quat_timestamp; 260} 261 262/** Gravity vector (gee) in body frame. 263* @param[out] values Gravity vector in body frame, length 3, (gee) 264* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, 265 while 3 is most accurate. 266* @param[out] timestamp The timestamp for this sensor. Derived from the 267 timestamp sent to inv_build_accel(). 268*/ 269void inv_get_sensor_type_gravity_float(float *values, int *accuracy, 270 inv_time_t * timestamp) 271{ 272 struct inv_single_sensor_t *pa = &dl_out.sc.accel; 273 274 if (values) { 275 long lgravity[3]; 276 (void)inv_get_gravity(lgravity); 277 values[0] = (float) lgravity[0] / (1L << 16); 278 values[1] = (float) lgravity[1] / (1L << 16); 279 values[2] = (float) lgravity[2] / (1L << 16); 280 } 281 if (accuracy) 282 *accuracy = pa->accuracy; 283 if (timestamp) 284 *timestamp = pa->timestamp; 285} 286 287/** 288* This corresponds to Sensor.TYPE_ROTATION_VECTOR. 289* The rotation vector represents the orientation of the device as a combination 290* of an angle and an axis, in which the device has rotated through an angle @f$\theta@f$ 291* around an axis {x, y, z}. <br> 292* The three elements of the rotation vector are 293* {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 294* vector is equal to sin(@f$\theta@f$/2), and the direction of the rotation vector is 295* equal to the direction of the axis of rotation. 296* 297* The three elements of the rotation vector are equal to the last three components of a unit quaternion 298* {x*sin(@f$\theta@f$/2), y*sin(@f$\theta@f$/2), z*sin(@f$\theta@f$/2)>. 299* 300* Elements of the rotation vector are unitless. The x,y and z axis are defined in the same way as the acceleration sensor. 301* The reference coordinate system is defined as a direct orthonormal basis, where: 302 303 -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). 304 -Y is tangential to the ground at the device's current location and points towards the magnetic North Pole. 305 -Z points towards the sky and is perpendicular to the ground. 306* @param[out] values 307* @param[out] accuracy Accuracy 0 to 3, 3 = most accurate 308* @param[out] timestamp Timestamp. In (ns) for Android. 309*/ 310void inv_get_sensor_type_rotation_vector_float(float *values, int *accuracy, 311 inv_time_t * timestamp) 312{ 313 if (accuracy) 314 *accuracy = dl_out.quat_accuracy; 315 if (timestamp) 316 *timestamp = dl_out.quat_timestamp; 317 if (values) { 318 if (dl_out.quat[0] >= 0) { 319 values[0] = dl_out.quat[1] * INV_TWO_POWER_NEG_30; 320 values[1] = dl_out.quat[2] * INV_TWO_POWER_NEG_30; 321 values[2] = dl_out.quat[3] * INV_TWO_POWER_NEG_30; 322 } else { 323 values[0] = -dl_out.quat[1] * INV_TWO_POWER_NEG_30; 324 values[1] = -dl_out.quat[2] * INV_TWO_POWER_NEG_30; 325 values[2] = -dl_out.quat[3] * INV_TWO_POWER_NEG_30; 326 } 327 } 328} 329 330/** Main callback to generate HAL outputs. Typically not called by library users. */ 331inv_error_t inv_generate_datalogger_outputs(struct inv_sensor_cal_t *sensor_cal) 332{ 333 memcpy(&dl_out.sc, sensor_cal, sizeof(struct inv_sensor_cal_t)); 334 inv_get_quaternion_set(dl_out.quat, &dl_out.quat_accuracy, 335 &dl_out.quat_timestamp); 336 return INV_SUCCESS; 337} 338 339/** Turns off generation of HAL outputs. */ 340inv_error_t inv_stop_datalogger_outputs(void) 341{ 342 return inv_unregister_data_cb(inv_generate_datalogger_outputs); 343} 344 345/** Turns on generation of HAL outputs. This should be called after inv_stop_dl_outputs() 346* to turn generation of HAL outputs back on. It is automatically called by inv_enable_dl_outputs().*/ 347inv_error_t inv_start_datalogger_outputs(void) 348{ 349 return inv_register_data_cb(inv_generate_datalogger_outputs, 350 INV_PRIORITY_HAL_OUTPUTS, INV_GYRO_NEW | INV_ACCEL_NEW | INV_MAG_NEW); 351} 352 353/** Initializes hal outputs class. This is called automatically by the 354* enable function. It may be called any time the feature is enabled, but 355* is typically not needed to be called by outside callers. 356*/ 357inv_error_t inv_init_datalogger_outputs(void) 358{ 359 memset(&dl_out, 0, sizeof(dl_out)); 360 return INV_SUCCESS; 361} 362 363/** Turns on creation and storage of HAL type results. 364*/ 365inv_error_t inv_enable_datalogger_outputs(void) 366{ 367 inv_error_t result; 368 result = inv_init_datalogger_outputs(); 369 if (result) 370 return result; 371 return inv_register_mpl_start_notification(inv_start_datalogger_outputs); 372} 373 374/** Turns off creation and storage of HAL type results. 375*/ 376inv_error_t inv_disable_datalogger_outputs(void) 377{ 378 inv_stop_datalogger_outputs(); 379 return inv_unregister_mpl_start_notification(inv_start_datalogger_outputs); 380} 381 382/** 383 * @} 384 */ 385