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

Copter: Delay release of I term until take off

This commit is contained in:
Leonard Hall 2019-11-04 13:24:47 +10:30 committed by Randy Mackay
parent aca7d67172
commit 6dc8dd2960
10 changed files with 18 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduCopter/mode_acro.cpp
View File

@ -30,17 +30,20 @@ void ModeAcro::run()
attitude_control->set_attitude_target_to_current_attitude(); attitude_control->set_attitude_target_to_current_attitude();
attitude_control->reset_rate_controller_I_terms(); attitude_control->reset_rate_controller_I_terms();
break; break;
case AP_Motors::SpoolState::GROUND_IDLE: case AP_Motors::SpoolState::GROUND_IDLE:
// Landed // Landed
attitude_control->set_attitude_target_to_current_attitude(); attitude_control->set_attitude_target_to_current_attitude();
attitude_control->reset_rate_controller_I_terms(); attitude_control->reset_rate_controller_I_terms();
break; break;
case AP_Motors::SpoolState::THROTTLE_UNLIMITED: case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
// clear landing flag above zero throttle // clear landing flag above zero throttle
if (!motors->limit.throttle_lower) { if (!motors->limit.throttle_lower) {
set_land_complete(false); set_land_complete(false);
} }
break; break;
case AP_Motors::SpoolState::SPOOLING_UP: case AP_Motors::SpoolState::SPOOLING_UP:
case AP_Motors::SpoolState::SPOOLING_DOWN: case AP_Motors::SpoolState::SPOOLING_DOWN:
// do nothing // do nothing

6
ArduCopter/mode_althold.cpp
View File

@ -48,25 +48,21 @@ void ModeAltHold::run()
switch (althold_state) { switch (althold_state) {
case AltHold_MotorStopped: case AltHold_MotorStopped:
attitude_control->reset_rate_controller_I_terms(); attitude_control->reset_rate_controller_I_terms();
attitude_control->set_yaw_target_to_current_heading(); attitude_control->set_yaw_target_to_current_heading();
pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
break; break;
case AltHold_Landed_Ground_Idle: case AltHold_Landed_Ground_Idle:
attitude_control->reset_rate_controller_I_terms();
attitude_control->set_yaw_target_to_current_heading(); attitude_control->set_yaw_target_to_current_heading();
// FALLTHROUGH // FALLTHROUGH
case AltHold_Landed_Pre_Takeoff: case AltHold_Landed_Pre_Takeoff:
attitude_control->reset_rate_controller_I_terms();
pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
break; break;
case AltHold_Takeoff: case AltHold_Takeoff:
// initiate take-off // initiate take-off
if (!takeoff.running()) { if (!takeoff.running()) {
takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));

3
ArduCopter/mode_drift.cpp
View File

@ -94,17 +94,20 @@ void ModeDrift::run()
attitude_control->set_yaw_target_to_current_heading(); attitude_control->set_yaw_target_to_current_heading();
attitude_control->reset_rate_controller_I_terms(); attitude_control->reset_rate_controller_I_terms();
break; break;
case AP_Motors::SpoolState::GROUND_IDLE: case AP_Motors::SpoolState::GROUND_IDLE:
// Landed // Landed
attitude_control->set_yaw_target_to_current_heading(); attitude_control->set_yaw_target_to_current_heading();
attitude_control->reset_rate_controller_I_terms(); attitude_control->reset_rate_controller_I_terms();
break; break;
case AP_Motors::SpoolState::THROTTLE_UNLIMITED: case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
// clear landing flag above zero throttle // clear landing flag above zero throttle
if (!motors->limit.throttle_lower) { if (!motors->limit.throttle_lower) {
set_land_complete(false); set_land_complete(false);
} }
break; break;
case AP_Motors::SpoolState::SPOOLING_UP: case AP_Motors::SpoolState::SPOOLING_UP:
case AP_Motors::SpoolState::SPOOLING_DOWN: case AP_Motors::SpoolState::SPOOLING_DOWN:
// do nothing // do nothing

1
ArduCopter/mode_flip.cpp
View File

@ -200,6 +200,7 @@ void ModeFlip::run()
Log_Write_Event(DATA_FLIP_END); Log_Write_Event(DATA_FLIP_END);
} }
break; break;
} }
case FlipState::Abandon: case FlipState::Abandon:
// restore original flight mode // restore original flight mode

4
ArduCopter/mode_flowhold.cpp
View File

@ -254,7 +254,6 @@ void ModeFlowHold::run()
switch (flowhold_state) { switch (flowhold_state) {
case AltHold_MotorStopped: case AltHold_MotorStopped:
copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN); copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
copter.attitude_control->reset_rate_controller_I_terms(); copter.attitude_control->reset_rate_controller_I_terms();
copter.attitude_control->set_yaw_target_to_current_heading(); copter.attitude_control->set_yaw_target_to_current_heading();
@ -283,12 +282,11 @@ void ModeFlowHold::run()
break; break;
case AltHold_Landed_Ground_Idle: case AltHold_Landed_Ground_Idle:
attitude_control->reset_rate_controller_I_terms();
attitude_control->set_yaw_target_to_current_heading(); attitude_control->set_yaw_target_to_current_heading();
// FALLTHROUGH // FALLTHROUGH
case AltHold_Landed_Pre_Takeoff: case AltHold_Landed_Pre_Takeoff:
attitude_control->reset_rate_controller_I_terms();
pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
break; break;

7
ArduCopter/mode_loiter.cpp
View File

@ -117,7 +117,6 @@ void ModeLoiter::run()
switch (loiter_state) { switch (loiter_state) {
case AltHold_MotorStopped: case AltHold_MotorStopped:
attitude_control->reset_rate_controller_I_terms(); attitude_control->reset_rate_controller_I_terms();
attitude_control->set_yaw_target_to_current_heading(); attitude_control->set_yaw_target_to_current_heading();
pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
@ -127,7 +126,6 @@ void ModeLoiter::run()
break; break;
case AltHold_Takeoff: case AltHold_Takeoff:
// initiate take-off // initiate take-off
if (!takeoff.running()) { if (!takeoff.running()) {
takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
@ -152,13 +150,11 @@ void ModeLoiter::run()
break; break;
case AltHold_Landed_Ground_Idle: case AltHold_Landed_Ground_Idle:
attitude_control->reset_rate_controller_I_terms();
attitude_control->set_yaw_target_to_current_heading(); attitude_control->set_yaw_target_to_current_heading();
// FALLTHROUGH // FALLTHROUGH
case AltHold_Landed_Pre_Takeoff: case AltHold_Landed_Pre_Takeoff:
attitude_control->reset_rate_controller_I_terms();
loiter_nav->init_target(); loiter_nav->init_target();
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f);
pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
@ -166,7 +162,6 @@ void ModeLoiter::run()
break; 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);

7
ArduCopter/mode_poshold.cpp
View File

@ -107,7 +107,6 @@ void ModePosHold::run()
switch (poshold_state) { switch (poshold_state) {
case AltHold_MotorStopped: case AltHold_MotorStopped:
attitude_control->reset_rate_controller_I_terms(); attitude_control->reset_rate_controller_I_terms();
attitude_control->set_yaw_target_to_current_heading(); attitude_control->set_yaw_target_to_current_heading();
pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
@ -121,7 +120,6 @@ void ModePosHold::run()
break; break;
case AltHold_Takeoff: case AltHold_Takeoff:
// initiate take-off // initiate take-off
if (!takeoff.running()) { if (!takeoff.running()) {
takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
@ -148,16 +146,14 @@ void ModePosHold::run()
break; break;
case AltHold_Landed_Ground_Idle: case AltHold_Landed_Ground_Idle:
loiter_nav->clear_pilot_desired_acceleration(); loiter_nav->clear_pilot_desired_acceleration();
loiter_nav->init_target(); loiter_nav->init_target();
loiter_nav->update(); loiter_nav->update();
attitude_control->reset_rate_controller_I_terms();
attitude_control->set_yaw_target_to_current_heading(); attitude_control->set_yaw_target_to_current_heading();
// FALLTHROUGH // FALLTHROUGH
case AltHold_Landed_Pre_Takeoff: case AltHold_Landed_Pre_Takeoff:
attitude_control->reset_rate_controller_I_terms();
pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
// set poshold state to pilot override // set poshold state to pilot override
@ -166,7 +162,6 @@ void ModePosHold::run()
break; 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);
#if AC_AVOID_ENABLED == ENABLED #if AC_AVOID_ENABLED == ENABLED

5
ArduCopter/mode_sport.cpp
View File

@ -79,14 +79,12 @@ void ModeSport::run()
switch (sport_state) { switch (sport_state) {
case AltHold_MotorStopped: case AltHold_MotorStopped:
attitude_control->reset_rate_controller_I_terms(); attitude_control->reset_rate_controller_I_terms();
attitude_control->set_yaw_target_to_current_heading(); attitude_control->set_yaw_target_to_current_heading();
pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
break; break;
case AltHold_Takeoff: case AltHold_Takeoff:
// initiate take-off // initiate take-off
if (!takeoff.running()) { if (!takeoff.running()) {
takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
@ -104,12 +102,11 @@ void ModeSport::run()
break; break;
case AltHold_Landed_Ground_Idle: case AltHold_Landed_Ground_Idle:
attitude_control->reset_rate_controller_I_terms();
attitude_control->set_yaw_target_to_current_heading(); attitude_control->set_yaw_target_to_current_heading();
// FALLTHROUGH // FALLTHROUGH
case AltHold_Landed_Pre_Takeoff: case AltHold_Landed_Pre_Takeoff:
attitude_control->reset_rate_controller_I_terms();
pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
break; break;

3
ArduCopter/mode_stabilize.cpp
View File

@ -34,17 +34,20 @@ void ModeStabilize::run()
attitude_control->set_yaw_target_to_current_heading(); attitude_control->set_yaw_target_to_current_heading();
attitude_control->reset_rate_controller_I_terms(); attitude_control->reset_rate_controller_I_terms();
break; break;
case AP_Motors::SpoolState::GROUND_IDLE: case AP_Motors::SpoolState::GROUND_IDLE:
// Landed // Landed
attitude_control->set_yaw_target_to_current_heading(); attitude_control->set_yaw_target_to_current_heading();
attitude_control->reset_rate_controller_I_terms(); attitude_control->reset_rate_controller_I_terms();
break; break;
case AP_Motors::SpoolState::THROTTLE_UNLIMITED: case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
// clear landing flag above zero throttle // clear landing flag above zero throttle
if (!motors->limit.throttle_lower) { if (!motors->limit.throttle_lower) {
set_land_complete(false); set_land_complete(false);
} }
break; break;
case AP_Motors::SpoolState::SPOOLING_UP: case AP_Motors::SpoolState::SPOOLING_UP:
case AP_Motors::SpoolState::SPOOLING_DOWN: case AP_Motors::SpoolState::SPOOLING_DOWN:
// do nothing // do nothing

3
ArduCopter/mode_systemid.cpp
View File

@ -128,6 +128,7 @@ void ModeSystemId::run()
attitude_control->set_yaw_target_to_current_heading(); attitude_control->set_yaw_target_to_current_heading();
attitude_control->reset_rate_controller_I_terms(); attitude_control->reset_rate_controller_I_terms();
break; break;
case AP_Motors::SpoolState::GROUND_IDLE: case AP_Motors::SpoolState::GROUND_IDLE:
// Landed // Landed
// Tradheli initializes targets when going from disarmed to armed state. // Tradheli initializes targets when going from disarmed to armed state.
@ -137,12 +138,14 @@ void ModeSystemId::run()
attitude_control->reset_rate_controller_I_terms(); attitude_control->reset_rate_controller_I_terms();
} }
break; break;
case AP_Motors::SpoolState::THROTTLE_UNLIMITED: case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
// clear landing flag above zero throttle // clear landing flag above zero throttle
if (!motors->limit.throttle_lower) { if (!motors->limit.throttle_lower) {
set_land_complete(false); set_land_complete(false);
} }
break; break;
case AP_Motors::SpoolState::SPOOLING_UP: case AP_Motors::SpoolState::SPOOLING_UP:
case AP_Motors::SpoolState::SPOOLING_DOWN: case AP_Motors::SpoolState::SPOOLING_DOWN:
// do nothing // do nothing