fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 14:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Un-inlined rotate_3

Browse Source
This commit is contained in:
Alexander Lerach
2024-11-15 12:01:06 -05:00
committed by Daniel Agar
parent 058fe540dd
commit 6d0b7f7ed1
2 changed files with 39 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/conversion/rotation.cpp
+28
View File
@@ -56,3 +56,31 @@ get_rot_quaternion(enum Rotation rot)
math::radians((float)rot_lookup[rot].pitch),
math::radians((float)rot_lookup[rot].yaw)}};
}
__EXPORT 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);
}
}
}
__EXPORT 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);
}
}
}
src/lib/conversion/rotation.h
+11 -33
View File
@@ -157,8 +157,18 @@ __EXPORT matrix::Dcmf get_rot_matrix(enum Rotation rot);
*/
__EXPORT matrix::Quatf get_rot_quaternion(enum Rotation rot);
/**
* rotate a 3 element int16_t vector in-place
*/
__EXPORT void rotate_3i(enum Rotation rot, int16_t &x, int16_t &y, int16_t &z);
/**
* rotate a 3 element float vector in-place
*/
__EXPORT void rotate_3f(enum Rotation rot, float &x, float &y, float &z);
template<typename T>
static constexpr bool rotate_3(enum Rotation rot, T &x, T &y, T &z)
static bool rotate_3(enum Rotation rot, T &x, T &y, T &z)
{
switch (rot) {
case ROTATION_NONE:
@@ -371,35 +381,3 @@ static constexpr bool rotate_3(enum Rotation rot, T &x, T &y, T &z)
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 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);
}
}
}