Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: DCM: add static create method

This commit is contained in:
Lucas De Marchi 2017-08-30 01:15:33 -07:00 committed by Francisco Ferreira
parent beabae6a98
commit cd1ff7b86f
1 changed files with 44 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

78
libraries/AP_AHRS/AP_AHRS_DCM.h
View File

@ -21,43 +21,19 @@
*
*/
class AP_AHRS_DCM : public AP_AHRS
{
class AP_AHRS_DCM : public AP_AHRS {
public:
// Constructors
AP_AHRS_DCM(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps) :
AP_AHRS(ins, baro, gps),
_omega_I_sum_time(0.0f),
_renorm_val_sum(0.0f),
_renorm_val_count(0),
_error_rp(1.0f),
_error_yaw(1.0f),
_gps_last_update(0),
_ra_deltat(0.0f),
_ra_sum_start(0),
_last_declination(0.0f),
_mag_earth(1,0),
_have_gps_lock(false),
_last_lat(0),
_last_lng(0),
_position_offset_north(0.0f),
_position_offset_east(0.0f),
_have_position(false),
_last_wind_time(0),
_last_airspeed(0.0f),
_last_consistent_heading(0),
_imu1_weight(0.5f),
_last_failure_ms(0),
_last_startup_ms(0)
{
_dcm_matrix.identity();
// these are experimentally derived from the simulator
// with large drift levels
_ki = 0.0087f;
_ki_yaw = 0.01f;
static AP_AHRS_DCM create(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps) {
return AP_AHRS_DCM{ins, baro, gps};
}
constexpr AP_AHRS_DCM(AP_AHRS_DCM &&other) = default;
/* 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;
@ -117,6 +93,40 @@ public:
// time that the AHRS has been up
uint32_t uptime_ms() const override;
protected:
AP_AHRS_DCM(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps)
: AP_AHRS(ins, baro, gps)
, _omega_I_sum_time(0.0f)
, _renorm_val_sum(0.0f)
, _renorm_val_count(0)
, _error_rp(1.0f)
, _error_yaw(1.0f)
, _gps_last_update(0)
, _ra_deltat(0.0f)
, _ra_sum_start(0)
, _last_declination(0.0f)
, _mag_earth(1, 0)
, _have_gps_lock(false)
, _last_lat(0)
, _last_lng(0)
, _position_offset_north(0.0f)
, _position_offset_east(0.0f)
, _have_position(false)
, _last_wind_time(0)
, _last_airspeed(0.0f)
, _last_consistent_heading(0)
, _imu1_weight(0.5f)
, _last_failure_ms(0)
, _last_startup_ms(0)
{
_dcm_matrix.identity();
// these are experimentally derived from the simulator
// with large drift levels
_ki = 0.0087f;
_ki_yaw = 0.01f;
}
private:
float _ki;
float _ki_yaw;