Skip to content

Commit

Permalink
Enable arbitrary euler angle for Mag rotation CAL_MAGx_{ROLL,PITCH,YAW}
Browse files Browse the repository at this point in the history 
 - 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]>
  • Loading branch information
@bresch @junwoo091400 @dagar
4 people authored Aug 26, 2023
1 parent 9551c1b commit 6a58f6c
Show file tree
Hide file tree
Showing 6 changed files with 140 additions and 7 deletions.
4 changes: 3 additions & 1 deletion src/lib/conversion/rotation.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down
67 changes: 62 additions & 5 deletions src/lib/sensor_calibration/Magnetometer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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>(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
Expand Down Expand Up @@ -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;
}

Expand Down
19 changes: 19 additions & 0 deletions src/lib/sensor_calibration/Magnetometer.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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; }
Expand Down Expand Up @@ -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;
Expand Down
9 changes: 9 additions & 0 deletions src/modules/commander/mag_calibration.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand Up @@ -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;
}
Expand Down
5 changes: 5 additions & 0 deletions src/modules/mavlink/mavlink_messages.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -228,8 +228,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");


static const StreamListItem streams_list[] = {
#if defined(HEARTBEAT_HPP)
Expand Down
43 changes: 42 additions & 1 deletion src/modules/sensors/module.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down Expand Up @@ -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
Expand Down

0 comments on commit 6a58f6c

Please sign in to comment.