zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Copter: Delay release of I term until take off

master
Leonard Hall 5 years ago committed by Randy Mackay
parent
aca7d67172
commit
6dc8dd2960
10 changed files with 18 additions and 24 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 3
      ArduCopter/mode_acro.cpp
  2. 6
      ArduCopter/mode_althold.cpp
  3. 3
      ArduCopter/mode_drift.cpp
  4. 1
      ArduCopter/mode_flip.cpp
  5. 4
      ArduCopter/mode_flowhold.cpp
  6. 7
      ArduCopter/mode_loiter.cpp
  7. 7
      ArduCopter/mode_poshold.cpp
  8. 5
      ArduCopter/mode_sport.cpp
  9. 3
      ArduCopter/mode_stabilize.cpp
  10. 3
      ArduCopter/mode_systemid.cpp

3
ArduCopter/mode_acro.cpp
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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

Write Preview
Loading…
Cancel
Save