Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-27 02:04:00 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: Broadcast POSITION_TARGET_GLOBAL_INT as part of the EXTENDED_STATUS stream

Browse Source
This commit is contained in:
Michael du Breuil 2016-04-13 22:22:13 -07:00 committed by Andrew Tridgell
parent a17ea5c121
commit 0fcfcdc169
2 changed files with 29 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
ArduPlane/GCS_Mavlink.cpp
View File

@ -374,6 +374,26 @@ void Plane::send_nav_controller_output(mavlink_channel_t chan)
nav_controller->crosstrack_error()); nav_controller->crosstrack_error());
} }
void Plane::send_position_target_global_int(mavlink_channel_t chan)
{
mavlink_msg_position_target_global_int_send(
chan,
AP_HAL::millis(), // time_boot_ms
MAV_FRAME_GLOBAL_INT, // targets are always global altitude
0xFFF8, // ignore everything except the x/y/z components
next_WP_loc.lat, // latitude as 1e7
next_WP_loc.lng, // longitude as 1e7
next_WP_loc.alt * 0.01f, // altitude is sent as a float
0.0f, // vx
0.0f, // vy
0.0f, // vz
0.0f, // afx
0.0f, // afy
0.0f, // afz
0.0f, // yaw
0.0f); // yaw_rate
}
void Plane::send_servo_out(mavlink_channel_t chan) void Plane::send_servo_out(mavlink_channel_t chan)
{ {
@ -673,6 +693,13 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
} }
break; break;
case MSG_POSITION_TARGET_GLOBAL_INT:
if (plane.control_mode != MANUAL) {
CHECK_PAYLOAD_SIZE(POSITION_TARGET_GLOBAL_INT);
plane.send_position_target_global_int(chan);
}
break;
case MSG_GPS_RAW: case MSG_GPS_RAW:
CHECK_PAYLOAD_SIZE(GPS_RAW_INT); CHECK_PAYLOAD_SIZE(GPS_RAW_INT);
plane.gcs[chan-MAVLINK_COMM_0].send_gps_raw(plane.gps); plane.gcs[chan-MAVLINK_COMM_0].send_gps_raw(plane.gps);
@ -1039,6 +1066,7 @@ GCS_MAVLINK::data_stream_send(void)
send_message(MSG_GPS_RAW); send_message(MSG_GPS_RAW);
send_message(MSG_NAV_CONTROLLER_OUTPUT); send_message(MSG_NAV_CONTROLLER_OUTPUT);
send_message(MSG_FENCE_STATUS); send_message(MSG_FENCE_STATUS);
send_message(MSG_POSITION_TARGET_GLOBAL_INT);
} }
if (plane.gcs_out_of_time) return; if (plane.gcs_out_of_time) return;

1
ArduPlane/Plane.h
View File

@ -762,6 +762,7 @@ private:
void send_extended_status1(mavlink_channel_t chan); void send_extended_status1(mavlink_channel_t chan);
void send_location(mavlink_channel_t chan); void send_location(mavlink_channel_t chan);
void send_nav_controller_output(mavlink_channel_t chan); void send_nav_controller_output(mavlink_channel_t chan);
void send_position_target_global_int(mavlink_channel_t chan);
void send_servo_out(mavlink_channel_t chan); void send_servo_out(mavlink_channel_t chan);
void send_radio_out(mavlink_channel_t chan); void send_radio_out(mavlink_channel_t chan);
void send_vfr_hud(mavlink_channel_t chan); void send_vfr_hud(mavlink_channel_t chan);