Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: support for making use of heading error correction selectable

This commit is contained in:
bnsgeyer 2024-11-17 23:49:44 -05:00
parent 626927f21e
commit a0df9fe15d
9 changed files with 94 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
ArduCopter/mode_acro_heli.cpp
View File

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

11
ArduCopter/mode_althold.cpp
View File

@ -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;

34
ArduCopter/mode_drift.cpp
View File

@ -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;
}

9
ArduCopter/mode_flowhold.cpp
View File

@ -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;

11
ArduCopter/mode_loiter.cpp
View File

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

15
ArduCopter/mode_poshold.cpp
View File

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

11
ArduCopter/mode_sport.cpp
View File

@ -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;

15
ArduCopter/mode_stabilize_heli.cpp
View File

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

11
ArduCopter/mode_zigzag.cpp
View File

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