Mirror/opentx
1
0
Fork 0
mirror of https://github.com/opentx/opentx.git synced 2025-07-15 04:15:26 +03:00
Code Issues Wiki Activity

[Crossfire] Upstream link development started

Browse source
This commit is contained in:
Bertrand Songis 2016-06-11 20:05:42 +02:00
parent 8523a2fdaf
commit 862fa5525e
11 changed files with 192 additions and 85 deletions
Download patch file Download diff file Expand all files Collapse all files

6
radio/sdcard/taranis/S6R/S6R.lua
View file

@ -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

74
radio/src/lua/api_general.cpp
View file

@ -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<LuaTelemetryPacket, 16>();
@ -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<LuaTelemetryPacket, 16>();
if (!luaInputTelemetryFifo) {
return 0;
}
}
LuaTelemetryPacket packet;
if (luaInputTelemetryFifo->pop(packet)) {
lua_pushnumber(L, packet.crossfire.command);
lua_newtable(L);
for (int i=0; i<packet.crossfire.length; i++) {
lua_pushinteger(L, i);
lua_pushinteger(L, packet.crossfire.data[i]);
lua_settable(L, -3);
}
lua_settable(L, -3);
return 2;
}
return 0;
}
static int luaCrossfireTelemetryPush(lua_State * L)
{
if (luaOutputTelemetryPacket.crossfire.command != 0) {
lua_pushboolean(L, false);
return 1;
}
luaOutputTelemetryPacket.crossfire.command = luaL_checkunsigned(L, 1);
luaL_checktype(L, 2, LUA_TTABLE);
luaOutputTelemetryPacket.crossfire.length = min<int>(sizeof(luaOutputTelemetryPacket.crossfire.data), luaL_len(L, 2));
for (int i=0; i<luaOutputTelemetryPacket.crossfire.length; i++) {
lua_rawgeti(L, 2, i+1);
luaOutputTelemetryPacket.crossfire.data[i] = luaL_checkunsigned(L, -1);
}
lua_pushboolean(L, true);
return 1;
@ -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 */
};

25
radio/src/pulses/crossfire.cpp
View file

@ -24,13 +24,25 @@
#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;
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; i<telemetryPacket->crossfire.length; i++) {
*buf++ = telemetryPacket->crossfire.data[i];
}
*buf++ = crc8(crc_start, telemetryPacket->crossfire.length + 1);
telemetryPacket->crossfire.clear();
}
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<CROSSFIRE_CHANNELS_COUNT; i++) {
@ -43,5 +55,8 @@ void createCrossfireFrame(uint8_t * frame, int16_t * pulses)
bitsavailable -= 8;
}
}
*buf = crc8(&frame[2], 23);
*buf++ = crc8(crc_start, 23);
}
return buf - frame;
}

8
radio/src/pulses/pulses_arm.cpp
View file

@ -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;

6
radio/src/pulses/pulses_arm.h
View file

@ -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

4
radio/src/targets/taranis/telemetry_driver.cpp
View file

@ -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;
}

15
radio/src/telemetry/crossfire.cpp
View file

@ -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<min<int>(sizeof(luaPacket.crossfire.data), luaPacket.crossfire.length); i++) {
luaPacket.crossfire.data[i] = telemetryRxBuffer[3+i];
}
luaInputTelemetryFifo->push(luaPacket);
}
break;
#endif
}
}

4
radio/src/telemetry/crossfire.h
View file

@ -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);

31
radio/src/telemetry/frsky.h
View file

@ -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 {
union {
struct {
uint8_t physicalId;
union {
PACK(struct {
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<LuaTelemetryPacket, 16> * luaInputTelemetryFifo;

22
radio/src/telemetry/frsky_sport.cpp
View file

@ -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

4
radio/src/telemetry/telemetry.cpp
View file

@ -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<LuaTelemetryPacket, 16> * luaInputTelemetryFifo = NULL;
LuaTelemetryPacket luaOutputTelemetryPacket = { 0x7E, 0x0, 0x0, 0x0 };
LuaTelemetryPacket luaOutputTelemetryPacket;
#endif