fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-31 23:31:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Mavlink: Start multiple uart listeners, HIL working

Browse Source
This commit is contained in:
Julian Oes
2014-02-14 13:36:59 +01:00
parent ef46cd5e90
commit 523637e0f1
6 changed files with 68 additions and 82 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_main.cpp
+32 -9
View File
@@ -309,6 +309,16 @@ int Mavlink::get_uart_fd(unsigned index)
return -1;
}
int Mavlink::get_uart_fd()
{
return _uart;
}
int Mavlink::get_channel()
{
return (int)_chan;
}
void
Mavlink::parameters_update()
{
@@ -945,7 +955,7 @@ void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8
wpa.target_component = compid;
wpa.type = type;
mavlink_msg_mission_ack_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpa);
mavlink_msg_mission_ack_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpa);
mavlink_missionlib_send_message(&msg);
if (verbose) warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system);
@@ -968,7 +978,7 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq)
wpc.seq = seq;
mavlink_msg_mission_current_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc);
mavlink_msg_mission_current_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpc);
mavlink_missionlib_send_message(&msg);
if (verbose) warnx("Broadcasted new current waypoint %u", wpc.seq);
@@ -988,7 +998,7 @@ void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uin
wpc.target_component = compid;
wpc.count = mission.count;
mavlink_msg_mission_count_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc);
mavlink_msg_mission_count_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpc);
mavlink_missionlib_send_message(&msg);
if (verbose) warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system);
@@ -1018,7 +1028,7 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t
wp.target_system = sysid;
wp.target_component = compid;
wp.seq = seq;
mavlink_msg_mission_item_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wp);
mavlink_msg_mission_item_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wp);
mavlink_missionlib_send_message(&msg);
if (verbose) warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system);
@@ -1036,7 +1046,7 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u
wpr.target_system = sysid;
wpr.target_component = compid;
wpr.seq = seq;
mavlink_msg_mission_request_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpr);
mavlink_msg_mission_request_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpr);
mavlink_missionlib_send_message(&msg);
if (verbose) warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system);
@@ -1061,7 +1071,7 @@ void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t seq)
wp_reached.seq = seq;
mavlink_msg_mission_item_reached_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wp_reached);
mavlink_msg_mission_item_reached_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wp_reached);
mavlink_missionlib_send_message(&msg);
if (verbose) warnx("Sent waypoint %u reached message", wp_reached.seq);
@@ -1318,7 +1328,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
mavlink_mission_item_t wp;
mavlink_msg_mission_item_decode(msg, &wp);
if (wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_wpm_comp_id) {
if (wp.target_system == mavlink_system.sysid && wp.target_component == _mavlink_wpm_comp_id) {
wpm->timestamp_lastaction = now;
@@ -1576,6 +1586,21 @@ Mavlink::task_main(int argc, char *argv[])
break;
}
switch(_mode) {
case MODE_OFFBOARD:
case MODE_HIL:
_mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
break;
case MODE_ONBOARD:
_mavlink_wpm_comp_id = MAV_COMP_ID_CAMERA;
break;
case MODE_TX_HEARTBEAT_ONLY:
default:
_mavlink_wpm_comp_id = MAV_COMP_ID_ALL;
warnx("Error: Unknown mode");
break;
}
/* Flush stdout in case MAVLink is about to take it over */
fflush(stdout);
@@ -1597,11 +1622,9 @@ Mavlink::task_main(int argc, char *argv[])
mavlink_update_system();
/* start the MAVLink receiver */
// MavlinkReceiver rcv(this);
receive_thread = MavlinkReceiver::receive_start(this);
/* start the ORB receiver */
// MavlinkOrbListener listener(this);
uorb_receive_thread = MavlinkOrbListener::uorb_receive_start(this);
/* initialize waypoint manager */