mirror of https://github.com/ArduPilot/ardupilot
111 lines
2.9 KiB
C++
111 lines
2.9 KiB
C++
#pragma once
|
|
|
|
#include <AP_HAL/AP_HAL_Boards.h>
|
|
#include <GCS_MAVLink/GCS_config.h>
|
|
|
|
#ifndef HAL_INS_ACCELCAL_ENABLED
|
|
#if HAL_GCS_ENABLED
|
|
#include <AP_InertialSensor/AP_InertialSensor_config.h>
|
|
#define HAL_INS_ACCELCAL_ENABLED AP_INERTIALSENSOR_ENABLED
|
|
#else
|
|
#define HAL_INS_ACCELCAL_ENABLED 0
|
|
#endif
|
|
#endif
|
|
|
|
#include <GCS_MAVLink/GCS_MAVLink.h>
|
|
#include "AccelCalibrator.h"
|
|
|
|
#define AP_ACCELCAL_MAX_NUM_CLIENTS 4
|
|
class GCS_MAVLINK;
|
|
class AP_AccelCal_Client;
|
|
|
|
class AP_AccelCal {
|
|
public:
|
|
AP_AccelCal():
|
|
_use_gcs_snoop(true),
|
|
_started(false),
|
|
_saving(false)
|
|
{ update_status(); }
|
|
|
|
// start all the registered calibrations
|
|
void start(GCS_MAVLINK *gcs);
|
|
|
|
// called on calibration cancellation
|
|
void cancel();
|
|
|
|
// Run an iteration of all registered calibrations
|
|
void update();
|
|
|
|
// get the status of the calibrator server as a whole
|
|
accel_cal_status_t get_status() { return _status; }
|
|
|
|
// Set vehicle position sent by the GCS
|
|
bool gcs_vehicle_position(float position);
|
|
|
|
// interface to the clients for registration
|
|
static void register_client(AP_AccelCal_Client* client);
|
|
|
|
#if HAL_GCS_ENABLED
|
|
void handle_command_ack(const mavlink_command_ack_t &packet);
|
|
#endif
|
|
|
|
// true if we are in a calibration process
|
|
bool running(void) const;
|
|
|
|
private:
|
|
class GCS_MAVLINK *_gcs;
|
|
bool _use_gcs_snoop;
|
|
bool _waiting_for_mavlink_ack = false;
|
|
uint32_t _last_position_request_ms;
|
|
uint8_t _step;
|
|
accel_cal_status_t _status;
|
|
accel_cal_status_t _last_result;
|
|
|
|
static uint8_t _num_clients;
|
|
static AP_AccelCal_Client* _clients[AP_ACCELCAL_MAX_NUM_CLIENTS];
|
|
|
|
// called on calibration success
|
|
void success();
|
|
|
|
// called on calibration failure
|
|
void fail();
|
|
|
|
// reset all the calibrators to there pre calibration stage so as to make them ready for next calibration request
|
|
void clear();
|
|
|
|
// proceed through the collection step for each of the registered calibrators
|
|
void collect_sample();
|
|
|
|
// update the state of the Accel calibrator server
|
|
void update_status();
|
|
|
|
// checks if no new sample has been received for considerable amount of time
|
|
bool check_for_timeout();
|
|
|
|
// check if client's calibrator is active
|
|
bool client_active(uint8_t client_num);
|
|
|
|
bool _started;
|
|
bool _saving;
|
|
|
|
uint8_t _num_active_calibrators;
|
|
|
|
AccelCalibrator* get_calibrator(uint8_t i);
|
|
};
|
|
|
|
class AP_AccelCal_Client {
|
|
friend class AP_AccelCal;
|
|
private:
|
|
// getters
|
|
virtual bool _acal_get_saving() { return false; }
|
|
virtual bool _acal_get_ready_to_sample() { return true; }
|
|
virtual bool _acal_get_fail() { return false; }
|
|
virtual AccelCalibrator* _acal_get_calibrator(uint8_t instance) = 0;
|
|
|
|
// events
|
|
virtual void _acal_save_calibrations() = 0;
|
|
virtual void _acal_event_success() {};
|
|
virtual void _acal_event_cancellation() {};
|
|
virtual void _acal_event_failure() {};
|
|
};
|