Tracker: add support for 4th mavlink channel

This commit is contained in:
Randy Mackay 2015-05-02 21:38:58 +09:00
parent facbfaf31f
commit 35275aab85
3 changed files with 13 additions and 0 deletions

View File

@ -92,6 +92,7 @@ public:
k_param_pitch_range, k_param_pitch_range,
k_param_distance_min, k_param_distance_min,
k_param_sysid_target, // 138 k_param_sysid_target, // 138
k_param_gcs3, // stream rates for fourth MAVLink port
// //
// 150: Telemetry control // 150: Telemetry control

View File

@ -222,6 +222,12 @@ const AP_Param::Info var_info[] PROGMEM = {
GOBJECTN(gcs[2], gcs2, "SR2_", GCS_MAVLINK), GOBJECTN(gcs[2], gcs2, "SR2_", GCS_MAVLINK),
#endif #endif
#if MAVLINK_COMM_NUM_BUFFERS > 3
// @Group: SR3_
// @Path: GCS_Mavlink.pde
GOBJECTN(gcs[3], gcs3, "SR3_", GCS_MAVLINK),
#endif
// @Group: INS_ // @Group: INS_
// @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp // @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp
GOBJECT(ins, "INS_", AP_InertialSensor), GOBJECT(ins, "INS_", AP_InertialSensor),

View File

@ -46,6 +46,12 @@ static void init_tracker()
gcs[2].set_snoop(mavlink_snoop); gcs[2].set_snoop(mavlink_snoop);
#endif #endif
#if MAVLINK_COMM_NUM_BUFFERS > 3
// setup serial port for fourth telemetry port (not used by default) and start snooping for vehicle data
gcs[3].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 2);
gcs[3].set_snoop(mavlink_snoop);
#endif
mavlink_system.sysid = g.sysid_this_mav; mavlink_system.sysid = g.sysid_this_mav;
if (g.compass_enabled==true) { if (g.compass_enabled==true) {