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

FrSky S.PORT upstream (#3426)

Browse Source 
FrSky S.PORT upstream
This commit is contained in:
Bertrand Songis
2016-04-29 18:44:25 +02:00
parent 56f59196ea
commit bd87028fd5
8 changed files with 303 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files
radio/src/lua/api_general.cpp
+27 -19
View File
@@ -296,40 +296,48 @@ bool luaFindFieldByName(const char * name, LuaField & field, unsigned int flags)
static int luaTelemetryPop(lua_State *L)
{
if (!luaInputTelemetryFifo) {
luaInputTelemetryFifo = new Fifo<LuaTelemetryValue, 16>();
luaInputTelemetryFifo = new Fifo<LuaTelemetryPacket, 16>();
if (!luaInputTelemetryFifo) {
return 0;
}
}
LuaTelemetryValue value;
if (luaInputTelemetryFifo->pop(value)) {
lua_pushnumber(L, value.id);
lua_pushunsigned(L, value.value);
return 2;
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);
return 4;
}
return 0;
}
#define BIT(x, index) (((x) >> index) & 0x01)
uint8_t getDataId(uint8_t physicalId)
{
uint8_t result = physicalId;
result += (BIT(physicalId, 0) ^ BIT(physicalId, 1) ^ BIT(physicalId, 2)) << 5;
result += (BIT(physicalId, 2) ^ BIT(physicalId, 3) ^ BIT(physicalId, 4)) << 6;
result += (BIT(physicalId, 0) ^ BIT(physicalId, 2) ^ BIT(physicalId, 4)) << 7;
return result;
}
static int luaTelemetryPush(lua_State *L)
{
if (!luaOutputTelemetryFifo) {
luaOutputTelemetryFifo = new Fifo<LuaTelemetryValue, 16>();
if (!luaOutputTelemetryFifo) {
return 0;
}
if (luaOutputTelemetryPacket.physicalId != 0x7E) {
lua_pushboolean(L, false);
return 1;
}
uint8_t id = luaL_checkunsigned(L, 1);
uint32_t value = luaL_checkunsigned(L, 2);
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);
#if defined __GNUC__
// TODO remove this ifdef when we have updated to MSVC recent version
luaOutputTelemetryFifo->push((LuaTelemetryValue){ id, value });
#endif
return 0;
lua_pushboolean(L, true);
return 1;
}
/*luadoc
radio/src/targets/taranis/bootloader/CMakeLists.txt
+1 -1
View File
@@ -62,7 +62,7 @@ set(BOOTLOADER_SRC
boot.cpp
)
remove_definitions(-DUSB_JOYSTICK -DUSB_SERIAL -DDEBUG)
remove_definitions(-DUSB_JOYSTICK -DUSB_SERIAL -DLUA -DDEBUG)
add_definitions(-DBOOT -DUSB_MASS_STORAGE)
set(CMAKE_EXE_LINKER_FLAGS "-mcpu=${MCU} -mthumb -nostartfiles -lm -T${RADIO_SRC_DIRECTORY}/targets/taranis/stm32_ramboot.ld -Wl,-Map=bootloader.map,--cref,--no-warn-mismatch,--gc-sections")
radio/src/targets/taranis/telemetry_driver.cpp
+10
View File
@@ -144,6 +144,16 @@ extern "C" void TELEMETRY_USART_IRQHandler(void)
}
else {
telemetryFifo.push(data);
#if defined(LUA)
if (telemetryProtocol == PROTOCOL_FRSKY_SPORT) {
static uint8_t prevdata;
if (prevdata == 0x7E && data == luaOutputTelemetryPacket.physicalId) {
sportSendLuaPacket(luaOutputTelemetryPacket);
luaOutputTelemetryPacket.physicalId = 0x7E;
}
prevdata = data;
}
#endif
}
status = TELEMETRY_USART->SR;
}
radio/src/telemetry/frsky.h
+27 -6
View File
@@ -418,7 +418,8 @@ void frskyDSendNextAlarm();
void frskyDProcessPacket(uint8_t *packet);
// FrSky S.PORT Protocol
void processSportPacket(uint8_t *packet);
void processSportPacket(uint8_t * packet);
#if defined(PCBTARANIS)
void sportFirmwareUpdate(ModuleIndex module, const char *filename);
#endif
@@ -483,14 +484,34 @@ void frskyUpdateCells();
void processFrskyTelemetryData(uint8_t data);
#if defined(LUA)
struct LuaTelemetryValue
struct LuaTelemetryPacket
{
uint8_t id;
uint32_t value;
LuaTelemetryPacket() { };
LuaTelemetryPacket(uint8_t physicalId, uint8_t primId, uint16_t dataId, uint32_t value):
physicalId(physicalId),
primId(primId),
dataId(dataId),
value(value)
{
}
PACK(struct {
uint8_t physicalId;
union {
PACK(struct {
uint8_t primId;
uint16_t dataId;
uint32_t value;
uint8_t crc;
});
uint8_t raw[8];
};
});
};
extern Fifo<LuaTelemetryValue, 16> * luaInputTelemetryFifo;
extern Fifo<LuaTelemetryValue, 16> * luaOutputTelemetryFifo;
extern Fifo<LuaTelemetryPacket, 16> * luaInputTelemetryFifo;
extern LuaTelemetryPacket luaOutputTelemetryPacket;
void sportSendLuaPacket(LuaTelemetryPacket & packet);
#endif
#endif // _FRSKY_H_
radio/src/telemetry/frsky_sport.cpp
+45 -12
View File
@@ -127,7 +127,7 @@ bool intPwr, extPwr;
uint16_t servosState;
uint16_t rboxState;
void processSportUpdatePacket(uint8_t *packet)
void processSportUpdatePacket(uint8_t * packet)
{
if (packet[0]==0x5E && packet[1]==0x50) {
switch (packet[2]) {
@@ -190,11 +190,40 @@ void processSportPacket(uint16_t id, uint8_t subId, uint8_t instance, uint32_t d
}
}
uint8_t sportUpdatePacket[16] __DMA;
#if defined(LUA)
// TODO merge it with S.PORT update function when finished
void sportSendLuaPacket(LuaTelemetryPacket & packet)
{
uint8_t * ptr = sportUpdatePacket;
// *ptr++ = 0x7E;
// *ptr++ = 0x1A;
uint16_t crc = 0;
for (uint8_t i=0; i<7; i++) {
if (packet.raw[i] == 0x7E || packet.raw[i] == 0x7D) {
*ptr++ = 0x7D;
*ptr++ = 0x20 ^ packet.raw[i];
}
else {
*ptr++ = packet.raw[i];
}
crc += packet.raw[i]; //0-1FF
crc += crc >> 8; //0-100
crc &= 0x00ff;
}
*ptr++ = 0xFF-crc;
sportSendBuffer(sportUpdatePacket, ptr-sportUpdatePacket);
}
#endif
void processSportPacket(uint8_t * packet)
{
uint8_t instance = (packet[0] & 0x1F) + 1;
uint8_t prim = packet[1];
uint16_t id = *((uint16_t *)(packet+2));
uint8_t physicalId = packet[0] & 0x1F;
uint8_t primId = packet[1];
uint16_t id = *((uint16_t *)(packet+2));
uint32_t data = SPORT_DATA_S32(packet);
#if defined(CPUSTM32) && !defined(SIMU)
if (sportUpdateState != SPORT_IDLE) {
@@ -209,9 +238,8 @@ void processSportPacket(uint8_t * packet)
return;
}
if (prim == DATA_FRAME) {
uint32_t data = SPORT_DATA_S32(packet);
if (primId == DATA_FRAME) {
uint8_t instance = physicalId + 1;
if (id == RSSI_ID) {
telemetryStreaming = TELEMETRY_TIMEOUT10ms; // reset counter only if valid packets are being detected
data = SPORT_DATA_U8(packet);
@@ -282,7 +310,7 @@ void processSportPacket(uint8_t * packet)
else if (id >= DIY_FIRST_ID && id <= DIY_LAST_ID) {
#if defined(LUA)
if (luaInputTelemetryFifo) {
luaInputTelemetryFifo->push((LuaTelemetryValue){(uint8_t)id, data});
luaInputTelemetryFifo->push(LuaTelemetryPacket(physicalId, primId, id, data));
}
#endif
}
@@ -292,6 +320,13 @@ void processSportPacket(uint8_t * packet)
}
}
}
#if defined(LUA)
else if (primId == 0x32) {
if (luaInputTelemetryFifo) {
luaInputTelemetryFifo->push(LuaTelemetryPacket(physicalId, primId, id, data));
}
}
#endif
}
void frskySportSetDefault(int index, uint16_t id, uint8_t subId, uint8_t instance)
@@ -364,16 +399,14 @@ bool sportWaitState(SportUpdateState state, int timeout)
#endif
}
void blankPacket(uint8_t *packet)
void blankPacket(uint8_t * packet)
{
memset(packet+2, 0, 6);
}
uint8_t sportUpdatePacket[16] __DMA;
void writePacket(uint8_t * packet)
{
uint8_t *ptr = sportUpdatePacket;
uint8_t * ptr = sportUpdatePacket;
*ptr++ = 0x7E;
*ptr++ = 0xFF;
packet[7] = crc16(packet, 7);
radio/src/telemetry/telemetry.cpp
+2 -2
View File
@@ -454,6 +454,6 @@ NOINLINE uint8_t getRssiAlarmValue(uint8_t alarm)
}
#if defined(LUA)
Fifo<LuaTelemetryValue, 16> * luaInputTelemetryFifo = NULL;
Fifo<LuaTelemetryValue, 16> * luaOutputTelemetryFifo = NULL;
Fifo<LuaTelemetryPacket, 16> * luaInputTelemetryFifo = NULL;
LuaTelemetryPacket luaOutputTelemetryPacket = { 0x7E, 0x0, 0x0, 0x0 };
#endif
radio/src/tests/frsky.cpp
+8 -5
View File
@@ -24,7 +24,7 @@ void frskyDProcessPacket(uint8_t *packet);
#if defined(FRSKY_SPORT)
bool checkSportPacket(uint8_t *packet);
void processSportPacket(uint8_t *packet);
void processSportPacket(uint8_t * packet);
bool checkSportPacket(uint8_t *packet);
void frskyCalculateCellStats(void);
void displayVoltagesScreen();
@@ -173,14 +173,17 @@ TEST(FrSky, HubAltNegative)
processHubPacket(BARO_ALT_AP_ID, 05);
EXPECT_EQ(telemetryItems[0].value, 120);
}
#endif // #if defined(FRSKY) && defined(CPUARM)
#endif // defined(FRSKY) && defined(CPUARM)
#if defined(FRSKY_SPORT)
TEST(FrSkySPORT, checkCrc)
{
// uint8_t pkt[] = { 0x7E, 0x98, 0x10, 0x10, 0x00, 0x7D, 0x5E, 0x02, 0x00, 0x00, 0x5F };
uint8_t pkt[] = { 0x7E, 0x98, 0x10, 0x10, 0x00, 0x7E, 0x02, 0x00, 0x00, 0x5F };
EXPECT_EQ(checkSportPacket(pkt+1), true);
// Packet downstream
uint8_t pkt1[] = { 0x7E, 0x98, 0x10, 0x10, 0x00, 0x7E, 0x02, 0x00, 0x00, 0x5F };
EXPECT_EQ(checkSportPacket(pkt1+1), true);
// Packet upstream
uint8_t pkt2[] = { 0x7E, 0x1C, 0x31, 0x00, 0x10, 0x85, 0x64, 0x00, 0x00, 0xD4 };
EXPECT_EQ(checkSportPacket(pkt2+1), true);
}
void setSportPacketCrc(uint8_t * packet)