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

Telemetry files split

Browse Source
This commit is contained in:
Bertrand Songis
2016-03-28 11:24:55 +02:00
parent 485d30f65b
commit 746dc450fa
25 changed files with 1713 additions and 1609 deletions
Download Patch File Download Diff File Expand all files Collapse all files
radio/src/telemetry/frsky.cpp
+15 -504
View File
@@ -18,113 +18,14 @@
* GNU General Public License for more details.
*/
#include "../opentx.h"
#include "opentx.h"
uint8_t frskyStreaming = 0;
#if defined(WS_HOW_HIGH)
uint8_t frskyUsrStreaming = 0;
#endif
uint8_t link_counter = 0;
#define FRSKY_RX_PACKET_SIZE 19
uint8_t frskyRxBuffer[FRSKY_RX_PACKET_SIZE]; // Receive buffer. 9 bytes (full packet), worst case 18 bytes with byte-stuffing (+1)
#if !defined(CPUARM) && !defined(PCBFLAMENCO)
uint8_t frskyTxBuffer[FRSKY_TX_PACKET_SIZE];
#endif
#if !defined(CPUARM)
uint8_t frskyTxBufferCount = 0;
#endif
#if defined(CPUARM)
uint8_t telemetryState = TELEMETRY_INIT;
#endif
uint8_t frskyRxBufferCount = 0;
FrskyData frskyData;
#if defined(CPUARM)
uint8_t telemetryProtocol = 255;
#if defined(REVX)
uint8_t serialInversion = 0;
#endif
#define IS_FRSKY_D_PROTOCOL() (telemetryProtocol == PROTOCOL_FRSKY_D)
#define IS_FRSKY_SPORT_PROTOCOL() (telemetryProtocol == PROTOCOL_FRSKY_SPORT)
#else
#define IS_FRSKY_D_PROTOCOL() (true)
#define IS_FRSKY_SPORT_PROTOCOL() (false)
#endif
#if defined(CPUARM)
void FrskyValueWithMin::reset()
{
memclear(this, sizeof(*this));
}
#endif
void FrskyValueWithMin::set(uint8_t value)
{
#if defined(CPUARM)
if (this->value == 0) {
memset(values, value, TELEMETRY_AVERAGE_COUNT);
this->value = value;
}
else {
//calculate the average from values[] and value
//also shift readings in values [] array
unsigned int sum = values[0];
for (int i=0; i<TELEMETRY_AVERAGE_COUNT-1; i++) {
uint8_t tmp = values[i+1];
values[i] = tmp;
sum += tmp;
}
values[TELEMETRY_AVERAGE_COUNT-1] = value;
sum += value;
this->value = sum/(TELEMETRY_AVERAGE_COUNT+1);
}
#else
if (this->value == 0) {
this->value = value;
}
else {
sum += value;
if (link_counter == 0) {
this->value = sum / (IS_FRSKY_D_PROTOCOL() ? FRSKY_D_AVERAGING : FRSKY_SPORT_AVERAGING);
sum = 0;
}
}
#endif
if (!min || value < min) {
min = value;
}
}
void FrskyValueWithMinMax::set(uint8_t value, uint8_t unit)
{
FrskyValueWithMin::set(value);
if (unit != UNIT_VOLTS) {
this->value = value;
}
if (!max || value > max) {
max = value;
}
}
#if !defined(CPUARM)
uint16_t getChannelRatio(source_t channel)
{
return (uint16_t)g_model.frsky.channels[channel].ratio << g_model.frsky.channels[channel].multiplier;
}
lcdint_t applyChannelRatio(source_t channel, lcdint_t val)
{
return ((int32_t)val+g_model.frsky.channels[channel].offset) * getChannelRatio(channel) * 2 / 51;
}
uint8_t frskyTxBuffer[FRSKY_TX_PACKET_SIZE];
uint8_t frskyTxBufferCount = 0;
#endif
#if defined(TELEMETREZ)
@@ -138,7 +39,7 @@ extern uint8_t TrotCount;
extern uint8_t TezRotary;
#endif
NOINLINE void processSerialData(uint8_t data)
NOINLINE void processFrskyTelemetryData(uint8_t data)
{
static uint8_t dataState = STATE_DATA_IDLE;
@@ -166,8 +67,7 @@ NOINLINE void processSerialData(uint8_t data)
}
#endif
switch (dataState)
{
switch (dataState) {
case STATE_DATA_START:
if (data == START_STOP) {
if (IS_FRSKY_SPORT_PROTOCOL()) {
@@ -262,412 +162,23 @@ NOINLINE void processSerialData(uint8_t data)
#endif
}
void telemetryWakeup()
{
#if defined(CPUARM)
uint8_t requiredTelemetryProtocol = MODEL_TELEMETRY_PROTOCOL();
#if defined(REVX)
uint8_t requiredSerialInversion = g_model.moduleData[EXTERNAL_MODULE].invertedSerial;
if (telemetryProtocol != requiredTelemetryProtocol || serialInversion != requiredSerialInversion) {
serialInversion = requiredSerialInversion;
#else
if (telemetryProtocol != requiredTelemetryProtocol) {
#endif
telemetryInit(requiredTelemetryProtocol);
telemetryProtocol = requiredTelemetryProtocol;
}
#endif
#if defined(CPUSTM32)
uint8_t data;
#if defined(LOG_TELEMETRY) && !defined(SIMU)
static tmr10ms_t lastTime = 0;
tmr10ms_t newTime = get_tmr10ms();
struct gtm utm;
gettime(&utm);
#endif
while (telemetryFifo.pop(data)) {
processSerialData(data);
#if defined(LOG_TELEMETRY) && !defined(SIMU)
extern FIL g_telemetryFile;
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
}
#elif defined(PCBSKY9X)
if (telemetryProtocol == PROTOCOL_FRSKY_D_SECONDARY) {
uint8_t data;
while (telemetrySecondPortReceive(data)) {
processSerialData(data);
}
}
else {
// Receive serial data here
rxPdcUsart(processSerialData);
}
#endif
#if !defined(CPUARM)
if (IS_FRSKY_D_PROTOCOL()) {
// 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
#if !defined(SIMU)
frskyDSendNextAlarm();
#endif
}
}
#endif
#if defined(CPUARM)
for (int i=0; i<MAX_SENSORS; i++) {
const TelemetrySensor & sensor = g_model.telemetrySensors[i];
if (sensor.type == TELEM_TYPE_CALCULATED) {
telemetryItems[i].eval(sensor);
}
}
#endif
#if defined(VARIO)
if (TELEMETRY_STREAMING() && !IS_FAI_ENABLED()) {
varioWakeup();
}
#endif
#if defined(PCBTARANIS) && defined(REVPLUS)
#define FRSKY_BAD_ANTENNA() (IS_VALID_XJT_VERSION() && frskyData.swr.value > 0x33)
#else
#define FRSKY_BAD_ANTENNA() (frskyData.swr.value > 0x33)
#endif
#if defined(CPUARM)
static tmr10ms_t alarmsCheckTime = 0;
#define SCHEDULE_NEXT_ALARMS_CHECK(seconds) alarmsCheckTime = get_tmr10ms() + (100*(seconds))
if (int32_t(get_tmr10ms() - alarmsCheckTime) > 0) {
SCHEDULE_NEXT_ALARMS_CHECK(1/*second*/);
uint8_t now = TelemetryItem::now();
bool sensor_lost = false;
for (int i=0; i<MAX_SENSORS; i++) {
if (isTelemetryFieldAvailable(i)) {
uint8_t lastReceived = telemetryItems[i].lastReceived;
if (lastReceived < TELEMETRY_VALUE_TIMER_CYCLE && uint8_t(now - lastReceived) > TELEMETRY_VALUE_OLD_THRESHOLD) {
sensor_lost = true;
telemetryItems[i].lastReceived = TELEMETRY_VALUE_OLD;
TelemetrySensor * sensor = & g_model.telemetrySensors[i];
if (sensor->unit == UNIT_DATETIME) {
telemetryItems[i].datetime.datestate = 0;
telemetryItems[i].datetime.timestate = 0;
}
}
}
}
if (sensor_lost && TELEMETRY_STREAMING()) {
audioEvent(AU_SENSOR_LOST);
}
#if defined(PCBTARANIS)
if ((g_model.moduleData[INTERNAL_MODULE].rfProtocol != RF_PROTO_OFF || g_model.moduleData[EXTERNAL_MODULE].type == MODULE_TYPE_XJT) && FRSKY_BAD_ANTENNA()) {
AUDIO_SWR_RED();
POPUP_WARNING(STR_ANTENNAPROBLEM);
SCHEDULE_NEXT_ALARMS_CHECK(10/*seconds*/);
}
#endif
if (TELEMETRY_STREAMING()) {
if (getRssiAlarmValue(1) && TELEMETRY_RSSI() < getRssiAlarmValue(1)) {
AUDIO_RSSI_RED();
SCHEDULE_NEXT_ALARMS_CHECK(10/*seconds*/);
}
else if (getRssiAlarmValue(0) && TELEMETRY_RSSI() < getRssiAlarmValue(0)) {
AUDIO_RSSI_ORANGE();
SCHEDULE_NEXT_ALARMS_CHECK(10/*seconds*/);
}
}
}
if (TELEMETRY_STREAMING()) {
if (telemetryState == TELEMETRY_KO) {
AUDIO_TELEMETRY_BACK();
}
telemetryState = TELEMETRY_OK;
}
else if (telemetryState == TELEMETRY_OK) {
telemetryState = TELEMETRY_KO;
AUDIO_TELEMETRY_LOST();
}
#endif
}
void telemetryInterrupt10ms()
{
#if defined(CPUARM)
#elif defined(FRSKY_HUB)
uint16_t voltage = 0; /* unit: 1/10 volts */
for (uint8_t i=0; i<frskyData.hub.cellsCount; i++)
voltage += frskyData.hub.cellVolts[i];
voltage /= (10 / TELEMETRY_CELL_VOLTAGE_MUTLIPLIER);
frskyData.hub.cellsSum = voltage;
if (frskyData.hub.cellsSum < frskyData.hub.minCells) {
frskyData.hub.minCells = frskyData.hub.cellsSum;
}
#endif
if (TELEMETRY_STREAMING()) {
if (!TELEMETRY_OPENXSENSOR()) {
#if defined(CPUARM)
for (int i=0; i<MAX_SENSORS; i++) {
const TelemetrySensor & sensor = g_model.telemetrySensors[i];
if (sensor.type == TELEM_TYPE_CALCULATED) {
telemetryItems[i].per10ms(sensor);
}
}
#else
// power calculation
uint8_t channel = g_model.frsky.voltsSource;
if (channel <= FRSKY_VOLTS_SOURCE_A2) {
voltage = applyChannelRatio(channel, frskyData.analog[channel].value) / 10;
}
#if defined(FRSKY_HUB)
else if (channel == FRSKY_VOLTS_SOURCE_FAS) {
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_CURRENT_SOURCE_A1;
if (channel < MAX_FRSKY_A_CHANNELS) {
current = applyChannelRatio(channel, frskyData.analog[channel].value) / 10;
}
frskyData.hub.power = ((current>>1) * (voltage>>1)) / 25;
frskyData.hub.currentPrescale += current;
if (frskyData.hub.currentPrescale >= 3600) {
frskyData.hub.currentConsumption += 1;
frskyData.hub.currentPrescale -= 3600;
}
#endif
}
#if !defined(CPUARM)
if (frskyData.hub.power > frskyData.hub.maxPower) {
frskyData.hub.maxPower = frskyData.hub.power;
}
#endif
}
#if defined(WS_HOW_HIGH)
if (frskyUsrStreaming > 0) {
frskyUsrStreaming--;
}
#endif
if (frskyStreaming > 0) {
frskyStreaming--;
}
else {
#if !defined(SIMU)
#if defined(CPUARM)
frskyData.rssi.reset();
#else
frskyData.rssi[0].set(0);
frskyData.rssi[1].set(0);
#endif
#endif
}
}
void telemetryReset()
{
memclear(&frskyData, sizeof(frskyData));
#if defined(CPUARM)
for (int index=0; index<MAX_SENSORS; index++) {
telemetryItems[index].clear();
}
#endif
frskyStreaming = 0; // reset counter only if valid frsky packets are being detected
link_counter = 0;
#if defined(CPUARM)
telemetryState = TELEMETRY_INIT;
#endif
#if defined(FRSKY_HUB) && !defined(CPUARM)
frskyData.hub.gpsLatitude_bp = 2;
frskyData.hub.gpsLongitude_bp = 2;
frskyData.hub.gpsFix = -1;
#endif
#if defined(SIMU)
#if defined(CPUARM)
frskyData.swr.value = 30;
frskyData.rssi.value = 75;
#else
frskyData.rssi[0].value = 75;
frskyData.rssi[1].value = 75;
frskyData.analog[TELEM_ANA_A1].set(120, UNIT_VOLTS);
frskyData.analog[TELEM_ANA_A2].set(240, UNIT_VOLTS);
#endif
#if !defined(CPUARM)
frskyData.hub.fuelLevel = 75;
frskyData.hub.rpm = 12000;
frskyData.hub.vfas = 100;
frskyData.hub.minVfas = 90;
#if defined(GPS)
frskyData.hub.gpsFix = 1;
frskyData.hub.gpsLatitude_bp = 4401;
frskyData.hub.gpsLatitude_ap = 7710;
frskyData.hub.gpsLongitude_bp = 1006;
frskyData.hub.gpsLongitude_ap = 8872;
frskyData.hub.gpsSpeed_bp = 200; //in knots
frskyData.hub.gpsSpeed_ap = 0;
getGpsPilotPosition();
frskyData.hub.gpsLatitude_bp = 4401;
frskyData.hub.gpsLatitude_ap = 7455;
frskyData.hub.gpsLongitude_bp = 1006;
frskyData.hub.gpsLongitude_ap = 9533;
getGpsDistance();
#endif
frskyData.hub.airSpeed = 1000; // 185.1 km/h
frskyData.hub.cellsCount = 6;
frskyData.hub.cellVolts[0] = 410/TELEMETRY_CELL_VOLTAGE_MUTLIPLIER;
frskyData.hub.cellVolts[1] = 420/TELEMETRY_CELL_VOLTAGE_MUTLIPLIER;
frskyData.hub.cellVolts[2] = 430/TELEMETRY_CELL_VOLTAGE_MUTLIPLIER;
frskyData.hub.cellVolts[3] = 440/TELEMETRY_CELL_VOLTAGE_MUTLIPLIER;
frskyData.hub.cellVolts[4] = 450/TELEMETRY_CELL_VOLTAGE_MUTLIPLIER;
frskyData.hub.cellVolts[5] = 460/TELEMETRY_CELL_VOLTAGE_MUTLIPLIER;
frskyData.hub.minCellVolts = 250/TELEMETRY_CELL_VOLTAGE_MUTLIPLIER;
frskyData.hub.minCell = 300; //unit 10mV
frskyData.hub.minCells = 220; //unit 100mV
//frskyData.hub.cellsSum = 261; //calculated from cellVolts[]
frskyData.hub.gpsAltitude_bp = 50;
frskyData.hub.baroAltitude_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 = 55;
frskyData.hub.maxCurrent = 65;
#endif
#endif
/*Add some default sensor values to the simulator*/
#if defined(CPUARM) && defined(SIMU)
for (int i=0; i<MAX_SENSORS; i++) {
const TelemetrySensor & sensor = g_model.telemetrySensors[i];
switch (sensor.id)
{
case RSSI_ID:
setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, RSSI_ID, 0, sensor.instance , 75, UNIT_RAW, 0);
break;
case ADC1_ID:
setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, ADC1_ID, 0, sensor.instance, 100, UNIT_RAW, 0);
break;
case ADC2_ID:
setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, ADC2_ID, 0, sensor.instance, 245, UNIT_RAW, 0);
break;
case SWR_ID:
setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, SWR_ID, 0, sensor.instance, 30, UNIT_RAW, 0);
break;
case BATT_ID:
setTelemetryValue(TELEM_PROTO_FRSKY_SPORT, BATT_ID, 0, sensor.instance, 100, UNIT_RAW, 0);
break;
}
}
#endif
}
#if defined(CPUARM)
// we don't reset the telemetry here as we would also reset the consumption after model load
void telemetryInit(uint8_t protocol)
{
if (protocol == PROTOCOL_FRSKY_D) {
telemetryPortInit(FRSKY_D_BAUDRATE);
}
#if defined(CROSSFIRE)
else if (protocol == PROTOCOL_PULSES_CROSSFIRE) {
telemetryPortInit(CROSSFIRE_BAUDRATE);
telemetryPortSetDirectionOutput();
}
#endif
else if (protocol == PROTOCOL_FRSKY_D_SECONDARY) {
telemetryPortInit(0);
serial2TelemetryInit(PROTOCOL_FRSKY_D_SECONDARY);
}
else {
telemetryPortInit(FRSKY_SPORT_BAUDRATE);
}
#if defined(REVX) && !defined(SIMU)
if (serialInversion) {
setMFP();
}
else {
clearMFP();
}
#endif
}
#else
void telemetryInit()
{
telemetryPortInit();
}
#endif
#if defined(CPUARM)
#elif defined(FRSKY_HUB)
void frskyUpdateCells(void)
{
// Voltage => Cell number + Cell voltage
uint8_t battnumber = ((frskyData.hub.volts & 0x00F0) >> 4);
uint8_t battnumber = ((telemetryData.hub.volts & 0x00F0) >> 4);
if (battnumber < 12) {
if (frskyData.hub.cellsCount < battnumber+1) {
frskyData.hub.cellsCount = battnumber+1;
if (telemetryData.hub.cellsCount < battnumber+1) {
telemetryData.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;
if (!frskyData.hub.minCell || frskyData.hub.minCellVolts<frskyData.hub.minCell)
frskyData.hub.minCell = frskyData.hub.minCellVolts;
uint8_t cellVolts = (uint8_t)(((((telemetryData.hub.volts & 0xFF00) >> 8) + ((telemetryData.hub.volts & 0x000F) << 8))) / 10);
telemetryData.hub.cellVolts[battnumber] = cellVolts;
if (!telemetryData.hub.minCellVolts || cellVolts<telemetryData.hub.minCellVolts || battnumber==telemetryData.hub.minCellIdx) {
telemetryData.hub.minCellIdx = battnumber;
telemetryData.hub.minCellVolts = cellVolts;
if (!telemetryData.hub.minCell || telemetryData.hub.minCellVolts<telemetryData.hub.minCell)
telemetryData.hub.minCell = telemetryData.hub.minCellVolts;
}
}
}
#endif
NOINLINE uint8_t getRssiAlarmValue(uint8_t alarm)
{
return (45 - 3*alarm + g_model.frsky.rssiAlarms[alarm].value);
}
#if defined(LUA)
Fifo<LuaTelemetryValue, 16> * luaInputTelemetryFifo = NULL;
Fifo<LuaTelemetryValue, 16> * luaOutputTelemetryFifo = NULL;
#endif