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

commander: quick mag cal with fixed heading use MAV_CMD_FIXED_MAG_CAL_YAW message

Browse Source 
- use proper Mavlink MAV_CMD_FIXED_MAG_CAL_YAW command for initiating magnetometer quick cal
 - MAV_CMD_FIXED_MAG_CAL_YAW allows specifying the yaw and optionally latitude and longitude if the vehicle doesn't have GPS
This commit is contained in:
Daniel Agar
2020-09-06 19:25:11 -04:00
committed by GitHub
parent 1ac31100cc
commit ca9b6bc137
5 changed files with 73 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+44 -8
View File
@@ -258,8 +258,8 @@ extern "C" __EXPORT int commander_main(int argc, char *argv[])
} else if (!strcmp(argv[2], "mag")) {
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);
// magnetometer quick calibration: VEHICLE_CMD_FIXED_MAG_CAL_YAW
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_FIXED_MAG_CAL_YAW);
} else {
// magnetometer calibration: param2 = 1
@@ -276,7 +276,6 @@ extern "C" __EXPORT int commander_main(int argc, char *argv[])
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f);
}
} else if (!strcmp(argv[2], "level")) {
// board level calibration: param5 = 2
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 2.f, 0.f, 0.f);
@@ -1115,6 +1114,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_LOCATION:
case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET:
case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_NONE:
case vehicle_command_s::VEHICLE_CMD_FIXED_MAG_CAL_YAW:
/* ignore commands that are handled by other parts of the system */
break;
@@ -3521,11 +3521,6 @@ 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);
@@ -3611,6 +3606,47 @@ void *commander_low_prio_loop(void *arg)
break;
}
case vehicle_command_s::VEHICLE_CMD_FIXED_MAG_CAL_YAW: {
// Magnetometer quick calibration using world magnetic model and known heading
if ((status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)
|| (status.arming_state == vehicle_status_s::ARMING_STATE_SHUTDOWN)
|| status_flags.condition_calibration_enabled) {
// reject if armed or shutting down
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED, command_ack_pub);
} else {
// parameter 1: Yaw in degrees
// parameter 3: Latitude
// parameter 4: Longitude
// assume vehicle pointing north (0 degrees) if heading isn't specified
const float heading_radians = PX4_ISFINITE(cmd.param1) ? math::radians(roundf(cmd.param1)) : 0.f;
float latitude = NAN;
float longitude = NAN;
if (PX4_ISFINITE(cmd.param3) && PX4_ISFINITE(cmd.param4)) {
// invalid if both lat & lon are 0 (current mavlink spec)
if ((fabsf(cmd.param3) > 0) && (fabsf(cmd.param4) > 0)) {
latitude = cmd.param3;
longitude = cmd.param4;
}
}
if (do_mag_calibration_quick(&mavlink_log_pub, heading_radians, latitude, longitude) == PX4_OK) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
tune_positive(true);
} else {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED, command_ack_pub);
tune_negative(true);
}
}
break;
}
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE: {
if ((status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)
src/modules/commander/mag_calibration.cpp
+20 -17
View File
@@ -901,30 +901,25 @@ 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)
int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radians, float latitude, float longitude)
{
// 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 (PX4_ISFINITE(latitude) && PX4_ISFINITE(longitude)) {
mag_earth_available = true;
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;
} else {
uORB::Subscription vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
vehicle_gps_position_s gps;
// 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 (vehicle_gps_position_sub.copy(&gps)) {
if ((gps.timestamp != 0) && (gps.eph < 1000)) {
latitude = gps.lat / 1.e7f;
longitude = gps.lon / 1.e7f;
mag_earth_available = true;
}
}
}
@@ -934,6 +929,14 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian
} else {
// magnetic field data returned by the geo library using the current GPS position
const float mag_declination_gps = get_mag_declination_radians(latitude, longitude);
const float mag_inclination_gps = get_mag_inclination_radians(latitude, longitude);
const float mag_strength_gps = get_mag_strength_gauss(latitude, longitude);
const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -mag_inclination_gps, mag_declination_gps)) * Vector3f(mag_strength_gps,
0, 0);
uORB::Subscription vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
vehicle_attitude_s attitude{};
vehicle_attitude_sub.copy(&attitude);
src/modules/commander/mag_calibration.h
+3 -1
View File
@@ -39,10 +39,12 @@
#ifndef MAG_CALIBRATION_H_
#define MAG_CALIBRATION_H_
#include <math.h>
#include <stdint.h>
#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);
int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radians = 0, float latitude = NAN,
float longitude = NAN);
#endif /* MAG_CALIBRATION_H_ */