Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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

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

60
libraries/AP_Mount/AP_Mount_SoloGimbal.cpp
View File

@ -40,52 +40,61 @@ void AP_Mount_SoloGimbal::update()
// move mount to a "retracted" position. we do not implement a separate servo based retract mechanism
case MAV_MOUNT_MODE_RETRACT:
_gimbal.set_lockedToBody(true);
// initialise _angle_rad to smooth transition if user changes to RC_TARGETTINg
_angle_rad = {0, 0, 0, false};
break;
// move mount to a neutral position, typically pointing forward
case MAV_MOUNT_MODE_NEUTRAL:
{
case MAV_MOUNT_MODE_NEUTRAL: {
_gimbal.set_lockedToBody(false);
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:
_gimbal.set_lockedToBody(false);
// 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;
}
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);
// update targets using pilot's rc inputs
update_targets_from_rc();
// 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;
}
break;
}
// point mount to a GPS point given by the mission planner
case MAV_MOUNT_MODE_GPS_POINT:
_gimbal.set_lockedToBody(false);
UNUSED_RESULT(calc_angle_to_roi_target(_angle_ef_target_rad, true, true, true));
IGNORE_RETURN(get_angle_target_to_roi(_angle_rad));
break;
case MAV_MOUNT_MODE_HOME_LOCATION:
_gimbal.set_lockedToBody(false);
// constantly update the home location:
if (!AP::ahrs().home_is_set()) {
break;
}
_roi_target = AP::ahrs().get_home();
_roi_target_set = true;
UNUSED_RESULT(calc_angle_to_roi_target(_angle_ef_target_rad, true, true, true));
IGNORE_RETURN(get_angle_target_to_home(_angle_rad));
break;
case MAV_MOUNT_MODE_SYSID_TARGET:
_gimbal.set_lockedToBody(false);
UNUSED_RESULT(calc_angle_to_sysid_target(_angle_ef_target_rad, true, true, true));
IGNORE_RETURN(get_angle_target_to_sysid(_angle_rad));
break;
default:
@ -94,13 +103,6 @@ void AP_Mount_SoloGimbal::update()
}
}
// has_pan_control - returns true if this mount can control it's pan (required for multicopters)
bool AP_Mount_SoloGimbal::has_pan_control() const
{
// we do not have yaw control
return false;
}
// set_mode - sets mount's mode
void AP_Mount_SoloGimbal::set_mode(enum MAV_MOUNT_MODE mode)
{
@ -117,9 +119,9 @@ void AP_Mount_SoloGimbal::set_mode(enum MAV_MOUNT_MODE mode)
void AP_Mount_SoloGimbal::send_mount_status(mavlink_channel_t chan)
{
if (_gimbal.aligned()) {
mavlink_msg_mount_status_send(chan, 0, 0, degrees(_angle_ef_target_rad.y)*100, degrees(_angle_ef_target_rad.x)*100, degrees(_angle_ef_target_rad.z)*100, _mode);
mavlink_msg_mount_status_send(chan, 0, 0, degrees(_angle_rad.roll)*100, degrees(_angle_rad.pitch)*100, degrees(get_bf_yaw_angle(_angle_rad))*100, _mode);
}
// block heartbeat from transmitting to the GCS
GCS_MAVLINK::disable_channel_routing(chan);
}
@ -129,7 +131,7 @@ void AP_Mount_SoloGimbal::send_mount_status(mavlink_channel_t chan)
*/
void AP_Mount_SoloGimbal::handle_gimbal_report(mavlink_channel_t chan, const mavlink_message_t &msg)
{
_gimbal.update_target(_angle_ef_target_rad);
_gimbal.update_target(Vector3f{_angle_rad.roll, _angle_rad.pitch, get_bf_yaw_angle(_angle_rad)});
_gimbal.receive_feedback(chan,msg);
AP_Logger *logger = AP_Logger::get_singleton();

4
libraries/AP_Mount/AP_Mount_SoloGimbal.h
View File

@ -30,7 +30,7 @@ public:
void update() override;
// has_pan_control - returns true if this mount can control it's pan (required for multicopters)
bool has_pan_control() const override;
bool has_pan_control() const override { return false; }
// set_mode - sets mount's mode
void set_mode(enum MAV_MOUNT_MODE mode) override;
@ -53,7 +53,7 @@ private:
void Log_Write_Gimbal(SoloGimbal &gimbal);
bool _params_saved;
MountTarget _angle_rad; // angle target
SoloGimbal _gimbal;
};