From de0dcdd9e47e6bc6762b7d4f5c8f836a97346221 Mon Sep 17 00:00:00 2001 From: Michelle Rossouw Date: Thu, 27 Jul 2023 15:05:11 +1000 Subject: [PATCH] Blimp: Fix inverted throttle --- Blimp/AP_Arming.cpp | 2 +- Blimp/Blimp.h | 2 +- Blimp/RC_Channel.cpp | 2 +- Blimp/mode.cpp | 2 +- Blimp/mode.h | 2 +- Blimp/motors.cpp | 2 +- Blimp/radio.cpp | 18 +++++++++--------- 7 files changed, 15 insertions(+), 15 deletions(-) diff --git a/Blimp/AP_Arming.cpp b/Blimp/AP_Arming.cpp index c72d250cc2..ea36c2c1f0 100644 --- a/Blimp/AP_Arming.cpp +++ b/Blimp/AP_Arming.cpp @@ -107,7 +107,7 @@ bool AP_Arming_Blimp::parameter_checks(bool display_failure) // failsafe parameter checks if (blimp.g.failsafe_throttle) { // check throttle min is above throttle failsafe trigger and that the trigger is above ppm encoder's loss-of-signal value of 900 - if (blimp.channel_down->get_radio_min() <= blimp.g.failsafe_throttle_value+10 || blimp.g.failsafe_throttle_value < 910) { + if (blimp.channel_up->get_radio_min() <= blimp.g.failsafe_throttle_value+10 || blimp.g.failsafe_throttle_value < 910) { check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Check FS_THR_VALUE"); return false; } diff --git a/Blimp/Blimp.h b/Blimp/Blimp.h index 763ef015a8..d106c85f68 100644 --- a/Blimp/Blimp.h +++ b/Blimp/Blimp.h @@ -111,7 +111,7 @@ private: // primary input control channels RC_Channel *channel_right; RC_Channel *channel_front; - RC_Channel *channel_down; + RC_Channel *channel_up; RC_Channel *channel_yaw; AP_Logger logger; diff --git a/Blimp/RC_Channel.cpp b/Blimp/RC_Channel.cpp index 12ef483847..7709867cf2 100644 --- a/Blimp/RC_Channel.cpp +++ b/Blimp/RC_Channel.cpp @@ -106,7 +106,7 @@ bool RC_Channel_Blimp::do_aux_function(const aux_func_t ch_option, const AuxSwit case AUX_FUNC::SAVE_TRIM: if ((ch_flag == AuxSwitchPos::HIGH) && (blimp.control_mode <= Mode::Number::MANUAL) && - (blimp.channel_down->get_control_in() == 0)) { + (blimp.channel_up->get_control_in() == 0)) { blimp.save_trim(); } break; diff --git a/Blimp/mode.cpp b/Blimp/mode.cpp index 64a08c829a..23f1328487 100644 --- a/Blimp/mode.cpp +++ b/Blimp/mode.cpp @@ -19,7 +19,7 @@ Mode::Mode(void) : loiter(blimp.loiter), channel_right(blimp.channel_right), channel_front(blimp.channel_front), - channel_down(blimp.channel_down), + channel_up(blimp.channel_up), channel_yaw(blimp.channel_yaw), G_Dt(blimp.G_Dt) { }; diff --git a/Blimp/mode.h b/Blimp/mode.h index b57c1d1152..107cc8c1a3 100644 --- a/Blimp/mode.h +++ b/Blimp/mode.h @@ -108,7 +108,7 @@ protected: Loiter *&loiter; RC_Channel *&channel_right; RC_Channel *&channel_front; - RC_Channel *&channel_down; + RC_Channel *&channel_up; RC_Channel *&channel_yaw; float &G_Dt; diff --git a/Blimp/motors.cpp b/Blimp/motors.cpp index c203477488..35ba08b90c 100644 --- a/Blimp/motors.cpp +++ b/Blimp/motors.cpp @@ -18,7 +18,7 @@ void Blimp::arm_motors_check() } // ensure throttle is down - if (channel_down->get_control_in() > 0) { //MIR what dow we do with this? + if (channel_up->get_control_in() > 0) { //MIR what dow we do with this? arming_counter = 0; return; } diff --git a/Blimp/radio.cpp b/Blimp/radio.cpp index e7640c44c2..861c057820 100644 --- a/Blimp/radio.cpp +++ b/Blimp/radio.cpp @@ -8,23 +8,23 @@ void Blimp::default_dead_zones() { channel_right->set_default_dead_zone(20); channel_front->set_default_dead_zone(20); - channel_down->set_default_dead_zone(30); + channel_up->set_default_dead_zone(30); channel_yaw->set_default_dead_zone(20); rc().channel(CH_6)->set_default_dead_zone(0); } void Blimp::init_rc_in() { - channel_right = rc().channel(rcmap.roll()-1); - channel_front = rc().channel(rcmap.pitch()-1); - channel_down = rc().channel(rcmap.throttle()-1); - channel_yaw = rc().channel(rcmap.yaw()-1); + channel_right = rc().channel(rcmap.roll()-1); + channel_front = rc().channel(rcmap.pitch()-1); + channel_up = rc().channel(rcmap.throttle()-1); + channel_yaw = rc().channel(rcmap.yaw()-1); // set rc channel ranges channel_right->set_angle(RC_SCALE); channel_front->set_angle(RC_SCALE); channel_yaw->set_angle(RC_SCALE); - channel_down->set_angle(RC_SCALE); + channel_up->set_angle(RC_SCALE); // set default dead zones default_dead_zones(); @@ -58,14 +58,14 @@ void Blimp::read_radio() if (rc().read_input()) { ap.new_radio_frame = true; - set_throttle_and_failsafe(channel_down->get_radio_in()); - set_throttle_zero_flag(channel_down->get_control_in()); + set_throttle_and_failsafe(channel_up->get_radio_in()); + set_throttle_zero_flag(channel_up->get_control_in()); // RC receiver must be attached if we've just got input ap.rc_receiver_present = true; const float dt = (tnow_ms - last_radio_update_ms)*1.0e-3f; - rc_throttle_control_in_filter.apply(channel_down->get_control_in(), dt); + rc_throttle_control_in_filter.apply(channel_up->get_control_in(), dt); last_radio_update_ms = tnow_ms; return; }