2016-02-17 21:25:13 -04:00
|
|
|
#pragma once
|
|
|
|
|
2013-08-29 02:34:34 -03:00
|
|
|
/*
|
|
|
|
This program is free software: you can redistribute it and/or modify
|
|
|
|
it under the terms of the GNU General Public License as published by
|
|
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
|
|
(at your option) any later version.
|
|
|
|
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
|
|
GNU General Public License for more details.
|
|
|
|
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
|
|
*/
|
|
|
|
|
2012-03-11 04:59:53 -03:00
|
|
|
/*
|
2012-08-21 23:19:51 -03:00
|
|
|
* DCM based AHRS (Attitude Heading Reference System) interface for
|
|
|
|
* ArduPilot
|
|
|
|
*
|
|
|
|
*/
|
2012-03-11 04:59:53 -03:00
|
|
|
|
2017-08-30 05:15:33 -03:00
|
|
|
class AP_AHRS_DCM : public AP_AHRS {
|
2010-12-01 03:58:04 -04:00
|
|
|
public:
|
2018-03-10 05:35:03 -04:00
|
|
|
AP_AHRS_DCM()
|
|
|
|
: AP_AHRS()
|
2017-12-12 21:06:11 -04:00
|
|
|
, _error_rp(1.0f)
|
|
|
|
, _error_yaw(1.0f)
|
|
|
|
, _mag_earth(1, 0)
|
|
|
|
, _imu1_weight(0.5f)
|
|
|
|
{
|
|
|
|
_dcm_matrix.identity();
|
2012-08-21 23:19:51 -03:00
|
|
|
|
2017-12-12 21:06:11 -04:00
|
|
|
// these are experimentally derived from the simulator
|
|
|
|
// with large drift levels
|
|
|
|
_ki = 0.0087f;
|
|
|
|
_ki_yaw = 0.01f;
|
|
|
|
}
|
2017-08-30 05:15:33 -03:00
|
|
|
|
|
|
|
/* Do not allow copies */
|
|
|
|
AP_AHRS_DCM(const AP_AHRS_DCM &other) = delete;
|
|
|
|
AP_AHRS_DCM &operator=(const AP_AHRS_DCM&) = delete;
|
|
|
|
|
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
// return the smoothed gyro vector corrected for drift
|
2017-02-23 06:27:21 -04:00
|
|
|
const Vector3f &get_gyro() const override {
|
2014-07-13 04:36:27 -03:00
|
|
|
return _omega;
|
2012-08-21 23:19:51 -03:00
|
|
|
}
|
2014-01-17 04:19:42 -04:00
|
|
|
|
|
|
|
// return rotation matrix representing rotaton from body to earth axes
|
2017-02-23 06:27:21 -04:00
|
|
|
const Matrix3f &get_rotation_body_to_ned() const override {
|
2014-01-17 04:19:42 -04:00
|
|
|
return _body_dcm_matrix;
|
2012-08-21 23:19:51 -03:00
|
|
|
}
|
|
|
|
|
2018-07-16 05:19:10 -03:00
|
|
|
// get rotation matrix specifically from DCM backend (used for compass calibrator)
|
|
|
|
const Matrix3f &get_DCM_rotation_body_to_ned(void) const override { return _body_dcm_matrix; }
|
2019-02-19 23:53:28 -04:00
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
// return the current drift correction integrator value
|
2017-02-23 06:27:21 -04:00
|
|
|
const Vector3f &get_gyro_drift() const override {
|
2012-08-21 23:19:51 -03:00
|
|
|
return _omega_I;
|
|
|
|
}
|
|
|
|
|
2014-10-28 08:22:48 -03:00
|
|
|
// reset the current gyro drift estimate
|
|
|
|
// should be called if gyro offsets are recalculated
|
2017-02-23 06:27:21 -04:00
|
|
|
void reset_gyro_drift() override;
|
2014-10-28 08:22:48 -03:00
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
// Methods
|
2017-04-02 22:03:45 -03:00
|
|
|
void update(bool skip_ins_update=false) override;
|
2017-02-23 06:27:21 -04:00
|
|
|
void reset(bool recover_eulers = false) override;
|
2012-08-21 23:19:51 -03:00
|
|
|
|
2013-11-22 21:37:23 -04:00
|
|
|
// reset the current attitude, used on new IMU calibration
|
2017-02-23 06:27:21 -04:00
|
|
|
void reset_attitude(const float &roll, const float &pitch, const float &yaw) override;
|
2013-11-22 21:37:23 -04:00
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
// dead-reckoning support
|
2017-02-23 06:27:21 -04:00
|
|
|
virtual bool get_position(struct Location &loc) const override;
|
2012-08-21 23:19:51 -03:00
|
|
|
|
|
|
|
// status reporting
|
2017-02-23 06:27:21 -04:00
|
|
|
float get_error_rp() const override {
|
2015-09-23 04:29:43 -03:00
|
|
|
return _error_rp;
|
|
|
|
}
|
2017-02-23 06:27:21 -04:00
|
|
|
float get_error_yaw() const override {
|
2015-09-23 04:29:43 -03:00
|
|
|
return _error_yaw;
|
|
|
|
}
|
2012-08-21 23:19:51 -03:00
|
|
|
|
|
|
|
// return a wind estimation vector, in m/s
|
2017-12-02 09:13:32 -04:00
|
|
|
Vector3f wind_estimate() const override {
|
2012-08-21 23:19:51 -03:00
|
|
|
return _wind;
|
|
|
|
}
|
2012-08-12 22:08:10 -03:00
|
|
|
|
2017-02-23 02:48:15 -04:00
|
|
|
void get_relative_position_D_home(float &posD) const override;
|
|
|
|
|
2012-08-24 08:22:58 -03:00
|
|
|
// return an airspeed estimate if available. return true
|
|
|
|
// if we have an estimate
|
2020-01-06 20:45:58 -04:00
|
|
|
bool airspeed_estimate(float &airspeed_ret) const override;
|
2012-08-24 08:22:58 -03:00
|
|
|
|
2020-08-18 12:08:35 -03:00
|
|
|
// return an airspeed estimate if available. return true
|
|
|
|
// if we have an estimate from a specific sensor index
|
|
|
|
bool airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const;
|
|
|
|
|
2020-07-28 01:29:57 -03:00
|
|
|
// return a synthetic airspeed estimate (one derived from sensors
|
|
|
|
// other than an actual airspeed sensor), if available. return
|
|
|
|
// true if we have a synthetic airspeed. ret will not be modified
|
|
|
|
// on failure.
|
|
|
|
bool synthetic_airspeed(float &ret) const override WARN_IF_UNUSED {
|
|
|
|
ret = _last_airspeed;
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2017-02-23 06:27:21 -04:00
|
|
|
bool use_compass() override;
|
2013-03-28 23:48:25 -03:00
|
|
|
|
2020-03-31 22:08:54 -03:00
|
|
|
// return the quaternion defining the rotation from NED to XYZ (body) axes
|
|
|
|
bool get_quaternion(Quaternion &quat) const override WARN_IF_UNUSED;
|
|
|
|
|
2018-05-29 20:47:10 -03:00
|
|
|
bool set_home(const Location &loc) override WARN_IF_UNUSED;
|
2014-02-08 04:11:12 -04:00
|
|
|
void estimate_wind(void);
|
2014-01-03 20:15:34 -04:00
|
|
|
|
2014-05-15 04:09:18 -03:00
|
|
|
// is the AHRS subsystem healthy?
|
2017-02-23 06:27:21 -04:00
|
|
|
bool healthy() const override;
|
2014-05-15 04:09:18 -03:00
|
|
|
|
2019-03-13 23:51:57 -03:00
|
|
|
bool get_velocity_NED(Vector3f &vec) const override;
|
|
|
|
|
2020-08-11 02:02:55 -03:00
|
|
|
// returns false if we fail arming checks, in which case the buffer will be populated with a failure message
|
|
|
|
bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override;
|
|
|
|
|
2010-12-01 03:58:04 -04:00
|
|
|
private:
|
2012-08-21 23:19:51 -03:00
|
|
|
float _ki;
|
|
|
|
float _ki_yaw;
|
|
|
|
|
|
|
|
// Methods
|
|
|
|
void matrix_update(float _G_Dt);
|
|
|
|
void normalize(void);
|
|
|
|
void check_matrix(void);
|
|
|
|
bool renorm(Vector3f const &a, Vector3f &result);
|
|
|
|
void drift_correction(float deltat);
|
|
|
|
void drift_correction_yaw(void);
|
|
|
|
float yaw_error_compass();
|
|
|
|
void euler_angles(void);
|
2013-07-07 22:25:40 -03:00
|
|
|
bool have_gps(void) const;
|
2015-04-27 21:26:36 -03:00
|
|
|
bool use_fast_gains(void) const;
|
2019-04-20 07:02:46 -03:00
|
|
|
void load_watchdog_home();
|
2019-04-21 00:08:29 -03:00
|
|
|
void backup_attitude(void);
|
2012-08-21 23:19:51 -03:00
|
|
|
|
2014-01-17 04:19:42 -04:00
|
|
|
// primary representation of attitude of board used for all inertial calculations
|
2012-08-21 23:19:51 -03:00
|
|
|
Matrix3f _dcm_matrix;
|
|
|
|
|
2014-01-17 04:19:42 -04:00
|
|
|
// primary representation of attitude of flight vehicle body
|
|
|
|
Matrix3f _body_dcm_matrix;
|
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
Vector3f _omega_P; // accel Omega proportional correction
|
|
|
|
Vector3f _omega_yaw_P; // proportional yaw correction
|
|
|
|
Vector3f _omega_I; // Omega Integrator correction
|
|
|
|
Vector3f _omega_I_sum;
|
|
|
|
float _omega_I_sum_time;
|
|
|
|
Vector3f _omega; // Corrected Gyro_Vector data
|
|
|
|
|
2013-11-04 00:50:33 -04:00
|
|
|
// variables to cope with delaying the GA sum to match GPS lag
|
2014-02-26 18:41:28 -04:00
|
|
|
Vector3f ra_delayed(uint8_t instance, const Vector3f &ra);
|
|
|
|
Vector3f _ra_delay_buffer[INS_MAX_INSTANCES];
|
2013-11-04 00:50:33 -04:00
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
// P term gain based on spin rate
|
|
|
|
float _P_gain(float spin_rate);
|
|
|
|
|
2014-01-18 03:18:12 -04:00
|
|
|
// P term yaw gain based on rate of change of horiz velocity
|
2014-02-26 18:41:28 -04:00
|
|
|
float _yaw_gain(void) const;
|
2014-01-18 03:18:12 -04:00
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
// state to support status reporting
|
|
|
|
float _renorm_val_sum;
|
|
|
|
uint16_t _renorm_val_count;
|
2015-04-21 08:42:17 -03:00
|
|
|
float _error_rp;
|
|
|
|
float _error_yaw;
|
2012-08-21 23:19:51 -03:00
|
|
|
|
|
|
|
// time in millis when we last got a GPS heading
|
|
|
|
uint32_t _gps_last_update;
|
|
|
|
|
|
|
|
// state of accel drift correction
|
2014-02-26 18:41:28 -04:00
|
|
|
Vector3f _ra_sum[INS_MAX_INSTANCES];
|
2012-08-21 23:19:51 -03:00
|
|
|
Vector3f _last_velocity;
|
|
|
|
float _ra_deltat;
|
|
|
|
uint32_t _ra_sum_start;
|
|
|
|
|
2013-05-04 12:25:59 -03:00
|
|
|
// the earths magnetic field
|
|
|
|
float _last_declination;
|
2013-05-05 00:51:45 -03:00
|
|
|
Vector2f _mag_earth;
|
2012-08-21 23:19:51 -03:00
|
|
|
|
|
|
|
// whether we have GPS lock
|
|
|
|
bool _have_gps_lock;
|
|
|
|
|
|
|
|
// the lat/lng where we last had GPS lock
|
|
|
|
int32_t _last_lat;
|
|
|
|
int32_t _last_lng;
|
|
|
|
|
|
|
|
// position offset from last GPS lock
|
|
|
|
float _position_offset_north;
|
|
|
|
float _position_offset_east;
|
|
|
|
|
|
|
|
// whether we have a position estimate
|
|
|
|
bool _have_position;
|
|
|
|
|
|
|
|
// support for wind estimation
|
|
|
|
Vector3f _last_fuse;
|
|
|
|
Vector3f _last_vel;
|
|
|
|
uint32_t _last_wind_time;
|
|
|
|
float _last_airspeed;
|
2013-10-21 23:06:27 -03:00
|
|
|
uint32_t _last_consistent_heading;
|
2012-08-21 23:19:51 -03:00
|
|
|
|
|
|
|
// estimated wind in m/s
|
|
|
|
Vector3f _wind;
|
2014-05-15 04:09:18 -03:00
|
|
|
|
2014-10-31 20:19:11 -03:00
|
|
|
float _imu1_weight;
|
|
|
|
|
2014-05-15 04:09:18 -03:00
|
|
|
// last time AHRS failed in milliseconds
|
|
|
|
uint32_t _last_failure_ms;
|
2015-04-27 21:26:36 -03:00
|
|
|
|
|
|
|
// time when DCM was last reset
|
|
|
|
uint32_t _last_startup_ms;
|
2010-12-01 03:58:04 -04:00
|
|
|
};
|