#pragma once #include "AP_Compass_config.h" #include #include #include #include #include #include #include #include #include #include "AP_Compass_Backend.h" #include "Compass_PerMotor.h" #include // 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 #define AP_COMPASS_MOT_COMP_PER_MOTOR 0x03 // setup default mag orientation for some board types #ifndef MAG_BOARD_ORIENTATION #if 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 #endif #ifndef COMPASS_MOT_ENABLED #define COMPASS_MOT_ENABLED 1 #endif #ifndef COMPASS_LEARN_ENABLED #define COMPASS_LEARN_ENABLED AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED #endif // define default compass calibration fitness and consistency checks #define AP_COMPASS_CALIBRATION_FITNESS_DEFAULT 16.0f #define AP_COMPASS_MAX_XYZ_ANG_DIFF radians(90.0f) #define AP_COMPASS_MAX_XY_ANG_DIFF radians(60.0f) #define AP_COMPASS_MAX_XY_LENGTH_DIFF 200.0f /** maximum number of compass instances available on this platform. If more than 1 then redundant sensors may be available */ #ifndef HAL_BUILD_AP_PERIPH #ifndef HAL_COMPASS_MAX_SENSORS #define HAL_COMPASS_MAX_SENSORS 3 #endif #if HAL_COMPASS_MAX_SENSORS > 1 #define COMPASS_MAX_UNREG_DEV 5 #else #define COMPASS_MAX_UNREG_DEV 0 #endif #else #ifndef HAL_COMPASS_MAX_SENSORS #define HAL_COMPASS_MAX_SENSORS 1 #endif #define COMPASS_MAX_UNREG_DEV 0 #endif #define COMPASS_MAX_INSTANCES HAL_COMPASS_MAX_SENSORS #define COMPASS_MAX_BACKEND HAL_COMPASS_MAX_SENSORS #define MAX_CONNECTED_MAGS (COMPASS_MAX_UNREG_DEV+COMPASS_MAX_INSTANCES) #include "CompassCalibrator.h" class CompassLearn; class Compass { friend class AP_Compass_Backend; public: Compass(); /* Do not allow copies */ CLASS_NO_COPY(Compass); // get singleton instance static Compass *get_singleton() { return _singleton; } friend class CompassLearn; /// Initialize the compass device. /// /// @returns True if the compass was initialized OK, false if it was not /// found or is not functioning. /// void init(); /// Read the compass and update the mag_ variables. /// bool read(); // available returns true if the compass is both enabled and has // been initialised bool available() const { return _enabled && init_done; } /// 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, _first_usable); } 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); #if AP_COMPASS_DIAGONALS_ENABLED void set_and_save_diagonals(uint8_t i, const Vector3f &diagonals); void set_and_save_offdiagonals(uint8_t i, const Vector3f &diagonals); #endif void set_and_save_scale_factor(uint8_t i, float scale_factor); void set_and_save_orientation(uint8_t i, Rotation orientation); /// 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 number of enabled sensors uint8_t get_num_enabled(void) const; /// Return the current field as a Vector3f in milligauss const Vector3f &get_field(uint8_t i) const { return _get_state(Priority(i)).field; } const Vector3f &get_field(void) const { return get_field(_first_usable); } /// Return true if we have set a scale factor for a compass bool have_scale_factor(uint8_t i) const; // compass calibrator interface void cal_update(); #if COMPASS_MOT_ENABLED // per-motor calibration access void per_motor_calibration_start(void) { _per_motor.calibration_start(); } void per_motor_calibration_update(void) { _per_motor.calibration_update(); } void per_motor_calibration_end(void) { _per_motor.calibration_end(); } #endif // start_calibration_all will only return false if there are no // compasses to calibrate. bool 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() const { return _cal_requires_reboot; } bool is_calibrating() const; // indicate which bit in LOG_BITMASK indicates we should log compass readings void set_log_bit(uint32_t log_bit) { _log_bit = log_bit; } /* handle an incoming MAG_CAL command */ MAV_RESULT handle_mag_cal_command(const mavlink_command_int_t &packet); bool send_mag_cal_progress(const class GCS_MAVLINK& link); bool send_mag_cal_report(const class GCS_MAVLINK& link); // 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; bool healthy(void) const { return healthy(_first_usable); } 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 _get_state(Priority(i)).offset; } const Vector3f &get_offsets(void) const { return get_offsets(_first_usable); } #if AP_COMPASS_DIAGONALS_ENABLED const Vector3f &get_diagonals(uint8_t i) const { return _get_state(Priority(i)).diagonals; } const Vector3f &get_diagonals(void) const { return get_diagonals(_first_usable); } const Vector3f &get_offdiagonals(uint8_t i) const { return _get_state(Priority(i)).offdiagonals; } const Vector3f &get_offdiagonals(void) const { return get_offdiagonals(_first_usable); } #endif // AP_COMPASS_DIAGONALS_ENABLED // learn offsets accessor bool learn_offsets_enabled() const { return _learn == LEARN_INFLIGHT; } /// 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; bool auto_declination_enabled() const { return _auto_declination != 0; } // set overall board orientation void set_board_orientation(enum Rotation orientation) { _board_orientation = orientation; } // get overall board orientation enum Rotation get_board_orientation(void) const { return _board_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 _get_state(Priority(i)).motor_compensation; } const Vector3f& get_motor_compensation(void) const { return get_motor_compensation(_first_usable); } /// 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 _get_state(Priority(i)).motor_offset; } const Vector3f &get_motor_offsets(void) const { return get_motor_offsets(_first_usable); } /// 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 = thr_pct; } } #if COMPASS_MOT_ENABLED /// Set the battery voltage for per-motor compensation void set_voltage(float voltage) { _per_motor.set_voltage(voltage); } #endif /// 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(char *failure_msg, uint8_t failure_msg_len); // return last update time in microseconds uint32_t last_update_usec(void) const { return last_update_usec(_first_usable); } uint32_t last_update_usec(uint8_t i) const { return _get_state(Priority(i)).last_update_usec; } uint32_t last_update_ms(void) const { return last_update_ms(_first_usable); } uint32_t last_update_ms(uint8_t i) const { return _get_state(Priority(i)).last_update_ms; } static const struct AP_Param::GroupInfo var_info[]; enum LearnType { LEARN_NONE=0, LEARN_INTERNAL=1, LEARN_EKF=2, LEARN_INFLIGHT=3 }; // return the chosen learning type enum LearnType get_learn_type(void) const { return (enum LearnType)_learn.get(); } // set the learning type void set_learn_type(enum LearnType type, bool save) { if (save) { _learn.set_and_save((int8_t)type); } else { _learn.set((int8_t)type); } } // return maximum allowed compass offsets uint16_t get_offsets_max(void) const { return (uint16_t)_offset_max.get(); } uint8_t get_filter_range() const { return uint8_t(_filter_range.get()); } #if AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED /* fast compass calibration given vehicle position and yaw */ bool mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask, float lat_deg, float lon_deg, bool force_use=false); #endif #if AP_COMPASS_MSP_ENABLED void handle_msp(const MSP::msp_compass_data_message_t &pkt); #endif #if AP_COMPASS_EXTERNALAHRS_ENABLED void handle_external(const AP_ExternalAHRS::mag_data_message_t &pkt); #endif // force save of current calibration as valid void force_save_calibration(void); // get the first compass marked for use by COMPASSx_USE uint8_t get_first_usable(void) const { return _first_usable; } private: static Compass *_singleton; // Use Priority and StateIndex typesafe index types // to distinguish between two different type of indexing // We use StateIndex for access by Backend // and Priority for access by Frontend DECLARE_TYPESAFE_INDEX(Priority, uint8_t); DECLARE_TYPESAFE_INDEX(StateIndex, uint8_t); /// Register a new compas driver, allocating an instance number /// /// @param dev_id Dev ID of compass to register against /// /// @return instance number saved against the dev id or first available empty instance number bool register_compass(int32_t dev_id, uint8_t& instance); // load backend drivers bool _add_backend(AP_Compass_Backend *backend); void _probe_external_i2c_compasses(void); void _detect_backends(void); void probe_i2c_spi_compasses(void); #if AP_COMPASS_DRONECAN_ENABLED void probe_dronecan_compasses(void); #endif // compass cal void _update_calibration_trampoline(); 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(); 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() const { 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; #if AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED /* get mag field with the effects of offsets, diagonals and off-diagonals removed */ bool get_uncorrected_field(uint8_t instance, Vector3f &field) const; #endif #if COMPASS_CAL_ENABLED //keep track of which calibrators have been saved RestrictIDTypeArray _cal_saved; bool _cal_autosave; #endif //autoreboot after compass calibration bool _compass_cal_autoreboot; bool _cal_requires_reboot; bool _cal_has_run; // enum of drivers for COMPASS_DISBLMSK enum DriverType { #if AP_COMPASS_HMC5843_ENABLED DRIVER_HMC5843 =0, #endif #if AP_COMPASS_LSM303D_ENABLED DRIVER_LSM303D =1, #endif #if AP_COMPASS_AK8963_ENABLED DRIVER_AK8963 =2, #endif #if AP_COMPASS_BMM150_ENABLED DRIVER_BMM150 =3, #endif #if AP_COMPASS_LSM9DS1_ENABLED DRIVER_LSM9DS1 =4, #endif #if AP_COMPASS_LIS3MDL_ENABLED DRIVER_LIS3MDL =5, #endif #if AP_COMPASS_AK09916_ENABLED DRIVER_AK09916 =6, #endif #if AP_COMPASS_IST8310_ENABLED DRIVER_IST8310 =7, #endif #if AP_COMPASS_ICM20948_ENABLED DRIVER_ICM20948 =8, #endif #if AP_COMPASS_MMC3416_ENABLED DRIVER_MMC3416 =9, #endif #if AP_COMPASS_DRONECAN_ENABLED DRIVER_UAVCAN =11, #endif #if AP_COMPASS_QMC5883L_ENABLED DRIVER_QMC5883L =12, #endif #if AP_COMPASS_SITL_ENABLED DRIVER_SITL =13, #endif #if AP_COMPASS_MAG3110_ENABLED DRIVER_MAG3110 =14, #endif #if AP_COMPASS_IST8308_ENABLED DRIVER_IST8308 =15, #endif #if AP_COMPASS_RM3100_ENABLED DRIVER_RM3100 =16, #endif #if AP_COMPASS_MSP_ENABLED DRIVER_MSP =17, #endif #if AP_COMPASS_EXTERNALAHRS_ENABLED DRIVER_EXTERNALAHRS =18, #endif #if AP_COMPASS_MMC5XX3_ENABLED DRIVER_MMC5XX3 =19, #endif #if AP_COMPASS_QMC5883P_ENABLED DRIVER_QMC5883P =20, #endif }; bool _driver_enabled(enum DriverType driver_type); // backend objects AP_Compass_Backend *_backends[COMPASS_MAX_BACKEND]; uint8_t _backend_count; // whether to enable the compass drivers at all AP_Int8 _enabled; // number of registered compasses. uint8_t _compass_count; // number of unregistered compasses. uint8_t _unreg_compass_count; // settable parameters AP_Int8 _learn; // board orientation from AHRS enum Rotation _board_orientation = ROTATION_NONE; // declination in radians AP_Float _declination; // enable automatic declination code AP_Int8 _auto_declination; // stores which bit is used to indicate we should log compass readings uint32_t _log_bit = -1; // motor compensation type // 0 = disabled, 1 = enabled for throttle, 2 = enabled for current AP_Int8 _motor_comp_type; // automatic compass orientation on calibration AP_Int8 _rotate_auto; // throttle expressed as a percentage from 0 ~ 1.0, used for motor compensation float _thr; struct mag_state { AP_Int8 external; bool healthy; bool registered; Compass::Priority priority; AP_Int8 orientation; AP_Vector3f offset; #if AP_COMPASS_DIAGONALS_ENABLED AP_Vector3f diagonals; AP_Vector3f offdiagonals; #endif AP_Float scale_factor; // 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; // Initialised when compass is detected int32_t detected_dev_id; // Initialised at boot from saved devid int32_t expected_dev_id; // 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; // board specific orientation enum Rotation rotation; // accumulated samples, protected by _sem, used by AP_Compass_Backend Vector3f accum; uint32_t accum_count; // We only copy persistent params void copy_from(const mag_state& state); }; //Create an Array of mag_state to be accessible by StateIndex only RestrictIDTypeArray _state; //Convert Priority to StateIndex StateIndex _get_state_id(Priority priority) const; //Get State Struct by Priority const struct mag_state& _get_state(Priority priority) const { return _state[_get_state_id(priority)]; } //Convert StateIndex to Priority Priority _get_priority(StateIndex state_id) { return _state[state_id].priority; } //Method to detect compass beyond initialisation stage void _detect_runtime(void); // This method reorganises devid list to match // priority list, only call before detection at boot #if COMPASS_MAX_INSTANCES > 1 void _reorder_compass_params(); #endif // Update Priority List for Mags, by default, we just // load them as they come up the first time Priority _update_priority_list(int32_t dev_id); // method to check if the mag with the devid // is a replacement mag bool is_replacement_mag(uint32_t dev_id); //remove the devid from unreg compass list void remove_unreg_dev_id(uint32_t devid); void _reset_compass_id(); //Create Arrays to be accessible by Priority only RestrictIDTypeArray _use_for_yaw; #if COMPASS_MAX_INSTANCES > 1 RestrictIDTypeArray _priority_did_stored_list; RestrictIDTypeArray _priority_did_list; #endif AP_Int16 _offset_max; // bitmask of options enum class Option : uint16_t { CAL_REQUIRE_GPS = (1U<<0), ALLOW_DRONECAN_AUTO_REPLACEMENT = (1U<<1), }; bool option_set(Option opt) const { return (_options.get() & uint16_t(opt)) != 0; } AP_Int16 _options; #if COMPASS_CAL_ENABLED RestrictIDTypeArray _calibrator; #endif #if COMPASS_MOT_ENABLED // per-motor compass compensation Compass_PerMotor _per_motor{*this}; #endif AP_Float _calibration_threshold; // mask of driver types to not load. Bit positions match DEVTYPE_ in backend AP_Int32 _driver_type_mask; #if COMPASS_MAX_UNREG_DEV // Put extra dev ids detected AP_Int32 extra_dev_id[COMPASS_MAX_UNREG_DEV]; uint32_t _previously_unreg_mag[COMPASS_MAX_UNREG_DEV]; #endif AP_Int8 _filter_range; CompassLearn *learn; bool learn_allocated; /// Sets the initial location used to get declination /// /// @param latitude GPS Latitude. /// @param longitude GPS Longitude. /// void try_set_initial_location(); bool _initial_location_set; bool _cal_thread_started; #if AP_COMPASS_MSP_ENABLED uint8_t msp_instance_mask; #endif bool init_done; bool suppress_devid_save; uint8_t _first_usable; // first compass usable based on COMPASSx_USE param }; namespace AP { Compass &compass(); };