Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: added handling of compass calibration mavlink messages

Browse Source
This commit is contained in:
Andrew Tridgell 2015-08-23 20:23:38 +10:00
parent 1ffbffa0e7
commit f4cdf57d8f
2 changed files with 84 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

79
libraries/AP_Compass/AP_Compass_Calibration.cpp
View File

@ -242,3 +242,82 @@ Compass::get_cal_mask() const
}
return cal_mask;
}
/*
handle an incoming MAG_CAL command
*/
uint8_t Compass::handle_mag_cal_command(const mavlink_command_long_t &packet)
{
uint8_t result = MAV_RESULT_FAILED;
switch (packet.command) {
case MAV_CMD_DO_START_MAG_CAL: {
result = MAV_RESULT_ACCEPTED;
if (hal.util->get_soft_armed() || packet.param1 < 0 || packet.param1 > 255) {
result = MAV_RESULT_FAILED;
break;
}
uint8_t mag_mask = packet.param1;
bool retry = packet.param2;
bool autosave = packet.param3;
float delay = packet.param4;
bool autoreboot = packet.param5;
if (mag_mask == 0) { // 0 means all
if (!start_calibration_all(retry, autosave, delay, autoreboot)) {
result = MAV_RESULT_FAILED;
}
} else {
if (!start_calibration_mask(mag_mask, retry, autosave, delay, autoreboot)) {
result = MAV_RESULT_FAILED;
}
}
break;
}
case MAV_CMD_DO_ACCEPT_MAG_CAL: {
result = MAV_RESULT_ACCEPTED;
if(packet.param1 < 0 || packet.param1 > 255) {
result = MAV_RESULT_FAILED;
break;
}
uint8_t mag_mask = packet.param1;
if (mag_mask == 0) { // 0 means all
if(!accept_calibration_all()) {
result = MAV_RESULT_FAILED;
}
break;
}
if(!accept_calibration_mask(mag_mask)) {
result = MAV_RESULT_FAILED;
}
break;
}
case MAV_CMD_DO_CANCEL_MAG_CAL: {
result = MAV_RESULT_ACCEPTED;
if(packet.param1 < 0 || packet.param1 > 255) {
result = MAV_RESULT_FAILED;
break;
}
uint8_t mag_mask = packet.param1;
if (mag_mask == 0) { // 0 means all
cancel_calibration_all();
break;
}
cancel_calibration_mask(mag_mask);
break;
}
}
return result;
}

5
libraries/AP_Compass/Compass.h
View File

@ -152,6 +152,11 @@ public:
uint8_t get_cal_mask() const;
bool is_calibrating() const;
/*
handle an incoming MAG_CAL command
*/
uint8_t handle_mag_cal_command(const mavlink_command_long_t &packet);
void send_mag_cal_progress(mavlink_channel_t chan);
void send_mag_cal_report(mavlink_channel_t chan);