Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Math: add roll +- 45 rotations

This commit is contained in:
Iampete1 2021-07-23 20:45:00 +01:00 committed by Andrew Tridgell
parent 4b9311d87d
commit 6c5424aad6
5 changed files with 31 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_Math/examples/rotations/rotations.cpp
View File

@ -239,6 +239,8 @@ static void test_eulers(void)
test_euler(ROTATION_ROLL_90_PITCH_180_YAW_90, 90, 180, 90);
test_euler(ROTATION_ROLL_90_YAW_270, 90, 0, 270);
test_euler(ROTATION_ROLL_90_PITCH_68_YAW_293,90,68.8,293.3);
test_euler(ROTATION_ROLL_45,45,0,0);
test_euler(ROTATION_ROLL_315,315,0,0);
test_euler(ROTATION_PITCH_7, 0, 7, 0);
}

12
libraries/AP_Math/quaternion.cpp
View File

@ -368,6 +368,18 @@ void QuaternionT<T>::from_rotation(enum Rotation rotation)
q3 = 0.06104854f;
return;
case ROTATION_ROLL_45:
q1 = 0.9238795325112867;
q2 = 0.3826834323650898;
q3 = q4 = 0.0;
return;
case ROTATION_ROLL_315:
q1 = 0.9238795325112867;
q2 = -0.3826834323650898;
q3 = q4 = 0.0;
return;
case ROTATION_CUSTOM:
// Error; custom rotations not supported
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);

4
libraries/AP_Math/rotations.h
View File

@ -67,6 +67,8 @@ enum Rotation : uint8_t {
ROTATION_PITCH_315 = 39,
ROTATION_ROLL_90_PITCH_315 = 40,
ROTATION_PITCH_7 = 41,
ROTATION_ROLL_45 = 42,
ROTATION_ROLL_315 = 43,
///////////////////////////////////////////////////////////////////////
// Do not add more rotations without checking that there is not a conflict
// with the MAVLink spec. MAV_SENSOR_ORIENTATION is expected to match our
@ -87,5 +89,5 @@ enum Rotation : uint8_t {
Here are the same values in a form suitable for a @Values attribute in
auto documentation:
@Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Roll180Yaw45,10:Roll180Yaw90,11:Roll180Yaw135,12:Pitch180,13:Roll180Yaw225,14:Roll180Yaw270,15:Roll180Yaw315,16:Roll90,17:Roll90Yaw45,18:Roll90Yaw90,19:Roll90Yaw135,20:Roll270,21:Roll270Yaw45,22:Roll270Yaw90,23:Roll270Yaw135,24:Pitch90,25:Pitch270,26:Pitch180Yaw90,27:Pitch180Yaw270,28:Roll90Pitch90,29:Roll180Pitch90,30:Roll270Pitch90,31:Roll90Pitch180,32:Roll270Pitch180,33:Roll90Pitch270,34:Roll180Pitch270,35:Roll270Pitch270,36:Roll90Pitch180Yaw90,37:Roll90Yaw270,38:Yaw293Pitch68Roll180,39:Pitch315,40:Roll90Pitch315,100:Custom
@Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Roll180Yaw45,10:Roll180Yaw90,11:Roll180Yaw135,12:Pitch180,13:Roll180Yaw225,14:Roll180Yaw270,15:Roll180Yaw315,16:Roll90,17:Roll90Yaw45,18:Roll90Yaw90,19:Roll90Yaw135,20:Roll270,21:Roll270Yaw45,22:Roll270Yaw90,23:Roll270Yaw135,24:Pitch90,25:Pitch270,26:Pitch180Yaw90,27:Pitch180Yaw270,28:Roll90Pitch90,29:Roll180Pitch90,30:Roll270Pitch90,31:Roll90Pitch180,32:Roll270Pitch180,33:Roll90Pitch270,34:Roll180Pitch270,35:Roll270Pitch270,36:Roll90Pitch180Yaw90,37:Roll90Yaw270,38:Yaw293Pitch68Roll180,39:Pitch315,40:Roll90Pitch315,42:Roll45,43:Roll315,100:Custom
*/

3
libraries/AP_Math/tests/test_math.cpp
View File

@ -70,7 +70,8 @@ TEST(VectorTest, Rotations)
TEST_ROTATION(ROTATION_PITCH_315, 0, 1, SQRT_2);
TEST_ROTATION(ROTATION_ROLL_90_PITCH_315, 0, -1, SQRT_2);
TEST_ROTATION(ROTATION_PITCH_7, 1.1144155f, 1, 0.87067682f);
TEST_ROTATION(ROTATION_ROLL_45, 1, 0, SQRT_2);
TEST_ROTATION(ROTATION_ROLL_315, 1, SQRT_2, 0);
EXPECT_EQ(ROTATION_MAX, rotation_count) << "All rotations are expect to be tested";
TEST_ROTATION(ROTATION_CUSTOM, 1, 1, 1); // TODO look at internal error ?
TEST_ROTATION(ROTATION_MAX, 1, 1, 1);

12
libraries/AP_Math/vector3.cpp
View File

@ -251,6 +251,18 @@ void Vector3<T>::rotate(enum Rotation rotation)
z = -sin_pitch * tmpx + cos_pitch * tmpz;
return;
}
case ROTATION_ROLL_45: {
tmp = HALF_SQRT_2*(ftype)(y - z);
z = HALF_SQRT_2*(ftype)(y + z);
y = tmp;
return;
}
case ROTATION_ROLL_315: {
tmp = HALF_SQRT_2*(ftype)(y + z);
z = HALF_SQRT_2*(ftype)(z - y);
y = tmp;
return;
}
case ROTATION_CUSTOM:
// Error: caller must perform custom rotations via matrix multiplication
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);