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