Skip to content

Commit

Permalink
EKF2 cleanup. Use MAX_NUM_x constants instead of hard-coded numbers
Browse files Browse the repository at this point in the history
Signed-off-by: Antonio Sanjurjo C <74329840+antonio-sc66@users.noreply.github.com>
  • Loading branch information
antonio-sc66 authored and bresch committed Jul 6, 2023
1 parent 39e04d9 commit c1cb48c
Showing 1 changed file with 8 additions and 6 deletions.
14 changes: 8 additions & 6 deletions src/modules/ekf2/EKF2.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -2568,11 +2568,12 @@ int EKF2::task_spawn(int argc, char *argv[])
// ekf selector requires SENS_IMU_MODE = 0
multi_mode = true;

// IMUs (1 - 4 supported)
// IMUs (1 - MAX_NUM_IMUS supported)
param_get(param_find("EKF2_MULTI_IMU"), &imu_instances);

if (imu_instances < 1 || imu_instances > 4) {
const int32_t imu_instances_limited = math::constrain(imu_instances, static_cast<int32_t>(1), static_cast<int32_t>(4));
if (imu_instances < 1 || imu_instances > MAX_NUM_IMUS) {
const int32_t imu_instances_limited = math::constrain(imu_instances, static_cast<int32_t>(1),
static_cast<int32_t>(MAX_NUM_IMUS));
PX4_WARN("EKF2_MULTI_IMU limited %" PRId32 " -> %" PRId32, imu_instances, imu_instances_limited);
param_set_no_notification(param_find("EKF2_MULTI_IMU"), &imu_instances_limited);
imu_instances = imu_instances_limited;
Expand All @@ -2586,9 +2587,10 @@ int EKF2::task_spawn(int argc, char *argv[])
const param_t param_ekf2_mult_mag = param_find("EKF2_MULTI_MAG");
param_get(param_ekf2_mult_mag, &mag_instances);

// Mags (1 - 4 supported)
if (mag_instances > 4) {
const int32_t mag_instances_limited = math::constrain(mag_instances, static_cast<int32_t>(1), static_cast<int32_t>(4));
// Mags (1 - MAX_NUM_MAGS supported)
if (mag_instances > MAX_NUM_MAGS) {
const int32_t mag_instances_limited = math::constrain(mag_instances, static_cast<int32_t>(1),
static_cast<int32_t>(MAX_NUM_MAGS));
PX4_WARN("EKF2_MULTI_MAG limited %" PRId32 " -> %" PRId32, mag_instances, mag_instances_limited);
param_set_no_notification(param_ekf2_mult_mag, &mag_instances_limited);
mag_instances = mag_instances_limited;
Expand Down

0 comments on commit c1cb48c

Please sign in to comment.