ardupilot/libraries/AP_Compass/Compass.h

417 lines
14 KiB
C
Raw Normal View History

/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#ifndef Compass_h
#define Compass_h
#include <inttypes.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
2015-03-18 20:42:30 -03:00
#include "CompassCalibrator.h"
#include <AP_Common/AP_Common.h>
#include <AP_Param/AP_Param.h>
#include <AP_Math/AP_Math.h>
#include <AP_Declination/AP_Declination.h> // ArduPilot Mega Declination Helper Library
#include <AP_HAL/AP_HAL.h>
#include "AP_Compass_Backend.h"
// compass product id
2014-11-21 12:26:58 -04:00
#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
2015-07-10 01:05:21 -03:00
#define AP_COMPASS_TYPE_AK8963_I2C 0x07
2015-08-17 21:09:52 -03:00
#define AP_COMPASS_TYPE_LSM303D 0x08
// 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_APM1
# define MAG_BOARD_ORIENTATION ROTATION_ROLL_180
2015-08-17 21:09:52 -03:00
#elif 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
#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
*/
2014-07-03 23:07:47 -03:00
#define COMPASS_MAX_INSTANCES 3
#define COMPASS_MAX_BACKEND 3
//MAXIMUM COMPASS REPORTS
#define MAX_CAL_REPORTS 10
#define CONTINUOUS_REPORTS 0
2015-08-14 21:27:23 -03:00
#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;
/// 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);
2015-03-18 20:18:47 -03:00
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()); }
// raw/unfiltered measurement interface
uint32_t raw_meas_time_us(uint8_t i) const { return _state[i].raw_meas_time_us; }
uint32_t raw_meas_time_us() const { return _state[get_primary()].raw_meas_time_us; }
uint32_t unfiltered_meas_time_us(uint8_t i) const { return _state[i].raw_meas_time_us; }
uint32_t unfiltered_meas_time_us() const { return _state[get_primary()].raw_meas_time_us; }
bool has_raw_field(uint8_t i) const { return _state[i].has_raw_field; }
bool has_raw_field() const { return has_raw_field(get_primary()); }
bool has_unfiltered_field(uint8_t i) const { return _state[i].has_unfiltered_field; }
bool has_unfiltered_field() const { return has_unfiltered_field(get_primary()); }
const Vector3f &get_raw_field(uint8_t i) const { return _state[i].raw_field; }
const Vector3f &get_raw_field(void) const { return get_raw_field(get_primary()); }
const Vector3f &get_unfiltered_field(uint8_t i) const { return _state[i].unfiltered_field; }
const Vector3f &get_unfiltered_field(void) const { return get_unfiltered_field(get_primary()); }
2015-03-18 20:42:30 -03:00
// compass calibrator interface
void compass_cal_update();
bool start_calibration(uint8_t i, bool retry=false, bool autosave=false, float delay_sec=0.0f, bool autoreboot = false);
bool start_calibration_all(bool retry=false, bool autosave=false, float delay_sec=0.0f, bool autoreboot = false);
bool start_calibration_mask(uint8_t mask, bool retry=false, bool autosave=false, float delay_sec=0.0f, bool autoreboot=false);
2015-03-18 20:42:30 -03:00
void cancel_calibration(uint8_t i);
void cancel_calibration_all();
void cancel_calibration_mask(uint8_t mask);
bool accept_calibration(uint8_t i);
bool accept_calibration_all();
bool accept_calibration_mask(uint8_t mask);
bool compass_cal_requires_reboot() { return _cal_complete_requires_reboot; }
bool auto_reboot() { return _compass_cal_autoreboot; }
2015-03-18 20:42:30 -03:00
uint8_t get_cal_mask() const;
bool is_calibrating() const;
/*
handle an incoming MAG_CAL command
*/
uint8_t handle_mag_cal_command(const mavlink_command_long_t &packet);
2015-03-18 20:42:30 -03:00
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;
2015-08-14 21:27:23 -03:00
/// Return the health of a compass
bool healthy(uint8_t i) const { return _state[i].healthy; }
bool healthy(void) const { return healthy(get_primary()); }
2015-03-18 20:42:30 -03:00
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
2014-09-18 09:56:13 -03:00
bool use_for_yaw(uint8_t i) const;
bool use_for_yaw(void) const;
Bug fix for compass. This is a fix for an interesting bug when a DCM matrix reset was added to the ground start. This bug only showed up if (A) a ground start were performed after an air start or due to use of the "Calibrate Gryo" action, (B) if the current orientation were sufficiently different from 0/0/0, and (C.) if the particular magnetometer had sufficiently large offsets. Why did resetting the DCM matrix to 0/0/0 pitch/roll/yaw at ground start cause a bug? The magnetometer offset nulling determines the proper offsets for the magnetometer by comparing the observed change in the magnetic field vector with the expected change due to rotation as calculated from the rotation in the DCM matrix. This comparison is made at 10Hz, and then filtered with a weight based on the amount of rotation to estimate the offsets. Normally it would take considerable time at normal in-flight rotation rates for the offset estimate to converge. If a DCM matrix reset occurs when the offset nulling algorithm is up and running, the algorithm sees the DCM reset as a instantaneous rotation, however the magnetic field vector did not change at all. Under certain conditions the algorithm would interpret this as indicating that the offset(s) should be very large. Since the "rotation" could also have been large the filter weighting would be large and it was possible for a large erroneous estimate of the offset(s) to be made based on this single (bad) data point. To fix this bug methods were added to the compass object to start and stop the offset nulling algorithm. Further, when the algorithm is started, it is set up to get fresh samples. The DCM matrix reset method now calls these new methods to stop the offset nulling before resetting the matrix, and resume after the matrix has been reset.
2012-01-12 17:43:39 -04:00
/// 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);
const Vector3f& getHIL(uint8_t instance) const;
void _setup_earth_field();
2015-03-13 08:32:35 -03:00
// 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; }
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;
2013-04-16 06:47:39 -03:00
private:
/// Register a new compas driver, allocating an instance number
///
/// @return number of compass instances
uint8_t register_compass(void);
// load backend drivers
void _add_backend(AP_Compass_Backend *backend);
void _detect_backends(void);
//keep track of number of calibration reports sent
uint8_t _reports_sent[COMPASS_MAX_INSTANCES];
//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;
2015-03-18 20:18:47 -03:00
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;
uint32_t raw_meas_time_us;
bool has_raw_field;
bool has_unfiltered_field;
bool updated_raw_field;
bool updated_unfiltered_field;
Vector3f raw_field;
Vector3f unfiltered_field;
} _state[COMPASS_MAX_INSTANCES];
2015-03-13 08:32:35 -03:00
2015-03-18 20:42:30 -03:00
CompassCalibrator _calibrator[COMPASS_MAX_INSTANCES];
2015-03-13 08:32:35 -03:00
// if we want HIL only
bool _hil_mode:1;
AP_Float _calibration_threshold;
};
#include "AP_Compass_Backend.h"
#include "AP_Compass_HMC5843.h"
#include "AP_Compass_HIL.h"
#include "AP_Compass_AK8963.h"
#include "AP_Compass_PX4.h"
2015-08-17 21:09:52 -03:00
#include "AP_Compass_LSM303D.h"
#endif