From 03ff3b18c2e337bec441b7a79c93a395d6c9ca9d Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 6 Feb 2024 01:01:02 +0000 Subject: [PATCH] Plane: output nav scripting throttle with rest of nav scripting stuff --- ArduPlane/Attitude.cpp | 1 + ArduPlane/mode_auto.cpp | 1 - ArduPlane/servos.cpp | 6 ------ 3 files changed, 1 insertion(+), 7 deletions(-) diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index b566d4ca730bf..0996a50b725ab 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -420,6 +420,7 @@ void Plane::stabilize() } SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, rudder); SRV_Channels::set_output_scaled(SRV_Channel::k_steering, rudder); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.nav_scripting.throttle_pct); #endif } else { plane.control_mode->run(); diff --git a/ArduPlane/mode_auto.cpp b/ArduPlane/mode_auto.cpp index f1a9f5de5540b..73753e1fa44ca 100644 --- a/ArduPlane/mode_auto.cpp +++ b/ArduPlane/mode_auto.cpp @@ -102,7 +102,6 @@ void ModeAuto::update() // NAV_SCRIPTING has a desired roll and pitch rate and desired throttle plane.nav_roll_cd = ahrs.roll_sensor; plane.nav_pitch_cd = ahrs.pitch_sensor; - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.nav_scripting.throttle_pct); #endif } else { // we are doing normal AUTO flight, the special cases diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index c0cab9f9de4f4..3f4310def0850 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -582,12 +582,6 @@ void Plane::set_throttle(void) // Update voltage scaling g2.fwd_batt_cmp.update(); -#if AP_SCRIPTING_ENABLED - if (nav_scripting_active()) { - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.nav_scripting.throttle_pct); - } -#endif - if (control_mode->use_battery_compensation()) { // Apply voltage compensation to throttle output from flight mode const float throttle = g2.fwd_batt_cmp.apply_throttle(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle));