/* * Authors (alphabetical order) * - Andre Bernet * - Andreas Weitl * - Bertrand Songis * - Bryan J. Rentoul (Gruvin) * - Cameron Weeks * - Erez Raviv * - Gabriel Birkus * - Jean-Pierre Parisy * - Karl Szmutny * - Michael Blandford * - Michal Hlavinka * - Pat Mackenzie * - Philip Moss * - Rob Thomson * - Romolo Manfredini * - Thomas Husterer * * opentx is based on code named * gruvin9x by Bryan J. Rentoul: http://code.google.com/p/gruvin9x/, * er9x by Erez Raviv: http://code.google.com/p/er9x/, * and the original (and ongoing) project by * Thomas Husterer, th9x: http://code.google.com/p/th9x/ * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License version 2 as * published by the Free Software Foundation. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * */ #include "../opentx.h" // FrSky PRIM IDs (1 byte) #define DATA_FRAME 0x10 // FrSky old DATA IDs (1 byte) #define GPS_ALT_BP_ID 0x01 #define TEMP1_ID 0x02 #define RPM_ID 0x03 #define FUEL_ID 0x04 #define TEMP2_ID 0x05 #define VOLTS_ID 0x06 #define GPS_ALT_AP_ID 0x09 #define BARO_ALT_BP_ID 0x10 #define GPS_SPEED_BP_ID 0x11 #define GPS_LONG_BP_ID 0x12 #define GPS_LAT_BP_ID 0x13 #define GPS_COURS_BP_ID 0x14 #define GPS_DAY_MONTH_ID 0x15 #define GPS_YEAR_ID 0x16 #define GPS_HOUR_MIN_ID 0x17 #define GPS_SEC_ID 0x18 #define GPS_SPEED_AP_ID 0x19 #define GPS_LONG_AP_ID 0x1A #define GPS_LAT_AP_ID 0x1B #define GPS_COURS_AP_ID 0x1C #define BARO_ALT_AP_ID 0x21 #define GPS_LONG_EW_ID 0x22 #define GPS_LAT_NS_ID 0x23 #define ACCEL_X_ID 0x24 #define ACCEL_Y_ID 0x25 #define ACCEL_Z_ID 0x26 #define CURRENT_ID 0x28 #define VARIO_ID 0x30 #define VFAS_ID 0x39 #define VOLTS_BP_ID 0x3A #define VOLTS_AP_ID 0x3B #define FRSKY_LAST_ID 0x3F // FrSky new DATA IDs (2 bytes) #define ALT_FIRST_ID 0x0100 #define ALT_LAST_ID 0x010f #define VARIO_FIRST_ID 0x0110 #define VARIO_LAST_ID 0x011f #define CURR_FIRST_ID 0x0200 #define CURR_LAST_ID 0x020f #define VFAS_FIRST_ID 0x0210 #define VFAS_LAST_ID 0x021f #define CELLS_FIRST_ID 0x0300 #define CELLS_LAST_ID 0x030f #define T1_FIRST_ID 0x0400 #define T1_LAST_ID 0x040f #define T2_FIRST_ID 0x0410 #define T2_LAST_ID 0x041f #define RPM_FIRST_ID 0x0500 #define RPM_LAST_ID 0x050f #define FUEL_FIRST_ID 0x0600 #define FUEL_LAST_ID 0x060f #define ACCX_FIRST_ID 0x0700 #define ACCX_LAST_ID 0x070f #define ACCY_FIRST_ID 0x0710 #define ACCY_LAST_ID 0x071f #define ACCZ_FIRST_ID 0x0720 #define ACCZ_LAST_ID 0x072f #define GPS_LONG_LATI_FIRST_ID 0x0800 #define GPS_LONG_LATI_LAST_ID 0x080f #define GPS_ALT_FIRST_ID 0x0820 #define GPS_ALT_LAST_ID 0x082f #define GPS_SPEED_FIRST_ID 0x0830 #define GPS_SPEED_LAST_ID 0x083f #define GPS_COURS_FIRST_ID 0x0840 #define GPS_COURS_LAST_ID 0x084f #define GPS_TIME_DATE_FIRST_ID 0x0850 #define GPS_TIME_DATE_LAST_ID 0x085f #define A3_FIRST_ID 0x0900 #define A3_LAST_ID 0x090f #define A4_FIRST_ID 0x0910 #define A4_LAST_ID 0x091f #define AIR_SPEED_FIRST_ID 0x0a00 #define AIR_SPEED_LAST_ID 0x0a0f #define RSSI_ID 0xf101 #define ADC1_ID 0xf102 #define ADC2_ID 0xf103 #define BATT_ID 0xf104 #define SWR_ID 0xf105 void setBaroAltitude(int32_t baroAltitude) { // First received barometer altitude => Altitude offset if (!frskyData.hub.baroAltitudeOffset) frskyData.hub.baroAltitudeOffset = -baroAltitude; baroAltitude += frskyData.hub.baroAltitudeOffset; frskyData.hub.baroAltitude = baroAltitude; baroAltitude /= 100; if (baroAltitude > frskyData.hub.maxAltitude) frskyData.hub.maxAltitude = baroAltitude; if (baroAltitude < frskyData.hub.minAltitude) frskyData.hub.minAltitude = baroAltitude; } void processHubPacket(uint8_t id, uint16_t value) { if (id > FRSKY_LAST_ID) return; if (id == GPS_LAT_BP_ID) { if (value) frskyData.hub.gpsFix = 1; else if (frskyData.hub.gpsFix > 0 && frskyData.hub.gpsLatitude_bp > 1) frskyData.hub.gpsFix = 0; } else if (id == GPS_LONG_BP_ID) { if (value) frskyData.hub.gpsFix = 1; else if (frskyData.hub.gpsFix > 0 && frskyData.hub.gpsLongitude_bp > 1) frskyData.hub.gpsFix = 0; } if (id == GPS_ALT_BP_ID || (id >= GPS_ALT_AP_ID && id <= GPS_LAT_NS_ID && id != BARO_ALT_BP_ID && id != BARO_ALT_AP_ID)) { // if we don't have a fix, we may discard the value if (frskyData.hub.gpsFix <= 0) return; } ((uint16_t*)&frskyData.hub)[id] = value; switch (id) { case RPM_ID: frskyData.hub.rpm *= (uint8_t)60/(g_model.frsky.blades+2); if (frskyData.hub.rpm > frskyData.hub.maxRpm) frskyData.hub.maxRpm = frskyData.hub.rpm; break; case TEMP1_ID: if (frskyData.hub.temperature1 > frskyData.hub.maxTemperature1) frskyData.hub.maxTemperature1 = frskyData.hub.temperature1; break; case TEMP2_ID: if (frskyData.hub.temperature2 > frskyData.hub.maxTemperature2) frskyData.hub.maxTemperature2 = frskyData.hub.temperature2; break; case CURRENT_ID: if ((int16_t)frskyData.hub.current > 0 && ((int16_t)frskyData.hub.current + g_model.frsky.fasOffset) > 0) frskyData.hub.current += g_model.frsky.fasOffset; else frskyData.hub.current = 0; if (frskyData.hub.current > frskyData.hub.maxCurrent) frskyData.hub.maxCurrent = frskyData.hub.current; break; case VOLTS_AP_ID: #if defined(FAS_BSS) frskyData.hub.vfas = (frskyData.hub.volts_bp * 10 + frskyData.hub.volts_ap); #else frskyData.hub.vfas = ((frskyData.hub.volts_bp * 100 + frskyData.hub.volts_ap * 10) * 21) / 110; #endif if (!frskyData.hub.minVfas || frskyData.hub.vfas < frskyData.hub.minVfas) frskyData.hub.minVfas = frskyData.hub.vfas; break; case BARO_ALT_AP_ID: if (frskyData.hub.baroAltitude_ap > 9) frskyData.hub.varioHighPrecision = true; if (!frskyData.hub.varioHighPrecision) frskyData.hub.baroAltitude_ap *= 10; setBaroAltitude((int32_t)100 * frskyData.hub.baroAltitude_bp + (frskyData.hub.baroAltitude_bp >= 0 ? frskyData.hub.baroAltitude_ap : -frskyData.hub.baroAltitude_ap)); break; case GPS_ALT_AP_ID: { frskyData.hub.gpsAltitude = (frskyData.hub.gpsAltitude_bp * 100) + frskyData.hub.gpsAltitude_ap; if (!frskyData.hub.gpsAltitudeOffset) frskyData.hub.gpsAltitudeOffset = -frskyData.hub.gpsAltitude; if (!frskyData.hub.baroAltitudeOffset) { int altitude = TELEMETRY_RELATIVE_GPS_ALT_BP; if (altitude > frskyData.hub.maxAltitude) frskyData.hub.maxAltitude = altitude; if (altitude < frskyData.hub.minAltitude) frskyData.hub.minAltitude = altitude; } if (!frskyData.hub.pilotLatitude && !frskyData.hub.pilotLongitude) { // First received GPS position => Pilot GPS position getGpsPilotPosition(); } else if (frskyData.hub.gpsDistNeeded || g_menuStack[g_menuStackPtr] == menuTelemetryFrsky) { getGpsDistance(); } break; } case GPS_SPEED_BP_ID: // Speed => Max speed frskyData.hub.gpsSpeed_bp = frskyData.hub.gpsSpeed_bp; if (frskyData.hub.gpsSpeed_bp > frskyData.hub.maxGpsSpeed) frskyData.hub.maxGpsSpeed = frskyData.hub.gpsSpeed_bp; break; case VOLTS_ID: { // Voltage => Cell number + Cell voltage uint8_t battnumber = ((frskyData.hub.volts & 0x00F0) >> 4); if (battnumber < 12) { if (frskyData.hub.cellsCount < battnumber+1) { frskyData.hub.cellsCount = battnumber+1; } uint16_t cellVolts = (uint16_t)(((((frskyData.hub.volts & 0xFF00) >> 8) + ((frskyData.hub.volts & 0x000F) << 8))) / 5); frskyData.hub.cellVolts[battnumber] = cellVolts; if (!frskyData.hub.minCellVolts || cellVolts> 8; //0-100 crc &= 0x00ff; crc += crc >> 8; //0-0FF crc &= 0x00ff; } return (crc == 0x00ff); } #define SPORT_DATA_U8(packet) (packet[4]) #define SPORT_DATA_S32(packet) (*((int32_t *)(packet+4))) #define SPORT_DATA_U32(packet) (*((uint32_t *)(packet+4))) #define HUB_DATA_U16(packet) (*((uint16_t *)(packet+4))) void frskySportProcessPacket(uint8_t *packet) { /* uint8_t dataId = packet[0]; */ uint8_t prim = packet[1]; uint16_t appId = *((uint16_t *)(packet+2)); if (!checkSportPacket(packet)) { return; } switch (prim) { case DATA_FRAME: if (appId == RSSI_ID) { frskyStreaming = FRSKY_TIMEOUT10ms; // reset counter only if valid frsky packets are being detected link_counter += 256 / FRSKY_SPORT_AVERAGING; frskyData.rssi[0].set(SPORT_DATA_U8(packet)); } if (appId == SWR_ID) { frskyData.swr.set(SPORT_DATA_U8(packet)); } else if (appId == ADC1_ID || appId == ADC2_ID) { // A1/A2 of DxR receivers uint8_t idx = appId - ADC1_ID; frskyData.analog[idx].set(SPORT_DATA_U8(packet), g_model.frsky.channels[idx].type); #if defined(VARIO) uint8_t varioSource = g_model.frsky.varioSource - VARIO_SOURCE_A1; if (varioSource == appId-ADC1_ID) { frskyData.hub.varioSpeed = applyChannelRatio(varioSource, frskyData.analog[varioSource].value); } #endif } else if (appId == BATT_ID) { frskyData.analog[TELEM_ANA_RxBatt].set((SPORT_DATA_U32(packet)*132/255)/5, UNIT_VOLTS); } else if ((appId >> 8) == 0) { // The old FrSky IDs uint8_t id = (uint8_t)appId; uint16_t value = HUB_DATA_U16(packet); processHubPacket(id, value); } else if (appId >= T1_FIRST_ID && appId <= T1_LAST_ID) { frskyData.hub.temperature1 = SPORT_DATA_S32(packet); if (frskyData.hub.temperature1 > frskyData.hub.maxTemperature1) frskyData.hub.maxTemperature1 = frskyData.hub.temperature1; } else if (appId >= T2_FIRST_ID && appId <= T2_LAST_ID) { frskyData.hub.temperature2 = SPORT_DATA_S32(packet); if (frskyData.hub.temperature2 > frskyData.hub.maxTemperature2) frskyData.hub.maxTemperature2 = frskyData.hub.temperature2; } else if (appId >= RPM_FIRST_ID && appId <= RPM_LAST_ID) { frskyData.hub.rpm = SPORT_DATA_U32(packet) / (g_model.frsky.blades+2); if (frskyData.hub.rpm > frskyData.hub.maxRpm) frskyData.hub.maxRpm = frskyData.hub.rpm; } else if (appId >= FUEL_FIRST_ID && appId <= FUEL_LAST_ID) { frskyData.hub.fuelLevel = SPORT_DATA_U32(packet); } else if (appId >= ALT_FIRST_ID && appId <= ALT_LAST_ID) { setBaroAltitude(SPORT_DATA_S32(packet)); } else if (appId >= VARIO_FIRST_ID && appId <= VARIO_LAST_ID) { frskyData.hub.varioSpeed = SPORT_DATA_S32(packet); } else if (appId >= ACCX_FIRST_ID && appId <= ACCX_LAST_ID) { frskyData.hub.accelX = SPORT_DATA_S32(packet); } else if (appId >= ACCY_FIRST_ID && appId <= ACCY_LAST_ID) { frskyData.hub.accelY = SPORT_DATA_S32(packet); } else if (appId >= ACCZ_FIRST_ID && appId <= ACCZ_LAST_ID) { frskyData.hub.accelZ = SPORT_DATA_S32(packet); } else if (appId >= CURR_FIRST_ID && appId <= CURR_LAST_ID) { frskyData.hub.current = SPORT_DATA_U32(packet); if (((int16_t)frskyData.hub.current + g_model.frsky.fasOffset)>0) frskyData.hub.current += g_model.frsky.fasOffset; else frskyData.hub.current = 0; if (frskyData.hub.current > frskyData.hub.maxCurrent) frskyData.hub.maxCurrent = frskyData.hub.current; } else if (appId >= VFAS_FIRST_ID && appId <= VFAS_LAST_ID) { frskyData.hub.vfas = SPORT_DATA_U32(packet)/10; //TODO: remove /10 and display with PREC2 when using SPORT } else if (appId >= AIR_SPEED_FIRST_ID && appId <= AIR_SPEED_LAST_ID) { frskyData.hub.airSpeed = SPORT_DATA_U32(packet); frskyData.hub.airSpeed /= 10; if (frskyData.hub.airSpeed > frskyData.hub.maxAirSpeed) frskyData.hub.maxAirSpeed = frskyData.hub.airSpeed; } else if (appId >= GPS_SPEED_FIRST_ID && appId <= GPS_SPEED_LAST_ID) { frskyData.hub.gpsSpeed_bp = SPORT_DATA_U32(packet); frskyData.hub.gpsSpeed_bp /= 1000; if (frskyData.hub.gpsSpeed_bp > frskyData.hub.maxGpsSpeed) frskyData.hub.maxGpsSpeed = frskyData.hub.gpsSpeed_bp; } else if (appId >= GPS_TIME_DATE_FIRST_ID && appId <= GPS_TIME_DATE_LAST_ID) { uint32_t gps_time_date = SPORT_DATA_U32(packet); if (gps_time_date & 0x000000ff) { frskyData.hub.year = (uint16_t) ((gps_time_date & 0xff000000) >> 24); frskyData.hub.month = (uint8_t) ((gps_time_date & 0x00ff0000) >> 16); frskyData.hub.day = (uint8_t) ((gps_time_date & 0x0000ff00) >> 8); } else { frskyData.hub.hour = (uint8_t) ((gps_time_date & 0xff000000) >> 24); frskyData.hub.min = (uint8_t) ((gps_time_date & 0x00ff0000) >> 16); frskyData.hub.sec = (uint16_t) ((gps_time_date & 0x0000ff00) >> 8); frskyData.hub.hour = ((uint8_t) (frskyData.hub.hour + g_eeGeneral.timezone + 24)) % 24; } } else if (appId >= GPS_COURS_FIRST_ID && appId <= GPS_COURS_LAST_ID) { uint32_t course = SPORT_DATA_U32(packet); frskyData.hub.gpsCourse_bp = course / 100; frskyData.hub.gpsCourse_ap = course % 100; } else if (appId >= GPS_ALT_FIRST_ID && appId <= GPS_ALT_LAST_ID) { frskyData.hub.gpsAltitude = SPORT_DATA_S32(packet); if (!frskyData.hub.gpsAltitudeOffset) frskyData.hub.gpsAltitudeOffset = -frskyData.hub.gpsAltitude; if (!frskyData.hub.baroAltitudeOffset) { int altitude = TELEMETRY_RELATIVE_GPS_ALT_BP; if (altitude > frskyData.hub.maxAltitude) frskyData.hub.maxAltitude = altitude; if (altitude < frskyData.hub.minAltitude) frskyData.hub.minAltitude = altitude; } if (frskyData.hub.gpsFix > 0) { if (!frskyData.hub.pilotLatitude && !frskyData.hub.pilotLongitude) { // First received GPS position => Pilot GPS position getGpsPilotPosition(); } else if (frskyData.hub.gpsDistNeeded || g_menuStack[g_menuStackPtr] == menuTelemetryFrsky) { getGpsDistance(); } } } else if (appId >= GPS_LONG_LATI_FIRST_ID && appId <= GPS_LONG_LATI_LAST_ID) { uint32_t gps_long_lati_data = SPORT_DATA_U32(packet); uint32_t gps_long_lati_b1w, gps_long_lati_a1w; gps_long_lati_b1w = (gps_long_lati_data & 0x3fffffff) / 10000; gps_long_lati_a1w = (gps_long_lati_data & 0x3fffffff) % 10000; switch ((gps_long_lati_data & 0xc0000000) >> 30) { case 0: frskyData.hub.gpsLatitude_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60); frskyData.hub.gpsLatitude_ap = gps_long_lati_a1w; frskyData.hub.gpsLatitudeNS = 'N'; break; case 1: frskyData.hub.gpsLatitude_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60); frskyData.hub.gpsLatitude_ap = gps_long_lati_a1w; frskyData.hub.gpsLatitudeNS = 'S'; break; case 2: frskyData.hub.gpsLongitude_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60); frskyData.hub.gpsLongitude_ap = gps_long_lati_a1w; frskyData.hub.gpsLongitudeEW = 'E'; break; case 3: frskyData.hub.gpsLongitude_bp = (gps_long_lati_b1w / 60 * 100) + (gps_long_lati_b1w % 60); frskyData.hub.gpsLongitude_ap = gps_long_lati_a1w; frskyData.hub.gpsLongitudeEW = 'W'; break; } if (frskyData.hub.gpsLongitudeEW && frskyData.hub.gpsLatitudeNS) { frskyData.hub.gpsFix = 1; } else if (frskyData.hub.gpsFix > 0) { frskyData.hub.gpsFix = 0; } } else if (appId >= A3_FIRST_ID && appId <= A3_LAST_ID) { frskyData.analog[TELEM_ANA_A3].set(SPORT_DATA_U32(packet), UNIT_VOLTS); } else if (appId >= A4_FIRST_ID && appId <= A4_LAST_ID) { frskyData.analog[TELEM_ANA_A4].set(SPORT_DATA_U32(packet), UNIT_VOLTS); } else if (appId >= CELLS_FIRST_ID && appId <= CELLS_LAST_ID) { uint32_t cells = SPORT_DATA_U32(packet); uint8_t battnumber = cells & 0xF; uint32_t minCell, minCellNum; //TODO: Use reported total voltages (bits 4-7)? frskyData.hub.cellVolts[battnumber] = ((cells & 0x000FFF00) >> 8) / 5; frskyData.hub.cellVolts[battnumber+1] = ((cells & 0xFFF00000) >> 20) / 5; if (frskyData.hub.cellsCount < battnumber+2) frskyData.hub.cellsCount = battnumber+2; if (frskyData.hub.cellVolts[battnumber+1] == 0) frskyData.hub.cellsCount--; if ((frskyData.hub.cellVolts[battnumber] < frskyData.hub.cellVolts[battnumber+1]) || (frskyData.hub.cellVolts[battnumber+1] == 0)) { minCell = frskyData.hub.cellVolts[battnumber]; minCellNum = battnumber; } else { minCell = frskyData.hub.cellVolts[battnumber+1]; minCellNum = battnumber+1; } if (!frskyData.hub.minCellVolts || minCell < frskyData.hub.minCellVolts || minCellNum==frskyData.hub.minCellIdx) { frskyData.hub.minCellIdx = minCellNum; frskyData.hub.minCellVolts = minCell; } } break; } }