Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: Hook up compass calibrator

This commit is contained in:
Jonathan Challinger 2015-03-07 07:51:53 -08:00 committed by Andrew Tridgell
parent 53fe649016
commit 3739318d2f
1 changed files with 74 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

74
ArduCopter/GCS_Mavlink.cpp
View File

@ -739,6 +739,14 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
case MSG_RETRY_DEFERRED:
break; // just here to prevent a warning
case MSG_MAG_CAL_PROGRESS:
compass.send_mag_cal_progress(chan);
break;
case MSG_MAG_CAL_REPORT:
compass.send_mag_cal_report(chan);
break;
}
return true;
@ -960,6 +968,8 @@ GCS_MAVLINK::data_stream_send(void)
send_message(MSG_MOUNT_STATUS);
send_message(MSG_OPTICAL_FLOW);
send_message(MSG_GIMBAL_REPORT);
send_message(MSG_MAG_CAL_REPORT);
send_message(MSG_MAG_CAL_PROGRESS);
send_message(MSG_EKF_STATUS_REPORT);
send_message(MSG_VIBRATION);
send_message(MSG_RPM);
@ -1456,6 +1466,70 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
}
case MAV_CMD_DO_START_MAG_CAL: {
result = MAV_RESULT_ACCEPTED;
if(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;
if (mag_mask == 0) { // 0 means all
compass.start_calibration_all(retry, autosave, delay);
break;
}
if(!compass.start_calibration_mask(mag_mask, retry, autosave, delay)) {
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(!compass.accept_calibration_all()) {
result = MAV_RESULT_FAILED;
}
break;
}
if(!compass.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
compass.cancel_calibration_all();
break;
}
compass.cancel_calibration_mask(mag_mask);
break;
}
default:
result = MAV_RESULT_UNSUPPORTED;
break;