192 uint8_t completion_pct = calibrator.get_completion_percent();
193 auto& completion_mask = calibrator.get_completion_mask();
197 mavlink_msg_mag_cal_progress_send(
199 compass_id, cal_mask,
200 cal_status, attempt, completion_pct, completion_mask,
201 direction.
x, direction.
y, direction.
z 225 mavlink_msg_mag_cal_report_send(
227 compass_id, cal_mask,
228 cal_status, autosaved,
231 diag.
x, diag.
y, diag.
z,
232 offdiag.
x, offdiag.
y, offdiag.
z 257 uint8_t cal_mask = 0;
272 MAV_RESULT
result = MAV_RESULT_FAILED;
274 switch (packet.command) {
275 case MAV_CMD_DO_START_MAG_CAL: {
276 result = MAV_RESULT_ACCEPTED;
279 result = MAV_RESULT_FAILED;
282 if (packet.param1 < 0 || packet.param1 > 255) {
283 result = MAV_RESULT_FAILED;
287 uint8_t mag_mask = packet.param1;
288 bool retry = !
is_zero(packet.param2);
289 bool autosave = !
is_zero(packet.param3);
290 float delay = packet.param4;
291 bool autoreboot = !
is_zero(packet.param5);
297 result = MAV_RESULT_FAILED;
304 case MAV_CMD_DO_ACCEPT_MAG_CAL: {
305 result = MAV_RESULT_ACCEPTED;
306 if(packet.param1 < 0 || packet.param1 > 255) {
307 result = MAV_RESULT_FAILED;
311 uint8_t mag_mask = packet.param1;
318 result = MAV_RESULT_FAILED;
323 case MAV_CMD_DO_CANCEL_MAG_CAL: {
324 result = MAV_RESULT_ACCEPTED;
325 if(packet.param1 < 0 || packet.param1 > 255) {
326 result = MAV_RESULT_FAILED;
330 uint8_t mag_mask = packet.param1;
bool get_soft_armed() const
void start(bool retry, float delay, uint16_t offset_max)
bool _start_calibration(uint8_t i, bool retry=false, float delay_sec=0.0f)
uint32_t compass_cal_failed
bool _cal_complete_requires_reboot
void set_tolerance(float tolerance)
AP_HAL::UARTDriver * console
bool _compass_cal_autoreboot
void set_and_save_offdiagonals(uint8_t i, const Vector3f &diagonals)
bool _cal_saved[COMPASS_MAX_INSTANCES]
Interface definition for the various Ground Control System.
void start_calibration_all(bool retry=false, bool autosave=false, float delay_sec=0.0f, bool autoreboot=false)
uint8_t get_attempt() const
void set_and_save_offsets(uint8_t i, const Vector3f &offsets)
void cancel_calibration_all()
uint8_t _get_cal_mask() const
uint8_t get_primary(void) const
void update(bool &failure)
CompassCalibrator _calibrator[COMPASS_MAX_INSTANCES]
MAV_RESULT handle_mag_cal_command(const mavlink_command_long_t &packet)
float get_fitness() const
virtual void delay(uint16_t ms)=0
bool _accept_calibration(uint8_t i)
void send_mag_cal_report(mavlink_channel_t chan)
#define HAVE_PAYLOAD_SPACE(chan, id)
void set_and_save_diagonals(uint8_t i, const Vector3f &diagonals)
virtual void printf(const char *,...) FMT_PRINTF(2
void get_calibration(Vector3f &offsets, Vector3f &diagonals, Vector3f &offdiagonals)
enum compass_cal_status_t get_status() const
uint32_t compass_cal_canceled
bool is_zero(const T fVal1)
bool _accept_calibration_mask(uint8_t mask)
bool _start_calibration_mask(uint8_t mask, bool retry=false, bool autosave=false, float delay_sec=0.0f, bool autoreboot=false)
uint16_t get_offsets_max(void) const
uint32_t compass_cal_running
uint32_t initiated_compass_cal
AP_Float _calibration_threshold
#define COMPASS_MAX_INSTANCES
virtual void reboot(bool hold_in_bootloader)=0
static struct notify_flags_and_values_type flags
void compass_cal_update()
AP_HAL::AnalogSource * chan
void _cancel_calibration(uint8_t i)
void _cancel_calibration_mask(uint8_t mask)
bool is_calibrating() const
void send_mag_cal_progress(mavlink_channel_t chan)
uint32_t compass_cal_saved
static struct notify_events_type events
bool use_for_yaw(void) const
return true if the compass should be used for yaw calculations
struct Compass::mag_state _state[COMPASS_MAX_INSTANCES]
AP_HAL::Scheduler * scheduler