From 891104ddbefbd39c19276d15cffe35a00f84d145 Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Tue, 9 Apr 2024 18:06:35 -0600 Subject: [PATCH] AP_Mount: Do not override default mode when first connecting to RC --- libraries/AP_Mount/AP_Mount_Backend.cpp | 14 +++++++++++--- libraries/AP_Mount/AP_Mount_Backend.h | 1 + 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 75eb1bba185ab1..45eedd89b915fe 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -618,8 +618,16 @@ void AP_Mount_Backend::set_rctargeting_on_rcinput_change() const int16_t pitch_in = (pitch_ch == nullptr) ? 0 : pitch_ch->get_radio_in(); const int16_t yaw_in = (yaw_ch == nullptr) ? 0 : yaw_ch->get_radio_in(); + if (!last_rc_input.initialised) + { + // The first time through, initial RC inputs should be set, but not used + last_rc_input.initialised = true; + last_rc_input.roll_in = roll_in; + last_rc_input.pitch_in = pitch_in; + last_rc_input.yaw_in = yaw_in; + } // if not in RC_TARGETING or RETRACT modes then check for RC change - if (get_mode() != MAV_MOUNT_MODE_RC_TARGETING && get_mode() != MAV_MOUNT_MODE_RETRACT) { + else if (get_mode() != MAV_MOUNT_MODE_RC_TARGETING && get_mode() != MAV_MOUNT_MODE_RETRACT) { // get dead zones const int16_t roll_dz = (roll_ch == nullptr) ? 10 : MAX(roll_ch->get_dead_zone(), 10); const int16_t pitch_dz = (pitch_ch == nullptr) ? 10 : MAX(pitch_ch->get_dead_zone(), 10); @@ -629,11 +637,11 @@ void AP_Mount_Backend::set_rctargeting_on_rcinput_change() if ((abs(last_rc_input.roll_in - roll_in) > roll_dz) || (abs(last_rc_input.pitch_in - pitch_in) > pitch_dz) || (abs(last_rc_input.yaw_in - yaw_in) > yaw_dz)) { - set_mode(MAV_MOUNT_MODE_RC_TARGETING); + set_mode(MAV_MOUNT_MODE_RC_TARGETING); } } - // if in RC_TARGETING or RETRACT mode then store last RC input + // if NOW in RC_TARGETING or RETRACT mode then store last RC input (mode might have changed) if (get_mode() == MAV_MOUNT_MODE_RC_TARGETING || get_mode() == MAV_MOUNT_MODE_RETRACT) { last_rc_input.roll_in = roll_in; last_rc_input.pitch_in = pitch_in; diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index d15b7cc1d3752c..56706d7463c98e 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -329,6 +329,7 @@ class AP_Mount_Backend // structure holding the last RC inputs struct { + bool initialised; int16_t roll_in; int16_t pitch_in; int16_t yaw_in;