fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: mag calibration add simple offset only quick cal using GPS & attitude

Browse Source
This commit is contained in:
Daniel Agar 2020-06-30 22:37:08 -04:00
parent f6ad9bdb64
commit c76cdaf8a8
4 changed files with 99 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.ci/Jenkinsfile-hardware
View File

@ -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"'

15
src/modules/commander/Commander.cpp
View File

@ -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);

83
src/modules/commander/mag_calibration.cpp
View File

@ -59,6 +59,7 @@
#include <uORB/SubscriptionBlocking.hpp>
#include <uORB/topics/sensor_mag.h>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_gps_position.h>
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;
}

1
src/modules/commander/mag_calibration.h
View File

@ -43,5 +43,6 @@
#include <uORB/uORB.h>
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_ */