mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-02 14:13:42 -04:00
c2672784be
this adds the necessary functions to allow for flying a quadplane on DCM as an emergency fallback. It sets the NavGainScalar to 0.5 to reduce the VTOL controller gains to allow planes to cope with the higher lag of DCM
225 lines
7.2 KiB
C++
225 lines
7.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
|
|
*
|
|
*/
|
|
|
|
class AP_AHRS_DCM : public AP_AHRS {
|
|
public:
|
|
AP_AHRS_DCM()
|
|
: AP_AHRS()
|
|
, _error_rp(1.0f)
|
|
, _error_yaw(1.0f)
|
|
, _mag_earth(1, 0)
|
|
, _imu1_weight(0.5f)
|
|
{
|
|
_dcm_matrix.identity();
|
|
|
|
// these are experimentally derived from the simulator
|
|
// with large drift levels
|
|
_ki = 0.0087f;
|
|
_ki_yaw = 0.01f;
|
|
}
|
|
|
|
/* 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(bool skip_ins_update=false) override;
|
|
void reset(bool recover_eulers = false) override;
|
|
|
|
// 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;
|
|
}
|
|
|
|
void get_relative_position_D_home(float &posD) const override;
|
|
|
|
// 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;
|
|
}
|
|
|
|
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;
|
|
|
|
bool set_home(const Location &loc) 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;
|
|
|
|
private:
|
|
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);
|
|
bool have_gps(void) const;
|
|
bool use_fast_gains(void) const;
|
|
void load_watchdog_home();
|
|
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
|
|
|
|
// 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;
|
|
|
|
// state to support status reporting
|
|
float _renorm_val_sum;
|
|
uint16_t _renorm_val_count;
|
|
float _error_rp;
|
|
float _error_yaw;
|
|
|
|
// 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;
|
|
|
|
// the earths magnetic field
|
|
float _last_declination;
|
|
Vector2f _mag_earth;
|
|
|
|
// 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;
|
|
|
|
// last time AHRS failed in milliseconds
|
|
uint32_t _last_failure_ms;
|
|
|
|
// time when DCM was last reset
|
|
uint32_t _last_startup_ms;
|
|
|
|
Location last_origin;
|
|
};
|