Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 22:58:10 +08:00
Code Issues Actions 15 Packages Projects Releases Wiki Activity

rotations: sync rotation enum, SENS_BOARD_ROT, CAL_MAGx_ROT with MAV_SENSOR_ORIENTATION

Browse Source
This commit is contained in:
Daniel Agar
2020-09-21 17:38:56 -04:00
parent 8b26b84fd1
commit 00e955cc3e
9 changed files with 25 additions and 89 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/conversion/RotationTest.cpp
-13
View File
@@ -97,25 +97,12 @@ TEST(Rotations, duplicates)
if (j == ROTATION_ROLL_180_YAW_90 && i == ROTATION_PITCH_180_YAW_270) { continue; }
// ROTATION_ROLL_180_PITCH_90 (29) = ROTATION_PITCH_90_YAW_180 (43)
if (i == ROTATION_ROLL_180_PITCH_90 && j == ROTATION_PITCH_90_YAW_180) { continue; }
if (j == ROTATION_ROLL_180_PITCH_90 && i == ROTATION_PITCH_90_YAW_180) { continue; }
// ROTATION_ROLL_90_PITCH_180 (31) = ROTATION_ROLL_270_YAW_180 (41)
if (i == ROTATION_ROLL_90_PITCH_180 && j == ROTATION_ROLL_270_YAW_180) { continue; }
if (j == ROTATION_ROLL_90_PITCH_180 && i == ROTATION_ROLL_270_YAW_180) { continue; }
// ROTATION_ROLL_90_PITCH_180_YAW_90 (36) = ROTATION_ROLL_270_YAW_270 (42)
if (i == ROTATION_ROLL_90_PITCH_180_YAW_90 && j == ROTATION_ROLL_270_YAW_270) { continue; }
if (j == ROTATION_ROLL_90_PITCH_180_YAW_90 && i == ROTATION_ROLL_270_YAW_270) { continue; }
// otherwise all rotations should be different
ASSERT_GT((transformed_1 - transformed_2).norm(), 0) << "Rotation " << i << " and " << j << " equal";
}
src/lib/conversion/rotation.cpp
+2 -25
View File
@@ -245,23 +245,6 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
return;
}
case ROTATION_PITCH_9_YAW_180: {
const float tmpx = x;
const float tmpy = y;
const float tmpz = z;
x = -0.987688f * tmpx + 0.000000f * tmpy + -0.156434f * tmpz;
y = 0.000000f * tmpx + -1.000000f * tmpy + 0.000000f * tmpz;
z = -0.156434f * tmpx + 0.000000f * tmpy + 0.987688f * tmpz;
return;
}
case ROTATION_PITCH_45: {
tmp = M_SQRT1_2_F * x + M_SQRT1_2_F * z;
z = M_SQRT1_2_F * z - M_SQRT1_2_F * x;
x = tmp;
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;
@@ -285,10 +268,7 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
return;
}
case ROTATION_ROLL_180_PITCH_90:
// FALLTHROUGH
case ROTATION_PITCH_90_YAW_180: {
case ROTATION_ROLL_180_PITCH_90: {
tmp = x;
x = -z;
y = -y;
@@ -339,10 +319,7 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
return;
}
case ROTATION_ROLL_90_PITCH_180_YAW_90:
// FALLTHROUGH
case ROTATION_ROLL_270_YAW_270: {
case ROTATION_ROLL_90_PITCH_180_YAW_90: {
tmp = x;
x = z;
z = -y;
src/lib/conversion/rotation.h
+1 -8
View File
@@ -91,10 +91,7 @@ enum Rotation {
ROTATION_PITCH_315 = 39,
ROTATION_ROLL_90_PITCH_315 = 40,
ROTATION_ROLL_270_YAW_180 = 41,
ROTATION_ROLL_270_YAW_270 = 42,
ROTATION_PITCH_90_YAW_180 = 43,
ROTATION_PITCH_9_YAW_180 = 44,
ROTATION_PITCH_45 = 45,
ROTATION_MAX
};
@@ -147,10 +144,6 @@ const rot_lookup_t rot_lookup[] = {
{ 0, 315, 0 },
{ 90, 315, 0 },
{270, 0, 180 },
{270, 0, 270 },
{ 0, 90, 180 },
{ 0, 9, 180 },
{ 0, 45, 0 },
};
/**
src/modules/commander/mag_calibration.cpp
-12
View File
@@ -739,15 +739,6 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
// FALLTHROUGH
case ROTATION_ROLL_270_YAW_180: // skip 41, same as 31 ROTATION_ROLL_90_PITCH_180
// FALLTHROUGH
case ROTATION_ROLL_270_YAW_270: // skip 42, same as 36 ROTATION_ROLL_90_PITCH_180_YAW_90
// FALLTHROUGH
case ROTATION_PITCH_90_YAW_180: // skip 43, same as 29 ROTATION_ROLL_180_PITCH_90
// FALLTHROUGH
case ROTATION_PITCH_9_YAW_180: // skip, too close to ROTATION_YAW_180
MSE[r] = FLT_MAX;
break;
@@ -804,9 +795,6 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
switch (worker_data.calibration[cur_mag].rotation_enum()) {
case ROTATION_ROLL_90_PITCH_68_YAW_293:
// FALLTHROUGH
case ROTATION_PITCH_9_YAW_180:
PX4_INFO("[cal] External Mag: %d (%d), keeping manually configured rotation %d", cur_mag,
worker_data.calibration[cur_mag].device_id(), worker_data.calibration[cur_mag].rotation_enum());
continue;
src/modules/sensors/sensor_params.c
+18 -11
View File
@@ -131,19 +131,26 @@ PARAM_DEFINE_FLOAT(SENS_DPRES_ANSC, 0);
* @value 23 Roll 270°, Yaw 135°
* @value 24 Pitch 90°
* @value 25 Pitch 270°
* @value 26 Roll 270°, Yaw 270°
* @value 27 Roll 180°, Pitch 270°
* @value 28 Pitch 90°, Yaw 180
* @value 29 Pitch 90°, Roll 90°
* @value 30 Yaw 293°, Pitch 68°, Roll 90° (Solo)
* @value 31 Pitch 90°, Roll 270°
* @value 32 Pitch 9°, Yaw 180°
* @value 33 Pitch 45°
* @value 34 Pitch 315°
* @value 35 Roll 90°, Yaw 270°
* @value 26 Pitch 180°, Yaw 90°
* @value 27 Pitch 180°, Yaw 270°
* @value 28 Roll 90°, Pitch 90°
* @value 29 Roll 180°, Pitch 90°
* @value 30 Roll 270°, Pitch 90°
* @value 31 Roll 90°, Pitch 180°
* @value 32 Roll 270°, Pitch 180°
* @value 33 Roll 90°, Pitch 270°
* @value 34 Roll 180°, Pitch 270°
* @value 35 Roll 270°, Pitch 270°
* @value 36 Roll 90°, Pitch 180°, Yaw 90°
* @value 37 Roll 90°, Yaw 270°
* @value 38 Roll 90°, Pitch 68°, Yaw 293°
* @value 39 Pitch 315°
* @value 40 Roll 90°, Pitch 315°
* @value 41 Roll 270°, Yaw 180°
*
* @min -1
* @max 41
* @reboot_required true
*
* @group Sensors
*/
PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);
src/modules/sensors/sensor_params_mag0.c
+1 -5
View File
@@ -105,13 +105,9 @@ PARAM_DEFINE_INT32(CAL_MAG0_PRIO, -1);
* @value 39 Pitch 315°
* @value 40 Roll 90°, Pitch 315°
* @value 41 Roll 270°, Yaw 180°
* @value 42 Roll 270°, Yaw 270°
* @value 43 Pitch 90°, Yaw 180°
* @value 44 Pitch 9°, Yaw 180°
* @value 45 Pitch 45°
*
* @min -1
* @max 45
* @max 41
* @reboot_required true
* @category system
* @group Sensor Calibration
src/modules/sensors/sensor_params_mag1.c
+1 -5
View File
@@ -105,13 +105,9 @@ PARAM_DEFINE_INT32(CAL_MAG1_PRIO, -1);
* @value 39 Pitch 315°
* @value 40 Roll 90°, Pitch 315°
* @value 41 Roll 270°, Yaw 180°
* @value 42 Roll 270°, Yaw 270°
* @value 43 Pitch 90°, Yaw 180°
* @value 44 Pitch 9°, Yaw 180°
* @value 45 Pitch 45°
*
* @min -1
* @max 45
* @max 41
* @reboot_required true
* @category system
* @group Sensor Calibration
src/modules/sensors/sensor_params_mag2.c
+1 -5
View File
@@ -105,13 +105,9 @@ PARAM_DEFINE_INT32(CAL_MAG2_PRIO, -1);
* @value 39 Pitch 315°
* @value 40 Roll 90°, Pitch 315°
* @value 41 Roll 270°, Yaw 180°
* @value 42 Roll 270°, Yaw 270°
* @value 43 Pitch 90°, Yaw 180°
* @value 44 Pitch 9°, Yaw 180°
* @value 45 Pitch 45°
*
* @min -1
* @max 45
* @max 41
* @reboot_required true
* @category system
* @group Sensor Calibration
src/modules/sensors/sensor_params_mag3.c
+1 -5
View File
@@ -105,13 +105,9 @@ PARAM_DEFINE_INT32(CAL_MAG3_PRIO, -1);
* @value 39 Pitch 315°
* @value 40 Roll 90°, Pitch 315°
* @value 41 Roll 270°, Yaw 180°
* @value 42 Roll 270°, Yaw 270°
* @value 43 Pitch 90°, Yaw 180°
* @value 44 Pitch 9°, Yaw 180°
* @value 45 Pitch 45°
*
* @min -1
* @max 45
* @max 41
* @reboot_required true
* @category system
* @group Sensor Calibration