From d98843f440a39da6455ab2743c58520beaccfe57 Mon Sep 17 00:00:00 2001 From: Mathieu Bresciani Date: Sat, 26 Aug 2023 20:52:39 +0200 Subject: [PATCH] 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 Co-authored-by: bresch <[brescianimathieu@gmail.com](mailto:brescianimathieu@gmail.com)> Co-authored-by: Daniel Agar --- src/lib/conversion/rotation.h | 4 +- src/lib/sensor_calibration/Magnetometer.cpp | 67 +++++++++++++++++++-- src/lib/sensor_calibration/Magnetometer.hpp | 19 ++++++ src/modules/commander/mag_calibration.cpp | 9 +++ src/modules/mavlink/mavlink_messages.cpp | 5 ++ src/modules/sensors/module.yaml | 43 ++++++++++++- 6 files changed, 140 insertions(+), 7 deletions(-) diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index 80a9910d8be7..b33e4a8383f0 100644 --- 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 index 4ea0e6982ea8..0c3a8b3102c5 100644 --- 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_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_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 index 24b9fa0ee052..2ad441880b72 100644 --- 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 index 7255581ff3f1..cb5b2195a10f 100644 --- 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 index fdef95417dfa..dd2144383a9e 100644 --- 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(ROTATION_PITCH_315), "Pitch: 315"); static_assert(MAV_SENSOR_ROTATION_ROLL_90_PITCH_315 == static_cast(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(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 index d64b87de9300..143210f31c0a 100644 --- 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