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

More telemetry info logged

Browse Source 
Negative numbers in logs fixed
[Taranis] SPORT telemetry
[Taranis] Crash when connecting USB during EEPROM write
This commit is contained in:
bernet.andre@gmail.com
2013-06-24 13:11:27 +00:00
parent d796623e3a
commit 475b8bc3e6
5 changed files with 898 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/logs.cpp
+7 -5
View File
@@ -117,7 +117,7 @@ const pm_char * openLogs()
#if defined(FRSKY_HUB)
if (IS_USR_PROTO_FRSKY_HUB())
f_puts("GPS Date,GPS Time,Long,Lat,Course,GPS Speed,GPS Alt,Baro Alt,Temp1,Temp2,RPM,Fuel,Volts,AccelX,AccelY,AccelZ,", &g_oLogFile);
f_puts("GPS Date,GPS Time,Long,Lat,Course,GPS Speed,GPS Alt,Baro Alt,Vertical Speed,Temp1,Temp2,RPM,Fuel,Cell volts,Cell 1,Cell 2,Cell 3,Cell 4,Cell 5,Cell 6,Current,Vfas,AccelX,AccelY,AccelZ,", &g_oLogFile);
#endif
#if defined(WS_HOW_HIGH)
@@ -193,7 +193,7 @@ void writeLogs()
#if defined(FRSKY_HUB)
if (IS_USR_PROTO_FRSKY_HUB()) {
f_printf(&g_oLogFile, "%4d-%02d-%02d,%02d:%02d:%02d,%03d.%04d%c,%03d.%04d%c,%03d.%d,%d.%d,%03d.%d,%d.%d,%d,%d,%d,%d,%d,%d,%d,%d,",
f_printf(&g_oLogFile, "%4d-%02d-%02d,%02d:%02d:%02d,%03d.%04d%c,%03d.%04d%c,%03d.%02d,%d.%02d,%d.%02d," TELEMETRY_ALT_FORMAT TELEMETRY_VSPEED_FORMAT "%d,%d,%d,%d," TELEMETRY_CELLS_FORMAT TELEMETRY_CURRENT_FORMAT TELEMETRY_VFAS_FORMAT "%d,%d,%d,",
frskyData.hub.year+2000,
frskyData.hub.month,
frskyData.hub.day,
@@ -212,13 +212,15 @@ void writeLogs()
TELEMETRY_GPS_SPEED_AP,
TELEMETRY_GPS_ALT_BP,
TELEMETRY_GPS_ALT_AP,
TELEMETRY_ALT_BP,
TELEMETRY_ALT_AP,
TELEMETRY_ALT,
TELEMETRY_VSPEED,
frskyData.hub.temperature1,
frskyData.hub.temperature2,
frskyData.hub.rpm,
frskyData.hub.fuelLevel,
frskyData.hub.volts,
TELEMETRY_CELLS,
TELEMETRY_CURRENT,
TELEMETRY_VFAS,
frskyData.hub.accelX,
frskyData.hub.accelY,
frskyData.hub.accelZ);
src/targets/taranis/usbd_usr.cpp
+1 -1
View File
@@ -108,7 +108,7 @@ void USBD_USR_DeviceResumed(void)
void USBD_USR_DeviceConnected (void)
{
sdDone();
eeCheck(true);
//eeCheck(true);
usbState = USB_CONNECTED;
}
src/telemetry/frsky.h
+16 -6
View File
@@ -217,12 +217,22 @@ bool FRSKY_alarmRaised(uint8_t idx);
void resetTelemetry();
#define TELEMETRY_ALT_BP frskyData.hub.baroAltitude_bp
#define TELEMETRY_ALT_AP frskyData.hub.baroAltitude_ap
#define TELEMETRY_GPS_ALT_AP frskyData.hub.gpsAltitude_ap
#define TELEMETRY_GPS_ALT_BP frskyData.hub.gpsAltitude_bp
#define TELEMETRY_GPS_SPEED_BP frskyData.hub.gpsSpeed_bp
#define TELEMETRY_GPS_SPEED_AP frskyData.hub.gpsSpeed_ap
#define TELEMETRY_ALT_BP frskyData.hub.baroAltitude_bp
#define TELEMETRY_ALT_AP frskyData.hub.baroAltitude_ap
#define TELEMETRY_GPS_SPEED_BP frskyData.hub.gpsSpeed_bp
#define TELEMETRY_GPS_SPEED_AP frskyData.hub.gpsSpeed_ap
#define TELEMETRY_GPS_ALT_AP frskyData.hub.gpsAltitude_ap
#define TELEMETRY_GPS_ALT_BP frskyData.hub.gpsAltitude_bp
#define TELEMETRY_ALT frskyData.hub.baroAltitude_bp, frskyData.hub.baroAltitude_ap
#define TELEMETRY_ALT_FORMAT "%d.%02d,"
#define TELEMETRY_CELLS frskyData.hub.cellsSum / 10, frskyData.hub.cellsSum % 10
#define TELEMETRY_CELLS_FORMAT "%d.%d,"
#define TELEMETRY_CURRENT frskyData.hub.current / 100, frskyData.hub.current % 100
#define TELEMETRY_CURRENT_FORMAT "%d.%02d,"
#define TELEMETRY_VFAS frskyData.hub.vfas / 10, frskyData.hub.vfas % 10
#define TELEMETRY_VFAS_FORMAT "%d.%d,"
#define TELEMETRY_VSPEED frskyData.hub.varioSpeed < 0 ? '-' : ' ', frskyData.hub.varioSpeed / 100, frskyData.hub.varioSpeed % 100
#define TELEMETRY_VSPEED_FORMAT "%c%d.%02d,"
#define TELEMETRY_STREAMING() (frskyStreaming > 0)
src/telemetry/frsky_sport.cpp
+663
View File
@@ -0,0 +1,663 @@
/*
* Authors (alphabetical order)
* - Andre Bernet <bernet.andre@gmail.com>
* - Andreas Weitl
* - Bertrand Songis <bsongis@gmail.com>
* - Bryan J. Rentoul (Gruvin) <gruvin@gmail.com>
* - Cameron Weeks <th9xer@gmail.com>
* - Erez Raviv
* - Gabriel Birkus
* - Jean-Pierre Parisy
* - Karl Szmutny
* - Michael Blandford
* - Michal Hlavinka
* - Pat Mackenzie
* - Philip Moss
* - Rob Thomson
* - Romolo Manfredini <romolo.manfredini@gmail.com>
* - 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"
#define START_STOP 0x7e
#define BYTESTUFF 0x7d
#define STUFF_MASK 0x20
// 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 RSSI_ID 0xf101
#define ADC1_ID 0xf102
#define ADC2_ID 0xf103
#define BATT_ID 0xf104
#define SWR_ID 0xf105
#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 ALT_FIRST_ID 0x0100
#define ALT_LAST_ID 0x010f
#define VARIO_FIRST_ID 0x0110
#define VARIO_LAST_ID 0x011f
#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 CURR_FIRST_ID 0x0200
#define CURR_LAST_ID 0x020f
#define VFAS_FIRST_ID 0x0210
#define VFAS_LAST_ID 0x021f
#define GPS_SPEED_FIRST_ID 0x0830
#define GPS_SPEED_LAST_ID 0x083f
#define CELLS_FIRST_ID 0x0300
#define CELLS_LAST_ID 0x030f
// FrSky wrong IDs ?
#define BETA_VARIO_ID 0x8030
#define BETA_BARO_ALT_ID 0x8010
uint8_t telemetryState = TELEMETRY_INIT;
uint8_t frskyRxBuffer[FRSKY_RX_PACKET_SIZE]; // Receive buffer. 9 bytes (full packet), worst case 18 bytes with byte-stuffing (+1)
uint8_t frskyTxBuffer[FRSKY_TX_PACKET_SIZE]; // Ditto for transmit buffer
#if !defined(CPUARM)
uint8_t frskyTxBufferCount = 0;
#endif
uint8_t link_counter = 0;
FrskyData frskyData;
#if defined(DEBUG)
extern FIL g_telemetryFile;
Fifo<512> debugTelemetryFifo;
#endif
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 (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
/* TODO later if (!frskyData.hub.minVfas || frskyData.hub.minVfas > frskyData.hub.vfas)
frskyData.hub.minVfas = frskyData.hub.vfas; */
break;
case BARO_ALT_AP_ID:
setBaroAltitude((int32_t)100 * frskyData.hub.baroAltitude_bp + frskyData.hub.baroAltitude_ap);
break;
case GPS_ALT_AP_ID:
if (!frskyData.hub.gpsAltitudeOffset)
frskyData.hub.gpsAltitudeOffset = -frskyData.hub.gpsAltitude_bp;
frskyData.hub.gpsAltitude_bp += frskyData.hub.gpsAltitudeOffset;
if (!frskyData.hub.baroAltitudeOffset) {
if (frskyData.hub.gpsAltitude_bp > frskyData.hub.maxAltitude)
frskyData.hub.maxAltitude = frskyData.hub.gpsAltitude_bp;
if (frskyData.hub.gpsAltitude_bp < frskyData.hub.minAltitude)
frskyData.hub.minAltitude = frskyData.hub.gpsAltitude_bp;
}
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
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;
}
uint8_t cellVolts = (uint8_t)(((((frskyData.hub.volts & 0xFF00) >> 8) + ((frskyData.hub.volts & 0x000F) << 8)))/10);
frskyData.hub.cellVolts[battnumber] = cellVolts;
if (!frskyData.hub.minCellVolts || cellVolts < frskyData.hub.minCellVolts || battnumber==frskyData.hub.minCellIdx) {
frskyData.hub.minCellIdx = battnumber;
frskyData.hub.minCellVolts = cellVolts;
}
}
break;
}
case GPS_HOUR_MIN_ID:
frskyData.hub.hour = ((uint8_t)(frskyData.hub.hour + g_eeGeneral.timezone + 24)) % 24;
break;
case ACCEL_X_ID:
case ACCEL_Y_ID:
case ACCEL_Z_ID:
((int16_t*)(&frskyData.hub))[id] /= 10;
break;
}
}
#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 processSportPacket(uint8_t *packet)
{
/* uint8_t dataId = packet[0]; */
uint8_t prim = packet[1];
uint16_t appId = *((uint16_t *)(packet+2));
switch (prim)
{
case DATA_FRAME:
if (appId == RSSI_ID) {
frskyData.rssi[0].set(SPORT_DATA_U8(packet));
}
if (appId == SWR_ID) {
frskyData.rssi[1].set(SPORT_DATA_U8(packet));
}
else if (appId == ADC1_ID || appId == ADC2_ID) {
// A1/A2 of DxR receivers
frskyData.analog[appId-ADC1_ID].set(SPORT_DATA_U8(packet));
}
else if (appId == BATT_ID) {
frskyData.analog[0].set(SPORT_DATA_U8(packet));
}
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 == BETA_BARO_ALT_ID) {
int32_t baroAltitude = SPORT_DATA_S32(packet);
setBaroAltitude(10 * (baroAltitude >> 8));
}
else if (appId == BETA_VARIO_ID) {
int32_t varioSpeed = SPORT_DATA_S32(packet);
frskyData.hub.varioSpeed = 10 * (varioSpeed >> 8);
}
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 (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 >= GPS_SPEED_FIRST_ID && appId <= GPS_SPEED_LAST_ID) {
frskyData.hub.gpsSpeed_bp = SPORT_DATA_U32(packet);
if (frskyData.hub.gpsSpeed_bp > frskyData.hub.maxGpsSpeed)
frskyData.hub.maxGpsSpeed = frskyData.hub.gpsSpeed_bp;
}
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) / 10;
frskyData.hub.cellVolts[battnumber+1] = ((cells & 0xFFF00000) >> 20) / 10;
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;
}
}
// Receive buffer state machine state enum
enum FrSkyDataState {
STATE_DATA_IDLE,
STATE_DATA_IN_FRAME,
STATE_DATA_XOR,
};
void processSerialData(uint8_t data)
{
static uint8_t numPktBytes = 0;
static uint8_t dataState = STATE_DATA_IDLE;
#if defined(BLUETOOTH)
// TODO if (g_model.bt_telemetry)
btPushByte(data);
#endif
#if defined(DEBUG)
debugTelemetryFifo.push(data);
#endif
if (data == START_STOP) {
dataState = STATE_DATA_IN_FRAME;
numPktBytes = 0;
}
else {
switch (dataState) {
case STATE_DATA_XOR:
frskyRxBuffer[numPktBytes++] = data ^ STUFF_MASK;
break;
case STATE_DATA_IN_FRAME:
if (data == BYTESTUFF)
dataState = STATE_DATA_XOR; // XOR next byte
else
frskyRxBuffer[numPktBytes++] = data;
break;
}
}
if (numPktBytes == FRSKY_RX_PACKET_SIZE) {
processSportPacket(frskyRxBuffer);
dataState = STATE_DATA_IDLE;
}
}
uint8_t frskyAlarmsSendState = 0 ;
void telemetryInterrupt10ms()
{
uint16_t voltage = 0; /* unit: 1/10 volts */
#if defined(FRSKY_HUB)
for (uint8_t i=0; i<frskyData.hub.cellsCount; i++)
voltage += frskyData.hub.cellVolts[i];
voltage /= 5;
frskyData.hub.cellsSum = voltage;
#endif
if (TELEMETRY_STREAMING()) {
uint8_t channel = g_model.frsky.voltsSource;
if (channel <= 1) {
voltage = applyChannelRatio(channel, frskyData.analog[channel].value) / 10;
}
#if defined(FRSKY_HUB)
else if (channel == 2) {
voltage = frskyData.hub.vfas;
}
#endif
#if defined(FRSKY_HUB)
uint16_t current = frskyData.hub.current; /* unit: 1/10 amps */
#else
uint16_t current = 0;
#endif
channel = g_model.frsky.currentSource - FRSKY_SOURCE_A1;
if (channel <= 1) {
current = applyChannelRatio(channel, frskyData.analog[channel].value) / 10;
}
frskyData.hub.power = (current * voltage) / 100;
if (frskyData.hub.power > frskyData.hub.maxPower)
frskyData.hub.maxPower = frskyData.hub.power;
frskyData.hub.currentPrescale += current;
if (frskyData.hub.currentPrescale >= 3600) {
frskyData.hub.currentConsumption += 1;
frskyData.hub.currentPrescale -= 3600;
}
}
}
inline bool alarmRaised(uint8_t channel, uint8_t idx)
{
return g_model.frsky.channels[channel].ratio > 0 && g_model.frsky.channels[channel].alarms_value[idx] > 0 && frskyData.analog[channel].value < g_model.frsky.channels[channel].alarms_value[idx];
}
void telemetryWakeup()
{
#if defined(PCBSKY9X)
rxPdcUsart(processSerialData); // Receive serial data here
#endif
#if 0
// Attempt to transmit any waiting Fr-Sky alarm set packets every 50ms (subject to packet buffer availability)
static uint8_t frskyTxDelay = 5;
if (frskyAlarmsSendState && (--frskyTxDelay == 0)) {
frskyTxDelay = 5; // 50ms
// frskySendNextAlarm();
}
#endif
#if defined(VARIO)
if (!IS_FAI_ENABLED())
varioWakeup();
#endif
#if defined(DEBUG)
static tmr10ms_t lastTime = 0;
tmr10ms_t newTime = get_tmr10ms();
uint8_t data;
struct gtm utm;
gettime(&utm);
while (debugTelemetryFifo.pop(data)) {
if (lastTime != newTime) {
f_printf(&g_telemetryFile, "\r\n%4d-%02d-%02d,%02d:%02d:%02d.%02d0: %02X", utm.tm_year+1900, utm.tm_mon+1, utm.tm_mday, utm.tm_hour, utm.tm_min, utm.tm_sec, g_ms100, data);
lastTime = newTime;
}
else {
f_printf(&g_telemetryFile, " %02X", data);
}
}
#endif
static tmr10ms_t alarmsCheckTime = 0;
static uint8_t alarmsCheckStep = 0;
if (int32_t(get_tmr10ms() - alarmsCheckTime) > 0) {
alarmsCheckTime = get_tmr10ms() + 100; /* next check in 1second */
if (alarmsCheckStep == 0) {
if (frskyData.rssi[1].value > 0x33) {
AUDIO_SWR_RED();
s_global_warning = PSTR(CENTER "Antenna problem!");
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3seconds */
}
}
else if (TELEMETRY_STREAMING()) {
if (alarmsCheckStep == 1) {
if (getRssiAlarmValue(1) && frskyData.rssi[0].value < getRssiAlarmValue(1)) {
AUDIO_RSSI_RED();
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3seconds */
}
else if (getRssiAlarmValue(0) && frskyData.rssi[0].value < getRssiAlarmValue(0)) {
AUDIO_RSSI_ORANGE();
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3seconds */
}
}
else if (alarmsCheckStep == 2) {
if (alarmRaised(1, 1) && g_model.moduleData[INTERNAL_MODULE].rfProtocol == RF_PROTO_D8) {
AUDIO_A2_RED();
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3seconds */
}
else if (alarmRaised(1, 0) && g_model.moduleData[INTERNAL_MODULE].rfProtocol == RF_PROTO_D8) {
AUDIO_A2_ORANGE();
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3seconds */
}
}
else if (alarmsCheckStep == 3) {
if (alarmRaised(0, 1)) {
AUDIO_A1_RED();
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3seconds */
}
else if (alarmRaised(0, 0)) {
AUDIO_A1_ORANGE();
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3seconds */
}
}
}
if (++alarmsCheckStep == 4) {
alarmsCheckStep = 0;
}
}
if (TELEMETRY_STREAMING()) {
if (telemetryState == TELEMETRY_KO) {
AUDIO_TELEMETRY_BACK();
}
telemetryState = TELEMETRY_OK;
}
else if (telemetryState == TELEMETRY_OK) {
telemetryState = TELEMETRY_KO;
AUDIO_TELEMETRY_LOST();
}
}
void FRSKY_Init(void)
{
// clear frsky variables
resetTelemetry();
}
void FRSKY_End(void)
{
}
void FrskyValueWithMinMax::set(uint8_t value)
{
this->value = value;
if (value) {
if (!max || value > max)
max = value;
if (!min || value < min)
min = value;
}
}
void resetTelemetry()
{
memclear(&frskyData, sizeof(frskyData));
#if defined(FRSKY_HUB)
frskyData.hub.gpsLatitude_bp = 2;
frskyData.hub.gpsLongitude_bp = 2;
frskyData.hub.gpsFix = -1;
#endif
#ifdef SIMU
frskyData.analog[0].set(120);
frskyData.analog[1].set(240);
frskyData.rssi[0].value = 75;
frskyData.rssi[1].value = 30;
frskyData.hub.fuelLevel = 75;
frskyData.hub.rpm = 12000;
frskyData.hub.gpsFix = 1;
frskyData.hub.gpsLatitude_bp = 4401;
frskyData.hub.gpsLatitude_ap = 7710;
frskyData.hub.gpsLongitude_bp = 1006;
frskyData.hub.gpsLongitude_ap = 8872;
getGpsPilotPosition();
frskyData.hub.gpsLatitude_bp = 4401;
frskyData.hub.gpsLatitude_ap = 7455;
frskyData.hub.gpsLongitude_bp = 1006;
frskyData.hub.gpsLongitude_ap = 9533;
getGpsDistance();
frskyData.hub.cellsCount = 6;
#if 0 // defined(FRSKY_SPORT)
frskyData.hub.baroAltitude = 50 * 100;
#else
frskyData.hub.baroAltitude_bp = 50;
#endif
frskyData.hub.gpsAltitude_bp = 50;
frskyData.hub.minAltitude = 10;
frskyData.hub.maxAltitude = 500;
frskyData.hub.accelY = 100;
frskyData.hub.temperature1 = -30;
frskyData.hub.maxTemperature1 = 100;
frskyData.hub.current = 5;
frskyData.hub.maxCurrent = 56;
#endif
}
src/telemetry/frsky_sport.h
+211
View File
@@ -0,0 +1,211 @@
/*
* Authors (alphabetical order)
* - Andre Bernet <bernet.andre@gmail.com>
* - Andreas Weitl
* - Bertrand Songis <bsongis@gmail.com>
* - Bryan J. Rentoul (Gruvin) <gruvin@gmail.com>
* - Cameron Weeks <th9xer@gmail.com>
* - Erez Raviv
* - Gabriel Birkus
* - Jean-Pierre Parisy
* - Karl Szmutny
* - Michael Blandford
* - Michal Hlavinka
* - Pat Mackenzie
* - Philip Moss
* - Rob Thomson
* - Romolo Manfredini <romolo.manfredini@gmail.com>
* - 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.
*
*/
#ifndef TELEMETRY_SPORT_H
#define TELEMETRY_SPORT_H
#include <inttypes.h>
#define FRSKY_RX_PACKET_SIZE 9
#define FRSKY_TX_PACKET_SIZE 12
// .50 seconds
#define FRSKY_TIMEOUT10ms 50
extern int8_t frskyStreaming;
#define TELEMETRY_INIT 0
#define TELEMETRY_OK 1
#define TELEMETRY_KO 2
extern uint8_t telemetryState;
enum AlarmLevel {
alarm_off = 0,
alarm_yellow = 1,
alarm_orange = 2,
alarm_red = 3
};
#define ALARM_GREATER(channel, alarm) ((g_model.frsky.channels[channel].alarms_greater >> alarm) & 1)
#define ALARM_LEVEL(channel, alarm) ((g_model.frsky.channels[channel].alarms_level >> (2*alarm)) & 3)
class FrskyValueWithMinMax {
public:
uint8_t value;
uint8_t min;
uint8_t max;
void set(uint8_t value);
};
PACK(struct FrskySerialData {
int16_t spare1;
int16_t gpsAltitude_bp; // 0x01 before punct
int16_t temperature1; // 0x02 -20 .. 250 deg. celcius
uint16_t rpm; // 0x03 0..60,000 revs. per minute
uint16_t fuelLevel; // 0x04 0, 25, 50, 75, 100 percent
int16_t temperature2; // 0x05 -20 .. 250 deg. celcius
uint16_t volts; // 0x06 1/500V increments (0..4.2V)
uint32_t distFromEarthAxis;// 2 spares reused
int16_t gpsAltitude_ap; // 0x01+8 after punct
uint16_t spare2[6];
int16_t baroAltitude_bp; // 0x10 0..9,999 meters
uint16_t gpsSpeed_bp; // 0x11 before punct
uint16_t gpsLongitude_bp; // 0x12 before punct
uint16_t gpsLatitude_bp; // 0x13 before punct
uint16_t gpsCourse_bp; // 0x14 before punct (0..359.99 deg. -- seemingly 2-decimal precision)
uint8_t day; // 0x15
uint8_t month; // 0x15
uint16_t year; // 0x16
uint8_t hour; // 0x17
uint8_t min; // 0x17
uint16_t sec; // 0x18
uint16_t gpsSpeed_ap; // 0x11+8
uint16_t gpsLongitude_ap; // 0x12+8
uint16_t gpsLatitude_ap; // 0x13+8
uint16_t gpsCourse_ap; // 0x14+8
uint32_t pilotLatitude; // 2 spares reused
uint32_t pilotLongitude; // 2 spares reused
uint16_t baroAltitude_ap; // 0x21 after punct
uint16_t gpsLongitudeEW; // 0x1A+8 East/West
uint16_t gpsLatitudeNS; // 0x1B+8 North/South
int16_t accelX; // 0x24 1/256th gram (-8g ~ +8g)
int16_t accelY; // 0x25 1/256th gram (-8g ~ +8g)
int16_t accelZ; // 0x26 1/256th gram (-8g ~ +8g)
int16_t spare3;
uint16_t current; // 0x28 Current
int16_t spare4[7];
int16_t varioSpeed; // 0x30 Vertical speed in cm/s
int32_t baroAltitudeOffset;
int32_t baroAltitude;
int32_t gpsAltitudeOffset;
uint32_t gpsDistance;
uint16_t vfas; // 0x39 Added to FrSky protocol for home made sensors with a better precision
uint16_t volts_bp; // 0x3A
uint16_t volts_ap; // 0x3B
uint16_t spare5[4];
// end of FrSky Hub data
/* these fields must keep this order! */
int16_t minAltitude;
int16_t maxAltitude;
uint16_t maxRpm;
int16_t maxTemperature1;
int16_t maxTemperature2;
uint16_t maxGpsSpeed;
uint16_t maxGpsDistance;
uint16_t maxCurrent;
uint16_t maxPower;
/* end */
uint8_t gpsDistNeeded;
int8_t gpsFix; // -1=never fixed, 0=fix lost, 1=fixed
uint16_t currentConsumption;
uint16_t currentPrescale;
uint16_t power;
uint8_t cellsCount;
uint16_t cellVolts[12];
int16_t cellsSum;
uint8_t minCellIdx;
uint16_t minCellVolts;
});
struct FrskyData {
FrskyValueWithMinMax analog[2];
FrskyValueWithMinMax rssi[2];
FrskySerialData hub;
};
#define SEND_RSSI_ALARMS 6
#define SEND_MODEL_ALARMS 4
extern uint8_t frskyAlarmsSendState;
extern FrskyData frskyData;
extern uint8_t frskyTxBuffer[FRSKY_TX_PACKET_SIZE];
extern uint8_t frskyTxBufferCount;
void FRSKY_Init(void);
void FRSKY_End(void);
void telemetryWakeup();
void telemetryInterrupt10ms(void);
inline void frskySendAlarms(void)
{
frskyAlarmsSendState = SEND_RSSI_ALARMS;
}
bool FRSKY_alarmRaised(uint8_t idx);
void resetTelemetry();
#define TELEMETRY_ALT_BP (frskyData.hub.baroAltitude / 100)
#define TELEMETRY_ALT_AP (frskyData.hub.baroAltitude % 100)
#define TELEMETRY_GPS_SPEED_BP frskyData.hub.gpsSpeed_bp
#define TELEMETRY_GPS_SPEED_AP frskyData.hub.gpsSpeed_ap
#define TELEMETRY_GPS_ALT_AP frskyData.hub.gpsAltitude_ap
#define TELEMETRY_GPS_ALT_BP frskyData.hub.gpsAltitude_bp
#define TELEMETRY_ALT frskyData.hub.baroAltitude < 0 ? '-' : ' ', frskyData.hub.baroAltitude / 100, abs(frskyData.hub.baroAltitude % 100)
#define TELEMETRY_ALT_FORMAT "%c%d.%02d,"
#define TELEMETRY_CELLS frskyData.hub.cellsSum / 10, frskyData.hub.cellsSum % 10, frskyData.hub.cellVolts[0]*2/100, frskyData.hub.cellVolts[0]*2%100, frskyData.hub.cellVolts[1]*2/100, frskyData.hub.cellVolts[1]*2%100, frskyData.hub.cellVolts[2]*2/100, frskyData.hub.cellVolts[2]*2%100, frskyData.hub.cellVolts[3]*2/100, frskyData.hub.cellVolts[3]*2%100, frskyData.hub.cellVolts[4]*2/100, frskyData.hub.cellVolts[4]*2%100, frskyData.hub.cellVolts[5]*2/100, frskyData.hub.cellVolts[5]*2%100
#define TELEMETRY_CELLS_FORMAT "%d.%d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,"
#define TELEMETRY_CURRENT frskyData.hub.current / 100, frskyData.hub.current % 100
#define TELEMETRY_CURRENT_FORMAT "%d.%02d,"
#define TELEMETRY_VFAS frskyData.hub.vfas / 10, frskyData.hub.vfas % 10
#define TELEMETRY_VFAS_FORMAT "%d.%d,"
#define TELEMETRY_VSPEED frskyData.hub.varioSpeed < 0 ? '-' : ' ', frskyData.hub.varioSpeed / 100, abs(frskyData.hub.varioSpeed % 100)
#define TELEMETRY_VSPEED_FORMAT "%c%d.%02d,"
/* Would be great, but f_printf() doesn't take floats...
#define TELEMETRY_ALT float(frskyData.hub.baroAltitude)/100
#define TELEMETRY_ALT_FORMAT "%.2f,"
#define TELEMETRY_CELLS float(frskyData.hub.cellsSum)/10, float(frskyData.hub.cellVolts[0])/100, float(frskyData.hub.cellVolts[1])/100, float(frskyData.hub.cellVolts[2])/100, float(frskyData.hub.cellVolts[3])/100, float(frskyData.hub.cellVolts[4])/100, float(frskyData.hub.cellVolts[5])/100
#define TELEMETRY_CELLS_FORMAT "%.1f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,"
#define TELEMETRY_CURRENT float(frskyData.hub.current)/100
#define TELEMETRY_CURRENT_FORMAT "%.2f,"
#define TELEMETRY_VFAS float(frskyData.hub.vfas)/10
#define TELEMETRY_VFAS_FORMAT "%.1f,"
#define TELEMETRY_VSPEED float(frskyData.hub.varioSpeed)/100
#define TELEMETRY_VSPEED_FORMAT "%.2f,"
*/
#define TELEMETRY_STREAMING() (frskyData.rssi[0].value > 0)
#endif