From 52f1718bb8d2e139a0888b8d26dbe4fc645ccce3 Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Tue, 23 May 2017 18:34:32 +0200 Subject: [PATCH] commander : warn if excess accels are connected --- src/modules/commander/accelerometer_calibration.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 7a4ac80353..c7a3da707d 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -466,9 +466,17 @@ calibrate_return do_accel_calibration_measurements(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 accel_count = orb_group_count(ORB_ID(sensor_accel)); + + // Warn that we will not calibrate more than max_accels accelerometers + if (accel_count > max_accel_sens) { + calibration_log_critical(mavlink_log_pub, "[cal] Detected %u accels, but will calibrate only %u", accel_count, max_accel_sens); + } + for (unsigned i = 0; i < accel_count; i++) { worker_data.subs[i] = orb_subscribe_multi(ORB_ID(sensor_accel), i); + if (worker_data.subs[i] < 0) { + calibration_log_critical(mavlink_log_pub, "[cal] Accel #%u not found, abort", i); result = calibrate_return_error; break; }