Copter: poshold clears wind est when disarmed or landed

mohanbabi · Nov 9, 2020 · 0f23458 · 0f23458
1 parent 9b44410
commit 0f23458
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 4 deletions.
diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h
@@ -973,6 +973,7 @@ class ModePosHold : public Mode {
     void update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw);
     float mix_controls(float mix_ratio, float first_control, float second_control);
     void update_brake_angle_from_velocity(float &brake_angle, float velocity);
+    void init_wind_comp_estimate();
     void update_wind_comp_estimate();
     void get_wind_comp_lean_angles(float &roll_angle, float &pitch_angle);
     void roll_controller_to_pilot_override();

diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp
@@ -59,10 +59,7 @@ bool ModePosHold::init(bool ignore_checks)
     loiter_nav->init_target();
 
     // initialise wind_comp each time PosHold is switched on
-    wind_comp_ef.zero();
-    wind_comp_timer = 0;
-    wind_comp_roll = 0.0f;
-    wind_comp_pitch = 0.0f;
+    init_wind_comp_estimate();
 
     return true;
 }
@@ -118,6 +115,9 @@ void ModePosHold::run()
         // set poshold state to pilot override
         roll_mode = RPMode::PILOT_OVERRIDE;
         pitch_mode = RPMode::PILOT_OVERRIDE;
+
+        // initialise wind compensation estimate
+        init_wind_comp_estimate();
         break;
 
     case AltHold_Takeoff:
@@ -151,6 +151,7 @@ void ModePosHold::run()
         loiter_nav->init_target();
         loiter_nav->update();
         attitude_control->set_yaw_target_to_current_heading();
+        init_wind_comp_estimate();
         FALLTHROUGH;
 
     case AltHold_Landed_Pre_Takeoff:
@@ -562,6 +563,15 @@ void ModePosHold::update_brake_angle_from_velocity(float &brake_angle, float vel
     brake_angle = constrain_float(brake_angle, -(float)g.poshold_brake_angle_max, (float)g.poshold_brake_angle_max);
 }
 
+// initialise wind compensation estimate back to zero
+void ModePosHold::init_wind_comp_estimate()
+{
+    wind_comp_ef.zero();
+    wind_comp_timer = 0;
+    wind_comp_roll = 0.0f;
+    wind_comp_pitch = 0.0f;
+}
+
 // update_wind_comp_estimate - updates wind compensation estimate
 //  should be called at the maximum loop rate when loiter is engaged
 void ModePosHold::update_wind_comp_estimate()