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 */ 28 struct 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 */ 38 static 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 */ 49 void 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 */ 64 void 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 */ 91 void 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 */ 107 void 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 */ 125 void 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 */ 141 void 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 */ 160 void 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 */ 207 void 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 */ 226 void 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 */ 251 void 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 */ 269 void 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 */ 310 void 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. */ 331 inv_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. */ 340 inv_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().*/ 347 inv_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 */ 357 inv_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 */ 365 inv_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 */ 376 inv_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
