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

Rover: send position-target-global-int mavlink messages

Browse Source
This commit is contained in:
Randy Mackay 2019-05-17 15:53:39 +09:00
parent 93b05d7d8a
commit 66fa948e27
2 changed files with 27 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
APMrover2/GCS_Mavlink.cpp
View File

@ -73,6 +73,30 @@ MAV_STATE GCS_MAVLINK_Rover::system_status() const
return MAV_STATE_ACTIVE;
}
void GCS_MAVLINK_Rover::send_position_target_global_int()
{
Location target;
if (!rover.control_mode->get_desired_location(target)) {
return;
}
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
target.lat, // latitude as 1e7
target.lng, // longitude as 1e7
target.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 GCS_MAVLINK_Rover::send_nav_controller_output() const
{
if (!rover.control_mode->is_autopilot_mode()) {
@ -438,6 +462,7 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = {
MSG_GPS2_RTK,
MSG_NAV_CONTROLLER_OUTPUT,
MSG_FENCE_STATUS,
MSG_POSITION_TARGET_GLOBAL_INT,
};
static const ap_message STREAM_POSITION_msgs[] = {
MSG_LOCATION,

2
APMrover2/GCS_Mavlink.h
View File

@ -21,6 +21,8 @@ protected:
MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet) override;
MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet) override;
void send_position_target_global_int() override;
virtual bool in_hil_mode() const override;
bool persist_streamrates() const override { return true; }