diff --git a/libraries/AP_Mount/AP_Mount_Alexmos.cpp b/libraries/AP_Mount/AP_Mount_Alexmos.cpp index 75bf82660c03d1..f7f39f6119be40 100644 --- a/libraries/AP_Mount/AP_Mount_Alexmos.cpp +++ b/libraries/AP_Mount/AP_Mount_Alexmos.cpp @@ -51,29 +51,38 @@ void AP_Mount_Alexmos::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 - if (get_rc_rate_target(mnt_target.rate_rads)) { - mnt_target.target_type = MountTargetType::RATE; - } else if (get_rc_angle_target(mnt_target.angle_rad)) { - mnt_target.target_type = MountTargetType::ANGLE; - } else { - mnt_target.target_type = MountTargetType::INVALID; + 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; } break; } // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: - mnt_target.target_type = get_angle_target_to_roi(mnt_target.angle_rad) ? MountTargetType::ANGLE : MountTargetType::INVALID; + if (get_angle_target_to_roi(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; + } break; // point mount to Home location case MAV_MOUNT_MODE_HOME_LOCATION: - mnt_target.target_type = get_angle_target_to_home(mnt_target.angle_rad) ? MountTargetType::ANGLE : MountTargetType::INVALID; + if (get_angle_target_to_home(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; + } break; // point mount to another vehicle case MAV_MOUNT_MODE_SYSID_TARGET: - mnt_target.target_type = get_angle_target_to_sysid(mnt_target.angle_rad) ? MountTargetType::ANGLE : MountTargetType::INVALID; + if (get_angle_target_to_sysid(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; + } break; default: @@ -86,7 +95,6 @@ void AP_Mount_Alexmos::update() case MountTargetType::RATE: update_angle_target_from_rate(mnt_target.rate_rads, mnt_target.angle_rad); FALLTHROUGH; - case MountTargetType::INVALID: case MountTargetType::ANGLE: // send latest angle targets to gimbal control_axis(mnt_target.angle_rad); diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 4102698cd5f386..ac4d2878eb7e93 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -423,61 +423,44 @@ void AP_Mount_Backend::get_rc_input(float& roll_in, float& pitch_in, float& yaw_ } } -// get rate targets (in rad/s) from pilot RC -// returns true on success (RC is providing rate targets), false on failure (RC is providing angle targets) -bool AP_Mount_Backend::get_rc_rate_target(MountTarget& rate_rads) const +// get angle or rate targets from pilot RC +// target_type will be either ANGLE or RATE, rpy will be the target angle in deg or rate in deg/s +void AP_Mount_Backend::get_rc_target(MountTargetType& target_type, MountTarget& target_rpy) const { - // exit immediately if RC is not providing rate targets - if (_params.rc_rate_max <= 0) { - return false; - } - // get RC input from pilot float roll_in, pitch_in, yaw_in; get_rc_input(roll_in, pitch_in, yaw_in); - // calculate rates - const float rc_rate_max_rads = radians(_params.rc_rate_max.get()); - rate_rads.roll = roll_in * rc_rate_max_rads; - rate_rads.pitch = pitch_in * rc_rate_max_rads; - rate_rads.yaw = yaw_in * rc_rate_max_rads; - // yaw frame - rate_rads.yaw_is_ef = _yaw_lock; - - return true; -} - -// get angle targets (in radians) from pilot RC -// returns true on success (RC is providing angle targets), false on failure (RC is providing rate targets) -bool AP_Mount_Backend::get_rc_angle_target(MountTarget& angle_rad) const -{ - // exit immediately if RC is not providing angle targets - if (_params.rc_rate_max > 0) { - return false; - } + target_rpy.yaw_is_ef = _yaw_lock; - // get RC input from pilot - float roll_in, pitch_in, yaw_in; - get_rc_input(roll_in, pitch_in, yaw_in); + // if RC_RATE is zero, targets are angle + if (_params.rc_rate_max <= 0) { + target_type = MountTargetType::ANGLE; - // roll angle - angle_rad.roll = radians(((roll_in + 1.0f) * 0.5f * (_params.roll_angle_max - _params.roll_angle_min) + _params.roll_angle_min)); + // roll angle + target_rpy.roll = radians(((roll_in + 1.0f) * 0.5f * (_params.roll_angle_max - _params.roll_angle_min) + _params.roll_angle_min)); - // pitch angle - angle_rad.pitch = radians(((pitch_in + 1.0f) * 0.5f * (_params.pitch_angle_max - _params.pitch_angle_min) + _params.pitch_angle_min)); + // pitch angle + target_rpy.pitch = radians(((pitch_in + 1.0f) * 0.5f * (_params.pitch_angle_max - _params.pitch_angle_min) + _params.pitch_angle_min)); - // yaw angle - angle_rad.yaw_is_ef = _yaw_lock; - if (angle_rad.yaw_is_ef) { - // if yaw is earth-frame pilot yaw input control angle from -180 to +180 deg - angle_rad.yaw = yaw_in * M_PI; - } else { - // yaw target in body frame so apply body frame limits - angle_rad.yaw = radians(((yaw_in + 1.0f) * 0.5f * (_params.yaw_angle_max - _params.yaw_angle_min) + _params.yaw_angle_min)); + // yaw angle + if (target_rpy.yaw_is_ef) { + // if yaw is earth-frame pilot yaw input control angle from -180 to +180 deg + target_rpy.yaw = yaw_in * M_PI; + } else { + // yaw target in body frame so apply body frame limits + target_rpy.yaw = radians(((yaw_in + 1.0f) * 0.5f * (_params.yaw_angle_max - _params.yaw_angle_min) + _params.yaw_angle_min)); + } + return; } - return true; + // calculate rate targets + target_type = MountTargetType::RATE; + const float rc_rate_max_rads = radians(_params.rc_rate_max.get()); + target_rpy.roll = roll_in * rc_rate_max_rads; + target_rpy.pitch = pitch_in * rc_rate_max_rads; + target_rpy.yaw = yaw_in * rc_rate_max_rads; } // get angle targets (in radians) to a Location diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 3bb6b1a1ab70dc..c663d0dadd7816 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -178,7 +178,6 @@ class AP_Mount_Backend protected: enum class MountTargetType { - INVALID, ANGLE, RATE, }; @@ -211,13 +210,9 @@ class AP_Mount_Backend // 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; - // get rate targets (in rad/s) from pilot RC - // returns true on success (RC is providing rate targets), false on failure (RC is providing angle targets) - bool get_rc_rate_target(MountTarget& rate_rads) const WARN_IF_UNUSED; - - // get angle targets (in radians) from pilot RC - // returns true on success (RC is providing angle targets), false on failure (RC is providing rate targets) - bool get_rc_angle_target(MountTarget& angle_rad) const WARN_IF_UNUSED; + // get angle or rate targets from pilot RC + // target_type will be either ANGLE or RATE, rpy will be the target angle in deg or rate in deg/s + void get_rc_target(MountTargetType& target_type, MountTarget& rpy) const; // get angle targets (in radians) to a Location // returns true on success, false on failure diff --git a/libraries/AP_Mount/AP_Mount_Gremsy.cpp b/libraries/AP_Mount/AP_Mount_Gremsy.cpp index 357aa3faf9e779..b9bf372a680646 100644 --- a/libraries/AP_Mount/AP_Mount_Gremsy.cpp +++ b/libraries/AP_Mount/AP_Mount_Gremsy.cpp @@ -46,34 +46,40 @@ 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 - if (get_rc_rate_target(mnt_target.rate_rads)) { - mnt_target.target_type = MountTargetType::RATE; - } else if (get_rc_angle_target(mnt_target.angle_rad)) { - mnt_target.target_type = MountTargetType::ANGLE; - } else { - mnt_target.target_type = MountTargetType::INVALID; + // 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; } break; } // point mount to a GPS point given by the mission planner - case MAV_MOUNT_MODE_GPS_POINT: { - mnt_target.target_type = get_angle_target_to_roi(mnt_target.angle_rad) ? MountTargetType::ANGLE : MountTargetType::INVALID; + case MAV_MOUNT_MODE_GPS_POINT: + if (get_angle_target_to_roi(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; + } break; - } // point mount to Home location - case MAV_MOUNT_MODE_HOME_LOCATION: { - mnt_target.target_type = get_angle_target_to_home(mnt_target.angle_rad) ? MountTargetType::ANGLE : MountTargetType::INVALID; + case MAV_MOUNT_MODE_HOME_LOCATION: + if (get_angle_target_to_home(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; + } break; - } // point mount to another vehicle - case MAV_MOUNT_MODE_SYSID_TARGET: { - mnt_target.target_type = get_angle_target_to_sysid(mnt_target.angle_rad) ? MountTargetType::ANGLE : MountTargetType::INVALID; + case MAV_MOUNT_MODE_SYSID_TARGET: + if (get_angle_target_to_sysid(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; + } break; - } default: // unknown mode so do nothing @@ -82,10 +88,6 @@ void AP_Mount_Gremsy::update() // send target angles or rates depending on the target type switch (mnt_target.target_type) { - case MountTargetType::INVALID: - // send zero rates - send_gimbal_device_set_rate(0, 0, 0, false); - break; case MountTargetType::ANGLE: send_gimbal_device_set_attitude(mnt_target.angle_rad.roll, mnt_target.angle_rad.pitch, mnt_target.angle_rad.yaw, mnt_target.angle_rad.yaw_is_ef); break; diff --git a/libraries/AP_Mount/AP_Mount_SToRM32.cpp b/libraries/AP_Mount/AP_Mount_SToRM32.cpp index 6c93101bac7c30..1c1e5da3eadc1e 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32.cpp +++ b/libraries/AP_Mount/AP_Mount_SToRM32.cpp @@ -51,35 +51,42 @@ void AP_Mount_SToRM32::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 - if (get_rc_rate_target(mnt_target.rate_rads)) { - mnt_target.target_type = MountTargetType::RATE; - update_angle_target_from_rate(mnt_target.rate_rads, mnt_target.angle_rad); - resend_now = true; - } else if (get_rc_angle_target(mnt_target.angle_rad)) { - mnt_target.target_type = MountTargetType::ANGLE; - resend_now = true; - } else { - mnt_target.target_type = MountTargetType::INVALID; + 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; } + resend_now = true; break; } - // point mount to a GPS location + // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: - resend_now = get_angle_target_to_roi(mnt_target.angle_rad); - mnt_target.target_type = resend_now ? MountTargetType::ANGLE : MountTargetType::INVALID; + if (get_angle_target_to_roi(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; + resend_now = true; + } break; // point mount to Home location case MAV_MOUNT_MODE_HOME_LOCATION: - resend_now = get_angle_target_to_home(mnt_target.angle_rad); - mnt_target.target_type = resend_now ? MountTargetType::ANGLE : MountTargetType::INVALID; + if (get_angle_target_to_home(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; + resend_now = true; + } break; // point mount to another vehicle case MAV_MOUNT_MODE_SYSID_TARGET: - resend_now = get_angle_target_to_sysid(mnt_target.angle_rad); - mnt_target.target_type = resend_now ? MountTargetType::ANGLE : MountTargetType::INVALID; + if (get_angle_target_to_sysid(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; + resend_now = true; + } break; default: diff --git a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp index 2ff1d34845be93..0fdcdd53c70fbf 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp +++ b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp @@ -62,35 +62,42 @@ void AP_Mount_SToRM32_serial::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 - if (get_rc_rate_target(mnt_target.rate_rads)) { - mnt_target.target_type = MountTargetType::RATE; - update_angle_target_from_rate(mnt_target.rate_rads, mnt_target.angle_rad); - resend_now = true; - } else if (get_rc_angle_target(mnt_target.angle_rad)) { - mnt_target.target_type = MountTargetType::ANGLE; - resend_now = true; - } else { - mnt_target.target_type = MountTargetType::INVALID; + 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; } + resend_now = true; break; } // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: - resend_now = get_angle_target_to_roi(mnt_target.angle_rad); - mnt_target.target_type = resend_now ? MountTargetType::ANGLE : MountTargetType::INVALID; + if (get_angle_target_to_roi(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; + resend_now = true; + } break; // point mount to Home location case MAV_MOUNT_MODE_HOME_LOCATION: - resend_now = get_angle_target_to_home(mnt_target.angle_rad); - mnt_target.target_type = resend_now ? MountTargetType::ANGLE : MountTargetType::INVALID; + if (get_angle_target_to_home(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; + resend_now = true; + } break; // point mount to another vehicle case MAV_MOUNT_MODE_SYSID_TARGET: - resend_now = get_angle_target_to_sysid(mnt_target.angle_rad); - mnt_target.target_type = resend_now ? MountTargetType::ANGLE : MountTargetType::INVALID; + if (get_angle_target_to_sysid(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; + resend_now = true; + } break; default: diff --git a/libraries/AP_Mount/AP_Mount_Scripting.cpp b/libraries/AP_Mount/AP_Mount_Scripting.cpp index e297fee7cb787b..9eb738503919bc 100644 --- a/libraries/AP_Mount/AP_Mount_Scripting.cpp +++ b/libraries/AP_Mount/AP_Mount_Scripting.cpp @@ -43,47 +43,41 @@ void AP_Mount_Scripting::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 - if (get_rc_rate_target(mnt_target.rate_rads)) { - mnt_target.target_type = MountTargetType::RATE; - } else if (get_rc_angle_target(mnt_target.angle_rad)) { - mnt_target.target_type = MountTargetType::ANGLE; - } else { - mnt_target.target_type = MountTargetType::INVALID; + // 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; } target_loc_valid = false; break; } - // point mount towards a GPS point - case MAV_MOUNT_MODE_GPS_POINT: { - target_loc_valid = _roi_target_set; - if (target_loc_valid) { - target_loc = _roi_target; - mnt_target.target_type = get_angle_target_to_roi(mnt_target.angle_rad) ? MountTargetType::ANGLE : MountTargetType::INVALID; + // point mount to a GPS point given by the mission planner + case MAV_MOUNT_MODE_GPS_POINT: + if (get_angle_target_to_roi(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; } break; - } - // point mount towards home - case MAV_MOUNT_MODE_HOME_LOCATION: { - target_loc_valid = AP::ahrs().home_is_set(); - if (target_loc_valid) { - target_loc = AP::ahrs().get_home(); - mnt_target.target_type = get_angle_target_to_home(mnt_target.angle_rad) ? MountTargetType::ANGLE : MountTargetType::INVALID; + // point mount to Home location + case MAV_MOUNT_MODE_HOME_LOCATION: + if (get_angle_target_to_home(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; } break; - } - // point mount towards another vehicle - case MAV_MOUNT_MODE_SYSID_TARGET: { - target_loc_valid = _target_sysid_location_set; - if (target_loc_valid) { - target_loc = _target_sysid_location; - mnt_target.target_type = get_angle_target_to_sysid(mnt_target.angle_rad) ? MountTargetType::ANGLE : MountTargetType::INVALID; + // point mount to another vehicle + case MAV_MOUNT_MODE_SYSID_TARGET: + if (get_angle_target_to_sysid(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; } break; - } default: // we do not know this mode so raise internal error diff --git a/libraries/AP_Mount/AP_Mount_Servo.cpp b/libraries/AP_Mount/AP_Mount_Servo.cpp index b4345c318562ff..6d1d45a7668a0f 100644 --- a/libraries/AP_Mount/AP_Mount_Servo.cpp +++ b/libraries/AP_Mount/AP_Mount_Servo.cpp @@ -54,33 +54,39 @@ 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 - if (get_rc_rate_target(mnt_target.rate_rads)) { - mnt_target.target_type = MountTargetType::RATE; - } else if (get_rc_angle_target(mnt_target.angle_rad)) { - mnt_target.target_type = MountTargetType::ANGLE; - } else { - mnt_target.target_type = MountTargetType::INVALID; + 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; } break; } // point mount to a GPS point given by the mission planner - case MAV_MOUNT_MODE_GPS_POINT: { - mnt_target.target_type = get_angle_target_to_roi(mnt_target.angle_rad) ? MountTargetType::ANGLE : MountTargetType::INVALID; + case MAV_MOUNT_MODE_GPS_POINT: + if (get_angle_target_to_roi(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; + } break; - } // point mount to Home location - case MAV_MOUNT_MODE_HOME_LOCATION: { - mnt_target.target_type = get_angle_target_to_home(mnt_target.angle_rad) ? MountTargetType::ANGLE : MountTargetType::INVALID; + case MAV_MOUNT_MODE_HOME_LOCATION: + if (get_angle_target_to_home(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; + } break; - } // point mount to another vehicle - case MAV_MOUNT_MODE_SYSID_TARGET: { - mnt_target.target_type = get_angle_target_to_sysid(mnt_target.angle_rad) ? MountTargetType::ANGLE : MountTargetType::INVALID; + case MAV_MOUNT_MODE_SYSID_TARGET: + if (get_angle_target_to_sysid(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; + } break; - } default: //do nothing @@ -92,8 +98,6 @@ void AP_Mount_Servo::update() case MountTargetType::RATE: update_angle_target_from_rate(mnt_target.rate_rads, mnt_target.angle_rad); FALLTHROUGH; - case MountTargetType::INVALID: - // use lastest angle targets case MountTargetType::ANGLE: // update _angle_bf_output_rad based on angle target if ((mount_mode != MAV_MOUNT_MODE_RETRACT) & (mount_mode != MAV_MOUNT_MODE_NEUTRAL)) { diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index 266d5976da39b8..f1dc4a1bfa6c8d 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.cpp +++ b/libraries/AP_Mount/AP_Mount_Siyi.cpp @@ -90,34 +90,40 @@ void AP_Mount_Siyi::update() } case MAV_MOUNT_MODE_RC_TARGETING: { - // update targets using pilot's rc inputs - if (get_rc_rate_target(mnt_target.rate_rads)) { - mnt_target.target_type = MountTargetType::RATE; - } else if (get_rc_angle_target(mnt_target.angle_rad)) { - mnt_target.target_type = MountTargetType::ANGLE; - } else { - mnt_target.target_type = MountTargetType::INVALID; + // 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; } break; } // point mount to a GPS point given by the mission planner - case MAV_MOUNT_MODE_GPS_POINT: { - mnt_target.target_type = get_angle_target_to_roi(mnt_target.angle_rad) ? MountTargetType::ANGLE : MountTargetType::INVALID; + case MAV_MOUNT_MODE_GPS_POINT: + if (get_angle_target_to_roi(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; + } break; - } // point mount to Home location - case MAV_MOUNT_MODE_HOME_LOCATION: { - mnt_target.target_type = get_angle_target_to_home(mnt_target.angle_rad) ? MountTargetType::ANGLE : MountTargetType::INVALID; + case MAV_MOUNT_MODE_HOME_LOCATION: + if (get_angle_target_to_home(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; + } break; - } // point mount to another vehicle - case MAV_MOUNT_MODE_SYSID_TARGET: { - mnt_target.target_type = get_angle_target_to_sysid(mnt_target.angle_rad) ? MountTargetType::ANGLE : MountTargetType::INVALID; + case MAV_MOUNT_MODE_SYSID_TARGET: + if (get_angle_target_to_sysid(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; + } break; - } default: // we do not know this mode so raise internal error @@ -127,10 +133,6 @@ void AP_Mount_Siyi::update() // send target angles or rates depending on the target type switch (mnt_target.target_type) { - case MountTargetType::INVALID: - // send zero rates - send_target_rates(0, 0, false); - break; case MountTargetType::ANGLE: send_target_angles(mnt_target.angle_rad.pitch, mnt_target.angle_rad.yaw, mnt_target.angle_rad.yaw_is_ef); break; diff --git a/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp index 2a7fecdcbb92e2..907b09a0d4574c 100644 --- a/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp +++ b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp @@ -61,37 +61,42 @@ void AP_Mount_SoloGimbal::update() // RC radio manual angle control, but with stabilization from the AHRS case MAV_MOUNT_MODE_RC_TARGETING: { _gimbal.set_lockedToBody(false); - if (get_rc_rate_target(mnt_target.rate_rads)) { - mnt_target.target_type = MountTargetType::RATE; - update_angle_target_from_rate(mnt_target.rate_rads, mnt_target.angle_rad); - } else if (get_rc_angle_target(mnt_target.angle_rad)) { - mnt_target.target_type = MountTargetType::ANGLE; - } else { - mnt_target.target_type = MountTargetType::INVALID; + 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; } break; } // point mount to a GPS point given by the mission planner - case MAV_MOUNT_MODE_GPS_POINT: { - _gimbal.set_lockedToBody(false); - mnt_target.target_type = get_angle_target_to_roi(mnt_target.angle_rad) ? MountTargetType::ANGLE : MountTargetType::INVALID; + case MAV_MOUNT_MODE_GPS_POINT: + if (get_angle_target_to_roi(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; + _gimbal.set_lockedToBody(false); + } break; - } // point mount to Home location - case MAV_MOUNT_MODE_HOME_LOCATION: { - _gimbal.set_lockedToBody(false); - mnt_target.target_type = get_angle_target_to_home(mnt_target.angle_rad) ? MountTargetType::ANGLE : MountTargetType::INVALID; + case MAV_MOUNT_MODE_HOME_LOCATION: + if (get_angle_target_to_home(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; + _gimbal.set_lockedToBody(false); + } break; - } // point mount to another vehicle - case MAV_MOUNT_MODE_SYSID_TARGET: { - _gimbal.set_lockedToBody(false); - mnt_target.target_type = get_angle_target_to_sysid(mnt_target.angle_rad) ? MountTargetType::ANGLE : MountTargetType::INVALID; + case MAV_MOUNT_MODE_SYSID_TARGET: + if (get_angle_target_to_sysid(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; + _gimbal.set_lockedToBody(false); + } break; - } default: // we do not know this mode so do nothing diff --git a/libraries/AP_Mount/AP_Mount_Viewpro.cpp b/libraries/AP_Mount/AP_Mount_Viewpro.cpp index 0ae7ff39f513c0..36965cb6911f3d 100644 --- a/libraries/AP_Mount/AP_Mount_Viewpro.cpp +++ b/libraries/AP_Mount/AP_Mount_Viewpro.cpp @@ -96,32 +96,40 @@ void AP_Mount_Viewpro::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 - if (get_rc_rate_target(mnt_target.rate_rads)) { - mnt_target.target_type = MountTargetType::RATE; - } else if (get_rc_angle_target(mnt_target.angle_rad)) { - mnt_target.target_type = MountTargetType::ANGLE; - } else { - mnt_target.target_type = MountTargetType::INVALID; + // 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; } break; } // point mount to a GPS point given by the mission planner - case MAV_MOUNT_MODE_GPS_POINT: { - mnt_target.target_type = get_angle_target_to_roi(mnt_target.angle_rad) ? MountTargetType::ANGLE : MountTargetType::INVALID; + case MAV_MOUNT_MODE_GPS_POINT: + if (get_angle_target_to_roi(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; + } break; - } - case MAV_MOUNT_MODE_HOME_LOCATION: { - mnt_target.target_type = get_angle_target_to_home(mnt_target.angle_rad) ? MountTargetType::ANGLE : MountTargetType::INVALID; + // point mount to Home location + case MAV_MOUNT_MODE_HOME_LOCATION: + if (get_angle_target_to_home(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; + } break; - } - case MAV_MOUNT_MODE_SYSID_TARGET:{ - mnt_target.target_type = get_angle_target_to_sysid(mnt_target.angle_rad) ? MountTargetType::ANGLE : MountTargetType::INVALID; + // point mount to another vehicle + case MAV_MOUNT_MODE_SYSID_TARGET: + if (get_angle_target_to_sysid(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; + } break; - } default: // we do not know this mode so raise internal error @@ -131,10 +139,6 @@ void AP_Mount_Viewpro::update() // send target angles or rates depending on the target type switch (mnt_target.target_type) { - case MountTargetType::INVALID: - // send zero rates - send_target_rates(0, 0, false); - break; case MountTargetType::ANGLE: send_target_angles(mnt_target.angle_rad.pitch, mnt_target.angle_rad.yaw, mnt_target.angle_rad.yaw_is_ef); break; diff --git a/libraries/AP_Mount/AP_Mount_Xacti.cpp b/libraries/AP_Mount/AP_Mount_Xacti.cpp index f28c2d0192fe1a..62ec3396a3e4a9 100644 --- a/libraries/AP_Mount/AP_Mount_Xacti.cpp +++ b/libraries/AP_Mount/AP_Mount_Xacti.cpp @@ -72,34 +72,40 @@ void AP_Mount_Xacti::update() } case MAV_MOUNT_MODE_RC_TARGETING: { - // update targets using pilot's rc inputs - if (get_rc_rate_target(mnt_target.rate_rads)) { - mnt_target.target_type = MountTargetType::RATE; - } else if (get_rc_angle_target(mnt_target.angle_rad)) { - mnt_target.target_type = MountTargetType::ANGLE; - } else { - mnt_target.target_type = MountTargetType::INVALID; + // 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; } break; } // point mount to a GPS point given by the mission planner - case MAV_MOUNT_MODE_GPS_POINT: { - mnt_target.target_type = get_angle_target_to_roi(mnt_target.angle_rad) ? MountTargetType::ANGLE : MountTargetType::INVALID; + case MAV_MOUNT_MODE_GPS_POINT: + if (get_angle_target_to_roi(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; + } break; - } // point mount to Home location - case MAV_MOUNT_MODE_HOME_LOCATION: { - mnt_target.target_type = get_angle_target_to_home(mnt_target.angle_rad) ? MountTargetType::ANGLE : MountTargetType::INVALID; + case MAV_MOUNT_MODE_HOME_LOCATION: + if (get_angle_target_to_home(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; + } break; - } // point mount to another vehicle - case MAV_MOUNT_MODE_SYSID_TARGET: { - mnt_target.target_type = get_angle_target_to_sysid(mnt_target.angle_rad) ? MountTargetType::ANGLE : MountTargetType::INVALID; + case MAV_MOUNT_MODE_SYSID_TARGET: + if (get_angle_target_to_sysid(mnt_target.angle_rad)) { + mnt_target.target_type = MountTargetType::ANGLE; + } break; - } default: // we do not know this mode so raise internal error @@ -109,9 +115,6 @@ void AP_Mount_Xacti::update() // send target angles or rates depending on the target type switch (mnt_target.target_type) { - case MountTargetType::INVALID: - send_target_rates(0, 0, false); - break; case MountTargetType::ANGLE: send_target_angles(mnt_target.angle_rad.pitch, mnt_target.angle_rad.yaw, mnt_target.angle_rad.yaw_is_ef); break;