@ -23,11 +23,38 @@
@@ -23,11 +23,38 @@
class AP_AHRS_DCM : public AP_AHRS {
public :
static AP_AHRS_DCM create ( AP_InertialSensor & ins , AP_Baro & baro , AP_GPS & gps ) {
return AP_AHRS_DCM { ins , baro , gps } ;
}
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 ( ) ;
constexpr AP_AHRS_DCM ( AP_AHRS_DCM & & other ) = default ;
// 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 ;
@ -93,40 +120,6 @@ public:
@@ -93,40 +120,6 @@ 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 ;