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

S.PORT Push now working on internal module

Browse source
This commit is contained in:
Bertrand Songis 2019-04-01 17:48:30 +02:00
parent 99c413dbac
commit 3bc75c1d55
5 changed files with 93 additions and 49 deletions
Download patch file Download diff file Expand all files Collapse all files

1
radio/sdcard/FrSky-utilities/FrSky SBEC.lua
View file

@ -244,7 +244,6 @@ local function runFieldsPage(event)
return 0
end
local function runSettingsPage(event)
fields = settingsFields
return runFieldsPage(event)

3
radio/src/datastructs.h
View file

@ -350,8 +350,7 @@ PACK(struct TelemetrySensor {
union {
PACK(struct {
uint8_t physID:5; // instance ID to allow handling multiple instances of same value type, for FrSky can be the physical ID of the sensor
uint8_t module:1;
uint8_t rx_uid:2;
uint8_t rxIndex:3;
}) frskyInstance;
uint8_t instance;
NOBACKUP(uint8_t formula);

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

@ -419,22 +419,61 @@ When called without parameters, it will only return the status of the output buf
@status current Introduced in 2.2.0
*/
TelemetryEndpoint getTelemetryEndpoint(uint8_t receiverIndex)
{
TelemetryEndpoint result;
for (uint8_t module=0; module<NUM_MODULES; module++) {
for (uint8_t receiver=0; receiver<PXX2_MAX_RECEIVERS_PER_MODULE; receiver++) {
uint8_t candidateIndex = g_model.moduleData[module].pxx2.getReceiverSlot(receiver) - 1;
if (candidateIndex == receiverIndex) {
result.module = module;
result.rxUid = candidateIndex;
return result;
}
}
}
result.value = TELEMETRY_ENDPOINT_NONE;
return result;
}
static int luaSportTelemetryPush(lua_State * L)
{
if (lua_gettop(L) == 0) {
lua_pushboolean(L, outputTelemetryBuffer.isAvailable());
return 1;
}
else if (outputTelemetryBuffer.isAvailable()) {
outputTelemetryBuffer.sport.physicalId = getDataId(luaL_checkunsigned(L, 1)); // ignored
if (outputTelemetryBuffer.isAvailable()) {
for (uint8_t i=0; i<MAX_TELEMETRY_SENSORS; i++) {
TelemetrySensor & sensor = g_model.telemetrySensors[i];
if (sensor.id == outputTelemetryBuffer.sport.dataId) {
if (sensor.frskyInstance.rxIndex == TELEMETRY_ENDPOINT_SPORT) {
SportTelemetryPacket packet;
packet.physicalId = getDataId(luaL_checkunsigned(L, 1));
packet.primId = luaL_checkunsigned(L, 2);
packet.dataId = luaL_checkunsigned(L, 3);
packet.value = luaL_checkunsigned(L, 4);
outputTelemetryBuffer.pushSportPacketWithBytestuffing(packet);
outputTelemetryBuffer.setDestination(TELEMETRY_ENDPOINT_SPORT);
}
else {
outputTelemetryBuffer.sport.physicalId = getDataId(luaL_checkunsigned(L, 1));
outputTelemetryBuffer.sport.primId = luaL_checkunsigned(L, 2);
outputTelemetryBuffer.sport.dataId = luaL_checkunsigned(L, 3);
outputTelemetryBuffer.sport.value = luaL_checkunsigned(L, 4);
outputTelemetryBuffer.setDestination(TELEMETRY_ENDPOINT_ANY);
TelemetryEndpoint destination = getTelemetryEndpoint(sensor.frskyInstance.rxIndex);
outputTelemetryBuffer.setDestination(destination.value);
}
lua_pushboolean(L, true);
return 1;
}
else {
}
}
lua_pushboolean(L, false);
}
return 1;
}

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

@ -150,42 +150,16 @@ 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)
void sportProcessTelemetryPacketWithoutCrc(uint8_t originValue, const uint8_t * packet)
{
TelemetryEndpoint & origin = (TelemetryEndpoint &)originValue;
uint8_t physicalId = packet[0] & 0x1F;
uint8_t primId = packet[1];
uint16_t dataId = *((uint16_t *)(packet+2));
uint32_t data = SPORT_DATA_S32(packet);
if (primId == DATA_FRAME) {
if (outputTelemetryBuffer.destination.value == TELEMETRY_ENDPOINT_ANY && dataId == outputTelemetryBuffer.sport.dataId) {
// outputTelemetryBuffer.sport.physicalId = packet[0];
if (origin == SPORT_MODULE)
sportOutputByteStuffAndCrc();
outputTelemetryBuffer.setDestination(origin);
}
uint8_t instance = physicalId + 1;
uint8_t instance = physicalId + (origin.rxUid << 5);
if (dataId == RSSI_ID && isValidIdAndInstance(RSSI_ID, instance)) {
telemetryStreaming = TELEMETRY_TIMEOUT10ms; // reset counter only if valid packets are being detected
data = SPORT_DATA_U8(packet);

47
radio/src/telemetry/telemetry.h
View file

@ -155,9 +155,8 @@ void logTelemetryWriteByte(uint8_t data);
#define LOG_TELEMETRY_WRITE_BYTE(data)
#endif
#define TELEMETRY_ENDPOINT_ANY 0xFF
#define TELEMETRY_ENDPOINT_NONE 0xFE
#define TELEMETRY_ENDPOINT_SPORT 0xFD
#define TELEMETRY_ENDPOINT_NONE 0xFF
#define TELEMETRY_ENDPOINT_SPORT 0x07
union TelemetryEndpoint {
PACK(struct {
@ -169,22 +168,56 @@ union TelemetryEndpoint {
class OutputTelemetryBuffer {
public:
OutputTelemetryBuffer() {
OutputTelemetryBuffer()
{
reset();
}
void setDestination(uint8_t value) {
void setDestination(uint8_t value)
{
destination.value = value;
}
void reset() {
void reset()
{
destination.value = TELEMETRY_ENDPOINT_NONE;
}
bool isAvailable() {
bool isAvailable()
{
return destination.value == TELEMETRY_ENDPOINT_NONE;
}
void pushByte(uint8_t byte)
{
data[size++] = byte;
}
void pushByteWithBytestuffing(uint8_t byte)
{
if (byte == 0x7E || byte == 0x7D) {
pushByte(0x7D);
pushByte(0x20 ^ byte);
}
else {
pushByte(byte);
}
}
void pushSportPacketWithBytestuffing(SportTelemetryPacket & packet)
{
uint16_t crc = 0;
sport.physicalId = packet.physicalId; // no bytestuffing, no CRC
for (uint8_t i=1; i<sizeof(SportTelemetryPacket); i++) {
uint8_t byte = packet.raw[i];
pushByteWithBytestuffing(byte);
crc += byte; // 0-1FF
crc += crc >> 8; // 0-100
crc &= 0x00ff;
}
pushByte(0xFF - crc);
}
public:
union {
SportTelemetryPacket sport;