From c62ccce9d8a2c072388fcf4b20946d60a3dd2607 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 8 Feb 2014 17:31:05 +1100 Subject: [PATCH] AP_AHRS: make estimate_wind() public this avoids it linking into copter --- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 8 ++------ libraries/AP_AHRS/AP_AHRS_DCM.h | 2 +- 2 files changed, 3 insertions(+), 7 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 20bac2126c..93079f3fb3 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -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 diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index 8c53c7e794..c76707b339 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -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: 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