Browse Source

AHRS: removed AHRS_BARO_USE option

this option has caused users too much trouble. The vertical velocity
is too noisy from the baro
mission-4.1.18
Andrew Tridgell 12 years ago
parent
commit
a4d25f5a82
  1. 7
      libraries/AP_AHRS/AP_AHRS.cpp
  2. 8
      libraries/AP_AHRS/AP_AHRS.h
  3. 10
      libraries/AP_AHRS/AP_AHRS_DCM.cpp

7
libraries/AP_AHRS/AP_AHRS.cpp

@ -49,12 +49,7 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = {
// @Increment: 1 // @Increment: 1
AP_GROUPINFO("WIND_MAX", 6, AP_AHRS, _wind_max, 0.0f), AP_GROUPINFO("WIND_MAX", 6, AP_AHRS, _wind_max, 0.0f),
// @Param: BARO_USE // NOTE: 7 was BARO_USE
// @DisplayName: AHRS Use Barometer
// @Description: This controls the use of the barometer for vertical acceleration compensation in AHRS. It is currently recommended that you set this value to zero unless you are a developer experimenting with the AHRS system.
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
AP_GROUPINFO("BARO_USE", 7, AP_AHRS, _baro_use, 0),
// @Param: TRIM_X // @Param: TRIM_X
// @DisplayName: AHRS Trim Roll // @DisplayName: AHRS Trim Roll

8
libraries/AP_AHRS/AP_AHRS.h

@ -28,8 +28,7 @@ public:
// Constructor // Constructor
AP_AHRS(AP_InertialSensor *ins, GPS *&gps) : AP_AHRS(AP_InertialSensor *ins, GPS *&gps) :
_ins(ins), _ins(ins),
_gps(gps), _gps(gps)
_barometer(NULL)
{ {
// load default values from var_info table // load default values from var_info table
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
@ -57,9 +56,6 @@ public:
_compass->set_board_orientation((enum Rotation)_board_orientation.get()); _compass->set_board_orientation((enum Rotation)_board_orientation.get());
} }
} }
void set_barometer(AP_Baro *barometer) {
_barometer = barometer;
}
void set_airspeed(AP_Airspeed *airspeed) { void set_airspeed(AP_Airspeed *airspeed) {
_airspeed = airspeed; _airspeed = airspeed;
} }
@ -171,7 +167,6 @@ public:
AP_Float _kp; AP_Float _kp;
AP_Float gps_gain; AP_Float gps_gain;
AP_Int8 _gps_use; AP_Int8 _gps_use;
AP_Int8 _baro_use;
AP_Int8 _wind_max; AP_Int8 _wind_max;
AP_Int8 _board_orientation; AP_Int8 _board_orientation;
@ -195,7 +190,6 @@ protected:
// IMU under us without our noticing. // IMU under us without our noticing.
AP_InertialSensor *_ins; AP_InertialSensor *_ins;
GPS *&_gps; GPS *&_gps;
AP_Baro *_barometer;
// a vector to capture the difference between the controller and body frames // a vector to capture the difference between the controller and body frames
AP_Vector3f _trim; AP_Vector3f _trim;

10
libraries/AP_AHRS/AP_AHRS_DCM.cpp

@ -494,16 +494,6 @@ AP_AHRS_DCM::drift_correction(float deltat)
_last_airspeed = airspeed.length(); _last_airspeed = airspeed.length();
} }
/*
* The barometer for vertical velocity is only enabled if we got
* at least 5 pressure samples for the reading. This ensures we
* don't use very noisy climb rate data
*/
if (_baro_use && _barometer != NULL && _barometer->get_pressure_samples() >= 5) {
// Z velocity is down
velocity.z = -_barometer->get_climb_rate();
}
// see if this is our first time through - in which case we // see if this is our first time through - in which case we
// just setup the start times and return // just setup the start times and return
if (_ra_sum_start == 0) { if (_ra_sum_start == 0) {

Loading…
Cancel
Save