diff options
Diffstat (limited to 'apps/hott_telemetry/messages.c')
| -rw-r--r-- | apps/hott_telemetry/messages.c | 12 |
1 files changed, 12 insertions, 0 deletions
diff --git a/apps/hott_telemetry/messages.c b/apps/hott_telemetry/messages.c index 8bfb99773..0e466e820 100644 --- a/apps/hott_telemetry/messages.c +++ b/apps/hott_telemetry/messages.c @@ -42,9 +42,11 @@ #include <string.h> #include <systemlib/systemlib.h> #include <unistd.h> +#include <uORB/topics/airspeed.h> #include <uORB/topics/battery_status.h> #include <uORB/topics/sensor_combined.h> +static int airspeed_sub = -1; static int battery_sub = -1; static int sensor_sub = -1; @@ -52,6 +54,7 @@ void messages_init(void) { battery_sub = orb_subscribe(ORB_ID(battery_status)); sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + airspeed_sub = orb_subscribe(ORB_ID(airspeed)); } void build_eam_response(uint8_t *buffer, int *size) @@ -81,6 +84,15 @@ void build_eam_response(uint8_t *buffer, int *size) msg.altitude_L = (uint8_t)alt & 0xff; msg.altitude_H = (uint8_t)(alt >> 8) & 0xff; + /* get a local copy of the current sensor values */ + 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 * 3.6); + msg.speed_L = (uint8_t)speed & 0xff; + msg.speed_H = (uint8_t)(speed >> 8) & 0xff; + msg.stop = STOP_BYTE; memcpy(buffer, &msg, *size); |