sergiotarxz/nuttx
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

drivers/sensors/: Add coordinate conversion function

Browse Source 
convert from body coordinate system to right-hand
coordinate system.

Example:
Compared to the standard coordinate system,
the x-axis and y-axis are interchanged and have opposite directions,
the z-axis remains normal.

  body coordinate   ---->     right-hand coordinate
              +x               +y
               |                |
               |                |
               |                |
               |                |
     -y<-------.                .------>+x
              /                /
             /                /
            /                /
           /                /
         +z                +z

So for the above conversion, using "P3" to represent transformation relationships
The front is 1 0 2, which represents the y x z axis.
The standard order is 0 1 2, so y and x are interchanged.

The following -1 1 1 indicates the direction of the axis.
The standard is 1 1 1. Because the current y-axis is opposite to
the standard x-axis, it is -1.

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 */
};

you can call the function sensor_remap_vector_raw16 and pass P3 parameters
to perform the conversion.

Signed-off-by: Jiuzhu Dong <dongjiuzhu1@xiaomi.com>
This commit is contained in:
Jiuzhu Dong 2021-12-31 11:15:28 +08:00 committed by Xiang Xiao
parent 00c0801743
commit 032a5c75a4
2 changed files with 190 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
drivers/sensors/sensor.c
View File

@ -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
*

137
include/nuttx/sensors/sensor.h
View File

@ -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
****************************************************************************/