Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-21 16:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Mount: SToRM32_serial restructure and support for ef/bf angle and rate

Browse Source
This commit is contained in:
Randy Mackay 2022-06-23 12:39:41 +09:00
parent f14f524ff5
commit 3b0869d098
2 changed files with 40 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

73
libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp
View File

@ -42,59 +42,66 @@ void AP_Mount_SToRM32_serial::update()
// update based on mount mode
switch(get_mode()) {
// move mount to a "retracted" position. To-Do: remove support and replace with a relaxed mode?
case MAV_MOUNT_MODE_RETRACT:
{
case MAV_MOUNT_MODE_RETRACT: {
const Vector3f &target = _state._retract_angles.get();
_angle_ef_target_rad.x = ToRad(target.x);
_angle_ef_target_rad.y = ToRad(target.y);
_angle_ef_target_rad.z = ToRad(target.z);
}
_angle_rad.roll = ToRad(target.x);
_angle_rad.pitch = ToRad(target.y);
_angle_rad.yaw = ToRad(target.z);
_angle_rad.yaw_is_ef = false;
break;
}
// move mount to a neutral position, typically pointing forward
case MAV_MOUNT_MODE_NEUTRAL:
{
case MAV_MOUNT_MODE_NEUTRAL: {
const Vector3f &target = _state._neutral_angles.get();
_angle_ef_target_rad.x = ToRad(target.x);
_angle_ef_target_rad.y = ToRad(target.y);
_angle_ef_target_rad.z = ToRad(target.z);
}
_angle_rad.roll = ToRad(target.x);
_angle_rad.pitch = ToRad(target.y);
_angle_rad.yaw = ToRad(target.z);
_angle_rad.yaw_is_ef = false;
break;
}
// point to the angles given by a mavlink message
case MAV_MOUNT_MODE_MAVLINK_TARGETING:
// do nothing because earth-frame angle targets (i.e. _angle_ef_target_rad) should have already been set by a MOUNT_CONTROL message from GCS
switch (mavt_target.target_type) {
case MountTargetType::ANGLE:
_angle_rad = mavt_target.angle_rad;
break;
case MountTargetType::RATE:
update_angle_target_from_rate(mavt_target.rate_rads, _angle_rad);
break;
}
resend_now = true;
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
update_targets_from_rc();
case MAV_MOUNT_MODE_RC_TARGETING: {
// update targets using pilot's RC inputs
MountTarget rc_target {};
if (get_rc_rate_target(rc_target)) {
update_angle_target_from_rate(rc_target, _angle_rad);
} else if (get_rc_angle_target(rc_target)) {
_angle_rad = rc_target;
}
resend_now = true;
break;
}
// point mount to a GPS point given by the mission planner
case MAV_MOUNT_MODE_GPS_POINT:
if (calc_angle_to_roi_target(_angle_ef_target_rad, true, true, true)) {
if (get_angle_target_to_roi(_angle_rad)) {
resend_now = true;
}
break;
case MAV_MOUNT_MODE_HOME_LOCATION:
// constantly update the home location:
if (!AP::ahrs().home_is_set()) {
break;
}
_roi_target = AP::ahrs().get_home();
_roi_target_set = true;
if (calc_angle_to_roi_target(_angle_ef_target_rad, true, true, true)) {
if (get_angle_target_to_home(_angle_rad)) {
resend_now = true;
}
break;
case MAV_MOUNT_MODE_SYSID_TARGET:
if (calc_angle_to_sysid_target(_angle_ef_target_rad, true, true, true)) {
if (get_angle_target_to_sysid(_angle_rad)) {
resend_now = true;
}
break;
@ -112,7 +119,7 @@ void AP_Mount_SToRM32_serial::update()
}
if (can_send(resend_now)) {
if (resend_now) {
send_target_angles(ToDeg(_angle_ef_target_rad.y), ToDeg(_angle_ef_target_rad.x), ToDeg(_angle_ef_target_rad.z));
send_target_angles(_angle_rad);
get_angles();
_reply_type = ReplyType_ACK;
_reply_counter = 0;
@ -162,7 +169,7 @@ bool AP_Mount_SToRM32_serial::can_send(bool with_control) {
// send_target_angles
void AP_Mount_SToRM32_serial::send_target_angles(float pitch_deg, float roll_deg, float yaw_deg)
void AP_Mount_SToRM32_serial::send_target_angles(const MountTarget& angle_target_rad)
{
static cmd_set_angles_struct cmd_set_angles_data = {
@ -186,14 +193,10 @@ void AP_Mount_SToRM32_serial::send_target_angles(float pitch_deg, float roll_deg
return;
}
// reverse pitch and yaw control
pitch_deg = -pitch_deg;
yaw_deg = -yaw_deg;
// send CMD_SETANGLE
cmd_set_angles_data.pitch = pitch_deg;
cmd_set_angles_data.roll = roll_deg;
cmd_set_angles_data.yaw = yaw_deg;
// send CMD_SETANGLE (Note: reversed pitch and yaw)
cmd_set_angles_data.pitch = -degrees(angle_target_rad.pitch);
cmd_set_angles_data.roll = degrees(angle_target_rad.roll);
cmd_set_angles_data.yaw = -degrees(get_bf_yaw_angle(angle_target_rad));
uint8_t* buf = (uint8_t*)&cmd_set_angles_data;

3
libraries/AP_Mount/AP_Mount_SToRM32_serial.h
View File

@ -44,7 +44,7 @@ public:
private:
// send_target_angles
void send_target_angles(float pitch_deg, float roll_deg, float yaw_deg);
void send_target_angles(const MountTarget& angle_target_rad);
// send read data request
void get_angles();
@ -137,6 +137,7 @@ private:
AP_HAL::UARTDriver *_port;
bool _initialised; // true once the driver has been initialised
MountTarget _angle_rad; // latest angle target
uint32_t _last_send; // system time of last do_mount_control sent to gimbal
uint8_t _reply_length;