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

GCS_MAVLink: support gimbal_manager_status and do_gimbal_manager_configure 

Co-authored-by: Randy Mackay <rmackay9@yahoo.com>
This commit is contained in:
davidsastresas 2023-05-09 21:25:33 +09:00 committed by Randy Mackay
parent cbec7ee47b
commit e4428f538d
3 changed files with 29 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/GCS_MAVLink/GCS.h
View File

@ -368,6 +368,7 @@ public:
void send_vibration() const;
void send_gimbal_device_attitude_status() const;
void send_gimbal_manager_information() const;
void send_gimbal_manager_status() const;
void send_named_float(const char *name, float value) const;
void send_home_position() const;
void send_gps_global_origin() const;
@ -638,7 +639,7 @@ protected:
MAV_RESULT handle_command_do_set_roi_sysid(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_do_set_roi_none();
virtual MAV_RESULT handle_command_mount(const mavlink_command_long_t &packet);
virtual MAV_RESULT handle_command_mount(const mavlink_command_long_t &packet, const mavlink_message_t &msg);
MAV_RESULT handle_command_mag_cal(const mavlink_command_long_t &packet);
virtual MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_camera(const mavlink_command_long_t &packet);

36
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -990,6 +990,7 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c
{ 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},
{ MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS, MSG_GIMBAL_MANAGER_STATUS},
#endif
#if AP_OPTICALFLOW_ENABLED
{ MAVLINK_MSG_ID_OPTICAL_FLOW, MSG_OPTICAL_FLOW},
@ -4540,14 +4541,14 @@ MAV_RESULT GCS_MAVLINK::handle_command_accelcal_vehicle_pos(const mavlink_comman
}
#endif // HAL_INS_ACCELCAL_ENABLED
MAV_RESULT GCS_MAVLINK::handle_command_mount(const mavlink_command_long_t &packet)
MAV_RESULT GCS_MAVLINK::handle_command_mount(const mavlink_command_long_t &packet, const mavlink_message_t &msg)
{
#if HAL_MOUNT_ENABLED
AP_Mount *mount = AP::mount();
if (mount == nullptr) {
return MAV_RESULT_UNSUPPORTED;
}
return mount->handle_command_long(packet);
return mount->handle_command_long(packet, msg);
#else
return MAV_RESULT_UNSUPPORTED;
#endif
@ -4678,12 +4679,6 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t
break;
#endif
case MAV_CMD_DO_MOUNT_CONFIGURE:
case MAV_CMD_DO_MOUNT_CONTROL:
case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
result = handle_command_mount(packet);
break;
#if AP_MAVLINK_MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES_ENABLED
case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: {
result = handle_command_request_autopilot_capabilities(packet);
@ -4892,6 +4887,14 @@ void GCS_MAVLINK::handle_command_long(const mavlink_message_t &msg)
result = handle_command_preflight_calibration(packet, msg);
break;
case MAV_CMD_DO_MOUNT_CONFIGURE:
case MAV_CMD_DO_MOUNT_CONTROL:
case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
case MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE:
// some mount commands require the source sysid and compid
result = handle_command_mount(packet, msg);
break;
default:
result = handle_command_long_packet(packet);
break;
@ -5334,9 +5337,7 @@ void GCS_MAVLINK::send_gimbal_device_attitude_status() const
}
mount->send_gimbal_device_attitude_status(chan);
}
#endif
#if HAL_MOUNT_ENABLED
void GCS_MAVLINK::send_gimbal_manager_information() const
{
AP_Mount *mount = AP::mount();
@ -5345,6 +5346,15 @@ void GCS_MAVLINK::send_gimbal_manager_information() const
}
mount->send_gimbal_manager_information(chan);
}
void GCS_MAVLINK::send_gimbal_manager_status() const
{
AP_Mount *mount = AP::mount();
if (mount == nullptr) {
return;
}
mount->send_gimbal_manager_status(chan);
}
#endif
void GCS_MAVLINK::send_set_position_target_global_int(uint8_t target_system, uint8_t target_component, const Location& loc)
@ -5675,6 +5685,12 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
#if HAL_MOUNT_ENABLED
CHECK_PAYLOAD_SIZE(GIMBAL_MANAGER_INFORMATION);
send_gimbal_manager_information();
#endif
break;
case MSG_GIMBAL_MANAGER_STATUS:
#if HAL_MOUNT_ENABLED
CHECK_PAYLOAD_SIZE(GIMBAL_MANAGER_STATUS);
send_gimbal_manager_status();
#endif
break;
case MSG_OPTICAL_FLOW:

1
libraries/GCS_MAVLink/ap_message.h
View File

@ -51,6 +51,7 @@ enum ap_message : uint8_t {
MSG_CAMERA_FEEDBACK,
MSG_GIMBAL_DEVICE_ATTITUDE_STATUS,
MSG_GIMBAL_MANAGER_INFORMATION,
MSG_GIMBAL_MANAGER_STATUS,
MSG_OPTICAL_FLOW,
MSG_MAG_CAL_PROGRESS,
MSG_MAG_CAL_REPORT,