fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-30 03:24:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

rotation: use Dcmf for all rotations that aren't direct swaps

Browse Source 
- increase optimization to ${MAX_CUSTOM_OPT_LEVEL} (max per board)
This commit is contained in:
Daniel Agar 2021-01-10 18:08:41 -05:00 committed by GitHub
parent 06b733bb86
commit 70e503cb91
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 90 additions and 174 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/lib/conversion/CMakeLists.txt
View File

@ -31,6 +31,11 @@
#
############################################################################
px4_add_library(conversion rotation.cpp)
px4_add_library(conversion
rotation.cpp
rotation.h
)
target_compile_options(conversion PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
px4_add_unit_gtest(SRC RotationTest.cpp LINKLIBS conversion)

243
src/lib/conversion/rotation.cpp
View File

@ -37,8 +37,6 @@
* Vector rotation library
*/
#include <px4_platform_common/defines.h>
#include "math.h"
#include "rotation.h"
__EXPORT matrix::Dcmf
@ -62,287 +60,198 @@ get_rot_quaternion(enum Rotation rot)
__EXPORT void
rotate_3f(enum Rotation rot, float &x, float &y, float &z)
{
float tmp;
switch (rot) {
case ROTATION_NONE:
case ROTATION_MAX:
return;
case ROTATION_YAW_45: {
tmp = M_SQRT1_2_F * (x - y);
y = M_SQRT1_2_F * (x + y);
x = tmp;
return;
}
break;
case ROTATION_YAW_90: {
tmp = x; x = -y; y = tmp;
return;
float tmp = x;
x = -y;
y = tmp;
}
break;
case ROTATION_YAW_135: {
tmp = -M_SQRT1_2_F * (x + y);
y = M_SQRT1_2_F * (x - y);
x = tmp;
return;
}
case ROTATION_YAW_180:
x = -x; y = -y;
return;
case ROTATION_YAW_225: {
tmp = M_SQRT1_2_F * (y - x);
y = -M_SQRT1_2_F * (x + y);
x = tmp;
return;
case ROTATION_YAW_180: {
x = -x;
y = -y;
}
break;
case ROTATION_YAW_270: {
tmp = x; x = y; y = -tmp;
return;
}
case ROTATION_YAW_315: {
tmp = M_SQRT1_2_F * (x + y);
y = M_SQRT1_2_F * (y - x);
x = tmp;
return;
float tmp = x;
x = y;
y = -tmp;
}
break;
case ROTATION_ROLL_180: {
y = -y; z = -z;
return;
}
case ROTATION_ROLL_180_YAW_45: {
tmp = M_SQRT1_2_F * (x + y);
y = M_SQRT1_2_F * (x - y);
x = tmp; z = -z;
return;
y = -y;
z = -z;
}
break;
case ROTATION_ROLL_180_YAW_90:
// FALLTHROUGH
case ROTATION_PITCH_180_YAW_270: {
tmp = x;
float tmp = x;
x = y;
y = tmp;
z = -z;
return;
}
case ROTATION_ROLL_180_YAW_135: {
tmp = M_SQRT1_2_F * (y - x);
y = M_SQRT1_2_F * (y + x);
x = tmp; z = -z;
return;
}
break;
case ROTATION_PITCH_180: {
x = -x; z = -z;
return;
}
case ROTATION_ROLL_180_YAW_225: {
tmp = -M_SQRT1_2_F * (x + y);
y = M_SQRT1_2_F * (y - x);
x = tmp; z = -z;
return;
x = -x;
z = -z;
}
break;
case ROTATION_ROLL_180_YAW_270:
// FALLTHROUGH
case ROTATION_PITCH_180_YAW_90: {
tmp = x;
float tmp = x;
x = -y;
y = -tmp;
z = -z;
return;
}
case ROTATION_ROLL_180_YAW_315: {
tmp = M_SQRT1_2_F * (x - y);
y = -M_SQRT1_2_F * (x + y);
x = tmp; z = -z;
return;
}
break;
case ROTATION_ROLL_90: {
tmp = z; z = y; y = -tmp;
return;
}
case ROTATION_ROLL_90_YAW_45: {
tmp = z; z = y; y = -tmp;
tmp = M_SQRT1_2_F * (x - y);
y = M_SQRT1_2_F * (x + y);
x = tmp;
return;
float tmp = z;
z = y;
y = -tmp;
}
break;
case ROTATION_ROLL_90_YAW_90: {
tmp = x;
float tmp = x;
x = z;
z = y;
y = tmp;
return;
}
case ROTATION_ROLL_90_YAW_135: {
tmp = z; z = y; y = -tmp;
tmp = -M_SQRT1_2_F * (x + y);
y = M_SQRT1_2_F * (x - y);
x = tmp;
return;
}
break;
case ROTATION_ROLL_270: {
tmp = z; z = -y; y = tmp;
return;
}
case ROTATION_ROLL_270_YAW_45: {
tmp = z; z = -y; y = tmp;
tmp = M_SQRT1_2_F * (x - y);
y = M_SQRT1_2_F * (x + y);
x = tmp;
return;
float tmp = z;
z = -y;
y = tmp;
}
break;
case ROTATION_ROLL_270_YAW_90: {
tmp = x;
float tmp = x;
x = -z;
z = -y;
y = tmp;
return;
}
case ROTATION_ROLL_270_YAW_135: {
tmp = z; z = -y; y = tmp;
tmp = -M_SQRT1_2_F * (x + y);
y = M_SQRT1_2_F * (x - y);
x = tmp;
return;
}
break;
case ROTATION_PITCH_90: {
tmp = z; z = -x; x = tmp;
return;
float tmp = z;
z = -x;
x = tmp;
}
break;
case ROTATION_PITCH_270: {
tmp = z; z = x; x = -tmp;
return;
float tmp = z;
z = x;
x = -tmp;
}
break;
case ROTATION_ROLL_180_PITCH_270: {
tmp = z; z = x; x = tmp;
y = -y;
return;
}
case ROTATION_PITCH_315: {
tmp = M_SQRT1_2_F * x - M_SQRT1_2_F * z;
z = M_SQRT1_2_F * z + M_SQRT1_2_F * x;
float tmp = z;
z = x;
x = tmp;
return;
y = -y;
}
break;
case ROTATION_ROLL_90_YAW_270: {
tmp = x;
float tmp = x;
x = -z;
z = y;
y = -tmp;
return;
}
break;
case ROTATION_ROLL_90_PITCH_90: {
tmp = x;
float tmp = x;
x = y;
y = -z;
z = -tmp;
return;
}
break;
case ROTATION_ROLL_180_PITCH_90: {
tmp = x;
float tmp = x;
x = -z;
y = -y;
z = -tmp;
return;
}
break;
case ROTATION_ROLL_270_PITCH_90: {
tmp = x;
float tmp = x;
x = -y;
y = z;
z = -tmp;
return;
}
break;
case ROTATION_ROLL_90_PITCH_180: {
tmp = y;
float tmp = y;
x = -x;
y = -z;
z = -tmp;
return;
}
break;
case ROTATION_ROLL_270_PITCH_180: {
tmp = y;
float tmp = y;
x = -x;
y = z;
z = tmp;
return;
}
break;
case ROTATION_ROLL_90_PITCH_270: {
tmp = x;
float tmp = x;
x = -y;
y = -z;
z = tmp;
return;
}
break;
case ROTATION_ROLL_270_PITCH_270: {
tmp = x;
float tmp = x;
x = y;
y = z;
z = tmp;
return;
}
break;
case ROTATION_ROLL_90_PITCH_180_YAW_90: {
tmp = x;
float tmp = x;
x = z;
z = -y;
y = -tmp;
return;
}
case ROTATION_ROLL_90_PITCH_68_YAW_293: {
const float tmpx = x;
const float tmpy = y;
const float tmpz = z;
x = 0.146371f * tmpx + 0.362280f * tmpy - 0.920505f * tmpz;
y = -0.344827f * tmpx - 0.853477f * tmpy - 0.390731f * tmpz;
z = -0.927184f * tmpx + 0.374607f * tmpy;
return;
}
case ROTATION_ROLL_90_PITCH_315: {
const float tmpx = x;
const float tmpy = y;
const float tmpz = z;
x = 0.707107f * tmpx - 0.707107f * tmpy;
y = -tmpz;
z = 0.707107f * tmpx + 0.707107f * tmpy;
return;
}
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;
}
}

14
src/lib/conversion/rotation.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Copyright (C) 2013-2020 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
@ -40,15 +40,17 @@
#ifndef ROTATION_H_
#define ROTATION_H_
#include <unistd.h>
#include <stdint.h>
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
#include <px4_platform_common/defines.h>
/**
* Enum for board and external compass rotations.
* This enum maps from board attitude to airframe attitude.
*/
enum Rotation {
enum Rotation : uint8_t {
ROTATION_NONE = 0,
ROTATION_YAW_45 = 1,
ROTATION_YAW_90 = 2,
@ -86,10 +88,10 @@ enum Rotation {
ROTATION_ROLL_180_PITCH_270 = 34,
ROTATION_ROLL_270_PITCH_270 = 35,
ROTATION_ROLL_90_PITCH_180_YAW_90 = 36,
ROTATION_ROLL_90_YAW_270 = 37,
ROTATION_ROLL_90_YAW_270 = 37,
ROTATION_ROLL_90_PITCH_68_YAW_293 = 38,
ROTATION_PITCH_315 = 39,
ROTATION_ROLL_90_PITCH_315 = 40,
ROTATION_PITCH_315 = 39,
ROTATION_ROLL_90_PITCH_315 = 40,
ROTATION_MAX
};