From 9b84516344539115f2f983f37cedcc6a5005c321 Mon Sep 17 00:00:00 2001 From: Airmamaf Date: Wed, 8 Oct 2014 16:28:26 +0200 Subject: [PATCH] FrSky telemetry update --- src/main/config/config.c | 6 ++ src/main/io/gps.c | 10 ++- src/main/io/gps.h | 5 ++ src/main/io/serial_cli.c | 6 ++ src/main/telemetry/frsky.c | 123 ++++++++++++++++++++++++++++++--- src/main/telemetry/telemetry.h | 14 ++++ 6 files changed, 151 insertions(+), 13 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index ffe3afa76..5cc4d43d0 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -201,6 +201,11 @@ void resetTelemetryConfig(telemetryConfig_t *telemetryConfig) telemetryConfig->telemetry_provider = TELEMETRY_PROVIDER_FRSKY; telemetryConfig->frsky_inversion = SERIAL_NOT_INVERTED; telemetryConfig->telemetry_switch = 0; + telemetryConfig->gpsNoFixLat = 0; + telemetryConfig->gpsNoFixLon = 0; + telemetryConfig->frsky_coordinate_format = FRSKY_FORMAT_DMS; + telemetryConfig->frsky_unit = FRSKY_UNIT_METRICS; + telemetryConfig->batterySize = 0; } void resetSerialConfig(serialConfig_t *serialConfig) @@ -305,6 +310,7 @@ static void resetConf(void) // gps/nav stuff masterConfig.gpsConfig.provider = GPS_NMEA; masterConfig.gpsConfig.sbasMode = SBAS_AUTO; + masterConfig.gpsConfig.gpsAutoConfig = GPS_AUTOCONFIG_ON; #endif resetSerialConfig(&masterConfig.serialConfig); diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 371eadaf1..ed2984e96 100644 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -255,7 +255,10 @@ void gpsInitUblox(void) if (gpsData.messageState == GPS_MESSAGE_STATE_INIT) { if (gpsData.state_position < sizeof(ubloxInit)) { - serialWrite(gpsPort, ubloxInit[gpsData.state_position]); + //Either use specific config file for GPS or let dynamically upload config + if( gpsConfig->gpsAutoConfig == GPS_AUTOCONFIG_ON ) { + serialWrite(gpsPort, ubloxInit[gpsData.state_position]); + } gpsData.state_position++; } else { gpsData.state_position = 0; @@ -265,7 +268,10 @@ void gpsInitUblox(void) if (gpsData.messageState == GPS_MESSAGE_STATE_SBAS) { if (gpsData.state_position < UBLOX_SBAS_MESSAGE_LENGTH) { - serialWrite(gpsPort, ubloxSbas[gpsConfig->sbasMode].message[gpsData.state_position]); + //Either use specific config file for GPS or let dynamically upload config + if( gpsConfig->gpsAutoConfig == GPS_AUTOCONFIG_ON ) { + serialWrite(gpsPort, ubloxSbas[gpsConfig->sbasMode].message[gpsData.state_position]); + } gpsData.state_position++; } else { gpsData.messageState++; diff --git a/src/main/io/gps.h b/src/main/io/gps.h index 61457cc3f..f0dd62abc 100644 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -45,11 +45,16 @@ typedef enum { GPS_BAUDRATE_9600 } gpsBaudRate_e; +typedef enum { + GPS_AUTOCONFIG_ON = 0, + GPS_AUTOCONFIG_OFF +} gpsAutoConfig_e; #define GPS_BAUDRATE_MAX GPS_BAUDRATE_9600 typedef struct gpsConfig_s { gpsProvider_e provider; sbasMode_e sbasMode; + gpsAutoConfig_e gpsAutoConfig; } gpsConfig_t; typedef enum { diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 60c597591..ca083bdf0 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -240,6 +240,7 @@ const clivalue_t valueTable[] = { { "gps_provider", VAR_UINT8 | MASTER_VALUE, &masterConfig.gpsConfig.provider, 0, GPS_PROVIDER_MAX }, { "gps_sbas_mode", VAR_UINT8 | MASTER_VALUE, &masterConfig.gpsConfig.sbasMode, 0, SBAS_MODE_MAX }, + { "gps_auto_config", VAR_UINT8 | MASTER_VALUE, &masterConfig.gpsConfig.gpsAutoConfig, 0, GPS_AUTOCONFIG_OFF }, { "gps_pos_p", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PIDPOS], 0, 200 }, @@ -263,6 +264,11 @@ const clivalue_t valueTable[] = { { "telemetry_provider", VAR_UINT8 | MASTER_VALUE, &masterConfig.telemetryConfig.telemetry_provider, 0, TELEMETRY_PROVIDER_MAX }, { "telemetry_switch", VAR_UINT8 | MASTER_VALUE, &masterConfig.telemetryConfig.telemetry_switch, 0, 1 }, { "frsky_inversion", VAR_UINT8 | MASTER_VALUE, &masterConfig.telemetryConfig.frsky_inversion, 0, 1 }, + { "frsky_default_lat", VAR_FLOAT | MASTER_VALUE, &masterConfig.telemetryConfig.gpsNoFixLat, -90.0, 90.0 }, + { "frsky_default_lon", VAR_FLOAT | MASTER_VALUE, &masterConfig.telemetryConfig.gpsNoFixLon, -180.0, 180.0 }, + { "frsky_coordinates_format", VAR_UINT8 | MASTER_VALUE, &masterConfig.telemetryConfig.frsky_coordinate_format, 0, FRSKY_FORMAT_NMEA }, + { "frsky_unit", VAR_UINT8 | MASTER_VALUE, &masterConfig.telemetryConfig.frsky_unit, 0, FRSKY_UNIT_IMPERIALS }, + { "frsky_battery_size", VAR_UINT16 | MASTER_VALUE, &masterConfig.telemetryConfig.batterySize, 0, 20000 }, { "vbat_scale", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatscale, VBAT_SCALE_MIN, VBAT_SCALE_MAX }, { "vbat_max_cell_voltage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatmaxcellvoltage, 10, 50 }, diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index 58c01aaf3..1afe3056a 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -35,6 +35,8 @@ #include "drivers/timer.h" #include "drivers/serial.h" #include "io/serial.h" +#include "rx/rx.h" +#include "io/rc_controls.h" #include "config/runtime_config.h" #include "config/config.h" @@ -100,6 +102,13 @@ extern int16_t telemTemperature1; // FIXME dependency on mw.c #define ID_VERT_SPEED 0x30 //opentx vario +#define GPS_BAD_QUALITY 300 +#define GPS_MAX_HDOP_VAL 9999 +#define DELAY_FOR_BARO_INITIALISATION (5 * 1000) //5s +#define BLADE_NUMBER_DIVIDER 5 //Should set 12 blades in Taranis + +static uint32_t lastCycleTime = 0; +static uint8_t cycleNum = 0; static void sendDataHead(uint8_t id) { serialWrite(frskyPort, PROTOCOL_HEADER); @@ -151,12 +160,68 @@ static void sendBaro(void) serialize16(abs(BaroAlt % 100)); } +static void sendGpsAltitude(void) +{ + uint16_t altitude = GPS_altitude; + //Send real GPS altitude only if it's reliable (there's a GPS fix) + if( !STATE(GPS_FIX) ) { + altitude = 0; + } + sendDataHead(ID_GPS_ALTIDUTE_BP); + serialize16(altitude); + sendDataHead(ID_GPS_ALTIDUTE_AP); + serialize16(0); +} + + +static void sendRpm(void) +{ + sendDataHead(ID_RPM); + if( ARMING_FLAG(ARMED) ) { + serialize16(rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER); + } else { + serialize16((telemetryConfig->batterySize / BLADE_NUMBER_DIVIDER)); + } + +} static void sendTemperature1(void) { sendDataHead(ID_TEMPRATURE1); + #ifdef BARO + serialize16((baroTemperature + 50)/ 100); //Airmamaf + #else serialize16(telemTemperature1 / 10); + #endif } +static void sendTemperature2(void) +{ + uint16_t satellite = GPS_numSat; + //Send instead Num sat and quality GPS + if( GPS_hdop > GPS_BAD_QUALITY && ( (cycleNum % 16 ) < 8) ) {//Every 1s + satellite = constrain(GPS_hdop, 0, GPS_MAX_HDOP_VAL); + } + sendDataHead(ID_TEMPRATURE2); + + if( telemetryConfig->frsky_unit == FRSKY_UNIT_METRICS ) { + serialize16(satellite); + } else { + float tmp = (satellite - 32) / 1.8; + //Round the value + tmp += (tmp < 0) ? -0.5 : 0.5; + serialize16(tmp); + } +} +static void sendSpeed(void) +{ + if( STATE(GPS_FIX) ) { + //Speed should be sent in m/s (GPS speed is in cm/s) + sendDataHead(ID_GPS_SPEED_BP); + serialize16((GPS_speed * 0.01 + 0.5)); + sendDataHead(ID_GPS_SPEED_AP); + serialize16(0); //Not dipslayed + } +} static void sendTime(void) { uint32_t seconds = millis() / 1000; @@ -180,28 +245,49 @@ static void GPStoDDDMM_MMMM(int32_t mwiigps, gpsCoordinateDDDMMmmmm_t *result) absgps = (absgps - deg * 10000000) * 60; // absgps = Minutes left * 10^7 min = absgps / 10000000; // minutes left - result->dddmm = deg * 100 + min; + if( telemetryConfig->frsky_coordinate_format == FRSKY_FORMAT_DMS ) { + result->dddmm = deg * 100 + min; + } else { + result->dddmm = deg * 60 + min; + } + result->mmmm = (absgps - min * 10000000) / 1000; } static void sendGPS(void) { + int32_t localGPS_coord[2] = {0,0}; + // Don't set dummy GPS data, if we already had a GPS fix + // it can be usefull to keep last valid coordinates + static uint8_t gpsFixOccured = 0; + + //Dummy data if no 3D fix, this way we can display heading in Taranis + if( STATE(GPS_FIX) || gpsFixOccured == 1 ) { + localGPS_coord[LAT] = GPS_coord[LAT]; + localGPS_coord[LON] = GPS_coord[LON]; + gpsFixOccured = 1; + } else { + // Send dummy GPS Data in order to display compass value + localGPS_coord[LAT] = (telemetryConfig->gpsNoFixLat * 10000000); + localGPS_coord[LON] = (telemetryConfig->gpsNoFixLon * 10000000); + } + gpsCoordinateDDDMMmmmm_t coordinate; - GPStoDDDMM_MMMM(GPS_coord[LAT], &coordinate); + GPStoDDDMM_MMMM(localGPS_coord[LAT], &coordinate); sendDataHead(ID_LATITUDE_BP); serialize16(coordinate.dddmm); sendDataHead(ID_LATITUDE_AP); serialize16(coordinate.mmmm); sendDataHead(ID_N_S); - serialize16(GPS_coord[LAT] < 0 ? 'S' : 'N'); + serialize16(localGPS_coord[LAT] < 0 ? 'S' : 'N'); - GPStoDDDMM_MMMM(GPS_coord[LON], &coordinate); + GPStoDDDMM_MMMM(localGPS_coord[LON], &coordinate); sendDataHead(ID_LONGITUDE_BP); serialize16(coordinate.dddmm); sendDataHead(ID_LONGITUDE_AP); serialize16(coordinate.mmmm); sendDataHead(ID_E_W); - serialize16(GPS_coord[LON] < 0 ? 'W' : 'E'); + serialize16(localGPS_coord[LON] < 0 ? 'W' : 'E'); } #endif @@ -281,8 +367,15 @@ static void sendAmperage(void) static void sendFuelLevel(void) { + uint16_t batterySize = telemetryConfig->batterySize; + sendDataHead(ID_FUEL_LEVEL); - serialize16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); + + if( batterySize > 0 ) { + serialize16((uint16_t)constrain((batterySize - constrain(mAhDrawn, 0, 0xFFFF)) * 100.0 / batterySize , 0, 100)); + } else { + serialize16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); + } } static void sendHeading(void) @@ -331,8 +424,6 @@ void configureFrSkyTelemetryPort(void) } } -static uint32_t lastCycleTime = 0; -static uint8_t cycleNum = 0; bool canSendFrSkyTelemetry(void) { @@ -366,13 +457,16 @@ void handleFrSkyTelemetry(void) sendTelemetryTail(); if ((cycleNum % 4) == 0) { // Sent every 500ms - sendBaro(); + if( lastCycleTime > DELAY_FOR_BARO_INITIALISATION ) { //Allow 5s to boot correctly + sendBaro(); + } sendHeading(); sendTelemetryTail(); } if ((cycleNum % 8) == 0) { // Sent every 1s sendTemperature1(); + sendRpm(); if (feature(FEATURE_VBAT)) { sendVoltage(); @@ -382,10 +476,17 @@ void handleFrSkyTelemetry(void) } #ifdef GPS - if (sensors(SENSOR_GPS)) - sendGPS(); + if (sensors(SENSOR_GPS)) { + sendSpeed(); + sendGpsAltitude(); + sendTemperature2(); + } #endif + // Send GPS information to display compass information + if( (telemetryConfig->gpsNoFixLat != 0 && telemetryConfig->gpsNoFixLon != 0) || sensors(SENSOR_GPS) ) { + sendGPS(); + } sendTelemetryTail(); } diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index add2dcb9a..7613bb33c 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -32,10 +32,24 @@ typedef enum { TELEMETRY_PROVIDER_MAX = TELEMETRY_PROVIDER_MSP } telemetryProvider_e; +typedef enum { + FRSKY_FORMAT_DMS = 0, + FRSKY_FORMAT_NMEA +} frskyGpsCoordFormat_e; + +typedef enum { + FRSKY_UNIT_METRICS = 0, + FRSKY_UNIT_IMPERIALS +} frskyUnit_e; typedef struct telemetryConfig_s { telemetryProvider_e telemetry_provider; uint8_t telemetry_switch; // Use aux channel to change serial output & baudrate( MSP / Telemetry ). It disables automatic switching to Telemetry when armed. serialInversion_e frsky_inversion; + float gpsNoFixLat; + float gpsNoFixLon; + frskyGpsCoordFormat_e frsky_coordinate_format; + frskyUnit_e frsky_unit; + uint16_t batterySize; } telemetryConfig_t; void checkTelemetryState(void);