From 6fa6e722509bbafe733797dad47cf05ef2bc7bc1 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Wed, 24 Feb 2016 21:19:30 -0700 Subject: [PATCH] add sPort GPS telemetry --- src/drivers/frsky_telemetry/frsky_telemetry.c | 39 ++++++++ src/drivers/frsky_telemetry/sPort_data.c | 94 +++++++++++++++---- src/drivers/frsky_telemetry/sPort_data.h | 7 +- 3 files changed, 119 insertions(+), 21 deletions(-) diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c index 323ff1a393..ddb73e4eb3 100644 --- a/src/drivers/frsky_telemetry/frsky_telemetry.c +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -268,6 +268,7 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) static hrt_abstime lastSPD = 0; static hrt_abstime lastFUEL = 0; static hrt_abstime lastVSPD = 0; + static hrt_abstime lastGPS = 0; switch (sbuf[1]) { @@ -344,6 +345,44 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) sPort_send_VSPD(uart, speed); } + break; + + case SMARTPORT_POLL_7: + + /* report GPS data elements at 2*5Hz */ + if (now - lastGPS > 100 * 1000) { + static int elementCount = 0; + + switch (elementCount) { + + case 0: + sPort_send_GPS_LON(uart); + elementCount++; + break; + + case 1: + sPort_send_GPS_LAT(uart); + elementCount++; + break; + + case 2: + sPort_send_GPS_COG(uart); + elementCount++; + break; + + case 3: + sPort_send_GPS_ALT(uart); + elementCount++; + break; + + case 4: + sPort_send_GPS_SPD(uart); + elementCount = 0; + break; + } + + } + break; } } diff --git a/src/drivers/frsky_telemetry/sPort_data.c b/src/drivers/frsky_telemetry/sPort_data.c index b079c2a00d..bfcb5ebac8 100644 --- a/src/drivers/frsky_telemetry/sPort_data.c +++ b/src/drivers/frsky_telemetry/sPort_data.c @@ -58,31 +58,26 @@ #define frac(f) (f - (int)f) -static int battery_sub = -1; +//static int battery_sub = -1; static int sensor_sub = -1; static int global_position_sub = -1; static int vehicle_status_sub = -1; +static struct vehicle_status_s vehicle_status; +static struct sensor_combined_s sensor_data; +static struct vehicle_global_position_s global_pos; + /** * Initializes the uORB subscriptions. */ void sPort_init() { - battery_sub = orb_subscribe(ORB_ID(battery_status)); +// battery_sub = orb_subscribe(ORB_ID(battery_status)); global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); } -/** - * Sends a 0x10 start byte. - */ -//static void sPort_send_start(int uart) -//{ -// static const uint8_t c = 0x10; -// write(uart, &c, 1); -//} - static void update_crc(uint16_t *crc, unsigned char b) { *crc += b; @@ -152,7 +147,6 @@ void sPort_send_data(int uart, uint16_t id, uint32_t data) void sPort_send_BATV(int uart) { /* get a local copy of the vehicle status data */ - struct vehicle_status_s vehicle_status; memset(&vehicle_status, 0, sizeof(vehicle_status)); orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); @@ -165,7 +159,6 @@ void sPort_send_BATV(int uart) void sPort_send_CUR(int uart) { /* get a local copy of the vehicle status data */ - struct vehicle_status_s vehicle_status; memset(&vehicle_status, 0, sizeof(vehicle_status)); orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); @@ -180,12 +173,11 @@ void sPort_send_CUR(int uart) void sPort_send_ALT(int uart) { /* get a local copy of the current sensor values */ - struct sensor_combined_s raw; - memset(&raw, 0, sizeof(raw)); - orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); + memset(&sensor_data, 0, sizeof(sensor_data)); + orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensor_data); /* send data */ - uint32_t alt = (int)(100 * raw.baro_alt_meter[0]); + uint32_t alt = (int)(100 * sensor_data.baro_alt_meter[0]); sPort_send_data(uart, SMARTPORT_ID_ALT, alt); } @@ -193,11 +185,10 @@ void sPort_send_ALT(int uart) void sPort_send_SPD(int uart) { /* get a local copy of the global position data */ - struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); - /* send data for A2 */ + /* send data */ float speed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e); uint32_t ispeed = (int)(10 * speed); sPort_send_data(uart, SMARTPORT_ID_GPS_SPD, ispeed); @@ -215,7 +206,6 @@ void sPort_send_VSPD(int uart, float speed) void sPort_send_FUEL(int uart) { /* get a local copy of the vehicle status data */ - struct vehicle_status_s vehicle_status; memset(&vehicle_status, 0, sizeof(vehicle_status)); orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); @@ -223,3 +213,67 @@ void sPort_send_FUEL(int uart) uint32_t fuel = (int)(100 * vehicle_status.battery_remaining); sPort_send_data(uart, SMARTPORT_ID_FUEL, fuel); } + +void sPort_send_GPS_LON(int uart) +{ + /* send longitude */ + /* get a local copy of the global position data */ + memset(&global_pos, 0, sizeof(global_pos)); + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); + + /* convert to 30 bit signed magnitude degrees*6E5 with MSb = 1 and bit 30=sign */ + /* precision is approximately 0.1m */ + uint32_t iLon = 6E5 * fabs(global_pos.lon); + iLon |= (1 << 31); + if (global_pos.lon < 0) { iLon |= (1 << 30); } + sPort_send_data(uart, SMARTPORT_ID_GPS_LON_LAT, iLon); +} + +void sPort_send_GPS_LAT(int uart) +{ + /* send latitude */ + /* get a local copy of the global position data */ + memset(&global_pos, 0, sizeof(global_pos)); + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); + + /* convert to 30 bit signed magnitude degrees*6E5 with MSb = 0 and bit 30=sign */ + uint32_t iLat = 6E5 * fabs(global_pos.lat); + if (global_pos.lat < 0) { iLat |= (1 << 30); } + sPort_send_data(uart, SMARTPORT_ID_GPS_LON_LAT, iLat); +} + +void sPort_send_GPS_ALT(int uart) +{ + /* send altitude */ + /* get a local copy of the global position data */ + memset(&global_pos, 0, sizeof(global_pos)); + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); + + /* convert to 100 * m/sec */ + uint32_t iAlt = 100 * global_pos.alt; + sPort_send_data(uart, SMARTPORT_ID_GPS_ALT, iAlt); +} + +void sPort_send_GPS_COG(int uart) +{ + /* send course */ + /* get a local copy of the global position data */ + memset(&global_pos, 0, sizeof(global_pos)); + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); + + /* convert to 30 bit signed magnitude degrees*6E5 with MSb = 1 and bit 30=sign */ + uint32_t iYaw = 100 * global_pos.yaw; + sPort_send_data(uart, SMARTPORT_ID_GPS_CRS, iYaw); +} + +void sPort_send_GPS_SPD(int uart) +{ + /* get a local copy of the global position data */ + memset(&global_pos, 0, sizeof(global_pos)); + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); + + /* send 100 * knots */ + float speed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e); + uint32_t ispeed = (int)(1944 * speed); + sPort_send_data(uart, SMARTPORT_ID_GPS_SPD, ispeed); +} diff --git a/src/drivers/frsky_telemetry/sPort_data.h b/src/drivers/frsky_telemetry/sPort_data.h index f3e49e8c7d..cd3f7e18a3 100644 --- a/src/drivers/frsky_telemetry/sPort_data.h +++ b/src/drivers/frsky_telemetry/sPort_data.h @@ -51,6 +51,7 @@ #define SMARTPORT_POLL_4 0x16 #define SMARTPORT_POLL_5 0xB7 #define SMARTPORT_POLL_6 0x00 +#define SMARTPORT_POLL_7 0x83 /* FrSky SmartPort sensor IDs */ #define SMARTPORT_ID_RSSI 0xf101 @@ -84,7 +85,11 @@ void sPort_send_CUR(int uart); void sPort_send_ALT(int uart); void sPort_send_SPD(int uart); void sPort_send_VSPD(int uart, float speed); - +void sPort_send_GPS_LON(int uart); +void sPort_send_GPS_LAT(int uart); +void sPort_send_GPS_ALT(int uart); +void sPort_send_GPS_COG(int uart); +void sPort_send_GPS_SPD(int uart); void sPort_send_FUEL(int uart); #endif /* _SPORT_TELEMETRY_H */