#pragma once #include #include #include #include #include #include #include #include "CompassCalibrator.h" #include "AP_Compass_Backend.h" // compass product id #define AP_COMPASS_TYPE_UNKNOWN 0x00 #define AP_COMPASS_TYPE_HIL 0x01 #define AP_COMPASS_TYPE_HMC5843 0x02 #define AP_COMPASS_TYPE_HMC5883L 0x03 #define AP_COMPASS_TYPE_PX4 0x04 #define AP_COMPASS_TYPE_VRBRAIN 0x05 #define AP_COMPASS_TYPE_AK8963_MPU9250 0x06 #define AP_COMPASS_TYPE_AK8963_I2C 0x07 #define AP_COMPASS_TYPE_LSM303D 0x08 #define AP_COMPASS_TYPE_LSM9DS1 0x09 #define AP_COMPASS_TYPE_BMM150 0x0A // motor compensation types (for use with motor_comp_enabled) #define AP_COMPASS_MOT_COMP_DISABLED 0x00 #define AP_COMPASS_MOT_COMP_THROTTLE 0x01 #define AP_COMPASS_MOT_COMP_CURRENT 0x02 // setup default mag orientation for some board types #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX && CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT # define MAG_BOARD_ORIENTATION ROTATION_ROLL_180 #elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX && CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP # define MAG_BOARD_ORIENTATION ROTATION_YAW_90 #elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX && (CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI) # define MAG_BOARD_ORIENTATION ROTATION_YAW_270 #else # define MAG_BOARD_ORIENTATION ROTATION_NONE #endif /** maximum number of compass instances available on this platform. If more than 1 then redundant sensors may be available */ #define COMPASS_MAX_INSTANCES 3 #define COMPASS_MAX_BACKEND 3 #define AP_COMPASS_MAX_XYZ_ANG_DIFF radians(50.0f) #define AP_COMPASS_MAX_XY_ANG_DIFF radians(30.0f) #define AP_COMPASS_MAX_XY_LENGTH_DIFF 100.0f class Compass { friend class AP_Compass_Backend; public: /// Constructor /// Compass(); /// Initialize the compass device. /// /// @returns True if the compass was initialized OK, false if it was not /// found or is not functioning. /// bool init(); /// Read the compass and update the mag_ variables. /// bool read(); /// use spare CPU cycles to accumulate values from the compass if /// possible (this method should also be implemented in the backends) void accumulate(); /// Calculate the tilt-compensated heading_ variables. /// /// @param dcm_matrix The current orientation rotation matrix /// /// @returns heading in radians /// float calculate_heading(const Matrix3f &dcm_matrix) const { return calculate_heading(dcm_matrix, get_primary()); } float calculate_heading(const Matrix3f &dcm_matrix, uint8_t i) const; /// Sets offset x/y/z values. /// /// @param i compass instance /// @param offsets Offsets to the raw mag_ values in milligauss. /// void set_offsets(uint8_t i, const Vector3f &offsets); /// Sets and saves the compass offset x/y/z values. /// /// @param i compass instance /// @param offsets Offsets to the raw mag_ values in milligauss. /// void set_and_save_offsets(uint8_t i, const Vector3f &offsets); void set_and_save_diagonals(uint8_t i, const Vector3f &diagonals); void set_and_save_offdiagonals(uint8_t i, const Vector3f &diagonals); /// Saves the current offset x/y/z values for one or all compasses /// /// @param i compass instance /// /// This should be invoked periodically to save the offset values maintained by /// ::learn_offsets. /// void save_offsets(uint8_t i); void save_offsets(void); // return the number of compass instances uint8_t get_count(void) const { return _compass_count; } /// Return the current field as a Vector3f in milligauss const Vector3f &get_field(uint8_t i) const { return _state[i].field; } const Vector3f &get_field(void) const { return get_field(get_primary()); } // compass calibrator interface void compass_cal_update(); void start_calibration_all(bool retry=false, bool autosave=false, float delay_sec=0.0f, bool autoreboot = false); void cancel_calibration_all(); bool compass_cal_requires_reboot() { return _cal_complete_requires_reboot; } bool is_calibrating() const; /* handle an incoming MAG_CAL command */ uint8_t 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); // check if the compasses are pointing in the same direction bool consistent() const; /// Return the health of a compass bool healthy(uint8_t i) const { return _state[i].healthy; } bool healthy(void) const { return healthy(get_primary()); } uint8_t get_healthy_mask() const; /// Returns the current offset values /// /// @returns The current compass offsets in milligauss. /// const Vector3f &get_offsets(uint8_t i) const { return _state[i].offset; } const Vector3f &get_offsets(void) const { return get_offsets(get_primary()); } /// Sets the initial location used to get declination /// /// @param latitude GPS Latitude. /// @param longitude GPS Longitude. /// void set_initial_location(int32_t latitude, int32_t longitude); /// Program new offset values. /// /// @param i compass instance /// @param x Offset to the raw mag_x value in milligauss. /// @param y Offset to the raw mag_y value in milligauss. /// @param z Offset to the raw mag_z value in milligauss. /// void set_and_save_offsets(uint8_t i, int x, int y, int z) { set_and_save_offsets(i, Vector3f(x, y, z)); } // learn offsets accessor bool learn_offsets_enabled() const { return _learn; } /// Perform automatic offset updates /// void learn_offsets(void); /// return true if the compass should be used for yaw calculations bool use_for_yaw(uint8_t i) const; bool use_for_yaw(void) const; /// Sets the local magnetic field declination. /// /// @param radians Local field declination. /// @param save_to_eeprom true to save to eeprom (false saves only to memory) /// void set_declination(float radians, bool save_to_eeprom = true); float get_declination() const; // set overall board orientation void set_board_orientation(enum Rotation orientation) { _board_orientation = orientation; } /// Set the motor compensation type /// /// @param comp_type 0 = disabled, 1 = enabled use throttle, 2 = enabled use current /// void motor_compensation_type(const uint8_t comp_type); /// get the motor compensation value. uint8_t get_motor_compensation_type() const { return _motor_comp_type; } /// Set the motor compensation factor x/y/z values. /// /// @param i instance of compass /// @param offsets Offsets multiplied by the throttle value and added to the raw mag_ values. /// void set_motor_compensation(uint8_t i, const Vector3f &motor_comp_factor); /// get motor compensation factors as a vector const Vector3f& get_motor_compensation(uint8_t i) const { return _state[i].motor_compensation; } const Vector3f& get_motor_compensation(void) const { return get_motor_compensation(get_primary()); } /// Saves the current motor compensation x/y/z values. /// /// This should be invoked periodically to save the offset values calculated by the motor compensation auto learning /// void save_motor_compensation(); /// Returns the current motor compensation offset values /// /// @returns The current compass offsets in milligauss. /// const Vector3f &get_motor_offsets(uint8_t i) const { return _state[i].motor_offset; } const Vector3f &get_motor_offsets(void) const { return get_motor_offsets(get_primary()); } /// Set the throttle as a percentage from 0.0 to 1.0 /// @param thr_pct throttle expressed as a percentage from 0 to 1.0 void set_throttle(float thr_pct) { if (_motor_comp_type == AP_COMPASS_MOT_COMP_THROTTLE) { _thr_or_curr = thr_pct; } } /// Set the current used by system in amps /// @param amps current flowing to the motors expressed in amps void set_current(float amps) { if (_motor_comp_type == AP_COMPASS_MOT_COMP_CURRENT) { _thr_or_curr = amps; } } /// Returns True if the compasses have been configured (i.e. offsets saved) /// /// @returns True if compass has been configured /// bool configured(uint8_t i); bool configured(void); /// Returns the instance of the primary compass /// /// @returns the instance number of the primary compass /// uint8_t get_primary(void) const { return _primary; } // HIL methods void setHIL(uint8_t instance, float roll, float pitch, float yaw); void setHIL(uint8_t instance, const Vector3f &mag, uint32_t last_update_usec); const Vector3f& getHIL(uint8_t instance) const; void _setup_earth_field(); // enable HIL mode void set_hil_mode(void) { _hil_mode = true; } // return last update time in microseconds uint32_t last_update_usec(void) const { return _state[get_primary()].last_update_usec; } uint32_t last_update_usec(uint8_t i) const { return _state[i].last_update_usec; } static const struct AP_Param::GroupInfo var_info[]; // HIL variables struct { Vector3f Bearth; float last_declination; bool healthy[COMPASS_MAX_INSTANCES]; Vector3f field[COMPASS_MAX_INSTANCES]; } _hil; enum LearnType { LEARN_NONE=0, LEARN_INTERNAL=1, LEARN_EKF=2 }; // return the chosen learning type enum LearnType get_learn_type(void) const { return (enum LearnType)_learn.get(); } private: /// Register a new compas driver, allocating an instance number /// /// @return number of compass instances uint8_t register_compass(void); // load backend drivers bool _add_backend(AP_Compass_Backend *backend, const char *name, bool external); void _detect_backends(void); // compass cal bool _accept_calibration(uint8_t i); bool _accept_calibration_mask(uint8_t mask); void _cancel_calibration(uint8_t i); void _cancel_calibration_mask(uint8_t mask); uint8_t _get_cal_mask() const; 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; } //keep track of which calibrators have been saved bool _cal_saved[COMPASS_MAX_INSTANCES]; bool _cal_autosave; //autoreboot after compass calibration bool _compass_cal_autoreboot; bool _cal_complete_requires_reboot; bool _cal_has_run; // backend objects AP_Compass_Backend *_backends[COMPASS_MAX_BACKEND]; uint8_t _backend_count; // number of registered compasses. uint8_t _compass_count; // settable parameters AP_Int8 _learn; // board orientation from AHRS enum Rotation _board_orientation; // primary instance AP_Int8 _primary; // declination in radians AP_Float _declination; // enable automatic declination code AP_Int8 _auto_declination; // first-time-around flag used by offset nulling bool _null_init_done; // used by offset correction static const uint8_t _mag_history_size = 20; // motor compensation type // 0 = disabled, 1 = enabled for throttle, 2 = enabled for current AP_Int8 _motor_comp_type; // throttle expressed as a percentage from 0 ~ 1.0 or current expressed in amps float _thr_or_curr; struct mag_state { AP_Int8 external; bool healthy; AP_Int8 orientation; AP_Vector3f offset; AP_Vector3f diagonals; AP_Vector3f offdiagonals; // device id detected at init. // saved to eeprom when offsets are saved allowing ram & // eeprom values to be compared as consistency check AP_Int32 dev_id; AP_Int8 use_for_yaw; uint8_t mag_history_index; Vector3i mag_history[_mag_history_size]; // factors multiplied by throttle and added to compass outputs AP_Vector3f motor_compensation; // latest compensation added to compass Vector3f motor_offset; // corrected magnetic field strength Vector3f field; // when we last got data uint32_t last_update_ms; uint32_t last_update_usec; } _state[COMPASS_MAX_INSTANCES]; CompassCalibrator _calibrator[COMPASS_MAX_INSTANCES]; // if we want HIL only bool _hil_mode:1; AP_Float _calibration_threshold; };