From c16e17f47b5ac50c72ca90c37ac6a5936f782027 Mon Sep 17 00:00:00 2001 From: alessandro Date: Fri, 13 Apr 2018 15:02:29 +0200 Subject: [PATCH] mag_calibration: only allocate as much memory as needed --- src/modules/commander/mag_calibration.cpp | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 7a2d3f64d7..08047441e6 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -569,7 +569,15 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub) char str[30]; - for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) { + // Get actual mag count and alloate only as much memory as needed + const unsigned orb_mag_count = orb_group_count(ORB_ID(sensor_mag)); + + // Warn that we will not calibrate more than max_mags magnetometers + if (orb_mag_count > max_mags) { + calibration_log_critical(mavlink_log_pub, "Detected %u mags, but will calibrate only %u", orb_mag_count, max_mags); + } + + for (size_t cur_mag = 0; cur_mag < orb_mag_count && cur_mag < max_mags; cur_mag++) { worker_data.x[cur_mag] = reinterpret_cast(malloc(sizeof(float) * calibration_points_maxcount)); worker_data.y[cur_mag] = reinterpret_cast(malloc(sizeof(float) * calibration_points_maxcount)); worker_data.z[cur_mag] = reinterpret_cast(malloc(sizeof(float) * calibration_points_maxcount)); @@ -585,13 +593,6 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub) if (result == calibrate_return_ok) { // We should not try to subscribe if the topic doesn't actually exist and can be counted. - const unsigned orb_mag_count = orb_group_count(ORB_ID(sensor_mag)); - - // Warn that we will not calibrate more than max_mags magnetometers - if (orb_mag_count > max_mags) { - calibration_log_critical(mavlink_log_pub, "Detected %u mags, but will calibrate only %u", orb_mag_count, max_mags); - } - for (unsigned cur_mag = 0; cur_mag < orb_mag_count && cur_mag < max_mags; cur_mag++) { // Lock in to correct ORB instance