ardupilot/libraries/AP_AHRS/AP_AHRS_DCM.h

257 lines
8.2 KiB
C++

#pragma once
/*
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/>.
*/
/*
* DCM based AHRS (Attitude Heading Reference System) interface for
* ArduPilot
*
*/
#include "AP_AHRS_Backend.h"
class AP_AHRS_DCM : public AP_AHRS_Backend {
public:
AP_AHRS_DCM()
: AP_AHRS_Backend()
{
_dcm_matrix.identity();
}
/* Do not allow copies */
AP_AHRS_DCM(const AP_AHRS_DCM &other) = delete;
AP_AHRS_DCM &operator=(const AP_AHRS_DCM&) = delete;
// return the smoothed gyro vector corrected for drift
const Vector3f &get_gyro() const override {
return _omega;
}
// return rotation matrix representing rotaton from body to earth axes
const Matrix3f &get_rotation_body_to_ned() const override {
return _body_dcm_matrix;
}
// 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; }
// return the current drift correction integrator value
const Vector3f &get_gyro_drift() const override {
return _omega_I;
}
// reset the current gyro drift estimate
// should be called if gyro offsets are recalculated
void reset_gyro_drift() override;
// Methods
void _update() override;
void reset(bool recover_eulers = false) override;
// return true if yaw has been initialised
bool yaw_initialised(void) const {
return have_initial_yaw;
}
// dead-reckoning support
virtual bool get_position(struct Location &loc) const override;
// status reporting
float get_error_rp() const override {
return _error_rp;
}
float get_error_yaw() const override {
return _error_yaw;
}
// return a wind estimation vector, in m/s
Vector3f wind_estimate() const override {
return _wind;
}
// return an airspeed estimate if available. return true
// if we have an estimate
bool airspeed_estimate(float &airspeed_ret) const override;
// 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;
// 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;
}
// return a ground vector estimate in meters/second, in North/East order
Vector2f groundspeed_vector() override;
bool use_compass() override;
// return the quaternion defining the rotation from NED to XYZ (body) axes
bool get_quaternion(Quaternion &quat) const override WARN_IF_UNUSED;
void estimate_wind(void);
// is the AHRS subsystem healthy?
bool healthy() const override;
bool get_velocity_NED(Vector3f &vec) const override;
// returns false if we fail arming checks, in which case the buffer will be populated with a failure message
// requires_position should be true if horizontal position configuration should be checked (not used)
bool pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const override;
// relative-origin functions for fallback in AP_InertialNav
bool get_origin_fallback(Location &ret) const;
bool get_relative_position_NED_origin(Vector3f &vec) const override;
bool get_relative_position_NE_origin(Vector2f &posNE) const override;
bool get_relative_position_D_origin(float &posD) const override;
protected:
// settable parameters
AP_Float _kp_yaw;
AP_Float _kp;
AP_Float gps_gain;
AP_Float beta;
AP_Int8 _gps_minsats;
private:
// these are experimentally derived from the simulator
// with large drift levels
static constexpr float _ki = 0.0087f;
static constexpr float _ki_yaw = 0.01f;
// 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(class Compass &compass);
void euler_angles(void);
bool have_gps(void) const;
bool use_fast_gains(void) const;
void backup_attitude(void);
// primary representation of attitude of board used for all inertial calculations
Matrix3f _dcm_matrix;
// primary representation of attitude of flight vehicle body
Matrix3f _body_dcm_matrix;
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
bool have_initial_yaw; // true if the yaw value has been initialised with a reference
// variables to cope with delaying the GA sum to match GPS lag
Vector3f ra_delayed(uint8_t instance, const Vector3f &ra);
Vector3f _ra_delay_buffer[INS_MAX_INSTANCES];
// P term gain based on spin rate
float _P_gain(float spin_rate);
// P term yaw gain based on rate of change of horiz velocity
float _yaw_gain(void) const;
/* returns true if attitude should be corrected from GPS-derived
* velocity-deltas. We turn this off for Copter and other similar
* vehicles while the vehicle is disarmed to avoid the HUD bobbing
* around while the vehicle is disarmed.
*/
bool should_correct_centrifugal() const;
// state to support status reporting
float _renorm_val_sum;
uint16_t _renorm_val_count;
float _error_rp{1.0f};
float _error_yaw{1.0f};
// time in microseconds of last compass update
uint32_t _compass_last_update;
// time in millis when we last got a GPS heading
uint32_t _gps_last_update;
// state of accel drift correction
Vector3f _ra_sum[INS_MAX_INSTANCES];
Vector3f _last_velocity;
float _ra_deltat;
uint32_t _ra_sum_start;
// which accelerometer instance is active
uint8_t _active_accel_instance;
// the earths magnetic field
float _last_declination;
Vector2f _mag_earth{1, 0};
// 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;
uint32_t _last_pos_ms;
// 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;
uint32_t _last_consistent_heading;
// estimated wind in m/s
Vector3f _wind;
float _imu1_weight{0.5f};
// last time AHRS failed in milliseconds
uint32_t _last_failure_ms;
// time when DCM was last reset
uint32_t _last_startup_ms;
// last origin we returned, for DCM fallback from EKF
Location last_origin;
// Declare filter states for HPF and LPF used by complementary
// filter in AP_AHRS::groundspeed_vector
Vector2f _lp; // ground vector low-pass filter
Vector2f _hp; // ground vector high-pass filter
Vector2f _lastGndVelADS; // previous HPF input
};