Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-02 19:53:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

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

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

78
libraries/AP_Mount/AP_Mount_SToRM32.cpp
View File

@ -29,59 +29,66 @@ void AP_Mount_SToRM32::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 = radians(target.x);
_angle_rad.pitch = radians(target.y);
_angle_rad.yaw = radians(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 = radians(target.x);
_angle_rad.pitch = radians(target.y);
_angle_rad.yaw = radians(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
// point mount to a GPS location
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;
@ -93,7 +100,7 @@ void AP_Mount_SToRM32::update()
// resend target angles at least once per second
if (resend_now || ((AP_HAL::millis() - _last_send) > AP_MOUNT_STORM32_RESEND_MS)) {
send_do_mount_control(ToDeg(_angle_ef_target_rad.y), ToDeg(_angle_ef_target_rad.x), ToDeg(_angle_ef_target_rad.z), MAV_MOUNT_MODE_MAVLINK_TARGETING);
send_do_mount_control(_angle_rad);
}
}
@ -120,7 +127,7 @@ void AP_Mount_SToRM32::set_mode(enum MAV_MOUNT_MODE mode)
void AP_Mount_SToRM32::send_mount_status(mavlink_channel_t chan)
{
// return target angles as gimbal's actual attitude. To-Do: retrieve actual gimbal attitude and send these instead
mavlink_msg_mount_status_send(chan, 0, 0, ToDeg(_angle_ef_target_rad.y)*100, ToDeg(_angle_ef_target_rad.x)*100, ToDeg(_angle_ef_target_rad.z)*100, _mode);
mavlink_msg_mount_status_send(chan, 0, 0, degrees(_angle_rad.pitch)*100, degrees(_angle_rad.roll)*100, degrees(get_bf_yaw_angle(_angle_rad))*100, _mode);
}
// search for gimbal in GCS_MAVLink routing table
@ -142,7 +149,7 @@ void AP_Mount_SToRM32::find_gimbal()
}
// send_do_mount_control - send a COMMAND_LONG containing a do_mount_control message
void AP_Mount_SToRM32::send_do_mount_control(float pitch_deg, float roll_deg, float yaw_deg, enum MAV_MOUNT_MODE mount_mode)
void AP_Mount_SToRM32::send_do_mount_control(const MountTarget& angle_target_rad)
{
// exit immediately if not initialised
if (!_initialised) {
@ -154,21 +161,18 @@ void AP_Mount_SToRM32::send_do_mount_control(float pitch_deg, float roll_deg, fl
return;
}
// reverse pitch and yaw control
pitch_deg = -pitch_deg;
yaw_deg = -yaw_deg;
// send command_long command containing a do_mount_control command
// Note: pitch and yaw are reversed
mavlink_msg_command_long_send(_chan,
_sysid,
_compid,
MAV_CMD_DO_MOUNT_CONTROL,
0, // confirmation of zero means this is the first time this message has been sent
pitch_deg,
roll_deg,
yaw_deg,
-degrees(angle_target_rad.pitch),
degrees(angle_target_rad.roll),
-degrees(get_bf_yaw_angle(angle_target_rad)),
0, 0, 0, // param4 ~ param6 unused
mount_mode);
MAV_MOUNT_MODE_MAVLINK_TARGETING);
// store time of send
_last_send = AP_HAL::millis();

5
libraries/AP_Mount/AP_Mount_SToRM32.h
View File

@ -43,8 +43,8 @@ private:
// search for gimbal in GCS_MAVLink routing table
void find_gimbal();
// send_do_mount_control - send a COMMAND_LONG containing a do_mount_control message
void send_do_mount_control(float pitch_deg, float roll_deg, float yaw_deg, enum MAV_MOUNT_MODE mount_mode);
// send_do_mount_control with latest angle targets
void send_do_mount_control(const MountTarget& angle_target_rad);
// internal variables
bool _initialised; // true once the driver has been initialised
@ -52,5 +52,6 @@ private:
uint8_t _compid; // component id of gimbal
mavlink_channel_t _chan; // mavlink channel used to communicate with gimbal
uint32_t _last_send; // system time of last do_mount_control sent to gimbal
MountTarget _angle_rad; // latest angle target
};
#endif // HAL_MOUNT_STORM32MAVLINK_ENABLED