diff --git a/drivers/sensors/sensor.c b/drivers/sensors/sensor.c index 6a38fc50f93..dde30ee77c0 100644 --- a/drivers/sensors/sensor.c +++ b/drivers/sensors/sensor.c @@ -55,6 +55,17 @@ * Private Types ****************************************************************************/ +struct sensor_axis_map_s +{ + int8_t src_x; + int8_t src_y; + int8_t src_z; + + int8_t sign_x; + int8_t sign_y; + int8_t sign_z; +}; + /* This structure describes sensor info */ struct sensor_info_s @@ -122,6 +133,18 @@ static ssize_t sensor_push_event(FAR void *priv, FAR const void *data, * Private Data ****************************************************************************/ +static const struct sensor_axis_map_s g_remap_tbl[] = +{ + { 0, 1, 2, 1, 1, 1 }, /* P0 */ + { 1, 0, 2, 1, -1, 1 }, /* P1 */ + { 0, 1, 2, -1, -1, 1 }, /* P2 */ + { 1, 0, 2, -1, 1, 1 }, /* P3 */ + { 0, 1, 2, -1, 1, -1 }, /* P4 */ + { 1, 0, 2, -1, -1, -1 }, /* P5 */ + { 0, 1, 2, 1, -1, -1 }, /* P6 */ + { 1, 0, 2, 1, 1, -1 }, /* P7 */ +}; + static const struct sensor_info_s g_sensor_info[] = { {0, NULL}, @@ -1010,6 +1033,36 @@ static void sensor_notify_event(FAR void *priv) * Public Functions ****************************************************************************/ +/**************************************************************************** + * Name: sensor_remap_vector_raw16 + * + * Description: + * This function remap the sensor data according to the place position on + * board. The value of place is determined base on g_remap_tbl. + * + * Input Parameters: + * in - A pointer to input data need remap. + * out - A pointer to output data. + * place - The place position of sensor on board, + * ex:SENSOR_BODY_COORDINATE_PX + * + ****************************************************************************/ + +void sensor_remap_vector_raw16(FAR const int16_t *in, FAR int16_t *out, + int place) +{ + FAR const struct sensor_axis_map_s *remap; + int16_t tmp[3]; + + DEBUGASSERT(place < (sizeof(g_remap_tbl) / sizeof(g_remap_tbl[0]))); + + remap = &g_remap_tbl[place]; + tmp[0] = in[remap->src_x] * remap->sign_x; + tmp[1] = in[remap->src_y] * remap->sign_y; + tmp[2] = in[remap->src_z] * remap->sign_z; + memcpy(out, tmp, sizeof(tmp)); +} + /**************************************************************************** * Name: sensor_register * diff --git a/include/nuttx/sensors/sensor.h b/include/nuttx/sensors/sensor.h index 52c5efd57a5..5272d1a9649 100644 --- a/include/nuttx/sensors/sensor.h +++ b/include/nuttx/sensors/sensor.h @@ -316,6 +316,125 @@ #define SENSOR_REMOTE (1u << 31) #define SENSOR_PERSIST (1u << 30) +/* Body coordinate system position P0: + * + * +y + * | + * | + * | + * | + * .------>+x + * / + * / + * / + * / + * +z + * + */ + +#define SENSOR_BODY_COORDINATE_P0 0 + +/* Body coordinate system position P1: + * + * .------>+y + * /| + * / | + * / | + * / | + * +z -x + * + */ + +#define SENSOR_BODY_COORDINATE_P1 1 + +/* Body coordinate system position P2: + * + * -x<------. + * /| + * / | + * / | + * / | + * +z -y + * + */ + +#define SENSOR_BODY_COORDINATE_P2 2 + +/* Body coordinate system position P3: + * + * +x + * | + * | + * | + * | + * -y<------. + * / + * / + * / + * / + * +z + * + */ + +#define SENSOR_BODY_COORDINATE_P3 3 + +/* Body coordinate system position P4: + * + * +y -z + * | / + * | / + * | / + * |/ + * -x<------. + * + */ + +#define SENSOR_BODY_COORDINATE_P4 4 + +/* Body coordinate system position P5: + * + * +y -z + * | / + * | / + * | / + * |/ + * -x<------. + * + */ + +#define SENSOR_BODY_COORDINATE_P5 5 + +/* Body coordinate system position P6: + * + * -z + * / + * / + * / + * / + * -x<------. + * | + * | + * | + * | + * -y + * + */ + +#define SENSOR_BODY_COORDINATE_P6 6 + +/* Body coordinate system position P7: + * + * +x -z + * | / + * | / + * | / + * |/ + * .------->y + * + */ + +#define SENSOR_BODY_COORDINATE_P7 7 + /**************************************************************************** * Inline Functions ****************************************************************************/ @@ -1058,6 +1177,24 @@ extern "C" #define EXTERN extern #endif +/**************************************************************************** + * Name: sensor_remap_vector_raw16 + * + * Description: + * This function remap the sensor data according to the place position on + * board. The value of place is determined base on g_remap_tbl. + * + * Input Parameters: + * in - A pointer to input data need remap. + * out - A pointer to output data. + * place - The place position of sensor on board, + * ex:SENSOR_BODY_COORDINATE_PX + * + ****************************************************************************/ + +void sensor_remap_vector_raw16(FAR const int16_t *in, FAR int16_t *out, + int place); + /**************************************************************************** * "Upper Half" Sensor Driver Interfaces ****************************************************************************/