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 d4d1ec112db6..95079371ec1e 100644 --- a/src/lib/sensor_calibration/Magnetometer.cpp +++ b/src/lib/sensor_calibration/Magnetometer.cpp @@ -159,12 +159,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) @@ -200,13 +227,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 @@ -311,6 +364,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 047cd3398bc7..8d328e7a7d11 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; } @@ -108,7 +121,13 @@ class Magnetometer 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 _thermal_offset; diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 184e10f876fb..3fff697d4940 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 43f7fab01645..ef8f1c3b5527 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -228,8 +228,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"); + static const StreamListItem streams_list[] = { #if defined(HEARTBEAT_HPP) diff --git a/src/modules/sensors/module.yaml b/src/modules/sensors/module.yaml index 70670f882c3a..5fa440d5ff24 100644 --- a/src/modules/sensors/module.yaml +++ b/src/modules/sensors/module.yaml @@ -360,6 +360,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: @@ -405,12 +406,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