Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 00:58:37 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: Only send a single MAG_CAL_* message per poll

Browse Source 
this fairly allocates bandwidth between the calibrators
This commit is contained in:
Michael du Breuil 2020-06-11 17:35:32 -07:00 committed by Andrew Tridgell
parent ab2dea5b86
commit f25d20f549
2 changed files with 36 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_Compass/AP_Compass.h
View File

@ -377,6 +377,8 @@ private:
bool _start_calibration(uint8_t i, bool retry=false, float delay_sec=0.0f);
bool _start_calibration_mask(uint8_t mask, bool retry=false, bool autosave=false, float delay_sec=0.0f, bool autoreboot=false);
bool _auto_reboot() { return _compass_cal_autoreboot; }
Priority next_cal_progress_idx[MAVLINK_COMM_NUM_BUFFERS];
Priority next_cal_report_idx[MAVLINK_COMM_NUM_BUFFERS];
// see if we already have probed a i2c driver by bus number and address
bool _have_i2c_driver(uint8_t bus_num, uint8_t address) const;

56
libraries/AP_Compass/AP_Compass_Calibration.cpp
View File

@ -230,19 +230,14 @@ bool Compass::_accept_calibration_mask(uint8_t mask)
bool Compass::send_mag_cal_progress(const GCS_MAVLINK& link)
{
uint8_t cal_mask = _get_cal_mask();
const mavlink_channel_t chan = link.get_chan();
for (Priority 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(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;
}
for (uint8_t i = 0; i < COMPASS_MAX_INSTANCES; i++) {
const Priority compass_id = (next_cal_progress_idx[chan] + 1) % COMPASS_MAX_INSTANCES;
auto& calibrator = _calibrator[compass_id];
if (calibrator == nullptr) {
next_cal_progress_idx[chan] = compass_id;
continue;
}
const CompassCalibrator::State cal_state = calibrator->get_state();
@ -250,12 +245,22 @@ bool Compass::send_mag_cal_progress(const GCS_MAVLINK& link)
if (cal_state.status == CompassCalibrator::Status::WAITING_TO_START ||
cal_state.status == CompassCalibrator::Status::RUNNING_STEP_ONE ||
cal_state.status == CompassCalibrator::Status::RUNNING_STEP_TWO) {
// ensure we don't try to send with no space available
if (!HAVE_PAYLOAD_SPACE(chan, MAG_CAL_PROGRESS)) {
return false;
}
next_cal_progress_idx[chan] = compass_id;
mavlink_msg_mag_cal_progress_send(
link.get_chan(),
uint8_t(compass_id), cal_mask,
uint8_t(compass_id),
_get_cal_mask(),
(uint8_t)cal_state.status, cal_state.attempt, cal_state.completion_pct, cal_state.completion_mask,
0.0f, 0.0f, 0.0f
);
} else {
next_cal_progress_idx[chan] = compass_id;
}
}
@ -264,28 +269,33 @@ bool Compass::send_mag_cal_progress(const GCS_MAVLINK& link)
bool Compass::send_mag_cal_report(const GCS_MAVLINK& link)
{
uint8_t cal_mask = _get_cal_mask();
const mavlink_channel_t chan = link.get_chan();
for (uint8_t i = 0; i < COMPASS_MAX_INSTANCES; i++) {
const Priority compass_id = (next_cal_report_idx[chan] + 1) % COMPASS_MAX_INSTANCES;
for (Priority 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(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;
}
if (_calibrator[compass_id] == nullptr) {
next_cal_report_idx[chan] = compass_id;
continue;
}
const CompassCalibrator::Report cal_report = _calibrator[compass_id]->get_report();
if (cal_report.status == CompassCalibrator::Status::SUCCESS ||
cal_report.status == CompassCalibrator::Status::FAILED ||
cal_report.status == CompassCalibrator::Status::BAD_ORIENTATION) {
uint8_t autosaved = _cal_saved[compass_id];
// ensure we don't try to send with no space available
if (!HAVE_PAYLOAD_SPACE(chan, MAG_CAL_REPORT)) {
return false;
}
next_cal_report_idx[chan] = compass_id;
mavlink_msg_mag_cal_report_send(
link.get_chan(),
uint8_t(compass_id), cal_mask,
(uint8_t)cal_report.status, autosaved,
uint8_t(compass_id),
_get_cal_mask(),
(uint8_t)cal_report.status,
_cal_saved[compass_id],
cal_report.fitness,
cal_report.ofs.x, cal_report.ofs.y, cal_report.ofs.z,
cal_report.diag.x, cal_report.diag.y, cal_report.diag.z,
@ -295,6 +305,8 @@ bool Compass::send_mag_cal_report(const GCS_MAVLINK& link)
cal_report.orientation,
cal_report.scale_factor
);
} else {
next_cal_report_idx[chan] = compass_id;
}
}
return true;