Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-24 00:33:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: poshold clears wind est when disarmed or landed

Browse Source
This commit is contained in:
Randy Mackay 2020-11-06 10:42:17 +09:00
parent 9b4441011c
commit 0f234583d6
2 changed files with 15 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/mode.h
View File

@ -973,6 +973,7 @@ private:
void update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw); 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); float mix_controls(float mix_ratio, float first_control, float second_control);
void update_brake_angle_from_velocity(float &brake_angle, float velocity); void update_brake_angle_from_velocity(float &brake_angle, float velocity);
void init_wind_comp_estimate();
void update_wind_comp_estimate(); void update_wind_comp_estimate();
void get_wind_comp_lean_angles(float &roll_angle, float &pitch_angle); void get_wind_comp_lean_angles(float &roll_angle, float &pitch_angle);
void roll_controller_to_pilot_override(); void roll_controller_to_pilot_override();

18
ArduCopter/mode_poshold.cpp
View File

@ -59,10 +59,7 @@ bool ModePosHold::init(bool ignore_checks)
loiter_nav->init_target(); loiter_nav->init_target();
// initialise wind_comp each time PosHold is switched on // initialise wind_comp each time PosHold is switched on
wind_comp_ef.zero(); init_wind_comp_estimate();
wind_comp_timer = 0;
wind_comp_roll = 0.0f;
wind_comp_pitch = 0.0f;
return true; return true;
} }
@ -118,6 +115,9 @@ void ModePosHold::run()
// set poshold state to pilot override // set poshold state to pilot override
roll_mode = RPMode::PILOT_OVERRIDE; roll_mode = RPMode::PILOT_OVERRIDE;
pitch_mode = RPMode::PILOT_OVERRIDE; pitch_mode = RPMode::PILOT_OVERRIDE;
// initialise wind compensation estimate
init_wind_comp_estimate();
break; break;
case AltHold_Takeoff: case AltHold_Takeoff:
@ -151,6 +151,7 @@ void ModePosHold::run()
loiter_nav->init_target(); loiter_nav->init_target();
loiter_nav->update(); loiter_nav->update();
attitude_control->set_yaw_target_to_current_heading(); attitude_control->set_yaw_target_to_current_heading();
init_wind_comp_estimate();
FALLTHROUGH; FALLTHROUGH;
case AltHold_Landed_Pre_Takeoff: 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); 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 // update_wind_comp_estimate - updates wind compensation estimate
// should be called at the maximum loop rate when loiter is engaged // should be called at the maximum loop rate when loiter is engaged
void ModePosHold::update_wind_comp_estimate() void ModePosHold::update_wind_comp_estimate()