diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 8545d75b90bf2..7e1f28d3d5df1 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -570,6 +570,47 @@ void AP_Mount_Backend::calculate_poi() } #endif +// change to RC_TARGETING mode if rc inputs have changed by more than the dead zone +// should be called on every update +void AP_Mount_Backend::set_rctargeting_on_rcinput_change() +{ + // exit immediately if no RC input + if (!rc().has_valid_input()) { + return; + } + + const RC_Channel *roll_ch = rc().find_channel_for_option(_instance == 0 ? RC_Channel::AUX_FUNC::MOUNT1_ROLL : RC_Channel::AUX_FUNC::MOUNT2_ROLL); + const RC_Channel *pitch_ch = rc().find_channel_for_option(_instance == 0 ? RC_Channel::AUX_FUNC::MOUNT1_PITCH : RC_Channel::AUX_FUNC::MOUNT2_PITCH); + const RC_Channel *yaw_ch = rc().find_channel_for_option(_instance == 0 ? RC_Channel::AUX_FUNC::MOUNT1_YAW : RC_Channel::AUX_FUNC::MOUNT2_YAW); + + // get rc input + const int16_t roll_in = (roll_ch == nullptr) ? 0 : roll_ch->get_radio_in(); + 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 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) { + // 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); + const int16_t yaw_dz = (yaw_ch == nullptr) ? 10 : MAX(yaw_ch->get_dead_zone(), 10); + + // check if RC input has changed by more than the dead zone + 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); + } + } + + // if in RC_TARGETING or RETRACT mode then store last RC input + 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; + last_rc_input.yaw_in = yaw_in; + } +} + // get pilot input (in the range -1 to +1) received through RC void AP_Mount_Backend::get_rc_input(float& roll_in, float& pitch_in, float& yaw_in) const { diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index b88ddf42bf760..d2f97273608b5 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -240,6 +240,10 @@ class AP_Mount_Backend void calculate_poi(); #endif + // change to RC_TARGETTING mode if rc inputs have changed by more than the dead zone + // should be called on every update + void set_rctargeting_on_rcinput_change(); + // get pilot input (in the range -1 to +1) received through RC void get_rc_input(float& roll_in, float& pitch_in, float& yaw_in) const; @@ -308,6 +312,13 @@ class AP_Mount_Backend uint32_t _last_warning_ms; // system time of last warning sent to GCS + // structure holding the last RC inputs + struct { + int16_t roll_in; + int16_t pitch_in; + int16_t yaw_in; + } last_rc_input; + // structure holding mavlink sysid and compid of controller of this gimbal // see MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE and GIMBAL_MANAGER_STATUS struct { diff --git a/libraries/AP_Mount/AP_Mount_Gremsy.cpp b/libraries/AP_Mount/AP_Mount_Gremsy.cpp index b9bf372a68064..6efe6b7ec4b39 100644 --- a/libraries/AP_Mount/AP_Mount_Gremsy.cpp +++ b/libraries/AP_Mount/AP_Mount_Gremsy.cpp @@ -20,6 +20,9 @@ void AP_Mount_Gremsy::update() return; } + // change to RC_TARGETING mode if RC input has changed + set_rctargeting_on_rcinput_change(); + // update based on mount mode switch (get_mode()) { diff --git a/libraries/AP_Mount/AP_Mount_SToRM32.cpp b/libraries/AP_Mount/AP_Mount_SToRM32.cpp index 1c1e5da3eadc1..de3c8035c989a 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32.cpp +++ b/libraries/AP_Mount/AP_Mount_SToRM32.cpp @@ -18,6 +18,9 @@ void AP_Mount_SToRM32::update() return; } + // change to RC_TARGETING mode if RC input has changed + set_rctargeting_on_rcinput_change(); + // flag to trigger sending target angles to gimbal bool resend_now = false; diff --git a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp index 518b698ef69e7..928a40a297496 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp +++ b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp @@ -29,6 +29,9 @@ void AP_Mount_SToRM32_serial::update() read_incoming(); // read the incoming messages from the gimbal + // change to RC_TARGETING mode if RC input has changed + set_rctargeting_on_rcinput_change(); + // flag to trigger sending target angles to gimbal bool resend_now = false; diff --git a/libraries/AP_Mount/AP_Mount_Scripting.cpp b/libraries/AP_Mount/AP_Mount_Scripting.cpp index 9eb738503919b..c2b93304e67ee 100644 --- a/libraries/AP_Mount/AP_Mount_Scripting.cpp +++ b/libraries/AP_Mount/AP_Mount_Scripting.cpp @@ -15,6 +15,9 @@ extern const AP_HAL::HAL& hal; // update mount position - should be called periodically void AP_Mount_Scripting::update() { + // change to RC_TARGETING mode if RC input has changed + set_rctargeting_on_rcinput_change(); + // update based on mount mode switch (get_mode()) { // move mount to a "retracted" position. To-Do: remove support and replace with a relaxed mode? diff --git a/libraries/AP_Mount/AP_Mount_Servo.cpp b/libraries/AP_Mount/AP_Mount_Servo.cpp index 196b79edff0a8..8c59eb6abfd28 100644 --- a/libraries/AP_Mount/AP_Mount_Servo.cpp +++ b/libraries/AP_Mount/AP_Mount_Servo.cpp @@ -27,6 +27,9 @@ void AP_Mount_Servo::init() // update mount position - should be called periodically void AP_Mount_Servo::update() { + // change to RC_TARGETING mode if RC input has changed + set_rctargeting_on_rcinput_change(); + auto mount_mode = get_mode(); switch (mount_mode) { // move mount to a "retracted position" or to a position where a fourth servo can retract the entire mount into the fuselage diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index 5cc1330bfb767..2854a28c7466c 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.cpp +++ b/libraries/AP_Mount/AP_Mount_Siyi.cpp @@ -103,6 +103,9 @@ void AP_Mount_Siyi::update() // run zoom control update_zoom_control(); + // change to RC_TARGETING mode if RC input has changed + set_rctargeting_on_rcinput_change(); + // Get the target angles or rates first depending on the current mount mode switch (get_mode()) { case MAV_MOUNT_MODE_RETRACT: { diff --git a/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp index 907b09a0d4574..6dbf8cdbd3b26 100644 --- a/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp +++ b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp @@ -31,6 +31,9 @@ void AP_Mount_SoloGimbal::update() return; } + // change to RC_TARGETING mode if RC input has changed + set_rctargeting_on_rcinput_change(); + // update based on mount mode switch(get_mode()) { // move mount to a "retracted" position. we do not implement a separate servo based retract mechanism diff --git a/libraries/AP_Mount/AP_Mount_Viewpro.cpp b/libraries/AP_Mount/AP_Mount_Viewpro.cpp index 07608d689bfff..a1180f85e3e58 100644 --- a/libraries/AP_Mount/AP_Mount_Viewpro.cpp +++ b/libraries/AP_Mount/AP_Mount_Viewpro.cpp @@ -76,6 +76,9 @@ void AP_Mount_Viewpro::update() // send vehicle attitude and position send_m_ahrs(); + // change to RC_TARGETING mode if RC input has changed + set_rctargeting_on_rcinput_change(); + // if tracking is active we do not send new targets to the gimbal if (_last_tracking_status == TrackingStatus::SEARCHING || _last_tracking_status == TrackingStatus::TRACKING) { return; diff --git a/libraries/AP_Mount/AP_Mount_Xacti.cpp b/libraries/AP_Mount/AP_Mount_Xacti.cpp index 9f210993d4350..adc752fd46947 100644 --- a/libraries/AP_Mount/AP_Mount_Xacti.cpp +++ b/libraries/AP_Mount/AP_Mount_Xacti.cpp @@ -110,6 +110,9 @@ void AP_Mount_Xacti::update() return; } + // change to RC_TARGETING mode if RC input has changed + set_rctargeting_on_rcinput_change(); + // update based on mount mode switch (get_mode()) { // move mount to a "retracted" position. To-Do: remove support and replace with a relaxed mode?