aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Compass: take GCS link in place of channel to calibration routines

Browse Source
This commit is contained in:
Peter Barker 2019-08-01 11:05:22 +10:00 committed by Peter Barker
parent cf0b7cf016
commit df01bed14f
2 changed files with 20 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_Compass/AP_Compass.h
View File

@ -150,8 +150,8 @@ public:
*/
MAV_RESULT 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);
bool send_mag_cal_progress(const class GCS_MAVLINK& link);
bool send_mag_cal_report(const class GCS_MAVLINK& link);
// check if the compasses are pointing in the same direction
bool consistent() const;

28
libraries/AP_Compass/AP_Compass_Calibration.cpp
View File

@ -186,15 +186,17 @@ Compass::_accept_calibration_mask(uint8_t mask)
return success;
}
void
Compass::send_mag_cal_progress(mavlink_channel_t chan)
bool Compass::send_mag_cal_progress(const GCS_MAVLINK& link)
{
uint8_t cal_mask = _get_cal_mask();
for (uint8_t compass_id=0; compass_id<COMPASS_MAX_INSTANCES; compass_id++) {
// ensure we don't try to send with no space available
if (!HAVE_PAYLOAD_SPACE(chan, MAG_CAL_PROGRESS)) {
return;
if (!HAVE_PAYLOAD_SPACE(link.get_chan(), MAG_CAL_PROGRESS)) {
// ideally we would only send one progress message per
// call. If we don't return true here we may end up
// hogging *all* the bandwidth
return true;
}
auto& calibrator = _calibrator[compass_id];
@ -205,27 +207,32 @@ Compass::send_mag_cal_progress(mavlink_channel_t chan)
cal_status == COMPASS_CAL_RUNNING_STEP_TWO) {
uint8_t completion_pct = calibrator.get_completion_percent();
auto& completion_mask = calibrator.get_completion_mask();
Vector3f direction(0.0f,0.0f,0.0f);
const Vector3f direction;
uint8_t attempt = _calibrator[compass_id].get_attempt();
mavlink_msg_mag_cal_progress_send(
chan,
link.get_chan(),
compass_id, cal_mask,
cal_status, attempt, completion_pct, completion_mask,
direction.x, direction.y, direction.z
);
}
}
return true;
}
void Compass::send_mag_cal_report(mavlink_channel_t chan)
bool Compass::send_mag_cal_report(const GCS_MAVLINK& link)
{
uint8_t cal_mask = _get_cal_mask();
for (uint8_t compass_id=0; compass_id<COMPASS_MAX_INSTANCES; compass_id++) {
// ensure we don't try to send with no space available
if (!HAVE_PAYLOAD_SPACE(chan, MAG_CAL_REPORT)) {
return;
if (!HAVE_PAYLOAD_SPACE(link.get_chan(), MAG_CAL_REPORT)) {
// ideally we would only send one progress message per
// call. If we don't return true here we may end up
// hogging *all* the bandwidth
return true;
}
uint8_t cal_status = _calibrator[compass_id].get_status();
@ -238,7 +245,7 @@ void Compass::send_mag_cal_report(mavlink_channel_t chan)
uint8_t autosaved = _cal_saved[compass_id];
mavlink_msg_mag_cal_report_send(
chan,
link.get_chan(),
compass_id, cal_mask,
cal_status, autosaved,
fitness,
@ -251,6 +258,7 @@ void Compass::send_mag_cal_report(mavlink_channel_t chan)
);
}
}
return true;
}
bool