From c76cdaf8a8e62fd64a67947e245ebca98740515e Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 30 Jun 2020 22:37:08 -0400 Subject: [PATCH] commander: mag calibration add simple offset only quick cal using GPS & attitude --- .ci/Jenkinsfile-hardware | 2 + src/modules/commander/Commander.cpp | 15 +++- src/modules/commander/mag_calibration.cpp | 83 +++++++++++++++++++++++ src/modules/commander/mag_calibration.h | 1 + 4 files changed, 99 insertions(+), 2 deletions(-) diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware index c05c3b3191..5312c7855e 100644 --- a/.ci/Jenkinsfile-hardware +++ b/.ci/Jenkinsfile-hardware @@ -842,6 +842,7 @@ void statusFTDI() { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander calibrate accel quick; sleep 2; param show CAL_ACC*"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander calibrate gyro; sleep 2; param show CAL_GYRO*"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander calibrate level; sleep 2; param show SENS_BOARD*"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "commander calibrate mag quick; sleep 2; param show CAL_MAG*"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dataman status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "df"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dmesg"' @@ -909,6 +910,7 @@ void statusSEGGER() { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "commander calibrate accel quick; sleep 2; param show CAL_ACC*"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "commander calibrate gyro; sleep 2; param show CAL_GYRO*"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "commander calibrate level; sleep 2; param show SENS_BOARD*"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "commander calibrate mag quick; sleep 2; param show CAL_MAG*"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "dataman status"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "df -h"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "dmesg"' diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 7e7804a55b..6e37e6e579 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -257,8 +257,14 @@ extern "C" __EXPORT int commander_main(int argc, char *argv[]) send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 1.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f); } else if (!strcmp(argv[2], "mag")) { - // magnetometer calibration: param2 = 1 - send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 0.f); + if (argc > 3 && (strcmp(argv[3], "quick") == 0)) { + // magnetometer quick calibration: param2 = 2 + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 2.f, 0.f, 0.f, 0.f, 0.f, 0.f); + + } else { + // magnetometer calibration: param2 = 1 + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 0.f); + } } else if (!strcmp(argv[2], "accel")) { if (argc > 3 && (strcmp(argv[3], "quick") == 0)) { @@ -3507,6 +3513,11 @@ void *commander_low_prio_loop(void *arg) answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); calib_ret = do_mag_calibration(&mavlink_log_pub); + } else if ((int)(cmd.param2) == 2) { + /* magnetometer calibration quick (requires GPS & attitude) */ + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub); + calib_ret = do_mag_calibration_quick(&mavlink_log_pub); + } else if ((int)(cmd.param3) == 1) { /* zero-altitude pressure calibration */ answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub); diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 4ba61d852f..3665957c16 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -59,6 +59,7 @@ #include #include #include +#include #include using namespace matrix; @@ -887,3 +888,85 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma return result; } + +int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radians) +{ + // magnetometer quick calibration + // if GPS available use world magnetic model to zero mag offsets + + Vector3f mag_earth_pred{}; + bool mag_earth_available = false; + + uORB::Subscription vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; + vehicle_gps_position_s gps{}; + + if (vehicle_gps_position_sub.copy(&gps)) { + if (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_declination_gps = get_mag_declination_radians(lat, lon); + const float mag_inclination_gps = get_mag_inclination_radians(lat, lon); + const float mag_strength_gps = get_mag_strength_gauss(lat, lon); + + mag_earth_pred = Dcmf(Eulerf(0, -mag_inclination_gps, mag_declination_gps)) * Vector3f(mag_strength_gps, 0, 0); + + mag_earth_available = true; + } + } + + if (!mag_earth_available) { + calibration_log_critical(mavlink_log_pub, "GPS required for mag quick cal"); + return calibrate_return_error; + + } else { + + uORB::Subscription vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + vehicle_attitude_s attitude{}; + vehicle_attitude_sub.copy(&attitude); + + if (hrt_elapsed_time(&attitude.timestamp) > 1_s) { + calibration_log_critical(mavlink_log_pub, "attitude required for mag quick cal"); + return calibrate_return_error; + } + + calibration_log_critical(mavlink_log_pub, "Assuming vehicle is facing heading %.1f degrees", + (double)math::radians(heading_radians)); + + matrix::Eulerf euler{matrix::Quatf{attitude.q}}; + euler(2) = heading_radians; + + const Vector3f expected_field = Dcmf(euler).transpose() * mag_earth_pred; + + for (uint8_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { + uORB::Subscription mag_sub{ORB_ID(sensor_mag), cur_mag}; + sensor_mag_s mag{}; + mag_sub.copy(&mag); + + if (mag_sub.advertised() && (hrt_elapsed_time(&mag.timestamp) < 1_s)) { + + calibration::Magnetometer cal{mag.device_id, mag.is_external}; + + // force calibration index to uORB index + cal.set_calibration_index(cur_mag); + + // use any existing scale and store the offset to the expected earth field + const Vector3f offset = Vector3f{mag.x, mag.y, mag.z} - (cal.scale().I() * cal.rotation().transpose() * expected_field); + cal.set_offset(offset); + + cal.PrintStatus(); + + // save new calibration + cal.ParametersSave(); + } + } + + param_notify_changes(); + + calibration_log_info(mavlink_log_pub, "Mag quick calibration finished"); + return calibrate_return_ok; + } + + return calibrate_return_error; +} diff --git a/src/modules/commander/mag_calibration.h b/src/modules/commander/mag_calibration.h index c37bf87639..dc22bd6ea5 100644 --- a/src/modules/commander/mag_calibration.h +++ b/src/modules/commander/mag_calibration.h @@ -43,5 +43,6 @@ #include int do_mag_calibration(orb_advert_t *mavlink_log_pub); +int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radians = 0); #endif /* MAG_CALIBRATION_H_ */