mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-19 22:13:34 +08:00
rotate accel/gyro FIFO before publish and fix angular velocity filter resets
- rotates accel & gyro FIFO data before publication both to simplify downstream usage (including log review) and fix other issues
- to best handle int16_t data rotations are now either performed with swaps if possible, otherwise promoted to float, rotated using the full rotation matrix, then rounded back to int16_t
- fix sensors/vehicle_angular_velocity filter reset both with proper rotation and new calibration uncorrect helper
- in FIFO case filtering is done before calibration is applied, but we need to handle a possible reset from a completely different sensor (vehicle body angular velocity -> sensor frame uncorrected data)
This commit is contained in:
Daniel Agar
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Copyright (C) 2013-2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@@ -56,202 +56,3 @@ get_rot_quaternion(enum Rotation rot)
|
||||
math::radians((float)rot_lookup[rot].pitch),
|
||||
math::radians((float)rot_lookup[rot].yaw)}};
|
||||
}
|
||||
|
||||
__EXPORT void
|
||||
rotate_3f(enum Rotation rot, float &x, float &y, float &z)
|
||||
{
|
||||
switch (rot) {
|
||||
case ROTATION_NONE:
|
||||
break;
|
||||
|
||||
case ROTATION_YAW_90: {
|
||||
float tmp = x;
|
||||
x = -y;
|
||||
y = tmp;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_YAW_180: {
|
||||
x = -x;
|
||||
y = -y;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_YAW_270: {
|
||||
float tmp = x;
|
||||
x = y;
|
||||
y = -tmp;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_ROLL_180: {
|
||||
y = -y;
|
||||
z = -z;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_ROLL_180_YAW_90:
|
||||
|
||||
// FALLTHROUGH
|
||||
case ROTATION_PITCH_180_YAW_270: {
|
||||
float tmp = x;
|
||||
x = y;
|
||||
y = tmp;
|
||||
z = -z;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_PITCH_180: {
|
||||
x = -x;
|
||||
z = -z;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_ROLL_180_YAW_270:
|
||||
|
||||
// FALLTHROUGH
|
||||
case ROTATION_PITCH_180_YAW_90: {
|
||||
float tmp = x;
|
||||
x = -y;
|
||||
y = -tmp;
|
||||
z = -z;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_ROLL_90: {
|
||||
float tmp = z;
|
||||
z = y;
|
||||
y = -tmp;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_ROLL_90_YAW_90: {
|
||||
float tmp = x;
|
||||
x = z;
|
||||
z = y;
|
||||
y = tmp;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_ROLL_270: {
|
||||
float tmp = z;
|
||||
z = -y;
|
||||
y = tmp;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_ROLL_270_YAW_90: {
|
||||
float tmp = x;
|
||||
x = -z;
|
||||
z = -y;
|
||||
y = tmp;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_PITCH_90: {
|
||||
float tmp = z;
|
||||
z = -x;
|
||||
x = tmp;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_PITCH_270: {
|
||||
float tmp = z;
|
||||
z = x;
|
||||
x = -tmp;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_ROLL_180_PITCH_270: {
|
||||
float tmp = z;
|
||||
z = x;
|
||||
x = tmp;
|
||||
y = -y;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_ROLL_90_YAW_270: {
|
||||
float tmp = x;
|
||||
x = -z;
|
||||
z = y;
|
||||
y = -tmp;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_ROLL_90_PITCH_90: {
|
||||
float tmp = x;
|
||||
x = y;
|
||||
y = -z;
|
||||
z = -tmp;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_ROLL_180_PITCH_90: {
|
||||
float tmp = x;
|
||||
x = -z;
|
||||
y = -y;
|
||||
z = -tmp;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_ROLL_270_PITCH_90: {
|
||||
float tmp = x;
|
||||
x = -y;
|
||||
y = z;
|
||||
z = -tmp;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_ROLL_90_PITCH_180: {
|
||||
float tmp = y;
|
||||
x = -x;
|
||||
y = -z;
|
||||
z = -tmp;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_ROLL_270_PITCH_180: {
|
||||
float tmp = y;
|
||||
x = -x;
|
||||
y = z;
|
||||
z = tmp;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_ROLL_90_PITCH_270: {
|
||||
float tmp = x;
|
||||
x = -y;
|
||||
y = -z;
|
||||
z = tmp;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_ROLL_270_PITCH_270: {
|
||||
float tmp = x;
|
||||
x = y;
|
||||
y = z;
|
||||
z = tmp;
|
||||
}
|
||||
break;
|
||||
|
||||
case ROTATION_ROLL_90_PITCH_180_YAW_90: {
|
||||
float tmp = x;
|
||||
x = z;
|
||||
z = -y;
|
||||
y = -tmp;
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
|
||||
// otherwise use full rotation matrix for valid rotations
|
||||
if (rot < ROTATION_MAX) {
|
||||
const matrix::Vector3f r{get_rot_matrix(rot) *matrix::Vector3f{x, y, z}};
|
||||
x = r(0);
|
||||
y = r(1);
|
||||
z = r(2);
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
+247
-12
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013-2020 PX4 Development Team. All rights reserved.
|
||||
* Copyright (C) 2013-2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@@ -37,8 +37,7 @@
|
||||
* Vector rotation library
|
||||
*/
|
||||
|
||||
#ifndef ROTATION_H_
|
||||
#define ROTATION_H_
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
@@ -149,20 +148,256 @@ static constexpr rot_lookup_t rot_lookup[ROTATION_MAX] = {
|
||||
/**
|
||||
* Get the rotation matrix
|
||||
*/
|
||||
__EXPORT matrix::Dcmf
|
||||
get_rot_matrix(enum Rotation rot);
|
||||
__EXPORT matrix::Dcmf get_rot_matrix(enum Rotation rot);
|
||||
|
||||
/**
|
||||
* Get the rotation quaternion
|
||||
*/
|
||||
__EXPORT matrix::Quatf
|
||||
get_rot_quaternion(enum Rotation rot);
|
||||
__EXPORT matrix::Quatf get_rot_quaternion(enum Rotation rot);
|
||||
|
||||
template<typename T>
|
||||
static constexpr bool rotate_3(enum Rotation rot, T &x, T &y, T &z)
|
||||
{
|
||||
switch (rot) {
|
||||
case ROTATION_NONE:
|
||||
return true;
|
||||
|
||||
case ROTATION_YAW_90: {
|
||||
T tmp = x;
|
||||
x = math::negate(y);
|
||||
y = tmp;
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_YAW_180: {
|
||||
x = math::negate(x);
|
||||
y = math::negate(y);
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_YAW_270: {
|
||||
T tmp = x;
|
||||
x = y;
|
||||
y = math::negate(tmp);
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_ROLL_180: {
|
||||
y = math::negate(y);
|
||||
z = math::negate(z);
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_ROLL_180_YAW_90:
|
||||
|
||||
// FALLTHROUGH
|
||||
case ROTATION_PITCH_180_YAW_270: {
|
||||
T tmp = x;
|
||||
x = y;
|
||||
y = tmp;
|
||||
z = math::negate(z);
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_PITCH_180: {
|
||||
x = math::negate(x);
|
||||
z = math::negate(z);
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_ROLL_180_YAW_270:
|
||||
|
||||
// FALLTHROUGH
|
||||
case ROTATION_PITCH_180_YAW_90: {
|
||||
T tmp = x;
|
||||
x = math::negate(y);
|
||||
y = math::negate(tmp);
|
||||
z = math::negate(z);
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_ROLL_90: {
|
||||
T tmp = z;
|
||||
z = y;
|
||||
y = math::negate(tmp);
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_ROLL_90_YAW_90: {
|
||||
T tmp = x;
|
||||
x = z;
|
||||
z = y;
|
||||
y = tmp;
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_ROLL_270: {
|
||||
T tmp = z;
|
||||
z = math::negate(y);
|
||||
y = tmp;
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_ROLL_270_YAW_90: {
|
||||
T tmp = x;
|
||||
x = math::negate(z);
|
||||
z = math::negate(y);
|
||||
y = tmp;
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_PITCH_90: {
|
||||
T tmp = z;
|
||||
z = math::negate(x);
|
||||
x = tmp;
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_PITCH_270: {
|
||||
T tmp = z;
|
||||
z = x;
|
||||
x = math::negate(tmp);
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_ROLL_180_PITCH_270: {
|
||||
T tmp = z;
|
||||
z = x;
|
||||
x = tmp;
|
||||
y = math::negate(y);
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_ROLL_90_YAW_270: {
|
||||
T tmp = x;
|
||||
x = math::negate(z);
|
||||
z = y;
|
||||
y = math::negate(tmp);
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_ROLL_90_PITCH_90: {
|
||||
T tmp = x;
|
||||
x = y;
|
||||
y = math::negate(z);
|
||||
z = math::negate(tmp);
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_ROLL_180_PITCH_90: {
|
||||
T tmp = x;
|
||||
x = math::negate(z);
|
||||
y = math::negate(y);
|
||||
z = math::negate(tmp);
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_ROLL_270_PITCH_90: {
|
||||
T tmp = x;
|
||||
x = math::negate(y);
|
||||
y = z;
|
||||
z = math::negate(tmp);
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_ROLL_90_PITCH_180: {
|
||||
T tmp = y;
|
||||
x = math::negate(x);
|
||||
y = math::negate(z);
|
||||
z = math::negate(tmp);
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_ROLL_270_PITCH_180: {
|
||||
T tmp = y;
|
||||
x = math::negate(x);
|
||||
y = z;
|
||||
z = tmp;
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_ROLL_90_PITCH_270: {
|
||||
T tmp = x;
|
||||
x = math::negate(y);
|
||||
y = math::negate(z);
|
||||
z = tmp;
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_ROLL_270_PITCH_270: {
|
||||
T tmp = x;
|
||||
x = y;
|
||||
y = z;
|
||||
z = tmp;
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
case ROTATION_ROLL_90_PITCH_180_YAW_90: {
|
||||
T tmp = x;
|
||||
x = z;
|
||||
z = math::negate(y);
|
||||
y = math::negate(tmp);
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* rotate a 3 element int16_t vector in-place
|
||||
*/
|
||||
__EXPORT inline void rotate_3i(enum Rotation rot, int16_t &x, int16_t &y, int16_t &z)
|
||||
{
|
||||
if (!rotate_3(rot, x, y, z)) {
|
||||
// otherwise use full rotation matrix for valid rotations
|
||||
if (rot < ROTATION_MAX) {
|
||||
const matrix::Vector3f r{get_rot_matrix(rot) *matrix::Vector3f{(float)x, (float)y, (float)z}};
|
||||
x = math::constrain(roundf(r(0)), (float)INT16_MIN, (float)INT16_MAX);
|
||||
y = math::constrain(roundf(r(1)), (float)INT16_MIN, (float)INT16_MAX);
|
||||
z = math::constrain(roundf(r(2)), (float)INT16_MIN, (float)INT16_MAX);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* rotate a 3 element float vector in-place
|
||||
*/
|
||||
__EXPORT void
|
||||
rotate_3f(enum Rotation rot, float &x, float &y, float &z);
|
||||
|
||||
|
||||
#endif /* ROTATION_H_ */
|
||||
__EXPORT inline void rotate_3f(enum Rotation rot, float &x, float &y, float &z)
|
||||
{
|
||||
if (!rotate_3(rot, x, y, z)) {
|
||||
// otherwise use full rotation matrix for valid rotations
|
||||
if (rot < ROTATION_MAX) {
|
||||
const matrix::Vector3f r{get_rot_matrix(rot) *matrix::Vector3f{x, y, z}};
|
||||
x = r(0);
|
||||
y = r(1);
|
||||
z = r(2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user