diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index f6b368f48c..b6ddfe7631 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -973,6 +973,7 @@ private: 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 index 7189b7f38f..a3424fd783 100644 --- 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()