From 9909a373b08b91134df19f89a6da610721e6b8fa Mon Sep 17 00:00:00 2001
From: Mohammed Kabir <kabir@uasys.io>
Date: Tue, 18 Apr 2017 12:45:39 +0200
Subject: [PATCH] commander : warn if more than 4 mags are connected
---
src/modules/commander/mag_calibration.cpp | 7 ++++++-
1 file changed, 6 insertions(+), 1 deletion(-)
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index 66f891d87a..6fee6b3a67 100644
--- a/src/modules/commander/mag_calibration.cpp
+++ b/src/modules/commander/mag_calibration.cpp
@@ -565,7 +565,12 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
// We should not try to subscribe if the topic doesn't actually exist and can be counted.
const unsigned mag_count = orb_group_count(ORB_ID(sensor_mag));
- for (unsigned cur_mag = 0; cur_mag < mag_count; cur_mag++) {
+ // Warn that we will not calibrate more than max_mags magnetometers
+ if (mag_count > max_mags) {
+ calibration_log_critical(mavlink_log_pub, "[cal] Detected %u mags, but will calibrate only %u", mag_count, max_mags);
+ }
+
+ for (unsigned cur_mag = 0; cur_mag < mag_count && cur_mag < max_mags; cur_mag++) {
// Mag in this slot is available
worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), cur_mag);
--
GitLab