Enable arbitrary euler angle for Mag rotation CAL_MAGx_{ROLL,PITCH,YAW}

- Enable arbitrary euler angle for Mag rotation - new CUSTOM rotation enum out of the normal enum range - mag_rot: automatically change to custom if euler rot is set - sensor_calibration: Magnetometer save custom rotation parameters - mag_cal: cross mention rotation parameters - This allows the user to see the RPY options when searching for the rotation parameter --------- Co-authored-by: Junwoo Hwang <[email protected]> Co-authored-by: bresch <[[email protected]](mailto:[email protected])> Co-authored-by: Daniel Agar <[email protected]>
aviant-tech · Sep 14, 2024 · a322a20 · a322a20
1 parent 0e017f6
commit a322a20
Show file tree

Hide file tree

Showing 6 changed files with 140 additions and 7 deletions.
diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h
@@ -91,8 +91,10 @@ enum Rotation : uint8_t {
 	ROTATION_ROLL_90_PITCH_68_YAW_293 = 38,
 	ROTATION_PITCH_315                = 39,
 	ROTATION_ROLL_90_PITCH_315        = 40,
+	ROTATION_MAX,
 
-	ROTATION_MAX
+	// Rotation Enum reserved for custom rotation using Euler Angles
+	ROTATION_CUSTOM                  = 100
 };
 
 struct rot_lookup_t {

diff --git a/src/lib/sensor_calibration/Magnetometer.cpp b/src/lib/sensor_calibration/Magnetometer.cpp
@@ -121,12 +121,39 @@ bool Magnetometer::set_offdiagonal(const Vector3f &offdiagonal)
 	return false;
 }
 
-void Magnetometer::set_rotation(Rotation rotation)
+void Magnetometer::set_rotation(const Rotation rotation)
 {
-	_rotation_enum = rotation;
+	if (rotation < ROTATION_MAX) {
+		_rotation_enum = rotation;
+
+	} else {
+		// invalid rotation, resetting
+		_rotation_enum = ROTATION_NONE;
+	}
+
+	// always apply level adjustments
+	_rotation = Dcmf(GetSensorLevelAdjustment()) * get_rot_matrix(_rotation_enum);
+
+	// clear any custom rotation
+	_rotation_custom_euler.zero();
+}
+
+void Magnetometer::set_custom_rotation(const Eulerf &rotation)
+{
+	_rotation_enum = ROTATION_CUSTOM;
+
+	// store custom rotation
+	_rotation_custom_euler = rotation;
 
 	// always apply board level adjustments
-	_rotation = Dcmf(GetSensorLevelAdjustment()) * get_rot_matrix(rotation);
+	_rotation = Dcmf(GetSensorLevelAdjustment()) * Dcmf(_rotation_custom_euler);
+
+	// TODO: Note that ideally this shouldn't be necessary for an external sensors, as the definition of *rotation
+	// between sensor frame & vehicle's body frame isn't affected by the rotation of the Autopilot.
+	// however, since while doing the 'level-calibration', users don't put the vehicle truly *horizontal, the
+	// measured board roll/pitch offset isn't true. So this affects external sensors as well (which is why we apply
+	// internal SensorLevelAdjustment to all the sensors). We need to figure out how to set the sensor board offset
+	// values properly (i.e. finding Vehicle's true Forward-Right-Down frame in a user's perspective)
 }
 
 bool Magnetometer::set_calibration_index(int calibration_index)
@@ -162,13 +189,39 @@ bool Magnetometer::ParametersLoad()
 		// CAL_MAGx_ROT
 		int32_t rotation_value = GetCalibrationParamInt32(SensorString(), "ROT", _calibration_index);
 
+		const float euler_roll_deg = GetCalibrationParamFloat(SensorString(), "ROLL", _calibration_index);
+		const float euler_pitch_deg = GetCalibrationParamFloat(SensorString(), "PITCH", _calibration_index);
+		const float euler_yaw_deg = GetCalibrationParamFloat(SensorString(), "YAW", _calibration_index);
+
 		if (_external) {
-			if ((rotation_value >= ROTATION_MAX) || (rotation_value < 0)) {
+			if (((rotation_value >= ROTATION_MAX) && (rotation_value != ROTATION_CUSTOM)) || (rotation_value < 0)) {
 				// invalid rotation, resetting
 				rotation_value = ROTATION_NONE;
 			}
 
-			set_rotation(static_cast<Rotation>(rotation_value));
+			// if CAL_MAGx_{ROLL,PITCH,YAW} manually set then CAL_MAGx_ROT needs to be ROTATION_CUSTOM
+			if ((rotation_value != ROTATION_CUSTOM)
+			    && ((fabsf(euler_roll_deg) > FLT_EPSILON)
+				|| (fabsf(euler_pitch_deg) > FLT_EPSILON)
+				|| (fabsf(euler_yaw_deg) > FLT_EPSILON))) {
+
+				rotation_value = ROTATION_CUSTOM;
+				SetCalibrationParam(SensorString(), "ROT", _calibration_index, rotation_value);
+			}
+
+			// Handle custom specified euler angle
+			if (rotation_value == ROTATION_CUSTOM) {
+
+				const matrix::Eulerf rotation_custom_euler{
+					math::radians(euler_roll_deg),
+					math::radians(euler_pitch_deg),
+					math::radians(euler_yaw_deg)};
+
+				set_custom_rotation(rotation_custom_euler);
+
+			} else {
+				set_rotation(static_cast<Rotation>(rotation_value));
+			}
 
 		} else {
 			// internal sensors follow board rotation
@@ -273,6 +326,10 @@ bool Magnetometer::ParametersSave(int desired_calibration_index, bool force)
 			success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1); // internal
 		}
 
+		success &= SetCalibrationParam(SensorString(), "ROLL", _calibration_index, math::degrees(_rotation_custom_euler(0)));
+		success &= SetCalibrationParam(SensorString(), "PITCH", _calibration_index, math::degrees(_rotation_custom_euler(1)));
+		success &= SetCalibrationParam(SensorString(), "YAW", _calibration_index, math::degrees(_rotation_custom_euler(2)));
+
 		return success;
 	}
 

diff --git a/src/lib/sensor_calibration/Magnetometer.hpp b/src/lib/sensor_calibration/Magnetometer.hpp
@@ -65,8 +65,21 @@ class Magnetometer
 	bool set_offset(const matrix::Vector3f &offset);
 	bool set_scale(const matrix::Vector3f &scale);
 	bool set_offdiagonal(const matrix::Vector3f &offdiagonal);
+
+	/**
+	 * @brief Set the rotation enum & corresponding rotation matrix for Magnetometer
+	 *
+	 * @param rotation Rotation enum
+	 */
 	void set_rotation(Rotation rotation);
 
+	/**
+	 * @brief Set the custom rotation & rotation enum to ROTATION_CUSTOM for Magnetometer
+	 *
+	 * @param rotation Rotation euler angles
+	 */
+	void set_custom_rotation(const matrix::Eulerf &rotation);
+
 	bool calibrated() const { return (_device_id != 0) && (_calibration_index >= 0); }
 	uint8_t calibration_count() const { return _calibration_count; }
 	int8_t calibration_index() const { return _calibration_index; }
@@ -103,7 +116,13 @@ class Magnetometer
 private:
 	Rotation _rotation_enum{ROTATION_NONE};
 
+	/**
+	 * @brief 3 x 3 Rotation matrix that translates from sensor frame (XYZ) to vehicle body frame (FRD)
+	 */
 	matrix::Dcmf _rotation;
+
+	matrix::Eulerf _rotation_custom_euler{0.f, 0.f, 0.f}; // custom rotation euler angles (optional)
+
 	matrix::Vector3f _offset;
 	matrix::Matrix3f _scale;
 	matrix::Vector3f _power_compensation;

diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
@@ -771,6 +771,10 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
 
 							// FALLTHROUGH
 							case ROTATION_PITCH_180_YAW_270: // skip 27, same as 10 ROTATION_ROLL_180_YAW_90
+
+							// FALLTHROUGH
+							case ROTATION_CUSTOM: // Skip, as we currently don't support detecting arbitrary euler angle orientation
+
 								MSE[r] = FLT_MAX;
 								break;
 
@@ -831,6 +835,11 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
 									 worker_data.calibration[cur_mag].device_id(), worker_data.calibration[cur_mag].rotation_enum());
 								continue;
 
+							case ROTATION_CUSTOM:
+								PX4_INFO("[cal] External Mag: %d (%" PRIu32 "), not setting rotation enum since it's specified by Euler Angle",
+									 cur_mag, worker_data.calibration[cur_mag].device_id());
+								continue; // Continue to the next mag loop
+
 							default:
 								break;
 							}

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
@@ -224,8 +224,13 @@ static_assert(MAV_SENSOR_ROTATION_ROLL_90_PITCH_68_YAW_293 == static_cast<MAV_SE
 static_assert(MAV_SENSOR_ROTATION_PITCH_315 == static_cast<MAV_SENSOR_ORIENTATION>(ROTATION_PITCH_315), "Pitch: 315");
 static_assert(MAV_SENSOR_ROTATION_ROLL_90_PITCH_315 == static_cast<MAV_SENSOR_ORIENTATION>(ROTATION_ROLL_90_PITCH_315),
 	      "Roll: 90, Pitch: 315");
+
+// Note: Update the number (41, as of writing) below to the number of 'normal' rotation enums in MAVLink spec:
+// https://mavlink.io/en/messages/common.html#MAV_SENSOR_ORIENTATION
 static_assert(41 == ROTATION_MAX, "Keep MAV_SENSOR_ROTATION and PX4 Rotation in sync");
 
+static_assert(MAV_SENSOR_ROTATION_CUSTOM == static_cast<MAV_SENSOR_ORIENTATION>(ROTATION_CUSTOM), "Custom Rotation");
+
 
 union px4_custom_mode get_px4_custom_mode(uint8_t nav_state)
 {

diff --git a/src/modules/sensors/module.yaml b/src/modules/sensors/module.yaml
@@ -348,6 +348,7 @@ parameters:
                 short: Magnetometer ${i} rotation relative to airframe
                 long: |
                     An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero.
+                    Set to "Custom Euler Angle" to define the rotation using CAL_MAG${i}_ROLL, CAL_MAG${i}_PITCH and CAL_MAG${i}_YAW.
             category: System
             type: enum
             values:
@@ -393,12 +394,52 @@ parameters:
                 38: Roll 90°, Pitch 68°, Yaw 293°
                 39: Pitch 315°
                 40: Roll 90°, Pitch 315°
+                100: Custom Euler Angle
             min: -1
-            max: 40
+            max: 100
             default: -1
             num_instances: *max_num_sensor_instances
             instance_start: 0
 
+        CAL_MAG${i}_ROLL:
+            description:
+                short: Magnetometer ${i} Custom Euler Roll Angle
+                long: Setting this parameter changes CAL_MAG${i}_ROT to "Custom Euler Angle"
+            category: System
+            type: float
+            default: 0.0
+            min: -180
+            max: 180
+            unit: deg
+            num_instances: *max_num_sensor_instances
+            instance_start: 0
+
+        CAL_MAG${i}_PITCH:
+            description:
+                short: Magnetometer ${i} Custom Euler Pitch Angle
+                long: Setting this parameter changes CAL_MAG${i}_ROT to "Custom Euler Angle"
+            category: System
+            type: float
+            default: 0.0
+            min: -180
+            max: 180
+            unit: deg
+            num_instances: *max_num_sensor_instances
+            instance_start: 0
+
+        CAL_MAG${i}_YAW:
+            description:
+                short: Magnetometer ${i} Custom Euler Yaw Angle
+                long: Setting this parameter changes CAL_MAG${i}_ROT to "Custom Euler Angle"
+            category: System
+            type: float
+            default: 0.0
+            min: -180
+            max: 180
+            unit: deg
+            num_instances: *max_num_sensor_instances
+            instance_start: 0
+
         CAL_MAG${i}_XOFF:
             description:
                 short: Magnetometer ${i} X-axis offset