diff --git a/src/drivers/frsky_telemetry/CMakeLists.txt b/src/drivers/frsky_telemetry/CMakeLists.txt index 1273790c88..ceda3d347a 100644 --- a/src/drivers/frsky_telemetry/CMakeLists.txt +++ b/src/drivers/frsky_telemetry/CMakeLists.txt @@ -43,4 +43,4 @@ px4_add_module( DEPENDS platforms__common ) -# vim: set noet ft=cmake fenc=utf-8 ff=unix : +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index 287e80f8c0..33607f0381 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -53,6 +53,7 @@ #include #include #include +#include #include @@ -93,10 +94,12 @@ static struct battery_status_s *battery_status; static struct vehicle_global_position_s *global_pos; +static struct vehicle_status_s *vehicle_status; static struct sensor_combined_s *sensor_combined; static int battery_status_sub = -1; -static int global_position_sub = -1; +static int vehicle_global_position_sub = -1; +static int vehicle_status_sub = -1; static int sensor_sub = -1; /** @@ -107,13 +110,15 @@ bool frsky_init() battery_status = malloc(sizeof(struct battery_status_s)); global_pos = malloc(sizeof(struct vehicle_global_position_s)); sensor_combined = malloc(sizeof(struct sensor_combined_s)); + vehicle_status = malloc(sizeof(struct vehicle_status_s)); - if (battery_status == NULL || global_pos == NULL || sensor_combined == NULL) { + if (battery_status == NULL || global_pos == NULL || sensor_combined == NULL || vehicle_status) { return false; } battery_status_sub = orb_subscribe(ORB_ID(battery_status)); - global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); return true; } @@ -123,6 +128,7 @@ void frsky_deinit() free(battery_status); free(global_pos); free(sensor_combined); + free(vehicle_status); } /** @@ -175,7 +181,6 @@ static void frsky_send_data(int uart, uint8_t id, int16_t data) void frsky_update_topics() { bool updated; - /* get a local copy of the current sensor values */ orb_check(sensor_sub, &updated); @@ -191,10 +196,17 @@ void frsky_update_topics() } /* get a local copy of the global position data */ - orb_check(global_position_sub, &updated); + orb_check(vehicle_global_position_sub, &updated); if (updated) { - orb_copy(ORB_ID(vehicle_global_position), global_position_sub, global_pos); + orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, global_pos); + } + + /* get a local copy of the vehicle status data */ + orb_check(vehicle_status_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, vehicle_status); } } @@ -374,4 +386,3 @@ bool frsky_parse_host(uint8_t *sbuf, int nbytes, struct adc_linkquality *v) return data_ready; } - diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c index 3cac256ddd..01bbd41b9e 100644 --- a/src/drivers/frsky_telemetry/frsky_telemetry.c +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -283,6 +283,8 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) // allow a minimum of 500usec before reply usleep(500); + sPort_update_topics(); + static hrt_abstime lastBATV = 0; static hrt_abstime lastCUR = 0; static hrt_abstime lastALT = 0; @@ -290,6 +292,9 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) static hrt_abstime lastFUEL = 0; static hrt_abstime lastVSPD = 0; static hrt_abstime lastGPS = 0; + static hrt_abstime lastNAV_STATE = 0; + static hrt_abstime lastGPS_FIX = 0; + switch (sbuf[1]) { @@ -370,7 +375,7 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) case SMARTPORT_POLL_7: - /* report GPS data elements at 2*5Hz */ + /* report GPS data elements at 5*5Hz */ if (now - lastGPS > 100 * 1000) { static int elementCount = 0; @@ -387,7 +392,7 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) break; case 2: - sPort_send_GPS_COG(uart); + sPort_send_GPS_CRS(uart); elementCount++; break; @@ -398,12 +403,33 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) case 4: sPort_send_GPS_SPD(uart); + elementCount++; + break; + + case 5: + sPort_send_GPS_TIME(uart); elementCount = 0; break; } } + case SMARTPORT_POLL_8: + + /* report nav_state as DIY_NAVSTATE at 1Hz */ + if (now - lastNAV_STATE > 1000 * 1000) { + lastNAV_STATE = now; + /* send T1 */ + sPort_send_NAV_STATE(uart); + } + + /* report satcount and fix as DIY_GPSFIX at 1Hz */ + else if (now - lastGPS_FIX > 1000 * 1000) { + lastGPS_FIX = now; + /* send T2 */ + sPort_send_GPS_FIX(uart); + } + break; } } diff --git a/src/drivers/frsky_telemetry/sPort_data.c b/src/drivers/frsky_telemetry/sPort_data.c index ae34e1577d..c266b84924 100644 --- a/src/drivers/frsky_telemetry/sPort_data.c +++ b/src/drivers/frsky_telemetry/sPort_data.c @@ -49,9 +49,11 @@ #include #include +#include #include #include -#include +#include +#include #include @@ -60,36 +62,93 @@ static int sensor_sub = -1; static int global_position_sub = -1; static int battery_status_sub = -1; -static struct sensor_combined_s *sensor_data; +static int vehicle_status_sub = -1; +static int gps_position_sub = -1; + +static struct sensor_combined_s *sensor_combined; static struct vehicle_global_position_s *global_pos; static struct battery_status_s *battery_status; +static struct vehicle_status_s *vehicle_status; +static struct vehicle_gps_position_s *gps_position; + /** * Initializes the uORB subscriptions. */ bool sPort_init() { - sensor_data = malloc(sizeof(struct sensor_combined_s)); + + sensor_combined = malloc(sizeof(struct sensor_combined_s)); global_pos = malloc(sizeof(struct vehicle_global_position_s)); battery_status = malloc(sizeof(struct battery_status_s)); + vehicle_status = malloc(sizeof(struct vehicle_status_s)); + gps_position = malloc(sizeof(struct vehicle_gps_position_s)); - if (sensor_data == NULL || global_pos == NULL || battery_status == NULL) { + + if (sensor_combined == NULL || global_pos == NULL || battery_status == NULL || vehicle_status == NULL + || gps_position == NULL) { return false; } + sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); battery_status_sub = orb_subscribe(ORB_ID(battery_status)); + vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + return true; } void sPort_deinit() { - free(sensor_data); + free(sensor_combined); free(global_pos); free(battery_status); + free(vehicle_status); + free(gps_position); } +void sPort_update_topics() +{ + bool updated; + /* get a local copy of the current sensor values */ + orb_check(sensor_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(sensor_combined), sensor_sub, sensor_combined); + } + + /* get a local copy of the battery data */ + orb_check(battery_status_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(battery_status), battery_status_sub, battery_status); + } + + /* get a local copy of the global position data */ + orb_check(global_position_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, global_pos); + } + + /* get a local copy of the vehicle status data */ + orb_check(vehicle_status_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, vehicle_status); + } + + /* get a local copy of the gps position data */ + orb_check(gps_position_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_gps_position), gps_position_sub, gps_position); + } +} + + static void update_crc(uint16_t *crc, unsigned char b) { *crc += b; @@ -158,24 +217,16 @@ void sPort_send_data(int uart, uint16_t id, uint32_t data) // scaling correct with OpenTX 2.1.7 void sPort_send_BATV(int uart) { - /* get a local copy of the battery data */ - orb_copy(ORB_ID(battery_status), battery_status_sub, battery_status); - /* send battery voltage as VFAS */ uint32_t voltage = (int)(100 * battery_status->voltage_v); - sPort_send_data(uart, SMARTPORT_ID_VFAS, voltage); } // verified scaling void sPort_send_CUR(int uart) { - /* get a local copy of the battery data */ - orb_copy(ORB_ID(battery_status), battery_status_sub, battery_status); - /* send data */ uint32_t current = (int)(10 * battery_status->current_a); - sPort_send_data(uart, SMARTPORT_ID_CURR, current); } @@ -184,18 +235,15 @@ void sPort_send_CUR(int uart) // the difference (altitude - field) void sPort_send_ALT(int uart) { - /* get a local copy of the current sensor values */ - orb_copy(ORB_ID(sensor_combined), sensor_sub, sensor_data); - /* send data */ - uint32_t alt = (int)(100 * sensor_data->baro_alt_meter[0]); + uint32_t alt = (int)(100 * sensor_combined->baro_alt_meter[0]); sPort_send_data(uart, SMARTPORT_ID_ALT, alt); } // verified scaling for "calculated" option void sPort_send_SPD(int uart) { - /* send data */ + /* send data for A2 */ 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); @@ -212,27 +260,20 @@ void sPort_send_VSPD(int uart, float speed) // verified scaling void sPort_send_FUEL(int uart) { - /* get a local copy of the battery data */ - orb_copy(ORB_ID(battery_status), battery_status_sub, battery_status); - /* send data */ uint32_t fuel = (int)(100 * battery_status->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 */ - 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); + uint32_t iLon = 6E5 * fabs(gps_position->lon); iLon |= (1 << 31); - if (global_pos->lon < 0) { iLon |= (1 << 30); } + if (gps_position->lon < 0) { iLon |= (1 << 30); } sPort_send_data(uart, SMARTPORT_ID_GPS_LON_LAT, iLon); } @@ -240,11 +281,10 @@ void sPort_send_GPS_LON(int uart) void sPort_send_GPS_LAT(int uart) { /* send latitude */ - /* convert to 30 bit signed magnitude degrees*6E5 with MSb = 0 and bit 30=sign */ - uint32_t iLat = 6E5 * fabs(global_pos->lat); + uint32_t iLat = 6E5 * fabs(gps_position->lat); - if (global_pos->lat < 0) { iLat |= (1 << 30); } + if (gps_position->lat < 0) { iLat |= (1 << 30); } sPort_send_data(uart, SMARTPORT_ID_GPS_LON_LAT, iLat); } @@ -252,13 +292,12 @@ void sPort_send_GPS_LAT(int uart) void sPort_send_GPS_ALT(int uart) { /* send altitude */ - /* convert to 100 * m/sec */ - uint32_t iAlt = 100 * global_pos->alt; + uint32_t iAlt = 100 * gps_position->alt; sPort_send_data(uart, SMARTPORT_ID_GPS_ALT, iAlt); } -void sPort_send_GPS_COG(int uart) +void sPort_send_GPS_CRS(int uart) { /* send course */ @@ -267,6 +306,11 @@ void sPort_send_GPS_COG(int uart) sPort_send_data(uart, SMARTPORT_ID_GPS_CRS, iYaw); } +void sPort_send_GPS_TIME(int uart) +{ + sPort_send_data(uart, SMARTPORT_ID_GPS_TIME, gps_position->time_utc_usec); +} + void sPort_send_GPS_SPD(int uart) { /* send 100 * knots */ @@ -274,3 +318,25 @@ void sPort_send_GPS_SPD(int uart) uint32_t ispeed = (int)(1944 * speed); sPort_send_data(uart, SMARTPORT_ID_GPS_SPD, ispeed); } + +/* + * Sends nav_state + 128 + */ +void sPort_send_NAV_STATE(int uart) +{ + uint32_t navstate = (int)(128 + vehicle_status->nav_state); + + /* send data */ + sPort_send_data(uart, SMARTPORT_ID_DIY_NAVSTATE, navstate); +} + +// verified scaling +// sends number of sats and type of gps fix +void sPort_send_GPS_FIX(int uart) +{ + /* send data */ + uint32_t satcount = (int)(gps_position->satellites_used); + uint32_t fixtype = (int)(gps_position->fix_type); + uint32_t t2 = satcount * 10 + fixtype; + sPort_send_data(uart, SMARTPORT_ID_DIY_GPSFIX, t2); +} diff --git a/src/drivers/frsky_telemetry/sPort_data.h b/src/drivers/frsky_telemetry/sPort_data.h index 67e7bd41d5..eb6b009969 100644 --- a/src/drivers/frsky_telemetry/sPort_data.h +++ b/src/drivers/frsky_telemetry/sPort_data.h @@ -53,45 +53,55 @@ #define SMARTPORT_POLL_5 0xB7 #define SMARTPORT_POLL_6 0x00 #define SMARTPORT_POLL_7 0x83 +#define SMARTPORT_POLL_8 0xBA -/* FrSky SmartPort sensor IDs */ +/* FrSky SmartPort sensor IDs. See more here: https://github.com/opentx/opentx/blob/master/radio/src/telemetry/frsky.h#L109 */ #define SMARTPORT_ID_RSSI 0xf101 #define SMARTPORT_ID_RXA1 0xf102 // supplied by RX #define SMARTPORT_ID_RXA2 0xf103 // supplied by RX #define SMARTPORT_ID_BATV 0xf104 -#define SMARTPORT_ID_SWR 0xf105 +#define SMARTPORT_ID_SWR 0xf105 // Standing Wave Ratio #define SMARTPORT_ID_T1 0x0400 #define SMARTPORT_ID_T2 0x0410 #define SMARTPORT_ID_RPM 0x0500 #define SMARTPORT_ID_FUEL 0x0600 #define SMARTPORT_ID_ALT 0x0100 -#define SMARTPORT_ID_VARIO 0x0110 +#define SMARTPORT_ID_VARIO 0x0110 //VSPEED #define SMARTPORT_ID_ACCX 0x0700 #define SMARTPORT_ID_ACCY 0x0710 #define SMARTPORT_ID_ACCZ 0x0720 #define SMARTPORT_ID_CURR 0x0200 -#define SMARTPORT_ID_VFAS 0x0210 +#define SMARTPORT_ID_VFAS 0x0210 //Volt per Cell #define SMARTPORT_ID_CELLS 0x0300 #define SMARTPORT_ID_GPS_LON_LAT 0x0800 #define SMARTPORT_ID_GPS_ALT 0x0820 #define SMARTPORT_ID_GPS_SPD 0x0830 #define SMARTPORT_ID_GPS_CRS 0x0840 #define SMARTPORT_ID_GPS_TIME 0x0850 +#define SMARTPORT_ID_DIY_FIRST 0x5000 +#define SMARTPORT_ID_DIY_LAST 0x50ff //We have 256 possible ID's for custom values :) +#define SMARTPORT_ID_DIY_NAVSTATE 0x5000 +#define SMARTPORT_ID_DIY_GPSFIX 0x5001 // Public functions bool sPort_init(void); void sPort_deinit(void); +void sPort_update_topics(void); void sPort_send_data(int uart, uint16_t id, uint32_t data); void sPort_send_BATV(int uart); 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_FUEL(int uart); 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); +void sPort_send_GPS_CRS(int uart); +void sPort_send_GPS_TIME(int uart); + +void sPort_send_NAV_STATE(int uart); +void sPort_send_GPS_FIX(int uart); #endif /* _SPORT_TELEMETRY_H */