piotr/edgetx-gx12
1
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

Fixes #1636

Browse Source
This commit is contained in:
bsongis
2014-08-25 11:29:08 +02:00
parent 3e31f33b64
commit 18ebb79f20
2 changed files with 186 additions and 181 deletions
Download Patch File Download Diff File Expand all files Collapse all files
radio/src/telemetry/frsky_d.cpp
+6 -3
View File
@@ -290,8 +290,9 @@ void frskyDProcessPacket(uint8_t *packet)
link_counter += 256 / FRSKY_D_AVERAGING;
#if defined(VARIO)
uint8_t varioSource = g_model.frsky.varioSource - VARIO_SOURCE_A1;
if (varioSource < 2)
if (varioSource < 2) {
frskyData.hub.varioSpeed = applyChannelRatio(varioSource, frskyData.analog[varioSource].value);
}
#endif
break;
}
@@ -300,12 +301,14 @@ void frskyDProcessPacket(uint8_t *packet)
uint8_t numBytes = 3 + (packet[1] & 0x07); // sanitize in case of data corruption leading to buffer overflow
for (uint8_t i=3; i<numBytes; i++) {
#if defined(FRSKY_HUB)
if (IS_USR_PROTO_FRSKY_HUB())
if (IS_USR_PROTO_FRSKY_HUB()) {
parseTelemHubByte(packet[i]);
}
#endif
#if defined(WS_HOW_HIGH)
if (IS_USR_PROTO_WS_HOW_HIGH())
if (IS_USR_PROTO_WS_HOW_HIGH()) {
parseTelemWSHowHighByte(packet[i]);
}
#endif
}
break;
radio/src/telemetry/frsky_sport.cpp
+180 -178
View File
@@ -301,191 +301,193 @@ void frskySportProcessPacket(uint8_t *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_A1].set(SPORT_DATA_U8(packet), 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
if (!frskyData.hub.minVfas || frskyData.hub.vfas < frskyData.hub.minVfas)
frskyData.hub.minVfas = frskyData.hub.vfas;
}
else if (appId >= AIR_SPEED_FIRST_ID && appId <= AIR_SPEED_LAST_ID) {
frskyData.hub.airSpeed = SPORT_DATA_U32(packet);
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 = (uint16_t) (SPORT_DATA_U32(packet)/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.rssi[0].value > 0) {
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);
}
else if (frskyData.hub.gpsDistNeeded || g_menuStack[g_menuStackPtr] == menuTelemetryFrsky) {
getGpsDistance();
#endif
}
else if (appId == BATT_ID) {
frskyData.analog[TELEM_ANA_A1].set(SPORT_DATA_U8(packet), 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
if (!frskyData.hub.minVfas || frskyData.hub.vfas < frskyData.hub.minVfas)
frskyData.hub.minVfas = frskyData.hub.vfas;
}
else if (appId >= AIR_SPEED_FIRST_ID && appId <= AIR_SPEED_LAST_ID) {
frskyData.hub.airSpeed = SPORT_DATA_U32(packet);
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 = (uint16_t) (SPORT_DATA_U32(packet)/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_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;
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;
}
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)*255+165)/330, UNIT_VOLTS);
}
else if (appId >= A4_FIRST_ID && appId <= A4_LAST_ID) {
frskyData.analog[TELEM_ANA_A4].set((SPORT_DATA_U32(packet)*255+165)/330, UNIT_VOLTS);
}
else if (appId >= CELLS_FIRST_ID && appId <= CELLS_LAST_ID) {
uint32_t data = SPORT_DATA_U32(packet);
uint8_t battnumber = data & 0xF;
uint8_t cells = (data & 0xF0) >> 4;
bool useSecondCell = (battnumber+1 < cells);
else if (appId >= GPS_ALT_FIRST_ID && appId <= GPS_ALT_LAST_ID) {
frskyData.hub.gpsAltitude = SPORT_DATA_S32(packet);
if (dataId == DATA_ID_FLVSS) {
// first sensor, remember its cell count
frskyData.hub.sensorCellsCount[0] = cells;
cells += frskyData.hub.sensorCellsCount[1];
}
else {
// second sensor connected
frskyData.hub.sensorCellsCount[1] = cells;
cells += frskyData.hub.sensorCellsCount[0];
battnumber += frskyData.hub.sensorCellsCount[0];
}
if (!frskyData.hub.gpsAltitudeOffset)
frskyData.hub.gpsAltitudeOffset = -frskyData.hub.gpsAltitude;
if (cells != frskyData.hub.cellsCount) {
frskySetCellsCount(cells);
}
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;
}
frskySetCellVoltage(battnumber, (frskyCellVoltage_t) ((data & 0x000FFF00) >> 8) / 5);
if (useSecondCell) {
frskySetCellVoltage(battnumber+1, (frskyCellVoltage_t) ((data & 0xFFF00000) >> 20) / 5);
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)*255+165)/330, UNIT_VOLTS);
}
else if (appId >= A4_FIRST_ID && appId <= A4_LAST_ID) {
frskyData.analog[TELEM_ANA_A4].set((SPORT_DATA_U32(packet)*255+165)/330, UNIT_VOLTS);
}
else if (appId >= CELLS_FIRST_ID && appId <= CELLS_LAST_ID) {
uint32_t data = SPORT_DATA_U32(packet);
uint8_t battnumber = data & 0xF;
uint8_t cells = (data & 0xF0) >> 4;
bool useSecondCell = (battnumber+1 < cells);
if (dataId == DATA_ID_FLVSS) {
// first sensor, remember its cell count
frskyData.hub.sensorCellsCount[0] = cells;
cells += frskyData.hub.sensorCellsCount[1];
}
else {
// second sensor connected
frskyData.hub.sensorCellsCount[1] = cells;
cells += frskyData.hub.sensorCellsCount[0];
battnumber += frskyData.hub.sensorCellsCount[0];
}
if (cells != frskyData.hub.cellsCount) {
frskySetCellsCount(cells);
}
frskySetCellVoltage(battnumber, (frskyCellVoltage_t) ((data & 0x000FFF00) >> 8) / 5);
if (useSecondCell) {
frskySetCellVoltage(battnumber+1, (frskyCellVoltage_t) ((data & 0xFFF00000) >> 20) / 5);
}
}
}
break;