dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Report airspeed over HoTT telemetry

This commit is contained in:
Simon Wilks 2013-06-16 20:44:11 +02:00
parent b714c5c9d1
commit dadac932da
1 changed files with 13 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
src/drivers/hott_telemetry/messages.c
View File

@ -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);
}