diff --git a/libraries/AP_Mount/AP_Mount_Alexmos.cpp b/libraries/AP_Mount/AP_Mount_Alexmos.cpp index 693d197860..7db333f6b7 100644 --- a/libraries/AP_Mount/AP_Mount_Alexmos.cpp +++ b/libraries/AP_Mount/AP_Mount_Alexmos.cpp @@ -100,20 +100,9 @@ void AP_Mount_Alexmos::update() break; // RC radio manual angle control, but with stabilization from the AHRS - case MAV_MOUNT_MODE_RC_TARGETING: { - // update targets using pilot's RC inputs - MountTarget rc_target; - get_rc_target(mnt_target.target_type, rc_target); - switch (mnt_target.target_type) { - case MountTargetType::ANGLE: - mnt_target.angle_rad = rc_target; - break; - case MountTargetType::RATE: - mnt_target.rate_rads = rc_target; - break; - } + case MAV_MOUNT_MODE_RC_TARGETING: + update_mnt_target_from_rc_target(); break; - } // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 9d064633b1..94f9e7e31b 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -66,6 +66,29 @@ bool AP_Mount_Backend::set_mode(MAV_MOUNT_MODE mode) return true; } +// called when mount mode is RC-targetting, updates the mnt_target object from RC inputs: +void AP_Mount_Backend::update_mnt_target_from_rc_target() +{ + if (rc().in_rc_failsafe()) { + if (option_set(Options::NEUTRAL_ON_RC_FS)) { + mnt_target.angle_rad.set(_params.neutral_angles.get() * DEG_TO_RAD, false); + mnt_target.target_type = MountTargetType::ANGLE; + return; + } + } + + MountTarget rc_target; + get_rc_target(mnt_target.target_type, rc_target); + switch (mnt_target.target_type) { + case MountTargetType::ANGLE: + mnt_target.angle_rad = rc_target; + break; + case MountTargetType::RATE: + mnt_target.rate_rads = rc_target; + break; + } +} + // set angle target in degrees // roll and pitch are in earth-frame // yaw_is_earth_frame (aka yaw_lock) should be true if yaw angle is earth-frame, false if body-frame diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index a25bd4870a..67b673d656 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -253,6 +253,9 @@ protected: }; bool option_set(Options opt) const { return (_params.options.get() & (uint8_t)opt) != 0; } + // called when mount mode is RC-targetting, updates the mnt_target object from RC inputs: + void update_mnt_target_from_rc_target(); + // returns true if user has configured a valid roll angle range // allows user to disable roll even on 3-axis gimbal bool roll_range_valid() const { return (_params.roll_angle_min < _params.roll_angle_max); } diff --git a/libraries/AP_Mount/AP_Mount_Gremsy.cpp b/libraries/AP_Mount/AP_Mount_Gremsy.cpp index 259de8e90c..631acc8778 100644 --- a/libraries/AP_Mount/AP_Mount_Gremsy.cpp +++ b/libraries/AP_Mount/AP_Mount_Gremsy.cpp @@ -48,20 +48,9 @@ void AP_Mount_Gremsy::update() } // RC radio manual angle control, but with stabilization from the AHRS - case MAV_MOUNT_MODE_RC_TARGETING: { - // update targets using pilot's RC inputs - MountTarget rc_target; - get_rc_target(mnt_target.target_type, rc_target); - switch (mnt_target.target_type) { - case MountTargetType::ANGLE: - mnt_target.angle_rad = rc_target; - break; - case MountTargetType::RATE: - mnt_target.rate_rads = rc_target; - break; - } + case MAV_MOUNT_MODE_RC_TARGETING: + update_mnt_target_from_rc_target(); break; - } // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: diff --git a/libraries/AP_Mount/AP_Mount_SToRM32.cpp b/libraries/AP_Mount/AP_Mount_SToRM32.cpp index 59e4da66bc..bd638fa42a 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32.cpp +++ b/libraries/AP_Mount/AP_Mount_SToRM32.cpp @@ -52,21 +52,10 @@ void AP_Mount_SToRM32::update() break; // RC radio manual angle control, but with stabilization from the AHRS - case MAV_MOUNT_MODE_RC_TARGETING: { - // update targets using pilot's RC inputs - MountTarget rc_target; - get_rc_target(mnt_target.target_type, rc_target); - switch (mnt_target.target_type) { - case MountTargetType::ANGLE: - mnt_target.angle_rad = rc_target; - break; - case MountTargetType::RATE: - mnt_target.rate_rads = rc_target; - break; - } + case MAV_MOUNT_MODE_RC_TARGETING: + update_mnt_target_from_rc_target(); resend_now = true; break; - } // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: diff --git a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp index 82387a0e93..9b003de0fe 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp +++ b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp @@ -49,21 +49,10 @@ void AP_Mount_SToRM32_serial::update() break; // RC radio manual angle control, but with stabilization from the AHRS - case MAV_MOUNT_MODE_RC_TARGETING: { - // update targets using pilot's RC inputs - MountTarget rc_target; - get_rc_target(mnt_target.target_type, rc_target); - switch (mnt_target.target_type) { - case MountTargetType::ANGLE: - mnt_target.angle_rad = rc_target; - break; - case MountTargetType::RATE: - mnt_target.rate_rads = rc_target; - break; - } + case MAV_MOUNT_MODE_RC_TARGETING: + update_mnt_target_from_rc_target(); resend_now = true; break; - } // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: diff --git a/libraries/AP_Mount/AP_Mount_Scripting.cpp b/libraries/AP_Mount/AP_Mount_Scripting.cpp index c2b93304e6..e70c030036 100644 --- a/libraries/AP_Mount/AP_Mount_Scripting.cpp +++ b/libraries/AP_Mount/AP_Mount_Scripting.cpp @@ -45,21 +45,10 @@ void AP_Mount_Scripting::update() break; // RC radio manual angle control, but with stabilization from the AHRS - case MAV_MOUNT_MODE_RC_TARGETING: { - // update targets using pilot's RC inputs - MountTarget rc_target; - get_rc_target(mnt_target.target_type, rc_target); - switch (mnt_target.target_type) { - case MountTargetType::ANGLE: - mnt_target.angle_rad = rc_target; - break; - case MountTargetType::RATE: - mnt_target.rate_rads = rc_target; - break; - } + case MAV_MOUNT_MODE_RC_TARGETING: + update_mnt_target_from_rc_target(); target_loc_valid = false; break; - } // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: diff --git a/libraries/AP_Mount/AP_Mount_Servo.cpp b/libraries/AP_Mount/AP_Mount_Servo.cpp index 7c97656c12..743708fa9d 100644 --- a/libraries/AP_Mount/AP_Mount_Servo.cpp +++ b/libraries/AP_Mount/AP_Mount_Servo.cpp @@ -55,28 +55,9 @@ void AP_Mount_Servo::update() } // RC radio manual angle control, but with stabilization from the AHRS - case MAV_MOUNT_MODE_RC_TARGETING: { - // update targets using pilot's rc inputs or go to neutral or retracted targets if no rc - if (rc().in_rc_failsafe()) { - if (option_set(Options::NEUTRAL_ON_RC_FS)) { - mnt_target.angle_rad.set(_angle_bf_output_rad, false); - mnt_target.target_type = MountTargetType::ANGLE; - } - } else { - // update targets using pilot's RC inputs - MountTarget rc_target; - get_rc_target(mnt_target.target_type, rc_target); - switch (mnt_target.target_type) { - case MountTargetType::ANGLE: - mnt_target.angle_rad = rc_target; - break; - case MountTargetType::RATE: - mnt_target.rate_rads = rc_target; - break; - } - } + case MAV_MOUNT_MODE_RC_TARGETING: + update_mnt_target_from_rc_target(); break; - } // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index d0e82cd503..1c91af6a43 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.cpp +++ b/libraries/AP_Mount/AP_Mount_Siyi.cpp @@ -121,20 +121,9 @@ void AP_Mount_Siyi::update() break; } - case MAV_MOUNT_MODE_RC_TARGETING: { - // update targets using pilot's RC inputs - MountTarget rc_target; - get_rc_target(mnt_target.target_type, rc_target); - switch (mnt_target.target_type) { - case MountTargetType::ANGLE: - mnt_target.angle_rad = rc_target; - break; - case MountTargetType::RATE: - mnt_target.rate_rads = rc_target; - break; - } + case MAV_MOUNT_MODE_RC_TARGETING: + update_mnt_target_from_rc_target(); break; - } // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: diff --git a/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp index 02c8c3f247..251c859693 100644 --- a/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp +++ b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp @@ -65,20 +65,10 @@ void AP_Mount_SoloGimbal::update() break; // RC radio manual angle control, but with stabilization from the AHRS - case MAV_MOUNT_MODE_RC_TARGETING: { + case MAV_MOUNT_MODE_RC_TARGETING: _gimbal.set_lockedToBody(false); - MountTarget rc_target; - get_rc_target(mnt_target.target_type, rc_target); - switch (mnt_target.target_type) { - case MountTargetType::ANGLE: - mnt_target.angle_rad = rc_target; - break; - case MountTargetType::RATE: - mnt_target.rate_rads = rc_target; - break; - } + update_mnt_target_from_rc_target(); break; - } // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: diff --git a/libraries/AP_Mount/AP_Mount_Topotek.cpp b/libraries/AP_Mount/AP_Mount_Topotek.cpp index 09682a8429..bfe93ec83a 100755 --- a/libraries/AP_Mount/AP_Mount_Topotek.cpp +++ b/libraries/AP_Mount/AP_Mount_Topotek.cpp @@ -156,20 +156,9 @@ void AP_Mount_Topotek::update() break; // RC radio manual angle control, but with stabilization from the AHRS - case MAV_MOUNT_MODE_RC_TARGETING: { - // update targets using pilot's RC inputs - MountTarget rc_target; - get_rc_target(mnt_target.target_type, rc_target); - switch (mnt_target.target_type) { - case MountTargetType::ANGLE: - mnt_target.angle_rad = rc_target; - break; - case MountTargetType::RATE: - mnt_target.rate_rads = rc_target; - break; - } + case MAV_MOUNT_MODE_RC_TARGETING: + update_mnt_target_from_rc_target(); break; - } // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: diff --git a/libraries/AP_Mount/AP_Mount_Viewpro.cpp b/libraries/AP_Mount/AP_Mount_Viewpro.cpp index f9b06f3b80..1de89fbc67 100644 --- a/libraries/AP_Mount/AP_Mount_Viewpro.cpp +++ b/libraries/AP_Mount/AP_Mount_Viewpro.cpp @@ -94,20 +94,9 @@ void AP_Mount_Viewpro::update() break; // RC radio manual angle control, but with stabilization from the AHRS - case MAV_MOUNT_MODE_RC_TARGETING: { - // update targets using pilot's RC inputs - MountTarget rc_target; - get_rc_target(mnt_target.target_type, rc_target); - switch (mnt_target.target_type) { - case MountTargetType::ANGLE: - mnt_target.angle_rad = rc_target; - break; - case MountTargetType::RATE: - mnt_target.rate_rads = rc_target; - break; - } + case MAV_MOUNT_MODE_RC_TARGETING: + update_mnt_target_from_rc_target(); break; - } // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: diff --git a/libraries/AP_Mount/AP_Mount_Xacti.cpp b/libraries/AP_Mount/AP_Mount_Xacti.cpp index 6353ce41d0..c5727f6c02 100644 --- a/libraries/AP_Mount/AP_Mount_Xacti.cpp +++ b/libraries/AP_Mount/AP_Mount_Xacti.cpp @@ -135,20 +135,9 @@ void AP_Mount_Xacti::update() break; } - case MAV_MOUNT_MODE_RC_TARGETING: { - // update targets using pilot's RC inputs - MountTarget rc_target; - get_rc_target(mnt_target.target_type, rc_target); - switch (mnt_target.target_type) { - case MountTargetType::ANGLE: - mnt_target.angle_rad = rc_target; - break; - case MountTargetType::RATE: - mnt_target.rate_rads = rc_target; - break; - } + case MAV_MOUNT_MODE_RC_TARGETING: + update_mnt_target_from_rc_target(); break; - } // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: