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

AP_Mount: support gimbal-manager-information requests

This commit is contained in:
Randy Mackay 2023-04-24 13:58:58 +09:00
parent 927f86ecbd
commit ddbc52d2db
5 changed files with 87 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_Mount/AP_Mount.cpp
View File

@ -474,6 +474,17 @@ void AP_Mount::send_gimbal_device_attitude_status(mavlink_channel_t chan)
}
}
// send a GIMBAL_MANAGER_INFORMATION message to GCS
void AP_Mount::send_gimbal_manager_information(mavlink_channel_t chan)
{
// call send_gimbal_device_attitude_status for each instance
for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
if (_backends[instance] != nullptr) {
_backends[instance]->send_gimbal_manager_information(chan);
}
}
}
// get mount's current attitude in euler angles in degrees. yaw angle is in body-frame
// returns true on success
bool AP_Mount::get_attitude_euler(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_bf_deg)

3
libraries/AP_Mount/AP_Mount.h
View File

@ -156,6 +156,9 @@ public:
// send a GIMBAL_DEVICE_ATTITUDE_STATUS message to GCS
void send_gimbal_device_attitude_status(mavlink_channel_t chan);
// send a GIMBAL_MANAGER_INFORMATION message to GCS
void send_gimbal_manager_information(mavlink_channel_t chan);
// get mount's current attitude in euler angles in degrees. yaw angle is in body-frame
// returns true on success
bool get_attitude_euler(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_bf_deg);

60
libraries/AP_Mount/AP_Mount_Backend.cpp
View File

@ -14,6 +14,18 @@ void AP_Mount_Backend::init()
_target_sysid = _params.sysid_default.get();
}
// return true if this mount accepts roll targets
bool AP_Mount_Backend::has_roll_control() const
{
return (_params.roll_angle_min < _params.roll_angle_max);
}
// return true if this mount accepts pitch targets
bool AP_Mount_Backend::has_pitch_control() const
{
return (_params.pitch_angle_min < _params.pitch_angle_max);
}
// set angle target in degrees
// yaw_is_earth_frame (aka yaw_lock) should be true if yaw angle is earth-frame, false if body-frame
void AP_Mount_Backend::set_angle_target(float roll_deg, float pitch_deg, float yaw_deg, bool yaw_is_earth_frame)
@ -111,6 +123,54 @@ void AP_Mount_Backend::send_gimbal_device_attitude_status(mavlink_channel_t chan
0); // failure flags (not supported)
}
// return gimbal manager capability flags used by GIMBAL_MANAGER_INFORMATION message
uint32_t AP_Mount_Backend::get_gimbal_manager_capability_flags() const
{
uint32_t cap_flags = GIMBAL_MANAGER_CAP_FLAGS_HAS_RETRACT |
GIMBAL_MANAGER_CAP_FLAGS_HAS_NEUTRAL |
GIMBAL_MANAGER_CAP_FLAGS_HAS_RC_INPUTS |
GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_LOCAL |
GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL;
// roll control
if (has_roll_control()) {
cap_flags |= GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_AXIS |
GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_FOLLOW |
GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_LOCK;
}
// pitch control
if (has_pitch_control()) {
cap_flags |= GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_AXIS |
GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_FOLLOW |
GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_LOCK;
}
// yaw control
if (has_pan_control()) {
cap_flags |= GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_AXIS |
GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_FOLLOW |
GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_LOCK;
}
return cap_flags;
}
// send a GIMBAL_MANAGER_INFORMATION message to GCS
void AP_Mount_Backend::send_gimbal_manager_information(mavlink_channel_t chan)
{
mavlink_msg_gimbal_manager_information_send(chan,
AP_HAL::millis(), // autopilot system time
get_gimbal_manager_capability_flags(), // bitmap of gimbal manager capability flags
_instance, // gimbal device id
radians(_params.roll_angle_min), // roll_min in radians
radians(_params.roll_angle_max), // roll_max in radians
radians(_params.pitch_angle_min), // pitch_min in radians
radians(_params.pitch_angle_max), // pitch_max in radians
radians(_params.yaw_angle_min), // yaw_min in radians
radians(_params.yaw_angle_max)); // yaw_max in radians
}
// process MOUNT_CONTROL messages received from GCS. deprecated.
void AP_Mount_Backend::handle_mount_control(const mavlink_mount_control_t &packet)
{

10
libraries/AP_Mount/AP_Mount_Backend.h
View File

@ -46,6 +46,10 @@ public:
// return true if healthy
virtual bool healthy() const { return true; }
// return true if this mount accepts roll or pitch targets
virtual bool has_roll_control() const;
virtual bool has_pitch_control() const;
// returns true if this mount can control its pan (required for multicopters)
virtual bool has_pan_control() const = 0;
@ -92,6 +96,12 @@ public:
// send a GIMBAL_DEVICE_ATTITUDE_STATUS message to GCS
void send_gimbal_device_attitude_status(mavlink_channel_t chan);
// return gimbal capabilities sent to GCS in the GIMBAL_MANAGER_INFORMATION
virtual uint32_t get_gimbal_manager_capability_flags() const;
// send a GIMBAL_MANAGER_INFORMATION message to GCS
void send_gimbal_manager_information(mavlink_channel_t chan);
// handle a GIMBAL_REPORT message
virtual void handle_gimbal_report(mavlink_channel_t chan, const mavlink_message_t &msg) {}

3
libraries/AP_Mount/AP_Mount_Siyi.h
View File

@ -48,6 +48,9 @@ public:
// return true if healthy
bool healthy() const override;
// return true if this mount accepts roll targets
bool has_roll_control() const override { return false; }
// has_pan_control - returns true if this mount can control its pan (required for multicopters)
bool has_pan_control() const override { return yaw_range_valid(); };