From 871661c65f44f6e7391c86a7a761b15b002b71bb Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Sat, 31 Mar 2018 20:05:23 +1100
Subject: [PATCH] SITL: SIM_Vicon: use existing channel for packing messages

Also correct sending of Vicon message; too many bytes were sent
---
 libraries/SITL/SIM_Vicon.cpp | 31 ++++++++++++++++++-------------
 libraries/SITL/SIM_Vicon.h   |  2 +-
 2 files changed, 19 insertions(+), 14 deletions(-)

diff --git a/libraries/SITL/SIM_Vicon.cpp b/libraries/SITL/SIM_Vicon.cpp
index bd8e4b9294..0f611f8b33 100644
--- a/libraries/SITL/SIM_Vicon.cpp
+++ b/libraries/SITL/SIM_Vicon.cpp
@@ -38,6 +38,10 @@ Vicon::Vicon()
     // make sure we don't screw the simulation up by blocking:
     fcntl(fd_my_end, F_SETFL, fcntl(fd_my_end, F_GETFL, 0) | O_NONBLOCK);
     fcntl(fd_their_end, F_SETFL, fcntl(fd_their_end, F_GETFL, 0) | O_NONBLOCK);
+
+    if (!valid_channel(mavlink_ch)) {
+        AP_HAL::panic("Invalid mavlink channel");
+    }
 }
 
 void Vicon::maybe_send_heartbeat()
@@ -100,20 +104,21 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
     observation.attitude.to_euler(roll, pitch, yaw);
 
     mavlink_message_t msg;
-    mavlink_msg_vicon_position_estimate_pack_chan(system_id,
-                                                  component_id,
-                                                  mavlink_ch,
-                                                  &msg,
-                                                  observation.time_ms*1000,
-                                                  observation.position.x,
-                                                  observation.position.y,
-                                                  observation.position.z,
-                                                  roll,
-                                                  pitch,
-                                                  yaw);
+    const uint16_t n = mavlink_msg_vicon_position_estimate_pack_chan(
+        system_id,
+        component_id,
+        mavlink_ch,
+        &msg,
+        observation.time_ms*1000,
+        observation.position.x,
+        observation.position.y,
+        observation.position.z,
+        roll,
+        pitch,
+        yaw);
 
-    // ::fprintf(stderr, "Vicon: writing pos=(%03.03f %03.03f %03.03f) att=(%01.03f %01.03f %01.03f)\n", observation.position.x, observation.position.y, observation.position.z, roll, pitch, yaw);
-    if (::write(fd_my_end, (void*)&msg, sizeof(msg)) != sizeof(msg)) {
+    // ::fprintf(stderr, "Vicon: %u: writing pos=(%03.03f %03.03f %03.03f) att=(%01.03f %01.03f %01.03f)\n", observation.time_ms, observation.position.x, observation.position.y, observation.position.z, roll, pitch, yaw);
+    if (::write(fd_my_end, (void*)&msg, n) != n) {
         ::fprintf(stderr, "Vicon: write failure\n");
         // abort();
     }
diff --git a/libraries/SITL/SIM_Vicon.h b/libraries/SITL/SIM_Vicon.h
index a83047a2d2..2526629320 100644
--- a/libraries/SITL/SIM_Vicon.h
+++ b/libraries/SITL/SIM_Vicon.h
@@ -47,7 +47,7 @@ private:
     const uint8_t component_id = 18;
 
     // we share channels with the ArduPilot binary!
-    const uint8_t mavlink_ch = 17;
+    const mavlink_channel_t mavlink_ch = (mavlink_channel_t)(MAVLINK_COMM_0+5);
 
     int fd_their_end;
     int fd_my_end;