Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Mount: support GIMBAL_MANAGER_INFORMATION message #23587

Merged
merged 3 commits into from
Apr 25, 2023
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 11 additions & 0 deletions libraries/AP_Mount/AP_Mount.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
3 changes: 3 additions & 0 deletions libraries/AP_Mount/AP_Mount.h
Original file line number Diff line number Diff line change
Expand Up @@ -156,6 +156,9 @@ class AP_Mount
// 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);
Expand Down
60 changes: 60 additions & 0 deletions libraries/AP_Mount/AP_Mount_Backend.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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)
{
Expand Down
10 changes: 10 additions & 0 deletions libraries/AP_Mount/AP_Mount_Backend.h
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,10 @@ class AP_Mount_Backend
// 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;

Expand Down Expand Up @@ -92,6 +96,12 @@ class AP_Mount_Backend
// 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) {}

Expand Down
3 changes: 3 additions & 0 deletions libraries/AP_Mount/AP_Mount_Siyi.h
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,9 @@ class AP_Mount_Siyi : public AP_Mount_Backend
// 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(); };

Expand Down
1 change: 1 addition & 0 deletions libraries/GCS_MAVLink/GCS.h
Original file line number Diff line number Diff line change
Expand Up @@ -371,6 +371,7 @@ class GCS_MAVLINK
void send_vfr_hud();
void send_vibration() const;
void send_gimbal_device_attitude_status() const;
void send_gimbal_manager_information() const;
void send_named_float(const char *name, float value) const;
void send_home_position() const;
void send_gps_global_origin() const;
Expand Down
19 changes: 18 additions & 1 deletion libraries/GCS_MAVLink/GCS_Common.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -989,6 +989,7 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c
#if HAL_MOUNT_ENABLED
{ MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS, MSG_GIMBAL_DEVICE_ATTITUDE_STATUS},
{ MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE, MSG_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE},
{ MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION, MSG_GIMBAL_MANAGER_INFORMATION},
#endif
#if AP_OPTICALFLOW_ENABLED
{ MAVLINK_MSG_ID_OPTICAL_FLOW, MSG_OPTICAL_FLOW},
Expand Down Expand Up @@ -5324,6 +5325,17 @@ void GCS_MAVLINK::send_gimbal_device_attitude_status() const
}
#endif

#if HAL_MOUNT_ENABLED
void GCS_MAVLINK::send_gimbal_manager_information() const
{
AP_Mount *mount = AP::mount();
if (mount == nullptr) {
return;
}
mount->send_gimbal_manager_information(chan);
}
#endif

void GCS_MAVLINK::send_set_position_target_global_int(uint8_t target_system, uint8_t target_component, const Location& loc)
{

Expand Down Expand Up @@ -5648,7 +5660,12 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
send_autopilot_state_for_gimbal_device();
#endif
break;

case MSG_GIMBAL_MANAGER_INFORMATION:
#if HAL_MOUNT_ENABLED
CHECK_PAYLOAD_SIZE(GIMBAL_MANAGER_INFORMATION);
send_gimbal_manager_information();
#endif
break;
case MSG_OPTICAL_FLOW:
#if AP_OPTICALFLOW_ENABLED
CHECK_PAYLOAD_SIZE(OPTICAL_FLOW);
Expand Down
1 change: 1 addition & 0 deletions libraries/GCS_MAVLink/ap_message.h
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,7 @@ enum ap_message : uint8_t {
MSG_BATTERY2,
MSG_CAMERA_FEEDBACK,
MSG_GIMBAL_DEVICE_ATTITUDE_STATUS,
MSG_GIMBAL_MANAGER_INFORMATION,
MSG_OPTICAL_FLOW,
MSG_MAG_CAL_PROGRESS,
MSG_MAG_CAL_REPORT,
Expand Down
2 changes: 1 addition & 1 deletion modules/mavlink