diff --git a/radio/sdcard/taranis/S6R/S6R.lua b/radio/sdcard/taranis/S6R/S6R.lua index caa682f97..589bf0c9a 100644 --- a/radio/sdcard/taranis/S6R/S6R.lua +++ b/radio/sdcard/taranis/S6R/S6R.lua @@ -120,11 +120,11 @@ local function redrawFieldsPage() end local function telemetryRead(field) - return telemetryPush(0x1A, 0x30, 0x0C30, field) + return sportTelemetryPush(0x1A, 0x30, 0x0C30, field) end local function telemetryWrite(field, value) - return telemetryPush(0x1A, 0x31, 0x0C30, field + value*256) + return sportTelemetryPush(0x1A, 0x31, 0x0C30, field + value*256) end local telemetryPopTimeout = 0 @@ -144,7 +144,7 @@ local function refreshNext() end end elseif refreshState == 1 then - physicalId, primId, dataId, value = telemetryPop() + physicalId, primId, dataId, value = sportTelemetryPop() if physicalId == 0x1A and primId == 0x32 and dataId == 0x0C30 then fieldId = value % 256 if calibrationState == 2 then diff --git a/radio/src/lua/api_general.cpp b/radio/src/lua/api_general.cpp index 7e9c46467..01762831b 100644 --- a/radio/src/lua/api_general.cpp +++ b/radio/src/lua/api_general.cpp @@ -85,7 +85,7 @@ minor: 1 rev: 7 ``` */ -static int luaGetVersion(lua_State *L) +static int luaGetVersion(lua_State * L) { lua_pushstring(L, VERSION); lua_pushstring(L, RADIO_VERSION); @@ -105,13 +105,13 @@ run time: 12.54 seconds, return value: 1254 @status current Introduced in 2.0.0 */ -static int luaGetTime(lua_State *L) +static int luaGetTime(lua_State * L) { lua_pushunsigned(L, get_tmr10ms()); return 1; } -static void luaPushDateTime(lua_State *L, uint32_t year, uint32_t mon, uint32_t day, +static void luaPushDateTime(lua_State * L, uint32_t year, uint32_t mon, uint32_t day, uint32_t hour, uint32_t min, uint32_t sec) { lua_createtable(L, 0, 6); @@ -136,7 +136,7 @@ Return current system date and time that is kept by the RTC unit * `min` (number) minutes * `sec` (number) seconds */ -static int luaGetDateTime(lua_State *L) +static int luaGetDateTime(lua_State * L) { struct gtm utm; gettime(&utm); @@ -296,7 +296,7 @@ bool luaFindFieldByName(const char * name, LuaField & field, unsigned int flags) return false; // not found } -static int luaTelemetryPop(lua_State *L) +static int luaSportTelemetryPop(lua_State * L) { if (!luaInputTelemetryFifo) { luaInputTelemetryFifo = new Fifo(); @@ -307,10 +307,10 @@ static int luaTelemetryPop(lua_State *L) LuaTelemetryPacket packet; if (luaInputTelemetryFifo->pop(packet)) { - lua_pushnumber(L, packet.physicalId); - lua_pushnumber(L, packet.primId); - lua_pushnumber(L, packet.dataId); - lua_pushunsigned(L, packet.value); + lua_pushnumber(L, packet.sport.physicalId); + lua_pushnumber(L, packet.sport.primId); + lua_pushnumber(L, packet.sport.dataId); + lua_pushunsigned(L, packet.sport.value); return 4; } @@ -327,17 +327,61 @@ uint8_t getDataId(uint8_t physicalId) return result; } -static int luaTelemetryPush(lua_State *L) +static int luaSportTelemetryPush(lua_State * L) { - if (luaOutputTelemetryPacket.physicalId != 0x7E) { + if (luaOutputTelemetryPacket.sport.physicalId != 0x7E) { lua_pushboolean(L, false); return 1; } - luaOutputTelemetryPacket.physicalId = getDataId(luaL_checkunsigned(L, 1)); - luaOutputTelemetryPacket.primId = luaL_checkunsigned(L, 2); - luaOutputTelemetryPacket.dataId = luaL_checkunsigned(L, 3); - luaOutputTelemetryPacket.value = luaL_checkunsigned(L, 4); + luaOutputTelemetryPacket.sport.physicalId = getDataId(luaL_checkunsigned(L, 1)); + luaOutputTelemetryPacket.sport.primId = luaL_checkunsigned(L, 2); + luaOutputTelemetryPacket.sport.dataId = luaL_checkunsigned(L, 3); + luaOutputTelemetryPacket.sport.value = luaL_checkunsigned(L, 4); + + lua_pushboolean(L, true); + return 1; +} + +static int luaCrossfireTelemetryPop(lua_State * L) +{ + if (!luaInputTelemetryFifo) { + luaInputTelemetryFifo = new Fifo(); + if (!luaInputTelemetryFifo) { + return 0; + } + } + + LuaTelemetryPacket packet; + if (luaInputTelemetryFifo->pop(packet)) { + lua_pushnumber(L, packet.crossfire.command); + lua_newtable(L); + for (int i=0; i(sizeof(luaOutputTelemetryPacket.crossfire.data), luaL_len(L, 2)); + for (int i=0; i= MAX_FLIGHT_MODES) { @@ -482,7 +526,7 @@ to the path (example: for English language: `/SOUNDS/en` is appended) @status current Introduced in 2.0.0, changed in 2.1.0 */ -static int luaPlayFile(lua_State *L) +static int luaPlayFile(lua_State * L) { const char * filename = luaL_checkstring(L, 1); if (filename[0] != '/') { @@ -575,7 +619,7 @@ OpenTX 2.1: | 24 | Seconds | 162 | */ -static int luaPlayNumber(lua_State *L) +static int luaPlayNumber(lua_State * L) { int number = luaL_checkinteger(L, 1); int unit = luaL_checkinteger(L, 2); @@ -597,7 +641,7 @@ Play a time value (text to speech) @status current Introduced in 2.1.0 */ -static int luaPlayDuration(lua_State *L) +static int luaPlayDuration(lua_State * L) { int duration = luaL_checkinteger(L, 1); bool playTime = (luaL_optinteger(L, 2, 0) != 0); @@ -627,7 +671,7 @@ The valid range is from -127 to 127. @status current Introduced in 2.1.0 */ -static int luaPlayTone(lua_State *L) +static int luaPlayTone(lua_State * L) { int frequency = luaL_checkinteger(L, 1); int length = luaL_checkinteger(L, 2); @@ -649,7 +693,7 @@ Stops key state machine. TODO table of events/masks */ -static int luaKillEvents(lua_State *L) +static int luaKillEvents(lua_State * L) { uint8_t key = EVT_KEY_MASK(luaL_checkinteger(L, 1)); // prevent killing maskable keys (only in telemetry scripts) @@ -669,7 +713,7 @@ Returns gray value which can be used in LCD functions @retval (number) a value that represents amount of *greyness* (from 0 to 15) */ -static int luaGrey(lua_State *L) +static int luaGrey(lua_State * L) { int index = luaL_checkinteger(L, 1); lua_pushunsigned(L, GREY(index)); @@ -694,7 +738,7 @@ Returns (some of) the general radio settings `language` and `voice` added int 2.2.0. */ -static int luaGetGeneralSettings(lua_State *L) +static int luaGetGeneralSettings(lua_State * L) { lua_newtable(L); lua_pushtablenumber(L, "battMin", double(90+g_eeGeneral.vBatMin)/10); @@ -731,7 +775,7 @@ Run function (key pressed) @status current Introduced in 2.0.0 */ -static int luaPopupInput(lua_State *L) +static int luaPopupInput(lua_State * L) { uint8_t event = luaL_checkinteger(L, 2); warningInputValue = luaL_checkinteger(L, 3); @@ -765,7 +809,7 @@ Get stick that is assigned to a channel. See Default Channel Order in General Se @status current Introduced in 2.0.0 */ -static int luaDefaultStick(lua_State *L) +static int luaDefaultStick(lua_State * L) { uint8_t channel = luaL_checkinteger(L, 1); lua_pushinteger(L, channel_order(channel+1)-1); @@ -785,7 +829,7 @@ Get channel assigned to stick. See Default Channel Order in General Settings @status current Introduced in 2.0.0 */ -static int luaDefaultChannel(lua_State *L) +static int luaDefaultChannel(lua_State * L) { uint8_t stick = luaL_checkinteger(L, 1); for (int i=1; i<=4; i++) { @@ -818,8 +862,12 @@ const luaL_Reg opentxLib[] = { #if !defined(COLORLCD) { "GREY", luaGrey }, #endif - { "telemetryPop", luaTelemetryPop }, - { "telemetryPush", luaTelemetryPush }, + { "sportTelemetryPop", luaSportTelemetryPop }, + { "sportTelemetryPush", luaSportTelemetryPush }, +#if defined(CROSSFIRE) + { "crossfireTelemetryPop", luaCrossfireTelemetryPop }, + { "crossfireTelemetryPush", luaCrossfireTelemetryPush }, +#endif { NULL, NULL } /* sentinel */ }; diff --git a/radio/src/pulses/crossfire.cpp b/radio/src/pulses/crossfire.cpp index 837cbe708..1810648ae 100644 --- a/radio/src/pulses/crossfire.cpp +++ b/radio/src/pulses/crossfire.cpp @@ -24,24 +24,39 @@ #define CROSSFIRE_CH_BITS 11 // Range for pulses (channels output) is [-1024:+1024] -void createCrossfireFrame(uint8_t * frame, int16_t * pulses) +uint8_t createCrossfireFrame(uint8_t * frame, int16_t * pulses, LuaTelemetryPacket * telemetryPacket) { uint8_t * buf = frame; - *buf++ = BROADCAST_ADDRESS; - *buf++ = 24; // 1(ID) + 22 + 1(CRC) - *buf++ = CHANNELS_ID; + *buf++ = MODULE_ADDRESS; - uint32_t bits = 0; - uint8_t bitsavailable = 0; - for (int i=0; i= 8) { - *buf++ = bits; - bits >>= 8; - bitsavailable -= 8; + if (telemetryPacket->crossfire.command) { + *buf++ = 2 + telemetryPacket->crossfire.length; // 1(ID) + telemetryPacket->crossfire.length + 1(CRC) + uint8_t * crc_start = buf; + *buf++ = telemetryPacket->crossfire.command; + for (int i=0; icrossfire.length; i++) { + *buf++ = telemetryPacket->crossfire.data[i]; } + *buf++ = crc8(crc_start, telemetryPacket->crossfire.length + 1); + telemetryPacket->crossfire.clear(); } - *buf = crc8(&frame[2], 23); + else { + *buf++ = 24; // 1(ID) + 22 + 1(CRC) + uint8_t * crc_start = buf; + *buf++ = CHANNELS_ID; + uint32_t bits = 0; + uint8_t bitsavailable = 0; + for (int i=0; i= 8) { + *buf++ = bits; + bits >>= 8; + bitsavailable -= 8; + } + } + *buf++ = crc8(crc_start, 23); + } + + return buf - frame; } diff --git a/radio/src/pulses/pulses_arm.cpp b/radio/src/pulses/pulses_arm.cpp index adfc3030c..a87c793ec 100644 --- a/radio/src/pulses/pulses_arm.cpp +++ b/radio/src/pulses/pulses_arm.cpp @@ -28,6 +28,10 @@ uint8_t moduleFlag[NUM_MODULES] = { 0 }; ModulePulsesData modulePulsesData[NUM_MODULES] __DMA; TrainerPulsesData trainerPulsesData __DMA; +#if defined(CROSSFIRE) +uint8_t createCrossfireFrame(uint8_t * frame, int16_t * pulses, LuaTelemetryPacket * telemetry); +#endif + uint8_t getRequiredProtocol(uint8_t port) { uint8_t required_protocol; @@ -177,8 +181,8 @@ void setupPulses(uint8_t port) case PROTO_CROSSFIRE: if (telemetryProtocol == PROTOCOL_PULSES_CROSSFIRE && !init_needed) { uint8_t * crossfire = modulePulsesData[port].crossfire.pulses; - createCrossfireFrame(crossfire, &channelOutputs[g_model.moduleData[port].channelsStart]); - sportSendBuffer(crossfire, CROSSFIRE_FRAME_LEN); + uint8_t len = createCrossfireFrame(crossfire, &channelOutputs[g_model.moduleData[port].channelsStart], &luaOutputTelemetryPacket); + sportSendBuffer(crossfire, len); } scheduleNextMixerCalculation(port, CROSSFIRE_FRAME_PERIOD); break; diff --git a/radio/src/pulses/pulses_arm.h b/radio/src/pulses/pulses_arm.h index eec7e37a2..b6925a1a9 100644 --- a/radio/src/pulses/pulses_arm.h +++ b/radio/src/pulses/pulses_arm.h @@ -100,10 +100,10 @@ PACK(struct Dsm2TimerPulsesData { #define CROSSFIRE_BAUDRATE 400000 #define CROSSFIRE_FRAME_PERIOD 4 // 4ms -#define CROSSFIRE_FRAME_LEN (3+22+1) +#define CROSSFIRE_FRAME_MAXLEN 64 #define CROSSFIRE_CHANNELS_COUNT 16 PACK(struct CrossfirePulsesData { - uint8_t pulses[CROSSFIRE_FRAME_LEN]; + uint8_t pulses[CROSSFIRE_FRAME_MAXLEN]; }); union ModulePulsesData { @@ -146,8 +146,6 @@ void sendByteDsm2(uint8_t b); void putDsm2Flush(); void putDsm2SerialBit(uint8_t bit); -void createCrossfireFrame(uint8_t * frame, int16_t * pulses); - #if defined(HUBSAN) void Hubsan_Init(); #endif diff --git a/radio/src/targets/taranis/telemetry_driver.cpp b/radio/src/targets/taranis/telemetry_driver.cpp index efaf25754..5f830376c 100644 --- a/radio/src/targets/taranis/telemetry_driver.cpp +++ b/radio/src/targets/taranis/telemetry_driver.cpp @@ -147,9 +147,9 @@ extern "C" void TELEMETRY_USART_IRQHandler(void) #if defined(LUA) if (telemetryProtocol == PROTOCOL_FRSKY_SPORT) { static uint8_t prevdata; - if (prevdata == 0x7E && data == luaOutputTelemetryPacket.physicalId) { + if (prevdata == 0x7E && data == luaOutputTelemetryPacket.sport.physicalId) { sportSendLuaPacket(luaOutputTelemetryPacket); - luaOutputTelemetryPacket.physicalId = 0x7E; + luaOutputTelemetryPacket.sport.clear(); } prevdata = data; } diff --git a/radio/src/telemetry/crossfire.cpp b/radio/src/telemetry/crossfire.cpp index 45c45b2b6..ec0418fa9 100644 --- a/radio/src/telemetry/crossfire.cpp +++ b/radio/src/telemetry/crossfire.cpp @@ -192,6 +192,21 @@ void processCrossfireTelemetryFrame() } break; } + +#if defined(LUA) + default: + if (luaInputTelemetryFifo) { + LuaTelemetryPacket luaPacket; + luaPacket.crossfire.command = id; + luaPacket.crossfire.length = telemetryRxBuffer[1]-2; + for (int i=0; i(sizeof(luaPacket.crossfire.data), luaPacket.crossfire.length); i++) { + luaPacket.crossfire.data[i] = telemetryRxBuffer[3+i]; + } + luaInputTelemetryFifo->push(luaPacket); + } + break; +#endif + } } diff --git a/radio/src/telemetry/crossfire.h b/radio/src/telemetry/crossfire.h index f201800a6..de9c08267 100644 --- a/radio/src/telemetry/crossfire.h +++ b/radio/src/telemetry/crossfire.h @@ -23,6 +23,7 @@ // Device address #define BROADCAST_ADDRESS 0x00 +#define MODULE_ADDRESS 0xEE // Frame id #define GPS_ID 0x02 @@ -31,6 +32,9 @@ #define CHANNELS_ID 0x16 #define ATTITUDE_ID 0x1E #define FLIGHT_MODE_ID 0x21 +#define PING_DEVICES_ID 0x28 +#define DEVICE_INFO_ID 0x29 +#define REQUEST_SETTINGS_ID 0x2A void processCrossfireTelemetryData(uint8_t data); void crossfireSetDefault(int index, uint8_t id, uint8_t subId); diff --git a/radio/src/telemetry/frsky.h b/radio/src/telemetry/frsky.h index 4f206fcc7..1041570e9 100644 --- a/radio/src/telemetry/frsky.h +++ b/radio/src/telemetry/frsky.h @@ -422,7 +422,7 @@ void frskyDProcessPacket(uint8_t *packet); void processSportPacket(uint8_t * packet); #if defined(PCBTARANIS) -void sportFirmwareUpdate(ModuleIndex module, const char *filename); +void sportFirmwareUpdate(ModuleIndex module, const char * filename); #endif void telemetryWakeup(); void telemetryReset(); @@ -487,27 +487,38 @@ void processFrskyTelemetryData(uint8_t data); #if defined(LUA) struct LuaTelemetryPacket { - LuaTelemetryPacket() { }; - LuaTelemetryPacket(uint8_t physicalId, uint8_t primId, uint16_t dataId, uint32_t value): - physicalId(physicalId), - primId(primId), - dataId(dataId), - value(value) - { - } + LuaTelemetryPacket() + { + } - PACK(struct { - uint8_t physicalId; - union { - PACK(struct { - uint8_t primId; - uint16_t dataId; - uint32_t value; - uint8_t crc; - }); - uint8_t raw[8]; - }; - }); + union { + struct { + uint8_t physicalId; + union { + struct { + uint8_t primId; + uint16_t dataId; + uint32_t value; + uint8_t crc; + }; + uint8_t raw[8]; + }; + void clear() + { + physicalId = 0x7E; + } + } sport; + + struct { + uint8_t command; + uint8_t length; + uint8_t data[8]; + void clear() + { + command = 0x00; + } + } crossfire; + }; }; extern Fifo * luaInputTelemetryFifo; diff --git a/radio/src/telemetry/frsky_sport.cpp b/radio/src/telemetry/frsky_sport.cpp index b42b0fb27..f7bd93767 100644 --- a/radio/src/telemetry/frsky_sport.cpp +++ b/radio/src/telemetry/frsky_sport.cpp @@ -201,14 +201,14 @@ void sportSendLuaPacket(LuaTelemetryPacket & packet) uint16_t crc = 0; for (uint8_t i=0; i<7; i++) { - if (packet.raw[i] == 0x7E || packet.raw[i] == 0x7D) { + if (packet.sport.raw[i] == 0x7E || packet.sport.raw[i] == 0x7D) { *ptr++ = 0x7D; - *ptr++ = 0x20 ^ packet.raw[i]; + *ptr++ = 0x20 ^ packet.sport.raw[i]; } else { - *ptr++ = packet.raw[i]; + *ptr++ = packet.sport.raw[i]; } - crc += packet.raw[i]; //0-1FF + crc += packet.sport.raw[i]; //0-1FF crc += crc >> 8; //0-100 crc &= 0x00ff; } @@ -313,7 +313,12 @@ void processSportPacket(uint8_t * packet) else if (id >= DIY_FIRST_ID && id <= DIY_LAST_ID) { #if defined(LUA) if (luaInputTelemetryFifo) { - luaInputTelemetryFifo->push(LuaTelemetryPacket(physicalId, primId, id, data)); + LuaTelemetryPacket luaPacket; + luaPacket.sport.physicalId = physicalId; + luaPacket.sport.primId = primId; + luaPacket.sport.dataId = id; + luaPacket.sport.value = data; + luaInputTelemetryFifo->push(luaPacket); } #endif } @@ -326,7 +331,12 @@ void processSportPacket(uint8_t * packet) #if defined(LUA) else if (primId == 0x32) { if (luaInputTelemetryFifo) { - luaInputTelemetryFifo->push(LuaTelemetryPacket(physicalId, primId, id, data)); + LuaTelemetryPacket luaPacket; + luaPacket.sport.physicalId = physicalId; + luaPacket.sport.primId = primId; + luaPacket.sport.dataId = id; + luaPacket.sport.value = data; + luaInputTelemetryFifo->push(luaPacket); } } #endif @@ -543,7 +553,7 @@ bool sportUpdateEnd() return sportWaitState(SPORT_COMPLETE, 2000); } -void sportFirmwareUpdate(ModuleIndex module, const char *filename) +void sportFirmwareUpdate(ModuleIndex module, const char * filename) { bool result = sportUpdatePowerOn(module); if (result) diff --git a/radio/src/telemetry/telemetry.cpp b/radio/src/telemetry/telemetry.cpp index 5ead4e842..0b0431ff4 100644 --- a/radio/src/telemetry/telemetry.cpp +++ b/radio/src/telemetry/telemetry.cpp @@ -421,6 +421,7 @@ void telemetryInit(uint8_t protocol) #if defined(CROSSFIRE) else if (protocol == PROTOCOL_PULSES_CROSSFIRE) { telemetryPortInit(CROSSFIRE_BAUDRATE); + luaOutputTelemetryPacket.crossfire.clear(); telemetryPortSetDirectionOutput(); } #endif @@ -430,6 +431,7 @@ void telemetryInit(uint8_t protocol) } else { telemetryPortInit(FRSKY_SPORT_BAUDRATE); + luaOutputTelemetryPacket.sport.clear(); } #if defined(REVX) && !defined(SIMU) @@ -455,5 +457,5 @@ NOINLINE uint8_t getRssiAlarmValue(uint8_t alarm) #if defined(LUA) Fifo * luaInputTelemetryFifo = NULL; -LuaTelemetryPacket luaOutputTelemetryPacket = { 0x7E, 0x0, 0x0, 0x0 }; +LuaTelemetryPacket luaOutputTelemetryPacket; #endif