From cc33d5833b8f2191a60a22c5e5188cf953ac1e6a Mon Sep 17 00:00:00 2001 From: Bertrand Songis Date: Sat, 30 Mar 2019 09:46:36 +0100 Subject: [PATCH] S.PORT PUSH should be OK now on both PXX2 and S.PORT lines --- radio/sdcard/FrSky-utilities/FrSky SBEC.lua | 8 +-- radio/src/gui/128x64/model_telemetry.cpp | 42 ++++++++------- radio/src/io/frsky_sport.cpp | 52 ------------------- radio/src/targets/common/arm/CMakeLists.txt | 1 - .../src/targets/taranis/telemetry_driver.cpp | 3 +- radio/src/telemetry/frsky_sport.cpp | 27 +++++++++- radio/src/telemetry/telemetry.h | 1 + 7 files changed, 53 insertions(+), 81 deletions(-) delete mode 100644 radio/src/io/frsky_sport.cpp diff --git a/radio/sdcard/FrSky-utilities/FrSky SBEC.lua b/radio/sdcard/FrSky-utilities/FrSky SBEC.lua index d6c051951..af5c1ccc1 100644 --- a/radio/sdcard/FrSky-utilities/FrSky SBEC.lua +++ b/radio/sdcard/FrSky-utilities/FrSky SBEC.lua @@ -156,19 +156,19 @@ local function redrawFieldsPage() end local function telemetryRead(fieldx) - return sportTelemetryPush(0, 0x30, 0x0e50, fieldx) + return sportTelemetryPush(0x17, 0x30, 0x0e50, fieldx) end local function telemetryIdle(field) - return sportTelemetryPush(0, 0x21, 0x0e50, field) + return sportTelemetryPush(0x17, 0x21, 0x0e50, field) end local function telemetryUnIdle(field) - return sportTelemetryPush(0, 0x20, 0x0e50, field) + return sportTelemetryPush(0x17, 0x20, 0x0e50, field) end local function telemetryWrite(fieldx, valuex) - return sportTelemetryPush(0, 0x31, 0x0e50, fieldx + valuex*256) + return sportTelemetryPush(0x17, 0x31, 0x0e50, fieldx + valuex*256) end local telemetryPopTimeout = 0 diff --git a/radio/src/gui/128x64/model_telemetry.cpp b/radio/src/gui/128x64/model_telemetry.cpp index 58e491846..598b18ef7 100644 --- a/radio/src/gui/128x64/model_telemetry.cpp +++ b/radio/src/gui/128x64/model_telemetry.cpp @@ -102,22 +102,18 @@ enum MenuModelTelemetryFrskyItems { #define VARIO_ROWS LABEL(Vario), 0, 1, 2, - #if defined (PCBTARANIS) #define TELEMETRY_TYPE_SHOW_TELEMETRY (! IS_INTERNAL_MODULE_ENABLED() && g_model.moduleData[EXTERNAL_MODULE].type == MODULE_TYPE_PPM) ? (uint8_t)0 : HIDDEN_ROW #else #define TELEMETRY_TYPE_SHOW_TELEMETRY (g_model.moduleData[EXTERNAL_MODULE].type == MODULE_TYPE_PPM) ? (uint8_t)0 : HIDDEN_ROW #endif - #if defined(REVX) #define TELEMETRY_TYPE_ROWS TELEMETRY_TYPE_SHOW_TELEMETRY, TELEMETRY_TYPE_SHOW_TELEMETRY, #else #define TELEMETRY_TYPE_ROWS TELEMETRY_TYPE_SHOW_TELEMETRY, #endif - #define TELEMETRY_SCREENS_ROWS - enum SensorFields { SENSOR_FIELD_NAME, SENSOR_FIELD_TYPE, @@ -137,28 +133,34 @@ enum SensorFields { SENSOR_FIELD_MAX }; -#define SENSOR_2ND_COLUMN (12*FW) -#define SENSOR_3RD_COLUMN (18*FW) - -#define SENSOR_UNIT_ROWS ((sensor->type == TELEM_TYPE_CALCULATED && (sensor->formula == TELEM_FORMULA_DIST)) || sensor->isConfigurable() ? (uint8_t)0 : HIDDEN_ROW) -#define SENSOR_PREC_ROWS (sensor->isPrecConfigurable() && sensor->unit != UNIT_FAHRENHEIT ? (uint8_t)0 : HIDDEN_ROW) -#define SENSOR_PARAM1_ROWS (sensor->unit >= UNIT_FIRST_VIRTUAL ? HIDDEN_ROW : (uint8_t)0) -#define SENSOR_PARAM2_ROWS (sensor->unit == UNIT_GPS || sensor->unit == UNIT_DATETIME || sensor->unit == UNIT_CELLS || (sensor->type==TELEM_TYPE_CALCULATED && (sensor->formula==TELEM_FORMULA_CONSUMPTION || sensor->formula==TELEM_FORMULA_TOTALIZE)) ? HIDDEN_ROW : (uint8_t)0) -#define SENSOR_PARAM3_ROWS (sensor->type == TELEM_TYPE_CALCULATED && sensor->formula < TELEM_FORMULA_MULTIPLY) ? (uint8_t)0 : HIDDEN_ROW -#define SENSOR_PARAM4_ROWS (sensor->type == TELEM_TYPE_CALCULATED && sensor->formula < TELEM_FORMULA_MULTIPLY) ? (uint8_t)0 : HIDDEN_ROW -#define SENSOR_AUTOOFFSET_ROWS (sensor->unit != UNIT_RPMS && sensor->isConfigurable() ? (uint8_t)0 : HIDDEN_ROW) -#define SENSOR_ONLYPOS_ROWS (sensor->isConfigurable() ? (uint8_t)0 : HIDDEN_ROW) -#define SENSOR_FILTER_ROWS (sensor->isConfigurable() ? (uint8_t)0 : HIDDEN_ROW) -#define SENSOR_PERSISTENT_ROWS (sensor->type == TELEM_TYPE_CALCULATED ? (uint8_t)0 : HIDDEN_ROW) +constexpr coord_t SENSOR_2ND_COLUMN = 12 * FW; +constexpr coord_t SENSOR_3RD_COLUMN = 18 * FW; void menuModelSensor(event_t event) { TelemetrySensor * sensor = & g_model.telemetrySensors[s_currIdx]; - SUBMENU(STR_MENUSENSOR, SENSOR_FIELD_MAX, {0, 0, sensor->type == TELEM_TYPE_CALCULATED ? (uint8_t)0 : (uint8_t)1, SENSOR_UNIT_ROWS, SENSOR_PREC_ROWS, SENSOR_PARAM1_ROWS, SENSOR_PARAM2_ROWS, SENSOR_PARAM3_ROWS, SENSOR_PARAM4_ROWS, SENSOR_AUTOOFFSET_ROWS, SENSOR_ONLYPOS_ROWS, SENSOR_FILTER_ROWS, SENSOR_PERSISTENT_ROWS, 0 }); + SUBMENU(STR_MENUSENSOR, SENSOR_FIELD_MAX, { + 0, // Name + 0, // Type + sensor->type == TELEM_TYPE_CALCULATED ? (uint8_t)0 : (uint8_t)1, // ID / Formula + ((sensor->type == TELEM_TYPE_CALCULATED && (sensor->formula == TELEM_FORMULA_DIST)) || sensor->isConfigurable() ? (uint8_t)0 : HIDDEN_ROW), // Unit + (sensor->isPrecConfigurable() && sensor->unit != UNIT_FAHRENHEIT ? (uint8_t)0 : HIDDEN_ROW), // Precision + (sensor->unit >= UNIT_FIRST_VIRTUAL ? HIDDEN_ROW : (uint8_t)0), // Param1 + (sensor->unit == UNIT_GPS || sensor->unit == UNIT_DATETIME || sensor->unit == UNIT_CELLS || (sensor->type==TELEM_TYPE_CALCULATED && (sensor->formula==TELEM_FORMULA_CONSUMPTION || sensor->formula==TELEM_FORMULA_TOTALIZE)) ? HIDDEN_ROW : (uint8_t)0), // Param2 + (sensor->type == TELEM_TYPE_CALCULATED && sensor->formula < TELEM_FORMULA_MULTIPLY) ? (uint8_t)0 : HIDDEN_ROW, // Param3 + (sensor->type == TELEM_TYPE_CALCULATED && sensor->formula < TELEM_FORMULA_MULTIPLY) ? (uint8_t)0 : HIDDEN_ROW, // Param4 + (sensor->unit != UNIT_RPMS && sensor->isConfigurable() ? (uint8_t)0 : HIDDEN_ROW), // Auto offset + (sensor->isConfigurable() ? (uint8_t)0 : HIDDEN_ROW), // Only positive + (sensor->isConfigurable() ? (uint8_t)0 : HIDDEN_ROW), // Filter + (sensor->type == TELEM_TYPE_CALCULATED ? (uint8_t)0 : HIDDEN_ROW), // Persistent + 0 // Logs + }); + lcdDrawNumber(PSIZE(TR_MENUSENSOR)*FW+1, 0, s_currIdx+1, INVERS|LEFT); - if (!isGPSSensor(s_currIdx+1)) drawSensorCustomValue(SENSOR_2ND_COLUMN, 0, s_currIdx, getValue(MIXSRC_FIRST_TELEM+3*s_currIdx), LEFT); + if (!isGPSSensor(s_currIdx+1)) + drawSensorCustomValue(SENSOR_2ND_COLUMN, 0, s_currIdx, getValue(MIXSRC_FIRST_TELEM+3*s_currIdx), LEFT); int8_t sub = menuVerticalPosition; @@ -417,7 +419,7 @@ void menuModelTelemetryFrsky(event_t event) } } - MENU(STR_MENUTELEMETRY, menuTabModel, MENU_MODEL_TELEMETRY_FRSKY, HEADER_LINE+ITEM_TELEMETRY_MAX, { HEADER_LINE_COLUMNS TELEMETRY_TYPE_ROWS CHANNELS_ROWS RSSI_ROWS SENSORS_ROWS USRDATA_ROWS VARIO_ROWS TELEMETRY_SCREENS_ROWS }); + MENU(STR_MENUTELEMETRY, menuTabModel, MENU_MODEL_TELEMETRY_FRSKY, HEADER_LINE+ITEM_TELEMETRY_MAX, { HEADER_LINE_COLUMNS TELEMETRY_TYPE_ROWS CHANNELS_ROWS RSSI_ROWS SENSORS_ROWS USRDATA_ROWS VARIO_ROWS}); uint8_t sub = menuVerticalPosition - HEADER_LINE; diff --git a/radio/src/io/frsky_sport.cpp b/radio/src/io/frsky_sport.cpp deleted file mode 100644 index 6dd6e9c77..000000000 --- a/radio/src/io/frsky_sport.cpp +++ /dev/null @@ -1,52 +0,0 @@ -/* - * Copyright (C) OpenTX - * - * Based on code named - * th9x - http://code.google.com/p/th9x - * er9x - http://code.google.com/p/er9x - * gruvin9x - http://code.google.com/p/gruvin9x - * - * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html - * - * 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" - -#warning "This need to be adapted" - -/*void sportOutputPushByte(uint8_t byte) -{ - if (byte == 0x7E || byte == 0x7D) { - outputTelemetryBuffer.push(0x7D); - outputTelemetryBuffer.push(0x20 ^ byte); - } - else { - outputTelemetryBuffer.push(byte); - } -} - -void sportOutputPushPacket(SportTelemetryPacket * packet) -{ - uint16_t crc = 0; - - for (uint8_t i=1; iraw[i]; - sportOutputPushByte(byte); - crc += byte; // 0-1FF - crc += crc >> 8; // 0-100 - crc &= 0x00ff; - } - - outputTelemetryBuffer.push(0xFF - crc); - outputTelemetryBuffer.setTrigger(packet->raw[0]); // physicalId - outputTelemetryBuffer.setDestination(SPORT_MODULE); -} -*/ \ No newline at end of file diff --git a/radio/src/targets/common/arm/CMakeLists.txt b/radio/src/targets/common/arm/CMakeLists.txt index d60248f3a..da513bb66 100644 --- a/radio/src/targets/common/arm/CMakeLists.txt +++ b/radio/src/targets/common/arm/CMakeLists.txt @@ -140,7 +140,6 @@ set(SRC main.cpp tasks.cpp audio.cpp - io/frsky_sport.cpp telemetry/telemetry.cpp telemetry/telemetry_holders.cpp telemetry/telemetry_sensors.cpp diff --git a/radio/src/targets/taranis/telemetry_driver.cpp b/radio/src/targets/taranis/telemetry_driver.cpp index 67056ee10..3fc7ca168 100644 --- a/radio/src/targets/taranis/telemetry_driver.cpp +++ b/radio/src/targets/taranis/telemetry_driver.cpp @@ -159,8 +159,7 @@ extern "C" void TELEMETRY_USART_IRQHandler(void) if (telemetryProtocol == PROTOCOL_TELEMETRY_FRSKY_SPORT) { static uint8_t prevdata; if (prevdata == 0x7E && outputTelemetryBuffer.destination.value == TELEMETRY_ENDPOINT_SPORT && data == outputTelemetryBuffer.sport.physicalId) { - #warning "bytestuffing + buffer send" - // sportSendBuffer(outputTelemetryBuffer.data, outputTelemetryBuffer.size); + sportSendBuffer(outputTelemetryBuffer.data, outputTelemetryBuffer.size); } prevdata = data; } diff --git a/radio/src/telemetry/frsky_sport.cpp b/radio/src/telemetry/frsky_sport.cpp index 29df3bd8c..4eb40661f 100644 --- a/radio/src/telemetry/frsky_sport.cpp +++ b/radio/src/telemetry/frsky_sport.cpp @@ -150,6 +150,27 @@ void sportProcessTelemetryPacket(const uint8_t * packet) sportProcessTelemetryPacketWithoutCrc(TELEMETRY_ENDPOINT_SPORT, packet); } +void sportOutputByteStuffAndCrc() +{ + uint16_t crc = 0; + + uint8_t count = sizeof(SportTelemetryPacket); + for (uint8_t i = 1/*no bytestuffing for the physicalID*/; i < count; i++) { + uint8_t byte = outputTelemetryBuffer.data[i]; + if (byte == 0x7E || byte == 0x7D) { + memmove(&outputTelemetryBuffer.data[i + 2], &outputTelemetryBuffer.data[i + 1], sizeof(SportTelemetryPacket) - i - 1); + outputTelemetryBuffer.data[i] = 0x7D; + outputTelemetryBuffer.data[i + 1] = 0x20 ^ byte; + i += 1, count += 1; + } + crc += byte; // 0-1FF + crc += crc >> 8; // 0-100 + crc &= 0x00ff; + } + outputTelemetryBuffer.data[count] = 0xFF - crc; + outputTelemetryBuffer.size = count + 1; +} + void sportProcessTelemetryPacketWithoutCrc(uint8_t origin, const uint8_t * packet) { uint8_t physicalId = packet[0] & 0x1F; @@ -159,8 +180,10 @@ void sportProcessTelemetryPacketWithoutCrc(uint8_t origin, const uint8_t * packe if (primId == DATA_FRAME) { if (outputTelemetryBuffer.destination.value == TELEMETRY_ENDPOINT_ANY && dataId == outputTelemetryBuffer.sport.dataId) { - outputTelemetryBuffer.sport.physicalId = packet[0]; - outputTelemetryBuffer.destination.value = origin; + // outputTelemetryBuffer.sport.physicalId = packet[0]; + if (origin == SPORT_MODULE) + sportOutputByteStuffAndCrc(); + outputTelemetryBuffer.setDestination(origin); } uint8_t instance = physicalId + 1; if (dataId == RSSI_ID && isValidIdAndInstance(RSSI_ID, instance)) { diff --git a/radio/src/telemetry/telemetry.h b/radio/src/telemetry/telemetry.h index 4cba60cc3..c8f16d62b 100644 --- a/radio/src/telemetry/telemetry.h +++ b/radio/src/telemetry/telemetry.h @@ -190,6 +190,7 @@ class OutputTelemetryBuffer { SportTelemetryPacket sport; uint8_t data[16]; }; + uint8_t size; TelemetryEndpoint destination; };