From ea9d35c99d15281fbec473cb0653d49e280419c4 Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Sun, 6 Feb 2022 20:23:17 -0600 Subject: [PATCH] ArduPlane: extend nav_scripting to all modes --- ArduPlane/Attitude.cpp | 29 ++++++++++++++++------------- ArduPlane/Plane.h | 6 +++++- ArduPlane/commands_logic.cpp | 27 ++++++++++++++++++++++++++- ArduPlane/mode.cpp | 5 +++++ ArduPlane/mode_acro.cpp | 1 - ArduPlane/servos.cpp | 4 ++++ 6 files changed, 56 insertions(+), 16 deletions(-) diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 14fe0a10ce..52d455af6d 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -495,6 +495,22 @@ void Plane::stabilize() if (control_mode == &mode_training) { stabilize_training(speed_scaler); +#if AP_SCRIPTING_ENABLED + } else if ((control_mode == &mode_auto && + mission.get_current_nav_cmd().id == MAV_CMD_NAV_SCRIPT_TIME) || (nav_scripting.enabled && nav_scripting.current_ms > 0)) { + // scripting is in control of roll and pitch rates and throttle + const float aileron = rollController.get_rate_out(nav_scripting.roll_rate_dps, speed_scaler); + const float elevator = pitchController.get_rate_out(nav_scripting.pitch_rate_dps, speed_scaler); + SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, aileron); + SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elevator); + if (yawController.rate_control_enabled()) { + const float rudder = yawController.get_rate_out(nav_scripting.yaw_rate_dps, speed_scaler, false); + steering_control.rudder = rudder; + } + if (AP_HAL::millis() - nav_scripting.current_ms > 50) { //set_target_throttle_rate_rpy has not been called from script in last 50ms + nav_scripting.current_ms = 0; + } +#endif } else if (control_mode == &mode_acro) { stabilize_acro(speed_scaler); #if HAL_QUADPLANE_ENABLED @@ -509,19 +525,6 @@ void Plane::stabilize() plane.stabilize_roll(speed_scaler); plane.stabilize_pitch(speed_scaler); } -#endif -#if AP_SCRIPTING_ENABLED - } else if (control_mode == &mode_auto && - mission.get_current_nav_cmd().id == MAV_CMD_NAV_SCRIPT_TIME) { - // scripting is in control of roll and pitch rates and throttle - const float aileron = rollController.get_rate_out(nav_scripting.roll_rate_dps, speed_scaler); - const float elevator = pitchController.get_rate_out(nav_scripting.pitch_rate_dps, speed_scaler); - SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, aileron); - SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elevator); - if (yawController.rate_control_enabled()) { - const float rudder = yawController.get_rate_out(nav_scripting.yaw_rate_dps, speed_scaler, false); - steering_control.rudder = rudder; - } #endif } else { if (allow_stick_mixing && g.stick_mixing == StickMixing::FBW && control_mode != &mode_stabilize) { diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index a94cc19f70..139820da36 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -517,14 +517,17 @@ private: #if AP_SCRIPTING_ENABLED // support for scripting nav commands, with verify struct { + bool enabled; uint16_t id; float roll_rate_dps; float pitch_rate_dps; float yaw_rate_dps; float throttle_pct; uint32_t start_ms; + uint32_t current_ms; bool done; } nav_scripting; + #endif struct { @@ -1138,8 +1141,9 @@ private: // command throttle percentage and roll, pitch, yaw target // rates. For use with scripting controllers bool set_target_throttle_rate_rpy(float throttle_pct, float roll_rate_dps, float pitch_rate_dps, float yaw_rate_dps) override; + bool nav_scripting_enable(uint8_t mode) override; #endif - + enum Failsafe_Action { Failsafe_Action_None = 0, Failsafe_Action_RTL = 1, diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index b2ecc8ce28..8fded3adbf 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -1181,7 +1181,7 @@ void Plane::nav_script_time_done(uint16_t id) } } -// support for NAV_SCRIPTING mission command +// support for NAV_SCRIPTING mission command and aerobatics in other allowed modes bool Plane::set_target_throttle_rate_rpy(float throttle_pct, float roll_rate_dps, float pitch_rate_dps, float yaw_rate_dps) { if (!nav_scripting_active()) { @@ -1191,6 +1191,31 @@ bool Plane::set_target_throttle_rate_rpy(float throttle_pct, float roll_rate_dps nav_scripting.pitch_rate_dps = constrain_float(pitch_rate_dps, -g.acro_pitch_rate, g.acro_pitch_rate); nav_scripting.yaw_rate_dps = constrain_float(yaw_rate_dps, -g.acro_yaw_rate, g.acro_yaw_rate); nav_scripting.throttle_pct = throttle_pct; + nav_scripting.current_ms = AP_HAL::millis(); return true; } + +// enable NAV_SCRIPTING takeover in modes other than AUTO using script time mission commands +bool Plane::nav_scripting_enable(uint8_t mode) +{ + uint8_t current_control_mode = control_mode->mode_number(); + if (current_control_mode == mode) { + switch (current_control_mode) { + case Mode::Number::CIRCLE: + case Mode::Number::STABILIZE: + case Mode::Number::ACRO: + case Mode::Number::FLY_BY_WIRE_A: + case Mode::Number::FLY_BY_WIRE_B: + case Mode::Number::CRUISE: + case Mode::Number::LOITER: + nav_scripting.enabled = true; + break; + default: + nav_scripting.enabled = false; + } + } else { + nav_scripting.enabled = false; + } + return nav_scripting.enabled; +} #endif // AP_SCRIPTING_ENABLED diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index 6d07d15fa7..f4ca11a06c 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -19,6 +19,11 @@ void Mode::exit() bool Mode::enter() { +#if AP_SCRIPTING_ENABLED + // reset nav_scripting.enabled + plane.nav_scripting.enabled = false; +#endif + // cancel inverted flight plane.auto_state.inverted_flight = false; diff --git a/ArduPlane/mode_acro.cpp b/ArduPlane/mode_acro.cpp index 53d569277d..77406c3592 100644 --- a/ArduPlane/mode_acro.cpp +++ b/ArduPlane/mode_acro.cpp @@ -5,7 +5,6 @@ bool ModeAcro::_enter() { plane.acro_state.locked_roll = false; plane.acro_state.locked_pitch = false; - return true; } diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index d8df5e4f2d..41b6f8ff45 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -551,6 +551,10 @@ void Plane::set_servos_controlled(void) // manual pass through of throttle while throttle is suppressed SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input(true)); } +#if AP_SCRIPTING_ENABLED + } else if (plane.nav_scripting.current_ms > 0 && nav_scripting.enabled) { + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.nav_scripting.throttle_pct); +#endif } else if (control_mode == &mode_stabilize || control_mode == &mode_training || control_mode == &mode_acro ||