Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-02 22:18:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: Reorder Alt_hold state switch statement for clarity

Browse Source
This commit is contained in:
Leonard Hall 2022-09-26 08:48:44 +09:30 committed by Randy Mackay
parent 4a4f361a17
commit 1a5b4fb7d2
3 changed files with 36 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
ArduCopter/mode_loiter.cpp
View File

@ -131,6 +131,17 @@ void ModeLoiter::run()
attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate, false); attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate, false);
break; break;
case AltHold_Landed_Ground_Idle:
attitude_control->reset_yaw_target_and_rate();
FALLTHROUGH;
case AltHold_Landed_Pre_Takeoff:
attitude_control->reset_rate_controller_I_terms_smoothly();
loiter_nav->init_target();
attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate, false);
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero
break;
case AltHold_Takeoff: case AltHold_Takeoff:
// initiate take-off // initiate take-off
if (!takeoff.running()) { if (!takeoff.running()) {
@ -150,17 +161,6 @@ void ModeLoiter::run()
attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate, false); attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate, false);
break; break;
case AltHold_Landed_Ground_Idle:
attitude_control->reset_yaw_target_and_rate();
FALLTHROUGH;
case AltHold_Landed_Pre_Takeoff:
attitude_control->reset_rate_controller_I_terms_smoothly();
loiter_nav->init_target();
attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate, false);
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero
break;
case AltHold_Flying: case AltHold_Flying:
// set motors to full range // set motors to full range
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);

32
ArduCopter/mode_poshold.cpp
View File

@ -115,6 +115,22 @@ void ModePosHold::run()
init_wind_comp_estimate(); init_wind_comp_estimate();
break; break;
case AltHold_Landed_Ground_Idle:
loiter_nav->clear_pilot_desired_acceleration();
loiter_nav->init_target();
attitude_control->reset_yaw_target_and_rate();
init_wind_comp_estimate();
FALLTHROUGH;
case AltHold_Landed_Pre_Takeoff:
attitude_control->reset_rate_controller_I_terms_smoothly();
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero
// set poshold state to pilot override
roll_mode = RPMode::PILOT_OVERRIDE;
pitch_mode = RPMode::PILOT_OVERRIDE;
break;
case AltHold_Takeoff: case AltHold_Takeoff:
// initiate take-off // initiate take-off
if (!takeoff.running()) { if (!takeoff.running()) {
@ -136,22 +152,6 @@ void ModePosHold::run()
pitch_mode = RPMode::PILOT_OVERRIDE; pitch_mode = RPMode::PILOT_OVERRIDE;
break; break;
case AltHold_Landed_Ground_Idle:
loiter_nav->clear_pilot_desired_acceleration();
loiter_nav->init_target();
attitude_control->reset_yaw_target_and_rate();
init_wind_comp_estimate();
FALLTHROUGH;
case AltHold_Landed_Pre_Takeoff:
attitude_control->reset_rate_controller_I_terms_smoothly();
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero
// set poshold state to pilot override
roll_mode = RPMode::PILOT_OVERRIDE;
pitch_mode = RPMode::PILOT_OVERRIDE;
break;
case AltHold_Flying: case AltHold_Flying:
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);

18
ArduCopter/mode_sport.cpp
View File

@ -80,6 +80,15 @@ void ModeSport::run()
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero
break; break;
case AltHold_Landed_Ground_Idle:
attitude_control->reset_yaw_target_and_rate();
FALLTHROUGH;
case AltHold_Landed_Pre_Takeoff:
attitude_control->reset_rate_controller_I_terms_smoothly();
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero
break;
case AltHold_Takeoff: case AltHold_Takeoff:
// initiate take-off // initiate take-off
if (!takeoff.running()) { if (!takeoff.running()) {
@ -93,15 +102,6 @@ void ModeSport::run()
takeoff.do_pilot_takeoff(target_climb_rate); takeoff.do_pilot_takeoff(target_climb_rate);
break; break;
case AltHold_Landed_Ground_Idle:
attitude_control->reset_yaw_target_and_rate();
FALLTHROUGH;
case AltHold_Landed_Pre_Takeoff:
attitude_control->reset_rate_controller_I_terms_smoothly();
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero
break;
case AltHold_Flying: case AltHold_Flying:
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);