Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-27 02:58:31 -04:00
Code Issues 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); MAV_RESULT handle_mag_cal_command(const mavlink_command_long_t &packet);
void send_mag_cal_progress(mavlink_channel_t chan); bool send_mag_cal_progress(const class GCS_MAVLINK& link);
void send_mag_cal_report(mavlink_channel_t chan); bool send_mag_cal_report(const class GCS_MAVLINK& link);
// check if the compasses are pointing in the same direction // check if the compasses are pointing in the same direction
bool consistent() const; 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; return success;
} }
void bool Compass::send_mag_cal_progress(const GCS_MAVLINK& link)
Compass::send_mag_cal_progress(mavlink_channel_t chan)
{ {
uint8_t cal_mask = _get_cal_mask(); uint8_t cal_mask = _get_cal_mask();
for (uint8_t compass_id=0; compass_id<COMPASS_MAX_INSTANCES; compass_id++) { for (uint8_t compass_id=0; compass_id<COMPASS_MAX_INSTANCES; compass_id++) {
// ensure we don't try to send with no space available // ensure we don't try to send with no space available
if (!HAVE_PAYLOAD_SPACE(chan, MAG_CAL_PROGRESS)) { if (!HAVE_PAYLOAD_SPACE(link.get_chan(), MAG_CAL_PROGRESS)) {
return; // 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]; 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) { cal_status == COMPASS_CAL_RUNNING_STEP_TWO) {
uint8_t completion_pct = calibrator.get_completion_percent(); uint8_t completion_pct = calibrator.get_completion_percent();
auto& completion_mask = calibrator.get_completion_mask(); 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(); uint8_t attempt = _calibrator[compass_id].get_attempt();
mavlink_msg_mag_cal_progress_send( mavlink_msg_mag_cal_progress_send(
chan, link.get_chan(),
compass_id, cal_mask, compass_id, cal_mask,
cal_status, attempt, completion_pct, completion_mask, cal_status, attempt, completion_pct, completion_mask,
direction.x, direction.y, direction.z 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(); uint8_t cal_mask = _get_cal_mask();
for (uint8_t compass_id=0; compass_id<COMPASS_MAX_INSTANCES; compass_id++) { for (uint8_t compass_id=0; compass_id<COMPASS_MAX_INSTANCES; compass_id++) {
// ensure we don't try to send with no space available // ensure we don't try to send with no space available
if (!HAVE_PAYLOAD_SPACE(chan, MAG_CAL_REPORT)) { if (!HAVE_PAYLOAD_SPACE(link.get_chan(), MAG_CAL_REPORT)) {
return; // 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(); 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]; uint8_t autosaved = _cal_saved[compass_id];
mavlink_msg_mag_cal_report_send( mavlink_msg_mag_cal_report_send(
chan, link.get_chan(),
compass_id, cal_mask, compass_id, cal_mask,
cal_status, autosaved, cal_status, autosaved,
fitness, fitness,
@ -251,6 +258,7 @@ void Compass::send_mag_cal_report(mavlink_channel_t chan)
); );
} }
} }
return true;
} }
bool bool