Skip to content

Commit

Permalink
- cleaned code
Browse files Browse the repository at this point in the history 
- fixed taranis radios
  • Loading branch information
@wimalopaan
wimalopaan committed Feb 2, 2022
1 parent 1e988cc commit 87eb219
Show file tree
Hide file tree
Showing 12 changed files with 191 additions and 103 deletions.
1 change: 1 addition & 0 deletions radio/src/dataconstants.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -192,6 +192,7 @@ enum TrainerMode {
TRAINER_MODE_SLAVE,
#if defined(PCBTARANIS) || defined(PCBNV14)
TRAINER_MODE_MASTER_SBUS_EXTERNAL_MODULE,
TRAINER_MODE_MASTER_IBUS_EXTERNAL_MODULE,
TRAINER_MODE_MASTER_CPPM_EXTERNAL_MODULE,
#endif
#if defined(PCBTARANIS) || defined(AUX_SERIAL) || defined(AUX2_SERIAL)
Expand Down
42 changes: 22 additions & 20 deletions radio/src/ibus.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,22 +1,22 @@
#include "ibus.h"
#include "opentx.h"
#include "trainer.h"

#include <algorithm>
#include <limits>

//#define IBUS_FRAME_GAP_DELAY 2000 // 1ms

#define IBUS_VALUE_MIN 988
#define IBUS_VALUE_MAX 2011
#define IBUS_VALUE_CENTER 1500
#ifdef __GNUC__
# pragma GCC diagnostic push
# pragma GCC diagnostic error "-Wswitch" // unfortunately the project uses -Wnoswitch
#endif

namespace IBus {
struct CheckSum final {
inline void reset() {
mSum = std::numeric_limits<uint16_t>::max();
}
inline uint8_t operator+=(const uint8_t b) {
mSum -= static_cast<uint8_t>(b);
mSum -= b;
return b;
}
inline uint8_t highByte() const {
Expand All @@ -40,27 +40,27 @@ namespace IBus {
uint16_t mSum = std::numeric_limits<uint16_t>::max();
};

uint16_t clamp(uint16_t v, uint16_t lower, uint16_t upper) {
return (v < lower) ? lower : ((v > upper) ? upper: v);
}

int16_t convertIbusToPuls(uint16_t const ibusValue) {
const uint16_t clamped = clamp(ibusValue, IBUS_VALUE_MIN, IBUS_VALUE_MAX);
return (clamped - IBUS_VALUE_CENTER);
}
struct Servo {
using IBus = Trainer::Protocol::IBus;
using MesgType = IBus::MesgType;

enum class State : uint8_t {Undefined, GotStart20, Data, CheckL, CheckH};
static inline void process(const uint8_t b, std::function<void()> f) {
switch(mState) {

static inline int16_t convertIbusToPuls(uint16_t const ibusValue) {
return Trainer::clamp(ibusValue - IBus::CenterValue);
}

static inline void process(const uint8_t b, const std::function<void()> f) {
switch(mState) { // enum-switch -> no default (intentional)
case State::Undefined:
csum.reset();
if (b == 0x20) {
if (b == IBus::StartByte1) {
csum += b;
mState = State::GotStart20;
}
break;
case State::GotStart20:
if (b == 0x40) {
if (b == IBus::StartByte2) {
csum += b;
mState = State::Data;
mIndex = 0;
Expand Down Expand Up @@ -93,7 +93,7 @@ namespace IBus {
break;
}
}
static inline void convert(int16_t* pulses) {
static inline void convert(int16_t* const pulses) {
for (size_t chi{0}; chi < MAX_TRAINER_CHANNELS; chi++) {
if (chi < 14) {
const uint8_t h = ibusFrame[2 * chi + 1] & 0x0f;
Expand All @@ -111,7 +111,6 @@ namespace IBus {
}
}
private:
using MesgType = std::array<uint8_t, 28>; // 0x20, 0x40 , 28 Bytes, checkH, checkL
static CheckSum csum;
static State mState;
static MesgType ibusFrame;
Expand Down Expand Up @@ -139,3 +138,6 @@ void processIbusInput() {
#endif
}

#ifdef __GNUC__
# pragma GCC diagnostic pop
#endif
1 change: 0 additions & 1 deletion radio/src/ibus.h
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,6 @@
#pragma once

#define IBUS_BAUDRATE 115200
#define IBUS_FRAME_SIZE 32

void processIbusInput();

124 changes: 62 additions & 62 deletions radio/src/sbus.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,30 +21,23 @@

#include "opentx.h"
#include "sbus.h"
#include "trainer.h"

#define SBUS_FRAME_GAP_DELAY 1000 // 500uS

//#define SBUS_START_BYTE 0x0F
//#define SBUS_END_BYTE 0x00
//#define SBUS_FLAGS_IDX 23
#define SBUS_FRAMELOST_BIT 2
#define SBUS_FAILSAFE_BIT 3

//#define SBUS_CH_BITS 11
//#define SBUS_CH_MASK ((1<<SBUS_CH_BITS) - 1)

#define SBUS_CH_CENTER 0x3E0

#ifdef __GNUC__
# pragma GCC diagnostic push
# pragma GCC diagnostic error "-Wswitch" // unfortunately the project uses -Wnoswitch
#endif

namespace SBus {
struct Servo {
using SBus = Trainer::Protocol::SBus;
using MesgType = SBus::MesgType;
static_assert(std::tuple_size<MesgType>::value == (SBUS_FRAME_SIZE - 2), "consistency check");

enum class State : uint8_t {Undefined, Data, GotEnd, WaitEnd};

static constexpr uint8_t mPauseCount{2}; // 2ms

static constexpr uint8_t mFrameLostMask{1 << SBUS_FRAMELOST_BIT};
static constexpr uint8_t mFailSafeMask{1 << SBUS_FAILSAFE_BIT};


static inline void tick1ms() {
if (mPauseCounter > 0) {
--mPauseCounter;
Expand All @@ -53,25 +46,30 @@ namespace SBus {
mState = State::Undefined;
}
}

static inline int16_t convertSbusToPuls(uint16_t const sbusValue) {
const int16_t centered = sbusValue - SBus::CenterValue;
return Trainer::clamp((centered * 5) / 8);
}

static inline void process(const uint8_t b, std::function<void()> f) {
static inline void process(const uint8_t b, const std::function<void()> f) {
mPauseCounter = mPauseCount;
switch(mState) {
switch(mState) { // enum-switch -> no default (intentional)
case State::Undefined:
if (b == 0x00) {
if (b == SBus::EndByte) {
mState = State::GotEnd;
}
else if (b == 0x0f) {
else if (b == SBus::StartByte) {
mState = State::Data;
mIndex = 0;
}
break;
case State::GotEnd:
if (b == 0x0f) {
if (b == SBus::StartByte) {
mState = State::Data;
mIndex = 0;
}
else if (b == 0x00) {
else if (b == SBus::EndByte) {
mState = State::GotEnd;
}
else {
Expand All @@ -80,17 +78,18 @@ namespace SBus {
break;
case State::Data:
mData[mIndex] = b;
if (mIndex >= (mData.size() - 1)) {
if (mIndex >= (mData.size() - 1)) { // got last byte
mState = State::WaitEnd;
}
else {
++mIndex;
}
break;
case State::WaitEnd:
if (b == 0x00) {
if (b == SBus::EndByte) {
mState = State::GotEnd;
if (!((mData[mData.size() - 1] & mFrameLostMask) || (mData[mData.size() - 1] & mFailSafeMask))) {
uint8_t& statusByte = mData[mData.size() - 1]; // last byte
if (!((statusByte & SBus::FrameLostMask) || (statusByte & SBus::FailSafeMask))) {
f();
++mPackages;
}
Expand All @@ -100,74 +99,75 @@ namespace SBus {
}
break;
}
}
static inline void convert(int16_t* pulses) {
pulses[0] = (uint16_t) (((mData[0] | mData[1] << 8)) & 0x07FF);
pulses[1] = (uint16_t) ((mData[1]>>3 | mData[2] <<5) & 0x07FF);
pulses[2] = (uint16_t) ((mData[2]>>6 | mData[3] <<2 |mData[4]<<10) & 0x07FF);
pulses[3] = (uint16_t) ((mData[4]>>1 | mData[5] <<7) & 0x07FF);
pulses[4] = (uint16_t) ((mData[5]>>4 | mData[6] <<4) & 0x07FF);
pulses[5] = (uint16_t) ((mData[6]>>7 | mData[7] <<1 |mData[8]<<9) & 0x07FF);
pulses[6] = (uint16_t) ((mData[8]>>2 | mData[9] <<6) & 0x07FF);
pulses[7] = (uint16_t) ((mData[9]>>5 | mData[10]<<3) & 0x07FF);
pulses[8] = (uint16_t) ((mData[11] | mData[12]<<8) & 0x07FF);
pulses[9] = (uint16_t) ((mData[12]>>3 | mData[13]<<5) & 0x07FF);
pulses[10] = (uint16_t) ((mData[13]>>6 | mData[14]<<2 |mData[15]<<10) & 0x07FF);
pulses[11] = (uint16_t) ((mData[15]>>1 | mData[16]<<7) & 0x07FF);
pulses[12] = (uint16_t) ((mData[16]>>4 | mData[17]<<4) & 0x07FF);
pulses[13] = (uint16_t) ((mData[17]>>7 | mData[18]<<1 |mData[19]<<9) & 0x07FF);
pulses[14] = (uint16_t) ((mData[19]>>2 | mData[20]<<6) & 0x07FF);
pulses[15] = (uint16_t) ((mData[20]>>5 | mData[21]<<3) & 0x07FF);
}
static inline void convert(int16_t* const pulses) {
static_assert(MAX_TRAINER_CHANNELS == 16);
pulses[0] = (uint16_t) (((mData[0] | mData[1] << 8)) & SBus::ValueMask);
pulses[1] = (uint16_t) ((mData[1]>>3 | mData[2] <<5) & SBus::ValueMask);
pulses[2] = (uint16_t) ((mData[2]>>6 | mData[3] <<2 | mData[4]<<10) & SBus::ValueMask);
pulses[3] = (uint16_t) ((mData[4]>>1 | mData[5] <<7) & SBus::ValueMask);
pulses[4] = (uint16_t) ((mData[5]>>4 | mData[6] <<4) & SBus::ValueMask);
pulses[5] = (uint16_t) ((mData[6]>>7 | mData[7] <<1 | mData[8]<<9) & SBus::ValueMask);
pulses[6] = (uint16_t) ((mData[8]>>2 | mData[9] <<6) & SBus::ValueMask);
pulses[7] = (uint16_t) ((mData[9]>>5 | mData[10]<<3) & SBus::ValueMask);
pulses[8] = (uint16_t) ((mData[11] | mData[12]<<8) & SBus::ValueMask);
pulses[9] = (uint16_t) ((mData[12]>>3 | mData[13]<<5) & SBus::ValueMask);
pulses[10] = (uint16_t) ((mData[13]>>6 | mData[14]<<2 | mData[15]<<10) & SBus::ValueMask);
pulses[11] = (uint16_t) ((mData[15]>>1 | mData[16]<<7) & SBus::ValueMask);
pulses[12] = (uint16_t) ((mData[16]>>4 | mData[17]<<4) & SBus::ValueMask);
pulses[13] = (uint16_t) ((mData[17]>>7 | mData[18]<<1 | mData[19]<<9) & SBus::ValueMask);
pulses[14] = (uint16_t) ((mData[19]>>2 | mData[20]<<6) & SBus::ValueMask);
pulses[15] = (uint16_t) ((mData[20]>>5 | mData[21]<<3) & SBus::ValueMask);

for(size_t i = 0; i < 16; ++i) {
pulses[i] -= SBUS_CH_CENTER;
pulses[i] *= 5;
pulses[i] /= 8;
for(size_t i = 0; i < MAX_TRAINER_CHANNELS; ++i) {
pulses[i] = convertSbusToPuls(pulses[i]);
}
}
using MesgType = std::array<uint8_t, 23>;
private:
static State mState;
static MesgType mData;
static uint8_t mIndex;
static uint16_t mPackages;
static uint8_t mPauseCounter;
};
Servo::State Servo::mState{State::Undefined};
Servo::State Servo::mState{Servo::State::Undefined};
Servo::MesgType Servo::mData;
uint8_t Servo::mIndex{};
uint16_t Servo::mPackages{};
uint8_t Servo::mIndex{0};
uint16_t Servo::mPackages{0};
uint8_t Servo::mPauseCounter{Servo::mPauseCount}; // 2 ms
}

void sbusTrainerPauseCheck() {
#if !defined(SIMU)
// GPIO_SetBits(EXTMODULE_TX_GPIO, EXTMODULE_TX_GPIO_PIN);

# if defined(AUX_SERIAL) || defined(AUX2_SERIAL) || defined(TRAINER_MODULE_SBUS)
if ((g_eeGeneral.auxSerialMode == UART_MODE_SBUS_TRAINER)
#ifdef AUX2_SERIAL
#if defined(AUX2_SERIAL)
|| (g_eeGeneral.aux2SerialMode == UART_MODE_SBUS_TRAINER)
#endif
#if defined(TRAINER_MODULE_SBUS)
|| (g_model.trainerData.mode == TRAINER_MODE_MASTER_SBUS_EXTERNAL_MODULE)
#endif
) {
SBus::Servo::tick1ms();
processSbusInput();
}
// GPIO_ResetBits(EXTMODULE_TX_GPIO, EXTMODULE_TX_GPIO_PIN);
# endif
#endif
}

void processSbusInput() {
#if !defined(SIMU)
uint8_t rxchar;

// GPIO_SetBits(EXTMODULE_TX_GPIO, EXTMODULE_TX_GPIO_PIN);
// GPIO_ResetBits(EXTMODULE_TX_GPIO, EXTMODULE_TX_GPIO_PIN);


while (sbusGetByte(&rxchar)) {
SBus::Servo::process(rxchar, [&](){
// GPIO_SetBits(EXTMODULE_TX_GPIO, EXTMODULE_TX_GPIO_PIN);
SBus::Servo::convert(ppmInput);
ppmInputValidityTimer = PPM_IN_VALID_TIMEOUT;
// GPIO_ResetBits(EXTMODULE_TX_GPIO, EXTMODULE_TX_GPIO_PIN);
});
}
#endif
}

#ifdef __GNUC__
# pragma GCC diagnostic pop
#endif
5 changes: 3 additions & 2 deletions radio/src/targets/common/arm/stm32/aux_serial_driver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -174,6 +174,7 @@ void auxSerialSbusInit()
{
auxSerialInit(UART_MODE_SBUS_TRAINER, 0);
}

void auxSerialIbusInit()
{
auxSerialInit(UART_MODE_IBUS_TRAINER, 0);
Expand Down Expand Up @@ -371,13 +372,13 @@ void aux2SerialPutc(char c)
USART_ITConfig(AUX2_SERIAL_USART, USART_IT_TXE, ENABLE);
#endif
}

void aux2SerialSbusInit()
{
aux2SerialInit(UART_MODE_SBUS_TRAINER, 0);
}
void aux2SerialIbusInit()
{
{
aux2SerialInit(UART_MODE_IBUS_TRAINER, 0);
}

Expand Down
8 changes: 4 additions & 4 deletions radio/src/targets/horus/trainer_driver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -147,14 +147,14 @@ int sbusGetByte(uint8_t * byte)
switch (currentTrainerMode) {
#if defined(AUX_SERIAL) || defined(AUX2_SERIAL)
case TRAINER_MODE_MASTER_BATTERY_COMPARTMENT:
#if defined(AUX_SERIAL)
# if defined(AUX_SERIAL)
if ((auxSerialMode == UART_MODE_SBUS_TRAINER) || (auxSerialMode == UART_MODE_IBUS_TRAINER))
return auxSerialRxFifo.pop(*byte);
#endif
#if defined(AUX2_SERIAL)
# endif
# if defined(AUX2_SERIAL)
if ((aux2SerialMode == UART_MODE_SBUS_TRAINER) || (aux2SerialMode == UART_MODE_IBUS_TRAINER))
return aux2SerialRxFifo.pop(*byte);
#endif
# endif
#endif
default:
return false;
Expand Down
2 changes: 2 additions & 0 deletions radio/src/targets/taranis/board.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -190,9 +190,11 @@ void extmoduleSendInvertedByte(uint8_t byte);
#endif
#if defined(TRAINER_MODULE_SBUS)
void init_trainer_module_sbus();
void init_trainer_module_ibus();
void stop_trainer_module_sbus();
#else
#define init_trainer_module_sbus()
#define init_trainer_module_ibus()
#define stop_trainer_module_sbus()
#endif

Expand Down
Loading

0 comments on commit 87eb219

Please sign in to comment.