Skip to content

Commit 2da05c1

Browse files
JMoore5353JMoore5353
authored
feat: add mag orientation params to firmware (#476)
* feat: add mag orientation params to firmware * cleanup: rename FC_roll|pitch|yaw params to IMU_roll|pitch|yaw
1 parent 6113955 commit 2da05c1

File tree

5 files changed

+65
-20
lines changed

5 files changed

+65
-20
lines changed

‎include/param.h‎

Lines changed: 7 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -366,9 +366,13 @@ enum : uint16_t
366366
PARAM_AILERON_REVERSE,
367367
PARAM_RUDDER_REVERSE,
368368

369-
PARAM_FC_ROLL,
370-
PARAM_FC_PITCH,
371-
PARAM_FC_YAW,
369+
PARAM_IMU_ROLL,
370+
PARAM_IMU_PITCH,
371+
PARAM_IMU_YAW,
372+
373+
PARAM_MAG_ROLL,
374+
PARAM_MAG_PITCH,
375+
PARAM_MAG_YAW,
372376

373377
/********************/
374378
/*** ARMING SETUP ***/

‎include/sensors.h‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -86,7 +86,9 @@ class Sensors : public ParamListenerInterface
8686
GnssStruct gnss_ = {};
8787

8888
void rotate_imu_in_place(ImuStruct * imu, turbomath::Quaternion q);
89-
turbomath::Quaternion fcu_orientation_ = {1, 0, 0, 0};
89+
void rotate_mag_in_place(MagStruct * mag, turbomath::Quaternion q);
90+
turbomath::Quaternion imu_orientation_ = {1, 0, 0, 0};
91+
turbomath::Quaternion mag_orientation_ = {1, 0, 0, 0};
9092

9193
static const int SENSOR_CAL_DELAY_CYCLES;
9294
static const int SENSOR_CAL_CYCLES;
@@ -114,6 +116,7 @@ class Sensors : public ParamListenerInterface
114116
bool calibrating_acc_flag_ = false;
115117
bool calibrating_gyro_flag_ = false;
116118
void init_imu();
119+
void init_mag();
117120
void calibrate_accel(void);
118121
void calibrate_gyro(void);
119122
void calibrate_baro(void);

‎src/param.cpp‎

Lines changed: 6 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -413,10 +413,13 @@ void Params::set_defaults(void)
413413
init_param_int(PARAM_AILERON_REVERSE, "AIL_REV", 0); // reverses aileron servo output | 0 | 1
414414
init_param_int(PARAM_RUDDER_REVERSE, "RUDDER_REV", 0); // reverses rudder servo output | 0 | 1
415415

416-
init_param_float(PARAM_FC_ROLL, "FC_ROLL", 0.0f); // roll angle (deg) of flight controller wrt aircraft body | 0 | 360
417-
init_param_float(PARAM_FC_PITCH, "FC_PITCH", 0.0f); // pitch angle (deg) of flight controller wrt aircraft body | 0 | 360
418-
init_param_float(PARAM_FC_YAW, "FC_YAW", 0.0f); // yaw angle (deg) of flight controller wrt aircraft body | 0 | 360
416+
init_param_float(PARAM_IMU_ROLL, "IMU_ROLL", 0.0f); // roll angle (deg) of IMU wrt aircraft body | 0 | 360
417+
init_param_float(PARAM_IMU_PITCH, "IMU_PITCH", 0.0f); // pitch angle (deg) of IMU wrt aircraft body | 0 | 360
418+
init_param_float(PARAM_IMU_YAW, "IMU_YAW", 0.0f); // yaw angle (deg) of IMU wrt aircraft body | 0 | 360
419419

420+
init_param_float(PARAM_MAG_ROLL, "MAG_ROLL", 0.0f); // roll angle (deg) of magnetometer wrt aircraft body | 0 | 360
421+
init_param_float(PARAM_MAG_PITCH, "MAG_PITCH", 0.0f); // pitch angle (deg) of magnetometer wrt aircraft body | 0 | 360
422+
init_param_float(PARAM_MAG_YAW, "MAG_YAW", 0.0f); // yaw angle (deg) of magnetometer wrt aircraft body | 0 | 360
420423

421424
/********************/
422425
/*** ARMING SETUP ***/

‎src/sensors.cpp‎

Lines changed: 42 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -63,17 +63,18 @@ void Sensors::init()
6363
rf_.board_.sensors_init();
6464

6565
init_imu();
66+
init_mag();
6667

6768
this->update_battery_monitor_multipliers();
6869
}
6970

7071
void Sensors::init_imu()
7172
{
72-
// Quaternion to compensate for FCU orientation
73-
float roll = rf_.params_.get_param_float(PARAM_FC_ROLL) * 0.017453293;
74-
float pitch = rf_.params_.get_param_float(PARAM_FC_PITCH) * 0.017453293;
75-
float yaw = rf_.params_.get_param_float(PARAM_FC_YAW) * 0.017453293;
76-
fcu_orientation_ = turbomath::Quaternion(roll, pitch, yaw);
73+
// Quaternion to compensate for IMU orientation
74+
float roll = rf_.params_.get_param_float(PARAM_IMU_ROLL) * 0.017453293;
75+
float pitch = rf_.params_.get_param_float(PARAM_IMU_PITCH) * 0.017453293;
76+
float yaw = rf_.params_.get_param_float(PARAM_IMU_YAW) * 0.017453293;
77+
imu_orientation_ = turbomath::Quaternion(roll, pitch, yaw);
7778

7879
// See if the IMU is uncalibrated, and throw an error if it is
7980
if (rf_.params_.get_param_float(PARAM_ACC_X_BIAS) == 0.0
@@ -86,14 +87,28 @@ void Sensors::init_imu()
8687
}
8788
}
8889

90+
void Sensors::init_mag()
91+
{
92+
// Quaternion to compensate for mag orientation
93+
float roll = rf_.params_.get_param_float(PARAM_MAG_ROLL) * 0.017453293;
94+
float pitch = rf_.params_.get_param_float(PARAM_MAG_PITCH) * 0.017453293;
95+
float yaw = rf_.params_.get_param_float(PARAM_MAG_YAW) * 0.017453293;
96+
mag_orientation_ = turbomath::Quaternion(roll, pitch, yaw);
97+
}
98+
8999
void Sensors::param_change_callback(uint16_t param_id)
90100
{
91101
switch (param_id) {
92-
case PARAM_FC_ROLL:
93-
case PARAM_FC_PITCH:
94-
case PARAM_FC_YAW:
102+
case PARAM_IMU_ROLL:
103+
case PARAM_IMU_PITCH:
104+
case PARAM_IMU_YAW:
95105
init_imu();
96106
break;
107+
case PARAM_MAG_ROLL:
108+
case PARAM_MAG_PITCH:
109+
case PARAM_MAG_YAW:
110+
init_mag();
111+
break;
97112
case PARAM_BATTERY_VOLTAGE_MULTIPLIER:
98113
case PARAM_BATTERY_CURRENT_MULTIPLIER:
99114
update_battery_monitor_multipliers();
@@ -135,14 +150,28 @@ void Sensors::rotate_imu_in_place(ImuStruct * imu, turbomath::Quaternion rotatio
135150
imu->gyro[2] = gyro.z;
136151
}
137152

153+
void Sensors::rotate_mag_in_place(MagStruct * mag, turbomath::Quaternion rotation)
154+
{
155+
turbomath::Vector flux;
156+
flux.x = mag->flux[0];
157+
flux.y = mag->flux[1];
158+
flux.z = mag->flux[2];
159+
160+
flux = rotation * flux;
161+
162+
mag->flux[0] = flux.x;
163+
mag->flux[1] = flux.y;
164+
mag->flux[2] = flux.z;
165+
}
166+
138167
got_flags Sensors::run()
139168
{
140169
got_flags got = {};
141170

142171
// IMU:
143172
if ((got.imu = rf_.board_.imu_read(&imu_))) {
144173
rf_.state_manager_.clear_error(StateManager::ERROR_IMU_NOT_RESPONDING);
145-
rotate_imu_in_place(&imu_, fcu_orientation_);
174+
rotate_imu_in_place(&imu_, imu_orientation_);
146175

147176
if (calibrating_acc_flag_) { calibrate_accel(); }
148177
if (calibrating_gyro_flag_) { calibrate_gyro(); }
@@ -160,7 +189,10 @@ got_flags Sensors::run()
160189
}
161190

162191
// MAGNETOMETER:
163-
if ((got.mag = rf_.board_.mag_read(&mag_))) { correct_mag(); }
192+
if ((got.mag = rf_.board_.mag_read(&mag_))) {
193+
rotate_mag_in_place(&mag_, mag_orientation_);
194+
correct_mag();
195+
}
164196

165197
// DIFF_PRESSURE:
166198
if ((got.diff_pressure = rf_.board_.diff_pressure_read(&diff_pressure_))) {

‎test/parameters_test.cpp‎

Lines changed: 6 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -214,9 +214,12 @@ TEST(Parameters, DefaultParameters)
214214
EXPECT_PARAM_EQ_INT(PARAM_ELEVATOR_REVERSE, 0); // reverses elevator servo output | 0 | 1
215215
EXPECT_PARAM_EQ_INT(PARAM_AILERON_REVERSE, 0); // reverses aileron servo output | 0 | 1
216216
EXPECT_PARAM_EQ_INT(PARAM_RUDDER_REVERSE, 0); // reverses rudder servo output | 0 | 1
217-
EXPECT_PARAM_EQ_FLOAT(PARAM_FC_ROLL, 0.0f); // roll angle (deg) of flight controller wrt aircraft body | 0 | 360
218-
EXPECT_PARAM_EQ_FLOAT(PARAM_FC_PITCH, 0.0f); // pitch angle (deg) of flight controller wrt aircraft body | 0 | 360
219-
EXPECT_PARAM_EQ_FLOAT(PARAM_FC_YAW, 0.0f); // yaw angle (deg) of flight controller wrt aircraft body | 0 | 360
217+
EXPECT_PARAM_EQ_FLOAT(PARAM_IMU_ROLL, 0.0f); // roll angle (deg) of IMU wrt aircraft body | 0 | 360
218+
EXPECT_PARAM_EQ_FLOAT(PARAM_IMU_PITCH, 0.0f); // pitch angle (deg) of IMU wrt aircraft body | 0 | 360
219+
EXPECT_PARAM_EQ_FLOAT(PARAM_IMU_YAW, 0.0f); // yaw angle (deg) of IMU wrt aircraft body | 0 | 360
220+
EXPECT_PARAM_EQ_FLOAT(PARAM_MAG_ROLL, 0.0f); // roll angle (deg) of magnetometer wrt aircraft body | 0 | 360
221+
EXPECT_PARAM_EQ_FLOAT(PARAM_MAG_PITCH, 0.0f); // pitch angle (deg) of magnetometer wrt aircraft body | 0 | 360
222+
EXPECT_PARAM_EQ_FLOAT(PARAM_MAG_YAW, 0.0f); // yaw angle (deg) of magnetometer wrt aircraft body | 0 | 360
220223
EXPECT_PARAM_EQ_FLOAT(PARAM_BATTERY_VOLTAGE_MULTIPLIER, 1.0f); // Multiplier for the voltage sensor | 0 | inf
221224
EXPECT_PARAM_EQ_FLOAT(PARAM_BATTERY_CURRENT_MULTIPLIER, 1.0f); // Multiplier for the current sensor | 0 | inf
222225
EXPECT_PARAM_EQ_INT(PARAM_OFFBOARD_TIMEOUT, 100); // Timeout in milliseconds for offboard commands, after which RC override is activated | 0 | 100000

0 commit comments

Comments
 (0)