aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions 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);
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();

18
ArduCopter/mode_poshold.cpp
View File

@ -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()