From 09af5e0637e626895f92e36e665eb7835c6730b7 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 20 Sep 2020 12:22:56 -0400 Subject: [PATCH] commander: mag calibration use expected WMM sphere radius to reject samples if GPS is available --- src/modules/commander/mag_calibration.cpp | 55 +++++++++++++---------- 1 file changed, 32 insertions(+), 23 deletions(-) diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 464af4e2d7..c45f699ea6 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -57,6 +57,7 @@ #include #include #include +#include #include #include #include @@ -135,9 +136,9 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub) } static bool reject_sample(float sx, float sy, float sz, float x[], float y[], float z[], unsigned count, - unsigned max_count) + unsigned max_count, float mag_sphere_radius) { - float min_sample_dist = fabsf(5.4f * MAG_SPHERE_RADIUS_DEFAULT / sqrtf(max_count)) / 3.0f; + float min_sample_dist = fabsf(5.4f * mag_sphere_radius / sqrtf(max_count)) / 3.0f; for (size_t i = 0; i < count; i++) { float dx = sx - x[i]; @@ -222,6 +223,28 @@ static calibrate_return check_calibration_result(float offset_x, float offset_y, return calibrate_return_ok; } +static float get_sphere_radius() +{ + // if GPS is available use real field intensity from world magnetic model + uORB::SubscriptionMultiArray gps_subs{ORB_ID::vehicle_gps_position}; + + for (auto &gps_sub : gps_subs) { + vehicle_gps_position_s gps; + + if (gps_sub.copy(&gps)) { + if (hrt_elapsed_time(&gps.timestamp) < 100_s && (gps.fix_type >= 2) && (gps.eph < 1000)) { + const double lat = gps.lat / 1.e7; + const double lon = gps.lon / 1.e7; + + // magnetic field data returned by the geo library using the current GPS position + return get_mag_strength_gauss(lat, lon); + } + } + } + + return MAG_SPHERE_RADIUS_DEFAULT; +} + static calibrate_return mag_calibration_worker(detect_orientation_return orientation, void *data) { const hrt_abstime calibration_started = hrt_absolute_time(); @@ -229,6 +252,8 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta mag_worker_data_t *worker_data = (mag_worker_data_t *)(data); + float mag_sphere_radius = get_sphere_radius(); + // notify user to start rotating set_tune(TONE_SINGLE_BEEP_TUNE); @@ -334,7 +359,8 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta bool reject = reject_sample(mag.x, mag.y, mag.z, worker_data->x[cur_mag], worker_data->y[cur_mag], worker_data->z[cur_mag], worker_data->calibration_counter_total[cur_mag], - worker_data->calibration_sides * worker_data->calibration_points_perside); + worker_data->calibration_sides * worker_data->calibration_points_perside, + mag_sphere_radius); if (!reject) { new_samples[cur_mag] = Vector3f{mag.x, mag.y, mag.z}; @@ -487,33 +513,16 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma Vector3f offdiag[MAX_MAGS]; float sphere_radius[MAX_MAGS]; + const float mag_sphere_radius = get_sphere_radius(); + for (size_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { - sphere_radius[cur_mag] = MAG_SPHERE_RADIUS_DEFAULT; + sphere_radius[cur_mag] = mag_sphere_radius; sphere[cur_mag].zero(); diag[cur_mag] = Vector3f{1.f, 1.f, 1.f}; offdiag[cur_mag].zero(); } if (result == calibrate_return_ok) { - - // if GPS is available use real field intensity from world magnetic model - uORB::SubscriptionData gps_sub{ORB_ID(vehicle_gps_position)}; - const vehicle_gps_position_s &gps = gps_sub.get(); - - if (hrt_elapsed_time(&gps.timestamp) < 10_s && (gps.fix_type >= 2) && (gps.eph < 1000)) { - const double lat = gps.lat / 1.e7; - const double lon = gps.lon / 1.e7; - - // magnetic field data returned by the geo library using the current GPS position - const float mag_strength_gps = get_mag_strength_gauss(lat, lon); - - PX4_INFO("[cal] using current GPS for field strength: %.4f Gauss", (double)mag_strength_gps); - - for (size_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { - sphere_radius[cur_mag] = mag_strength_gps; - } - } - // Sphere fit the data to get calibration values for (uint8_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { if (worker_data.calibration[cur_mag].device_id() != 0) {