Skip to content

Commit

Permalink
AP_Mount: remove need for invalid target type
Browse files Browse the repository at this point in the history
  • Loading branch information
@rmackay9
rmackay9 committed Jul 24, 2023
1 parent bdc2196 commit 533c709
Show file tree
Hide file tree
Showing 12 changed files with 249 additions and 235 deletions.
28 changes: 18 additions & 10 deletions libraries/AP_Mount/AP_Mount_Alexmos.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand All @@ -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);
Expand Down
69 changes: 26 additions & 43 deletions libraries/AP_Mount/AP_Mount_Backend.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
11 changes: 3 additions & 8 deletions libraries/AP_Mount/AP_Mount_Backend.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -178,7 +178,6 @@ class AP_Mount_Backend
protected:

enum class MountTargetType {
INVALID,
ANGLE,
RATE,
};
Expand Down Expand Up @@ -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
Expand Down
42 changes: 22 additions & 20 deletions libraries/AP_Mount/AP_Mount_Gremsy.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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;
Expand Down
39 changes: 23 additions & 16 deletions libraries/AP_Mount/AP_Mount_SToRM32.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down
37 changes: 22 additions & 15 deletions libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down
Loading

0 comments on commit 533c709

Please sign in to comment.