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 */
inv_get_sensor_type_gyro_raw_short(short * values,inv_time_t * timestamp)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 */
inv_get_sensor_type_gyro_raw_body_float(float * values,inv_time_t * timestamp)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 */
inv_get_sensor_type_gyro_float(float * values,int8_t * accuracy,inv_time_t * timestamp)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 */
inv_get_sensor_type_accel_raw_short(short * values,inv_time_t * timestamp)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 */
inv_get_sensor_type_accel_float(float * values,int8_t * accuracy,inv_time_t * timestamp)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 */
inv_get_sensor_type_compass_raw_short(short * values,inv_time_t * timestamp)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 */
inv_get_sensor_type_compass_float(float * mag_north,float * true_north,float * values,int8_t * accuracy,inv_time_t * timestamp)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 */
inv_get_sensor_type_temperature_float(float * value,inv_time_t * timestamp)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 */
inv_get_sensor_type_quat_float(float * values,int * accuracy,inv_time_t * timestamp)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 */
inv_get_sensor_type_gravity_float(float * values,int * accuracy,inv_time_t * timestamp)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 */
inv_get_sensor_type_rotation_vector_float(float * values,int * accuracy,inv_time_t * timestamp)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. */
inv_generate_datalogger_outputs(struct inv_sensor_cal_t * sensor_cal)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. */
inv_stop_datalogger_outputs(void)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().*/
inv_start_datalogger_outputs(void)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 */
inv_init_datalogger_outputs(void)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 */
inv_enable_datalogger_outputs(void)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 */
inv_disable_datalogger_outputs(void)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