Report airspeed over HoTT telemetry

1 files changed, 13 insertions, 0 deletions

diff --git a/src/drivers/hott_telemetry/messages.c b/src/drivers/hott_telemetry/messages.c
index 369070f8c..d2634ef41 100644
--- a/src/drivers/hott_telemetry/messages.c
+++ b/src/drivers/hott_telemetry/messages.c
@@ -44,6 +44,7 @@
 #include <string.h>
 #include <systemlib/geo/geo.h>
 #include <unistd.h>
+#include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/sensor_combined.h>
@@ -56,6 +57,7 @@ static int battery_sub = -1;
 static int gps_sub = -1;
 static int home_sub = -1;
 static int sensor_sub = -1;
+static int airspeed_sub = -1;
 
 static bool home_position_set = false;
 static double home_lat = 0.0d;
@@ -68,6 +70,7 @@ messages_init(void)
 	gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
 	home_sub = orb_subscribe(ORB_ID(home_position));
 	sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+	airspeed_sub = orb_subscribe(ORB_ID(airspeed));
 }
 
 void 
@@ -100,6 +103,16 @@ build_eam_response(uint8_t *buffer, size_t *size)
 	msg.altitude_L = (uint8_t)alt & 0xff;
 	msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
 
+	/* get a local copy of the airspeed data */
+	struct airspeed_s airspeed;
+	memset(&airspeed, 0, sizeof(airspeed));
+	orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed);
+
+	uint16_t speed = (uint16_t)(airspeed.indicated_airspeed_m_s);
+	msg.speed_L = (uint8_t)speed & 0xff;
+	msg.speed_H = (uint8_t)(speed >> 8) & 0xff;
+    
+
 	msg.stop = STOP_BYTE;
 	memcpy(buffer, &msg, *size);
 }