diff options
| -rw-r--r-- | board/samus/board.c | 24 | ||||
| -rw-r--r-- | common/lid_angle.c | 8 | ||||
| -rw-r--r-- | common/math_util.c | 168 | ||||
| -rw-r--r-- | common/motion_lid.c | 63 | ||||
| -rw-r--r-- | common/motion_sense.c | 10 | ||||
| -rw-r--r-- | include/lid_angle.h | 2 | ||||
| -rw-r--r-- | include/math_util.h | 86 | ||||
| -rw-r--r-- | include/motion_lid.h | 7 | ||||
| -rw-r--r-- | test/math_util.c | 2 | ||||
| -rw-r--r-- | test/motion_lid.c | 13 |
10 files changed, 258 insertions, 125 deletions
diff --git a/board/samus/board.c b/board/samus/board.c index 704b3102b8..9dfcd1a8b4 100644 --- a/board/samus/board.c +++ b/board/samus/board.c @@ -249,15 +249,15 @@ struct kxcj9_data g_kxcj9_data; /* Four Motion sensors */ /* Matrix to rotate accelrator into standard reference frame */ const matrix_3x3_t base_standard_ref = { - {-1, 0, 0}, - { 0, -1, 0}, - { 0, 0, -1} + {FLOAT_TO_FP(-1), 0, 0}, + { 0, FLOAT_TO_FP(-1), 0}, + { 0, 0, FLOAT_TO_FP(-1)} }; const matrix_3x3_t lid_standard_ref = { - { 0, 1, 0}, - {-1, 0, 0}, - { 0, 0, -1} + { 0, FLOAT_TO_FP(1), 0}, + {FLOAT_TO_FP(-1), 0, 0}, + { 0, 0, FLOAT_TO_FP(-1)} }; struct motion_sensor_t motion_sensors[] = { @@ -289,14 +289,14 @@ const unsigned int motion_sensor_count = ARRAY_SIZE(motion_sensors); const struct accel_orientation acc_orient = { /* Hinge aligns with y axis. */ .rot_hinge_90 = { - { 1, 0, 0}, - { 0, 1, 0}, - { 0, 0, 1} + { FLOAT_TO_FP(1), 0, 0}, + { 0, FLOAT_TO_FP(1), 0}, + { 0, 0, FLOAT_TO_FP(1)} }, .rot_hinge_180 = { - { 1, 0, 0}, - { 0, 1, 0}, - { 0, 0, 1} + { FLOAT_TO_FP(1), 0, 0}, + { 0, FLOAT_TO_FP(1), 0}, + { 0, 0, FLOAT_TO_FP(1)} }, .hinge_axis = {0, 1, 0}, }; diff --git a/common/lid_angle.c b/common/lid_angle.c index a55cd5d291..eb6f6c94b5 100644 --- a/common/lid_angle.c +++ b/common/lid_angle.c @@ -59,7 +59,7 @@ static const int kb_wake_small_angle = 13; * * @return true/false */ -static int lid_in_range_to_accept_keys(float ang) +static int lid_in_range_to_accept_keys(int ang) { /* * If the keyboard wake large angle is min or max, then this @@ -82,7 +82,7 @@ static int lid_in_range_to_accept_keys(float ang) * * @return true/false */ -static int lid_in_range_to_ignore_keys(float ang) +static int lid_in_range_to_ignore_keys(int ang) { /* * If the keyboard wake large angle is min or max, then this @@ -114,9 +114,9 @@ void lid_angle_set_kb_wake_angle(int ang) kb_wake_large_angle = ang; } -void lidangle_keyscan_update(float lid_ang) +void lidangle_keyscan_update(int lid_ang) { - static float lidangle_buffer[KEY_SCAN_LID_ANGLE_BUFFER_SIZE]; + static int lidangle_buffer[KEY_SCAN_LID_ANGLE_BUFFER_SIZE]; static int index; int i; diff --git a/common/math_util.c b/common/math_util.c index 56688283f4..120d13d63f 100644 --- a/common/math_util.c +++ b/common/math_util.c @@ -10,44 +10,56 @@ #include "math_util.h" #include "util.h" +/* Some useful math functions. Use with integers only! */ +#define SQ(x) ((x) * (x)) + /* For cosine lookup table, define the increment and the size of the table. */ #define COSINE_LUT_INCR_DEG 5 #define COSINE_LUT_SIZE ((180 / COSINE_LUT_INCR_DEG) + 1) -#ifdef CONFIG_FPU /* Lookup table for the value of cosine from 0 degrees to 180 degrees. */ -static const float cos_lut[] = { - 1.00000, 0.99619, 0.98481, 0.96593, 0.93969, - 0.90631, 0.86603, 0.81915, 0.76604, 0.70711, - 0.64279, 0.57358, 0.50000, 0.42262, 0.34202, - 0.25882, 0.17365, 0.08716, 0.00000, -0.08716, - -0.17365, -0.25882, -0.34202, -0.42262, -0.50000, - -0.57358, -0.64279, -0.70711, -0.76604, -0.81915, - -0.86603, -0.90631, -0.93969, -0.96593, -0.98481, - -0.99619, -1.00000, +static const fp_t cos_lut[] = { + FLOAT_TO_FP( 1.00000), FLOAT_TO_FP( 0.99619), FLOAT_TO_FP( 0.98481), + FLOAT_TO_FP( 0.96593), FLOAT_TO_FP( 0.93969), FLOAT_TO_FP( 0.90631), + FLOAT_TO_FP( 0.86603), FLOAT_TO_FP( 0.81915), FLOAT_TO_FP( 0.76604), + FLOAT_TO_FP( 0.70711), FLOAT_TO_FP( 0.64279), FLOAT_TO_FP( 0.57358), + FLOAT_TO_FP( 0.50000), FLOAT_TO_FP( 0.42262), FLOAT_TO_FP( 0.34202), + FLOAT_TO_FP( 0.25882), FLOAT_TO_FP( 0.17365), FLOAT_TO_FP( 0.08716), + FLOAT_TO_FP( 0.00000), FLOAT_TO_FP(-0.08716), FLOAT_TO_FP(-0.17365), + FLOAT_TO_FP(-0.25882), FLOAT_TO_FP(-0.34202), FLOAT_TO_FP(-0.42262), + FLOAT_TO_FP(-0.50000), FLOAT_TO_FP(-0.57358), FLOAT_TO_FP(-0.64279), + FLOAT_TO_FP(-0.70711), FLOAT_TO_FP(-0.76604), FLOAT_TO_FP(-0.81915), + FLOAT_TO_FP(-0.86603), FLOAT_TO_FP(-0.90631), FLOAT_TO_FP(-0.93969), + FLOAT_TO_FP(-0.96593), FLOAT_TO_FP(-0.98481), FLOAT_TO_FP(-0.99619), + FLOAT_TO_FP(-1.00000), }; BUILD_ASSERT(ARRAY_SIZE(cos_lut) == COSINE_LUT_SIZE); -float arc_cos(float x) +fp_t arc_cos(fp_t x) { int i; /* Cap x if out of range. */ - if (x < -1.0) - x = -1.0; - else if (x > 1.0) - x = 1.0; + if (x < FLOAT_TO_FP(-1.0)) + x = FLOAT_TO_FP(-1.0); + else if (x > FLOAT_TO_FP(1.0)) + x = FLOAT_TO_FP(1.0); /* * Increment through lookup table to find index and then linearly * interpolate for precision. */ /* TODO(crosbug.com/p/25600): Optimize with binary search. */ - for (i = 0; i < COSINE_LUT_SIZE-1; i++) - if (x >= cos_lut[i+1]) - return COSINE_LUT_INCR_DEG * - (i + (cos_lut[i] - x) / (cos_lut[i] - cos_lut[i+1])); + for (i = 0; i < COSINE_LUT_SIZE-1; i++) { + if (x >= cos_lut[i+1]) { + const fp_t interp = fp_div(cos_lut[i] - x, + cos_lut[i] - cos_lut[i + 1]); + + return fp_mul(INT_TO_FP(COSINE_LUT_INCR_DEG), + INT_TO_FP(i) + interp); + } + } /* * Shouldn't be possible to get here because inputs are clipped to @@ -59,56 +71,114 @@ float arc_cos(float x) return 0; } +/** + * Integer square root. + */ +int int_sqrtf(int64_t x) +{ + int rmax = 0x7fffffff; + int rmin = 0; + + /* Short cut if x is 32-bit value */ + if (x < rmax) + rmax = 0x7fff; + + /* + * Just binary-search. There are better algorithms, but we call this + * infrequently enough it doesn't matter. + */ + if (x <= 0) + return 0; /* Yeah, for imaginary numbers too */ + else if (x > (int64_t)rmax * rmax) + return rmax; + + while (1) { + int r = (rmax + rmin) / 2; + int64_t r2 = (int64_t)r * r; + + if (r2 > x) { + /* Guessed too high */ + rmax = r; + } else if (r2 < x) { + /* Guessed too low. Watch out for rounding! */ + if (rmin == r) + return r; + + rmin = r; + } else { + /* Bullseye! */ + return r; + } + } +} + int vector_magnitude(const vector_3_t v) { - return sqrtf(SQ(v[0]) + SQ(v[1]) + SQ(v[2])); + int64_t sum = (int64_t)v[0] * v[0] + + (int64_t)v[1] * v[1] + + (int64_t)v[2] * v[2]; + + return int_sqrtf(sum); } -float cosine_of_angle_diff(const vector_3_t v1, const vector_3_t v2) +fp_t cosine_of_angle_diff(const vector_3_t v1, const vector_3_t v2) { - int dotproduct; - float denominator; + int64_t dotproduct; + int64_t denominator; /* * Angle between two vectors is acos(A dot B / |A|*|B|). To return * cosine of angle between vectors, then don't do acos operation. */ - dotproduct = v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2]; + dotproduct = (int64_t)v1[0] * v2[0] + + (int64_t)v1[1] * v2[1] + + (int64_t)v1[2] * v2[2]; - denominator = vector_magnitude(v1) * vector_magnitude(v2); + denominator = (int64_t)vector_magnitude(v1) * vector_magnitude(v2); /* Check for divide by 0 although extremely unlikely. */ - if (ABS(denominator) < 0.01F) - return 0.0; + if (!denominator) + return 0; - return (float)dotproduct / (denominator); + /* + * We must shift the dot product first, so that we can represent + * fractions. The answer is always a number with magnitude < 1.0, so + * if we don't shift, it will always round down to 0. + * + * Note that overflow is possible if the dot product is large (that is, + * if the vector components are of size (31 - FP_BITS/2) bits. If that + * ever becomes a problem, we could detect this by counting the leading + * zeroes of the dot product and shifting the denominator down + * partially instead of shifting the dot product up. With the current + * FP_BITS=16, that happens if the vector components are ~2^23. Which + * we're a long way away from; the vector components used in + * accelerometer calculations are ~2^11. + */ + return (dotproduct << FP_BITS) / denominator; } -#endif /* * rotate a vector v * - support input v and output res are the same vector */ -void rotate(const vector_3_t v, const matrix_3x3_t R, - vector_3_t res) +void rotate(const vector_3_t v, const matrix_3x3_t R, vector_3_t res) { - vector_3_t t; - - /* copy input v to temp vector t */ - t[0] = v[0]; - t[1] = v[1]; - t[2] = v[2]; - - /* start rotate */ - res[0] = t[0] * R[0][0] + - t[1] * R[1][0] + - t[2] * R[2][0]; - res[1] = t[0] * R[0][1] + - t[1] * R[1][1] + - t[2] * R[2][1]; - res[2] = t[0] * R[0][2] + - t[1] * R[1][2] + - t[2] * R[2][2]; + int64_t t[3]; + + /* Rotate */ + t[0] = (int64_t)v[0] * R[0][0] + + (int64_t)v[1] * R[1][0] + + (int64_t)v[2] * R[2][0]; + t[1] = (int64_t)v[0] * R[0][1] + + (int64_t)v[1] * R[1][1] + + (int64_t)v[2] * R[2][1]; + t[2] = (int64_t)v[0] * R[0][2] + + (int64_t)v[1] * R[1][2] + + (int64_t)v[2] * R[2][2]; + + /* Scale by fixed point shift when writing back to result */ + res[0] = t[0] >> FP_BITS; + res[1] = t[1] >> FP_BITS; + res[2] = t[2] >> FP_BITS; } - diff --git a/common/motion_lid.c b/common/motion_lid.c index 4b991dee6b..0dfb8763d7 100644 --- a/common/motion_lid.c +++ b/common/motion_lid.c @@ -32,7 +32,7 @@ enum { }; /* Current acceleration vectors and current lid angle. */ -static float lid_angle_deg; +static int lid_angle_deg; static int lid_angle_is_reliable; /* @@ -41,7 +41,7 @@ static int lid_angle_is_reliable; * efficiency, value is given unit-less, so if you want the threshold to be * at 15 degrees, the value would be cos(15 deg) = 0.96593. */ -#define HINGE_ALIGNED_WITH_GRAVITY_THRESHOLD 0.96593F +#define HINGE_ALIGNED_WITH_GRAVITY_THRESHOLD FLOAT_TO_FP(0.96593) /* Pointer to constant acceleration orientation data. */ @@ -61,11 +61,12 @@ struct motion_sensor_t *accel_lid = &motion_sensors[CONFIG_SENSOR_LID]; * @return flag representing if resulting lid angle calculation is reliable. */ static int calculate_lid_angle(const vector_3_t base, const vector_3_t lid, - float *lid_angle) + int *lid_angle) { vector_3_t v; - float ang_lid_to_base, ang_lid_90, ang_lid_270; - float lid_to_base, base_to_hinge; + fp_t ang_lid_to_base, cos_lid_90, cos_lid_270; + fp_t lid_to_base, base_to_hinge; + fp_t denominator; int reliable = 1; /* @@ -82,64 +83,66 @@ static int calculate_lid_angle(const vector_3_t base, const vector_3_t lid, * If hinge aligns too closely with gravity, then result may be * unreliable. */ - if (ABS(base_to_hinge) > HINGE_ALIGNED_WITH_GRAVITY_THRESHOLD) + if (fp_abs(base_to_hinge) > HINGE_ALIGNED_WITH_GRAVITY_THRESHOLD) reliable = 0; - base_to_hinge = SQ(base_to_hinge); + base_to_hinge = fp_sq(base_to_hinge); /* Check divide by 0. */ - if (ABS(1.0F - base_to_hinge) < 0.01F) { - *lid_angle = 0.0; + denominator = FLOAT_TO_FP(1.0) - base_to_hinge; + if (fp_abs(denominator) < FLOAT_TO_FP(0.01)) { + *lid_angle = 0; return 0; } - ang_lid_to_base = arc_cos( - (lid_to_base - base_to_hinge) / (1 - base_to_hinge)); + ang_lid_to_base = arc_cos(fp_div(lid_to_base - base_to_hinge, + denominator)); /* * The previous calculation actually has two solutions, a positive and * a negative solution. To figure out the sign of the answer, calculate - * the angle between the actual lid angle and the estimated vector if - * the lid were open to 90 deg, ang_lid_90. Also calculate the angle - * between the actual lid angle and the estimated vector if the lid - * were open to 270 deg, ang_lid_270. The smaller of the two angles - * represents which one is closer. If the lid is closer to the - * estimated 270 degree vector then the result is negative, otherwise - * it is positive. + * the cosine of the angle between the actual lid angle and the + * estimated vector if the lid were open to 90 deg, cos_lid_90. Also + * calculate the cosine of the angle between the actual lid angle and + * the estimated vector if the lid were open to 270 deg, + * cos_lid_270. The smaller of the two angles represents which one is + * closer. If the lid is closer to the estimated 270 degree vector then + * the result is negative, otherwise it is positive. */ rotate(base, p_acc_orient->rot_hinge_90, v); - ang_lid_90 = cosine_of_angle_diff(v, lid); + cos_lid_90 = cosine_of_angle_diff(v, lid); rotate(v, p_acc_orient->rot_hinge_180, v); - ang_lid_270 = cosine_of_angle_diff(v, lid); + cos_lid_270 = cosine_of_angle_diff(v, lid); /* - * Note that ang_lid_90 and ang_lid_270 are not in degrees, because + * Note that cos_lid_90 and cos_lid_270 are not in degrees, because * the arc_cos() was never performed. But, since arc_cos() is * monotonically decreasing, we can do this comparison without ever * taking arc_cos(). But, since the function is monotonically * decreasing, the logic of this comparison is reversed. */ - if (ang_lid_270 > ang_lid_90) + if (cos_lid_270 > cos_lid_90) ang_lid_to_base = -ang_lid_to_base; /* Place lid angle between 0 and 360 degrees. */ if (ang_lid_to_base < 0) - ang_lid_to_base += 360; + ang_lid_to_base += FLOAT_TO_FP(360); + + /* + * Round to nearest int by adding 0.5. Note, only works because lid + * angle is known to be positive. + */ + *lid_angle = FP_TO_INT(ang_lid_to_base + FLOAT_TO_FP(0.5)); - *lid_angle = ang_lid_to_base; return reliable; } int motion_lid_get_angle(void) { if (lid_angle_is_reliable) - /* - * Round to nearest int by adding 0.5. Note, only works because - * lid angle is known to be positive. - */ - return (int)(lid_angle_deg + 0.5F); + return lid_angle_deg; else - return (int)LID_ANGLE_UNRELIABLE; + return LID_ANGLE_UNRELIABLE; } /* diff --git a/common/motion_sense.c b/common/motion_sense.c index fd9c64bc7d..5e61b8fcec 100644 --- a/common/motion_sense.c +++ b/common/motion_sense.c @@ -285,6 +285,12 @@ void motion_sense_task(void) gesture_calc(); #endif #ifdef CONFIG_LID_ANGLE + /* + * TODO (crosbug.com/p/36132): Checking for ALL sensors on is + * overkill. It should just check for ACCEL in BASE and ACCEL + * in LID, since those are the only ones needed for the lid + * calculation. + */ if (rd_cnt == motion_sensor_count) motion_lid_calc(); #endif @@ -299,7 +305,7 @@ void motion_sense_task(void) sensor->xyz[Z]); } #ifdef CONFIG_LID_ANGLE - CPRINTF("a=%-6.1d", 10 * motion_lid_get_angle()); + CPRINTF("a=%-4d", motion_lid_get_angle()); #endif CPRINTF("]\n"); } @@ -778,5 +784,3 @@ DECLARE_CONSOLE_COMMAND(accelint, command_accelerometer_interrupt, #endif /* CONFIG_ACCEL_INTERRUPTS */ #endif /* CONFIG_CMD_ACCELS */ - - diff --git a/include/lid_angle.h b/include/lid_angle.h index cd13265c93..e45a5683a4 100644 --- a/include/lid_angle.h +++ b/include/lid_angle.h @@ -15,7 +15,7 @@ * * @lid_ang Lid angle. */ -void lidangle_keyscan_update(float lid_ang); +void lidangle_keyscan_update(int lid_ang); /** * Getter and setter methods for the keyboard wake angle. In S3, when the diff --git a/include/math_util.h b/include/math_util.h index 6ae36a995b..dc0eb431c3 100644 --- a/include/math_util.h +++ b/include/math_util.h @@ -8,20 +8,74 @@ #ifndef __CROS_MATH_UTIL_H #define __CROS_MATH_UTIL_H -#ifdef CONFIG_FPU -typedef float matrix_3x3_t[3][3]; -#else -typedef int matrix_3x3_t[3][3]; -#endif +/* Fixed-point type */ +typedef int32_t fp_t; -typedef int vector_3_t[3]; +/* Number of bits left of decimal point for fixed-point */ +#define FP_BITS 16 +/* Conversion to/from fixed-point */ +#define INT_TO_FP(x) ((fp_t)(x) << FP_BITS) +#define FP_TO_INT(x) ((int32_t)((x) >> FP_BITS)) +/* Float to fixed-point, only for compile-time constants and unit tests */ +#define FLOAT_TO_FP(x) ((fp_t)((x) * (float)(1<<FP_BITS))) +/* Fixed-point to float, for unit tests */ +#define FP_TO_FLOAT(x) ((float)(x) / (float)(1<<FP_BITS)) -/* Some useful math functions. */ -#define SQ(x) ((x) * (x)) -#define ABS(x) ((x) >= 0 ? (x) : -(x)) +/* + * Fixed-point addition and subtraction can be done directly, because they + * work identically. + */ + +/** + * Multiplication - return (a * b) + */ +static inline fp_t fp_mul(fp_t a, fp_t b) +{ + return (fp_t)(((int64_t)a * b) >> FP_BITS); +} + +/** + * Division - return (a / b) + */ +static inline fp_t fp_div(fp_t a, fp_t b) +{ + return (fp_t)(((int64_t)a << FP_BITS) / b); +} + +/** + * Square (a * a) + */ +static inline fp_t fp_sq(fp_t a) +{ + return fp_mul(a, a); +} -#ifdef CONFIG_FPU +/** + * Absolute value + */ +static inline fp_t fp_abs(fp_t a) +{ + return (a >= 0 ? a : -a); +} + +/* + * Fixed point matrix + * + * Note that constant matrices MUST be initialized using FLOAT_TO_FP() + * or INT_TO_FP() for all non-zero values. + */ +typedef fp_t matrix_3x3_t[3][3]; + +/* Integer vector */ +typedef int vector_3_t[3]; + +/* + * Return absolute value of x. Note that as a macro expansion, this may have + * side effects if x includes function calls, which is why inline functions + * like fp_abs() are preferred. + */ +#define ABS(x) ((x) >= 0 ? (x) : -(x)) /** * Find acos(x) in degrees. Argument is clipped to [-1.0, 1.0]. @@ -30,19 +84,20 @@ typedef int vector_3_t[3]; * * @return acos(x) in degrees. */ -float arc_cos(float x); +fp_t arc_cos(fp_t x); /** * Find the cosine of the angle between two vectors. * + * The implementation assumes no vector component is greater than + * 2^(31 - FP_BITS/2). For example, 2^23, for FP_BITS=16. + * * @param v1 * @param v2 * * @return Cosine of the angle between v1 and v2. */ -float cosine_of_angle_diff(const vector_3_t v1, const vector_3_t v2); - -#endif +fp_t cosine_of_angle_diff(const vector_3_t v1, const vector_3_t v2); /** * Rotate vector v by rotation matrix R. @@ -51,8 +106,7 @@ float cosine_of_angle_diff(const vector_3_t v1, const vector_3_t v2); * @param R Rotation matrix. * @param res Resultant vector. */ -void rotate(const vector_3_t v, const matrix_3x3_t R, - vector_3_t res); +void rotate(const vector_3_t v, const matrix_3x3_t R, vector_3_t res); diff --git a/include/motion_lid.h b/include/motion_lid.h index b3348c020e..8ccf27191e 100644 --- a/include/motion_lid.h +++ b/include/motion_lid.h @@ -3,13 +3,13 @@ * found in the LICENSE file. */ -/* Header for motion_sense.c */ +/* Header for motion_lid.h */ #ifndef __CROS_EC_MOTION_LID_H #define __CROS_EC_MOTION_LID_H /* Anything outside of lid angle range [-180, 180] should work. */ -#define LID_ANGLE_UNRELIABLE 500.0F +#define LID_ANGLE_UNRELIABLE 500 /** * This structure defines all of the data needed to specify the orientation @@ -37,7 +37,8 @@ extern const struct accel_orientation acc_orient; * Get last calculated lid angle. Note, the lid angle calculated by the EC * is un-calibrated and is an approximate angle. * - * @return lid angle in degrees in range [0, 360]. + * @return lid angle in degrees in range [0, 360], or LID_ANGLE_UNRELIABLE + * if the lid angle can't be determined. */ int motion_lid_get_angle(void); diff --git a/test/math_util.c b/test/math_util.c index 46087e9f33..e563d8dd02 100644 --- a/test/math_util.c +++ b/test/math_util.c @@ -37,7 +37,7 @@ static int test_acos(void) /* Test a handful of values. */ for (test = -1.0; test <= 1.0; test += 0.01) { - a = arc_cos(test); + a = FP_TO_FLOAT(arc_cos(FLOAT_TO_FP(test))); b = acos(test) * RAD_TO_DEG; TEST_ASSERT(IS_FLOAT_EQUAL(a, b, ACOS_TOLERANCE_DEG)); } diff --git a/test/motion_lid.c b/test/motion_lid.c index e0f1e68e7c..a358a5056c 100644 --- a/test/motion_lid.c +++ b/test/motion_lid.c @@ -6,6 +6,7 @@ */ #include <math.h> +#include <stdio.h> #include "accelgyro.h" #include "common.h" @@ -89,15 +90,15 @@ const struct accelgyro_drv test_motion_sense = { }; const matrix_3x3_t base_standard_ref = { - { 1, 0, 0}, - { 0, 1, 0}, - { 0, 0, 1} + { FLOAT_TO_FP(1), 0, 0}, + { 0, FLOAT_TO_FP(1), 0}, + { 0, 0, FLOAT_TO_FP(1)} }; const matrix_3x3_t lid_standard_ref = { - { 1, 0, 0}, - { 1, 0, 0}, - { 0, 0, 1} + { FLOAT_TO_FP(1), 0, 0}, + { FLOAT_TO_FP(1), 0, 0}, + { 0, 0, FLOAT_TO_FP(1)} }; struct motion_sensor_t motion_sensors[] = { |