Skip to content

Commit

Permalink
Copter: support for making use of heading error correction selectable
Browse files Browse the repository at this point in the history
  • Loading branch information
@bnsgeyer
bnsgeyer committed Dec 4, 2024
1 parent 626927f commit a0df9fe
Show file tree
Hide file tree
Showing 9 changed files with 94 additions and 38 deletions.
15 changes: 9 additions & 6 deletions ArduCopter/mode_acro_heli.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,26 +45,29 @@ void ModeAcro_Heli::run()
switch (motors->get_spool_state()) {
case AP_Motors::SpoolState::SHUT_DOWN:
// Motors Stopped
attitude_control->reset_target_and_rate(false);
attitude_control->reset_target_and_rate();
attitude_control->reset_rate_controller_I_terms();
break;
case AP_Motors::SpoolState::GROUND_IDLE:
// If aircraft is landed, set target heading to current and reset the integrator
// IF aircraft is landed and not using leaky integrator
// OR initializing targets on arming and using leaky integrator,
// THEN set target heading to current and reset the integrator
// Otherwise motors could be at ground idle for practice autorotation
if ((motors->init_targets_on_arming() && motors->using_leaky_integrator()) || (copter.ap.land_complete && !motors->using_leaky_integrator())) {
attitude_control->reset_target_and_rate(false);
attitude_control->reset_rate_controller_I_terms_smoothly();
}
break;
case AP_Motors::SpoolState::SPOOLING_UP:
case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
case AP_Motors::SpoolState::SPOOLING_DOWN:
if (copter.ap.land_complete && !motors->using_leaky_integrator()) {
if (!motors->using_hdg_error_correction()) {
attitude_control->reset_yaw_target_and_rate(false);
}
attitude_control->reset_rate_controller_I_terms_smoothly();
}
break;
case AP_Motors::SpoolState::SPOOLING_UP:
case AP_Motors::SpoolState::SPOOLING_DOWN:
// do nothing
break;
}

if (!motors->has_flybar()){
Expand Down
11 changes: 8 additions & 3 deletions ArduCopter/mode_althold.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,15 +50,20 @@ void ModeAltHold::run()

case AltHoldModeState::MotorStopped:
attitude_control->reset_rate_controller_I_terms();
attitude_control->reset_yaw_target_and_rate(false);
attitude_control->reset_yaw_target_and_rate();
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero
break;

case AltHoldModeState::Landed_Ground_Idle:
attitude_control->reset_yaw_target_and_rate();
FALLTHROUGH;
attitude_control->reset_yaw_target_and_rate(false);
attitude_control->reset_rate_controller_I_terms_smoothly();
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero
break;

case AltHoldModeState::Landed_Pre_Takeoff:
if (!motors->using_hdg_error_correction()) {
attitude_control->reset_target_and_rate(false);
}
attitude_control->reset_rate_controller_I_terms_smoothly();
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero
break;
Expand Down
34 changes: 23 additions & 11 deletions ArduCopter/mode_drift.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,26 +91,38 @@ void ModeDrift::run()
switch (motors->get_spool_state()) {
case AP_Motors::SpoolState::SHUT_DOWN:
// Motors Stopped
attitude_control->reset_yaw_target_and_rate(false);
attitude_control->reset_yaw_target_and_rate();
attitude_control->reset_rate_controller_I_terms();
break;

case AP_Motors::SpoolState::GROUND_IDLE:
// Landed
attitude_control->reset_yaw_target_and_rate();
attitude_control->reset_rate_controller_I_terms_smoothly();
break;

case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
// clear landing flag above zero throttle
if (!motors->limit.throttle_lower) {
set_land_complete(false);
// For helicopters, IF aircraft is landed and not using leaky integrator
// OR initializing targets on arming and using leaky integrator,
// THEN set target heading to current and reset the integrator
// Otherwise motors could be at ground idle for practice autorotation
// If a multirotor then it is always done
if (!copter.is_tradheli() || (motors->init_targets_on_arming() && motors->using_leaky_integrator()) || (copter.ap.land_complete && !motors->using_leaky_integrator())) {
attitude_control->reset_yaw_target_and_rate(false);
attitude_control->reset_rate_controller_I_terms_smoothly();
}
break;

case AP_Motors::SpoolState::SPOOLING_UP:
case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
case AP_Motors::SpoolState::SPOOLING_DOWN:
// do nothing
// clear landing flag above zero throttle
if (!copter.is_tradheli()) {
if (!motors->limit.throttle_lower) {
set_land_complete(false);
}
} else {
if (copter.ap.land_complete && !motors->using_leaky_integrator()) {
if (!motors->using_hdg_error_correction()) {
attitude_control->reset_yaw_target_and_rate(false);
}
attitude_control->reset_rate_controller_I_terms_smoothly();
}
}
break;
}

Expand Down
9 changes: 7 additions & 2 deletions ArduCopter/mode_flowhold.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -288,10 +288,15 @@ void ModeFlowHold::run()
break;

case AltHoldModeState::Landed_Ground_Idle:
attitude_control->reset_yaw_target_and_rate();
FALLTHROUGH;
attitude_control->reset_yaw_target_and_rate(false);
attitude_control->reset_rate_controller_I_terms_smoothly();
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero
break;

case AltHoldModeState::Landed_Pre_Takeoff:
if (!motors->using_hdg_error_correction()) {
attitude_control->reset_target_and_rate(false);
}
attitude_control->reset_rate_controller_I_terms_smoothly();
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero
break;
Expand Down
11 changes: 9 additions & 2 deletions ArduCopter/mode_loiter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -132,10 +132,17 @@ void ModeLoiter::run()
break;

case AltHoldModeState::Landed_Ground_Idle:
attitude_control->reset_yaw_target_and_rate();
FALLTHROUGH;
attitude_control->reset_yaw_target_and_rate(false);
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 AltHoldModeState::Landed_Pre_Takeoff:
if (!motors->using_hdg_error_correction()) {
attitude_control->reset_target_and_rate(false);
}
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);
Expand Down
15 changes: 12 additions & 3 deletions ArduCopter/mode_poshold.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -102,7 +102,7 @@ void ModePosHold::run()

case AltHoldModeState::MotorStopped:
attitude_control->reset_rate_controller_I_terms();
attitude_control->reset_yaw_target_and_rate(false);
attitude_control->reset_yaw_target_and_rate();
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero
loiter_nav->clear_pilot_desired_acceleration();
loiter_nav->init_target();
Expand All @@ -116,13 +116,22 @@ void ModePosHold::run()
break;

case AltHoldModeState::Landed_Ground_Idle:
attitude_control->reset_yaw_target_and_rate(false);
attitude_control->reset_rate_controller_I_terms_smoothly();
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero
loiter_nav->clear_pilot_desired_acceleration();
loiter_nav->init_target();
attitude_control->reset_yaw_target_and_rate();
init_wind_comp_estimate();
FALLTHROUGH;

// set poshold state to pilot override
roll_mode = RPMode::PILOT_OVERRIDE;
pitch_mode = RPMode::PILOT_OVERRIDE;
break;

case AltHoldModeState::Landed_Pre_Takeoff:
if (!motors->using_hdg_error_correction()) {
attitude_control->reset_target_and_rate(false);
}
attitude_control->reset_rate_controller_I_terms_smoothly();
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero

Expand Down
11 changes: 8 additions & 3 deletions ArduCopter/mode_sport.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,15 +76,20 @@ void ModeSport::run()

case AltHoldModeState::MotorStopped:
attitude_control->reset_rate_controller_I_terms();
attitude_control->reset_yaw_target_and_rate(false);
attitude_control->reset_yaw_target_and_rate();
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero
break;

case AltHoldModeState::Landed_Ground_Idle:
attitude_control->reset_yaw_target_and_rate();
FALLTHROUGH;
attitude_control->reset_yaw_target_and_rate(false);
attitude_control->reset_rate_controller_I_terms_smoothly();
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero
break;

case AltHoldModeState::Landed_Pre_Takeoff:
if (!motors->using_hdg_error_correction()) {
attitude_control->reset_target_and_rate(false);
}
attitude_control->reset_rate_controller_I_terms_smoothly();
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero
break;
Expand Down
15 changes: 9 additions & 6 deletions ArduCopter/mode_stabilize_heli.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,26 +52,29 @@ void ModeStabilize_Heli::run()
switch (motors->get_spool_state()) {
case AP_Motors::SpoolState::SHUT_DOWN:
// Motors Stopped
attitude_control->reset_yaw_target_and_rate(false);
attitude_control->reset_yaw_target_and_rate();
attitude_control->reset_rate_controller_I_terms();
break;
case AP_Motors::SpoolState::GROUND_IDLE:
// If aircraft is landed, set target heading to current and reset the integrator
// IF aircraft is landed and not using leaky integrator
// OR initializing targets on arming and using leaky integrator,
// THEN set target heading to current and reset the integrator
// Otherwise motors could be at ground idle for practice autorotation
if ((motors->init_targets_on_arming() && motors->using_leaky_integrator()) || (copter.ap.land_complete && !motors->using_leaky_integrator())) {
attitude_control->reset_yaw_target_and_rate(false);
attitude_control->reset_rate_controller_I_terms_smoothly();
}
break;
case AP_Motors::SpoolState::SPOOLING_UP:
case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
case AP_Motors::SpoolState::SPOOLING_DOWN:
if (copter.ap.land_complete && !motors->using_leaky_integrator()) {
if (!motors->using_hdg_error_correction()) {
attitude_control->reset_target_and_rate(false);
}
attitude_control->reset_rate_controller_I_terms_smoothly();
}
break;
case AP_Motors::SpoolState::SPOOLING_UP:
case AP_Motors::SpoolState::SPOOLING_DOWN:
// do nothing
break;
}

// call attitude controller
Expand Down
11 changes: 9 additions & 2 deletions ArduCopter/mode_zigzag.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -356,10 +356,17 @@ void ModeZigZag::manual_control()
break;

case AltHoldModeState::Landed_Ground_Idle:
attitude_control->reset_yaw_target_and_rate();
FALLTHROUGH;
attitude_control->reset_yaw_target_and_rate(false);
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);
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero
break;

case AltHoldModeState::Landed_Pre_Takeoff:
if (!motors->using_hdg_error_correction()) {
attitude_control->reset_target_and_rate(false);
}
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);
Expand Down

0 comments on commit a0df9fe

Please sign in to comment.