Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-26 18:48:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: move handling of gimbal messages up

Browse Source 
GCS_MAVLink: move handling of mav_cmd_do_mount_control up

GCS_MAVLink: move handling of deprecated mount messages up

GCS_MAVLink: move handling of command_do_set_roi up

GCS_MAVLink: move handling of command_do_set_roi up

GCS_MAVLink: handle MAV_CMD_DO_MOUNT_CONFIGURE

GCS_MAVLink: call renamed AP_Mount send_mount_status msg

GCS_MAVLink: add support for MAV_CMD_DO_SET_ROI_LOCATION
This commit is contained in:
Peter Barker 2018-10-14 09:24:24 +11:00 committed by Peter Barker
parent 4ae6aeed7e
commit 890a62fc51
2 changed files with 156 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/GCS_MAVLink/GCS.h
View File

@ -190,7 +190,9 @@ public:
void send_local_position() const;
void send_vfr_hud();
void send_vibration() const;
void send_mount_status() const;
void send_named_float(const char *name, float value) const;
void send_gimbal_report() const;
void send_home() const;
void send_ekf_origin() const;
virtual void send_position_target_global_int() { };
@ -302,7 +304,8 @@ protected:
void handle_common_rally_message(mavlink_message_t *msg);
void handle_rally_fetch_point(mavlink_message_t *msg);
void handle_rally_point(mavlink_message_t *msg);
void handle_gimbal_report(AP_Mount &mount, mavlink_message_t *msg) const;
virtual void handle_mount_message(const mavlink_message_t *msg);
void handle_param_value(mavlink_message_t *msg);
void handle_radio_status(mavlink_message_t *msg, DataFlash_Class &dataflash, bool log_radio);
void handle_serial_control(const mavlink_message_t *msg);
void handle_vision_position_delta(mavlink_message_t *msg);
@ -354,10 +357,14 @@ protected:
void handle_command_long(mavlink_message_t* msg);
MAV_RESULT handle_command_accelcal_vehicle_pos(const mavlink_command_long_t &packet);
virtual MAV_RESULT handle_command_mount(const mavlink_command_long_t &packet);
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);
MAV_RESULT handle_command_do_send_banner(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_do_set_roi(const mavlink_command_int_t &packet);
MAV_RESULT handle_command_do_set_roi(const mavlink_command_long_t &packet);
virtual MAV_RESULT handle_command_do_set_roi(const Location &roi_loc);
MAV_RESULT handle_command_do_gripper(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_do_set_mode(const mavlink_command_long_t &packet);
MAV_RESULT handle_command_get_home_position(const mavlink_command_long_t &packet);

151
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -618,13 +618,28 @@ void GCS_MAVLINK::handle_mission_write_partial_list(AP_Mission &mission, mavlink
/*
handle a GIMBAL_REPORT mavlink packet
pass mavlink messages to the AP_Mount singleton
*/
void GCS_MAVLINK::handle_gimbal_report(AP_Mount &mount, mavlink_message_t *msg) const
void GCS_MAVLINK::handle_mount_message(const mavlink_message_t *msg)
{
mount.handle_gimbal_report(chan, msg);
AP_Mount *mount = AP::mount();
if (mount == nullptr) {
return;
}
mount->handle_message(chan, msg);
}
/*
pass parameter value messages through to mount library
*/
void GCS_MAVLINK::handle_param_value(mavlink_message_t *msg)
{
AP_Mount *mount = AP::mount();
if (mount == nullptr) {
return;
}
mount->handle_param_value(msg);
}
void GCS_MAVLINK::send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list)
{
@ -2060,6 +2075,7 @@ MAV_RESULT GCS_MAVLINK::handle_command_camera(const mavlink_command_long_t &pack
return result;
}
// sets ekf_origin if it has not been set.
// should only be used when there is no GPS to provide an absolute position
void GCS_MAVLINK::set_ekf_origin(const Location& loc)
@ -2353,6 +2369,14 @@ void GCS_MAVLINK::handle_common_message(mavlink_message_t *msg)
handle_command_int(msg);
break;
case MAVLINK_MSG_ID_GIMBAL_REPORT:
handle_mount_message(msg);
break;
case MAVLINK_MSG_ID_PARAM_VALUE:
handle_param_value(msg);
break;
case MAVLINK_MSG_ID_SERIAL_CONTROL:
handle_serial_control(msg);
break;
@ -2373,6 +2397,11 @@ void GCS_MAVLINK::handle_common_message(mavlink_message_t *msg)
AP_Notify::handle_led_control(msg);
break;
case MAVLINK_MSG_ID_MOUNT_CONFIGURE: // deprecated. Use MAV_CMD_DO_MOUNT_CONFIGURE
case MAVLINK_MSG_ID_MOUNT_CONTROL: // deprecated. Use MAV_CMD_DO_MOUNT_CONTROL
handle_mount_message(msg);
break;
case MAVLINK_MSG_ID_PLAY_TUNE:
// send message to Notify
AP_Notify::handle_play_tune(msg);
@ -2715,6 +2744,15 @@ MAV_RESULT GCS_MAVLINK::handle_command_accelcal_vehicle_pos(const mavlink_comman
return MAV_RESULT_ACCEPTED;
}
MAV_RESULT GCS_MAVLINK::handle_command_mount(const mavlink_command_long_t &packet)
{
AP_Mount *mount = AP::mount();
if (mount == nullptr) {
return MAV_RESULT_UNSUPPORTED;
}
return mount->handle_command_long(packet);
}
MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t &packet)
{
MAV_RESULT result = MAV_RESULT_FAILED;
@ -2758,11 +2796,21 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t
result = handle_command_do_gripper(packet);
break;
case MAV_CMD_DO_MOUNT_CONFIGURE:
case MAV_CMD_DO_MOUNT_CONTROL:
result = handle_command_mount(packet);
break;
case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: {
result = handle_command_request_autopilot_capabilities(packet);
break;
}
case MAV_CMD_DO_SET_ROI_LOCATION:
case MAV_CMD_DO_SET_ROI:
result = handle_command_do_set_roi(packet);
break;
case MAV_CMD_PREFLIGHT_CALIBRATION:
result = handle_command_preflight_calibration(packet);
break;
@ -2819,8 +2867,77 @@ void GCS_MAVLINK::handle_command_long(mavlink_message_t *msg)
mavlink_msg_command_ack_send_buf(msg, chan, packet.command, result);
}
MAV_RESULT GCS_MAVLINK::handle_command_do_set_roi(const Location &roi_loc)
{
AP_Mount *mount = AP::mount();
if (mount == nullptr) {
return MAV_RESULT_UNSUPPORTED;
}
// sanity check location
if (!check_latlng(roi_loc)) {
return MAV_RESULT_FAILED;
}
if (roi_loc.lat == 0 && roi_loc.lng == 0 && roi_loc.alt == 0) {
// switch off the camera tracking if enabled
if (mount->get_mode() == MAV_MOUNT_MODE_GPS_POINT) {
mount->set_mode_to_default();
}
} else {
// send the command to the camera mount
mount->set_roi_target(roi_loc);
}
return MAV_RESULT_ACCEPTED;
}
MAV_RESULT GCS_MAVLINK::handle_command_do_set_roi(const mavlink_command_int_t &packet)
{
// be aware that this method is called for both MAV_CMD_DO_SET_ROI
// and MAV_CMD_DO_SET_ROI_LOCATION. If you intend to support any
// of the extra fields in the former then you will need to split
// off support for MAV_CMD_DO_SET_ROI_LOCATION (which doesn't
// support the extra fields).
// param1 : /* Region of interest mode (not used)*/
// param2 : /* MISSION index/ target ID (not used)*/
// param3 : /* ROI index (not used)*/
// param4 : /* empty */
// x : lat
// y : lon
// z : alt
Location roi_loc;
roi_loc.lat = packet.x;
roi_loc.lng = packet.y;
roi_loc.alt = (int32_t)(packet.z * 100.0f);
return handle_command_do_set_roi(roi_loc);
}
MAV_RESULT GCS_MAVLINK::handle_command_do_set_roi(const mavlink_command_long_t &packet)
{
// be aware that this method is called for both MAV_CMD_DO_SET_ROI
// and MAV_CMD_DO_SET_ROI_LOCATION. If you intend to support any
// of the extra fields in the former then you will need to split
// off support for MAV_CMD_DO_SET_ROI_LOCATION (which doesn't
// support the extra fields).
Location roi_loc;
roi_loc.lat = (int32_t)(packet.param5 * 1.0e7f);
roi_loc.lng = (int32_t)(packet.param6 * 1.0e7f);
roi_loc.alt = (int32_t)(packet.param7 * 100.0f);
return handle_command_do_set_roi(roi_loc);
}
MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &packet)
{
switch (packet.command) {
case MAV_CMD_DO_SET_ROI:
case MAV_CMD_DO_SET_ROI_LOCATION:
return handle_command_do_set_roi(packet);
default:
break;
}
return MAV_RESULT_UNSUPPORTED;
}
@ -2941,6 +3058,24 @@ void GCS_MAVLINK::send_global_position_int()
ahrs.yaw_sensor); // compass heading in 1/100 degree
}
void GCS_MAVLINK::send_gimbal_report() const
{
AP_Mount *mount = AP::mount();
if (mount == nullptr) {
return;
}
mount->send_gimbal_report(chan);
}
void GCS_MAVLINK::send_mount_status() const
{
AP_Mount *mount = AP::mount();
if (mount == nullptr) {
return;
}
mount->send_mount_status(chan);
}
bool GCS_MAVLINK::try_send_message(const enum ap_message id)
{
if (telemetry_delayed()) {
@ -2961,6 +3096,11 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
queued_param_send();
break;
case MSG_GIMBAL_REPORT:
CHECK_PAYLOAD_SIZE(GIMBAL_REPORT);
send_gimbal_report();
break;
case MSG_HEARTBEAT:
CHECK_PAYLOAD_SIZE(HEARTBEAT);
last_heartbeat_time = AP_HAL::millis();
@ -3053,6 +3193,11 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
send_local_position();
break;
case MSG_MOUNT_STATUS:
CHECK_PAYLOAD_SIZE(MOUNT_STATUS);
send_mount_status();
break;
case MSG_POSITION_TARGET_GLOBAL_INT:
CHECK_PAYLOAD_SIZE(POSITION_TARGET_GLOBAL_INT);
send_position_target_global_int();