Browse Source

AP_AHRS: make estimate_wind() public

this avoids it linking into copter
mission-4.1.18
Andrew Tridgell 11 years ago
parent
commit
c62ccce9d8
  1. 8
      libraries/AP_AHRS/AP_AHRS_DCM.cpp
  2. 2
      libraries/AP_AHRS/AP_AHRS_DCM.h

8
libraries/AP_AHRS/AP_AHRS_DCM.cpp

@ -742,20 +742,16 @@ AP_AHRS_DCM::drift_correction(float deltat) @@ -742,20 +742,16 @@ AP_AHRS_DCM::drift_correction(float deltat)
// remember the velocity for next time
_last_velocity = velocity;
if (_have_gps_lock && _flags.fly_forward) {
// update wind estimate
estimate_wind(velocity);
}
}
// update our wind speed estimate
void AP_AHRS_DCM::estimate_wind(Vector3f &velocity)
void AP_AHRS_DCM::estimate_wind(void)
{
if (!_flags.wind_estimation) {
return;
}
const Vector3f &velocity = _last_velocity;
// this is based on the wind speed estimation code from MatrixPilot by
// Bill Premerlani. Adaption for ArduPilot by Jon Challinger

2
libraries/AP_AHRS/AP_AHRS_DCM.h

@ -82,6 +82,7 @@ public: @@ -82,6 +82,7 @@ public:
Vector3f get_velocity_NED(void);
Vector3f get_relative_position_NED(void);
void estimate_wind(void);
private:
float _ki;
@ -96,7 +97,6 @@ private: @@ -96,7 +97,6 @@ private:
void drift_correction_yaw(void);
float yaw_error_compass();
void euler_angles(void);
void estimate_wind(Vector3f &velocity);
bool have_gps(void) const;
// primary representation of attitude of board used for all inertial calculations

Loading…
Cancel
Save