fix: Trainer jack initialisation and driver cleanup (#3811)

This commit is contained in:
3djc
2023-08-09 21:08:42 +10:00
committed by GitHub
parent f7037a92b0
commit ae8e1fff8a
16 changed files with 57 additions and 48 deletions
+3 -3
View File
@@ -141,11 +141,11 @@ void Bluetooth::processTrainerFrame(const uint8_t * buffer)
{ {
for (uint8_t channel=0, i=1; channel<BLUETOOTH_TRAINER_CHANNELS; channel+=2, i+=3) { for (uint8_t channel=0, i=1; channel<BLUETOOTH_TRAINER_CHANNELS; channel+=2, i+=3) {
// +-500 != 512, but close enough. // +-500 != 512, but close enough.
ppmInput[channel] = buffer[i] + ((buffer[i+1] & 0xf0) << 4) - 1500; trainerInput[channel] = buffer[i] + ((buffer[i+1] & 0xf0) << 4) - 1500;
ppmInput[channel+1] = ((buffer[i+1] & 0x0f) << 4) + ((buffer[i+2] & 0xf0) >> 4) + ((buffer[i+2] & 0x0f) << 8) - 1500; trainerInput[channel+1] = ((buffer[i+1] & 0x0f) << 4) + ((buffer[i+2] & 0xf0) >> 4) + ((buffer[i+2] & 0x0f) << 8) - 1500;
} }
ppmInputValidityTimer = PPM_IN_VALID_TIMEOUT; trainerInputValidityTimer = TRAINER_IN_VALID_TIMEOUT;
} }
void Bluetooth::appendTrainerByte(uint8_t data) void Bluetooth::appendTrainerByte(uint8_t data)
+2 -2
View File
@@ -287,12 +287,12 @@ void displayTopBar()
} }
if (SLAVE_MODE()) { if (SLAVE_MODE()) {
if (is_trainer_connected()) { if (is_trainer_dsc_connected()) {
LCD_NOTIF_ICON(x, ICON_TRAINEE); LCD_NOTIF_ICON(x, ICON_TRAINEE);
x -= 12; x -= 12;
} }
} }
else if (IS_TRAINER_INPUT_VALID()) { else if (is_trainer_connected()) {
LCD_NOTIF_ICON(x, ICON_TRAINER); LCD_NOTIF_ICON(x, ICON_TRAINER);
x -= 12; x -= 12;
} }
+2 -2
View File
@@ -86,7 +86,7 @@ void RadioTrainerPage::build(FormWindow * form)
#endif #endif
new DynamicNumber<int16_t>(line, rect_t{}, new DynamicNumber<int16_t>(line, rect_t{},
[=]() { return (ppmInput[i] - g_eeGeneral.trainer.calib[i]) * 2; }, [=]() { return (trainerInput[i] - g_eeGeneral.trainer.calib[i]) * 2; },
LEFT | PPM_PRECISION | COLOR_THEME_PRIMARY1); LEFT | PPM_PRECISION | COLOR_THEME_PRIMARY1);
} }
@@ -110,7 +110,7 @@ void RadioTrainerPage::build(FormWindow * form)
// Trainer calibration // Trainer calibration
auto btn = new TextButton(line, rect_t{0, 0, 0, 30}, std::string(STR_CAL), [=]() -> uint8_t { auto btn = new TextButton(line, rect_t{0, 0, 0, 30}, std::string(STR_CAL), [=]() -> uint8_t {
memcpy(g_eeGeneral.trainer.calib, ppmInput, memcpy(g_eeGeneral.trainer.calib, trainerInput,
sizeof(g_eeGeneral.trainer.calib)); sizeof(g_eeGeneral.trainer.calib));
SET_DIRTY(); SET_DIRTY();
return 0; return 0;
@@ -93,7 +93,7 @@ void menuRadioTrainer(event_t event)
lcdDrawText(0*FW, MENU_HEADER_HEIGHT+1+6*FH, STR_CAL, attr); lcdDrawText(0*FW, MENU_HEADER_HEIGHT+1+6*FH, STR_CAL, attr);
for (uint8_t i = 0; i < 4; i++) { for (uint8_t i = 0; i < 4; i++) {
uint8_t x = 8*FW + (i * TRAINER_CALIB_COLUMN_WIDTH); uint8_t x = 8*FW + (i * TRAINER_CALIB_COLUMN_WIDTH);
int32_t chVal = ppmInput[i] - g_eeGeneral.trainer.calib[i]; int32_t chVal = trainerInput[i] - g_eeGeneral.trainer.calib[i];
chVal *= g_eeGeneral.trainer.mix[i].studWeight * 10; chVal *= g_eeGeneral.trainer.mix[i].studWeight * 10;
chVal /= 512; chVal /= 512;
#if defined (PPM_UNIT_PERCENT_PREC1) #if defined (PPM_UNIT_PERCENT_PREC1)
@@ -106,7 +106,7 @@ void menuRadioTrainer(event_t event)
if (attr) { if (attr) {
s_editMode = 0; s_editMode = 0;
if (event==EVT_KEY_LONG(KEY_ENTER)){ if (event==EVT_KEY_LONG(KEY_ENTER)){
memcpy(g_eeGeneral.trainer.calib, ppmInput, sizeof(g_eeGeneral.trainer.calib)); memcpy(g_eeGeneral.trainer.calib, trainerInput, sizeof(g_eeGeneral.trainer.calib));
storageDirty(EE_GENERAL); storageDirty(EE_GENERAL);
AUDIO_WARNING1(); AUDIO_WARNING1();
} }
+8 -1
View File
@@ -33,7 +33,14 @@ void init_trainer_capture();
void stop_trainer_capture(); void stop_trainer_capture();
// Cable inserted? // Cable inserted?
bool is_trainer_connected(); bool is_trainer_dsc_connected();
// Active signal received
extern uint8_t trainerInputValidityTimer;
inline bool is_trainer_connected()
{
return (trainerInputValidityTimer != 0);
}
#if defined(TRAINER_MODULE_CPPM) #if defined(TRAINER_MODULE_CPPM)
void init_trainer_module_cppm(); void init_trainer_module_cppm();
+8 -7
View File
@@ -25,6 +25,7 @@
#include "input_mapping.h" #include "input_mapping.h"
#include "hal/adc_driver.h" #include "hal/adc_driver.h"
#include "hal/trainer_driver.h"
#include "hal/switch_driver.h" #include "hal/switch_driver.h"
uint8_t s_mixer_first_run_done = false; uint8_t s_mixer_first_run_done = false;
@@ -162,7 +163,7 @@ void applyExpos(int16_t * anas, uint8_t mode, uint8_t ovwrIdx, int16_t ovwrValue
continue; continue;
if (ed->flightModes & (1<<mixerCurrentFlightMode)) if (ed->flightModes & (1<<mixerCurrentFlightMode))
continue; continue;
if (ed->srcRaw >= MIXSRC_FIRST_TRAINER && ed->srcRaw <= MIXSRC_LAST_TRAINER && !IS_TRAINER_INPUT_VALID()) if (ed->srcRaw >= MIXSRC_FIRST_TRAINER && ed->srcRaw <= MIXSRC_LAST_TRAINER && !is_trainer_connected())
continue; continue;
if (getSwitch(ed->swtch)) { if (getSwitch(ed->swtch)) {
int32_t v; int32_t v;
@@ -235,8 +236,8 @@ int16_t applyLimits(uint8_t channel, int32_t value)
} }
#endif #endif
if (isFunctionActive(FUNCTION_TRAINER_CHANNELS) && IS_TRAINER_INPUT_VALID()) { if (isFunctionActive(FUNCTION_TRAINER_CHANNELS) && is_trainer_connected()) {
return ppmInput[channel] * 2; return trainerInput[channel] * 2;
} }
LimitData * lim = limitAddress(channel); LimitData * lim = limitAddress(channel);
@@ -407,7 +408,7 @@ getvalue_t getValue(mixsrc_t i, bool* valid)
else if (i <= MIXSRC_LAST_LOGICAL_SWITCH) { else if (i <= MIXSRC_LAST_LOGICAL_SWITCH) {
return getSwitch(SWSRC_FIRST_LOGICAL_SWITCH + i - MIXSRC_FIRST_LOGICAL_SWITCH) ? 1024 : -1024; return getSwitch(SWSRC_FIRST_LOGICAL_SWITCH + i - MIXSRC_FIRST_LOGICAL_SWITCH) ? 1024 : -1024;
} else if (i <= MIXSRC_LAST_TRAINER) { } else if (i <= MIXSRC_LAST_TRAINER) {
int16_t x = ppmInput[i - MIXSRC_FIRST_TRAINER]; int16_t x = trainerInput[i - MIXSRC_FIRST_TRAINER];
if (i < MIXSRC_FIRST_TRAINER + NUM_CAL_PPM) { if (i < MIXSRC_FIRST_TRAINER + NUM_CAL_PPM) {
x -= g_eeGeneral.trainer.calib[i - MIXSRC_FIRST_TRAINER]; x -= g_eeGeneral.trainer.calib[i - MIXSRC_FIRST_TRAINER];
} }
@@ -568,13 +569,13 @@ void evalInputs(uint8_t mode)
if (mode <= e_perout_mode_inactive_flight_mode && if (mode <= e_perout_mode_inactive_flight_mode &&
isFunctionActive(FUNCTION_TRAINER_STICK1 + ch) && isFunctionActive(FUNCTION_TRAINER_STICK1 + ch) &&
IS_TRAINER_INPUT_VALID()) { is_trainer_connected()) {
// trainer mode // trainer mode
TrainerMix* td = &g_eeGeneral.trainer.mix[ch]; TrainerMix* td = &g_eeGeneral.trainer.mix[ch];
if (td->mode) { if (td->mode) {
uint8_t chStud = td->srcChn; uint8_t chStud = td->srcChn;
int32_t vStud = int32_t vStud =
(ppmInput[chStud] - g_eeGeneral.trainer.calib[chStud]); (trainerInput[chStud] - g_eeGeneral.trainer.calib[chStud]);
vStud *= td->studWeight; vStud *= td->studWeight;
vStud /= 50; vStud /= 50;
switch (td->mode) { switch (td->mode) {
@@ -762,7 +763,7 @@ void evalFlightModeMixes(uint8_t mode, uint8_t tick10ms)
#define MIXER_LINE_DISABLE() (mixCondition = true, mixEnabled = 0) #define MIXER_LINE_DISABLE() (mixCondition = true, mixEnabled = 0)
if (mixEnabled && md->srcRaw >= MIXSRC_FIRST_TRAINER && md->srcRaw <= MIXSRC_LAST_TRAINER && !IS_TRAINER_INPUT_VALID()) { if (mixEnabled && md->srcRaw >= MIXSRC_FIRST_TRAINER && md->srcRaw <= MIXSRC_LAST_TRAINER && !is_trainer_connected()) {
MIXER_LINE_DISABLE(); MIXER_LINE_DISABLE();
} }
+1 -1
View File
@@ -146,7 +146,7 @@ void per10ms()
#endif #endif
if (trimsCheckTimer) trimsCheckTimer--; if (trimsCheckTimer) trimsCheckTimer--;
if (ppmInputValidityTimer) ppmInputValidityTimer--; if (trainerInputValidityTimer) trainerInputValidityTimer--;
if (trimsDisplayTimer) if (trimsDisplayTimer)
trimsDisplayTimer--; trimsDisplayTimer--;
+2 -2
View File
@@ -91,7 +91,7 @@ void processSbusFrame(uint8_t * sbus, int16_t * pulses, uint32_t size)
inputbits >>= SBUS_CH_BITS; inputbits >>= SBUS_CH_BITS;
} }
ppmInputValidityTimer = PPM_IN_VALID_TIMEOUT; trainerInputValidityTimer = TRAINER_IN_VALID_TIMEOUT;
} }
void processSbusInput() void processSbusInput()
@@ -124,7 +124,7 @@ void processSbusInput()
// Check if end-of-frame is detected // Check if end-of-frame is detected
if (SbusIndex) { if (SbusIndex) {
if ((uint16_t)(getTmr2MHz() - SbusTimer) > SBUS_FRAME_GAP_DELAY) { if ((uint16_t)(getTmr2MHz() - SbusTimer) > SBUS_FRAME_GAP_DELAY) {
processSbusFrame(SbusFrame, ppmInput, SbusIndex); processSbusFrame(SbusFrame, trainerInput, SbusIndex);
SbusIndex = 0; SbusIndex = 0;
} }
} }
@@ -271,7 +271,7 @@ void stop_trainer_capture()
#endif // TRAINER_GPIO #endif // TRAINER_GPIO
bool is_trainer_connected() bool is_trainer_dsc_connected()
{ {
#if defined(TRAINER_DETECT_GPIO_PIN) #if defined(TRAINER_DETECT_GPIO_PIN)
bool set = LL_GPIO_IsInputPinSet(TRAINER_DETECT_GPIO, TRAINER_DETECT_GPIO_PIN); bool set = LL_GPIO_IsInputPinSet(TRAINER_DETECT_GPIO, TRAINER_DETECT_GPIO_PIN);
+1 -1
View File
@@ -49,7 +49,7 @@ void stop_trainer_ppm() {}
void init_trainer_capture() {} void init_trainer_capture() {}
void stop_trainer_capture() {} void stop_trainer_capture() {}
bool is_trainer_connected() { return false; } bool is_trainer_dsc_connected() { return false; }
void init_trainer_module_cppm() {} void init_trainer_module_cppm() {}
void stop_trainer_module_cppm() {} void stop_trainer_module_cppm() {}
+3 -3
View File
@@ -241,10 +241,10 @@ void OpenTxSimulator::setTrim(unsigned int idx, int value)
void OpenTxSimulator::setTrainerInput(unsigned int inputNumber, int16_t value) void OpenTxSimulator::setTrainerInput(unsigned int inputNumber, int16_t value)
{ {
static unsigned dim = DIM(ppmInput); static unsigned dim = DIM(trainerInput);
//setTrainerTimeout(100); //setTrainerTimeout(100);
if (inputNumber < dim) if (inputNumber < dim)
ppmInput[inputNumber] = qMin(qMax((int16_t)-512, value), (int16_t)512); trainerInput[inputNumber] = qMin(qMax((int16_t)-512, value), (int16_t)512);
} }
void OpenTxSimulator::setInputValue(int type, uint8_t index, int16_t value) void OpenTxSimulator::setInputValue(int type, uint8_t index, int16_t value)
@@ -411,7 +411,7 @@ void OpenTxSimulator::lcdFlushed()
void OpenTxSimulator::setTrainerTimeout(uint16_t ms) void OpenTxSimulator::setTrainerTimeout(uint16_t ms)
{ {
ppmInputValidityTimer = ms; trainerInputValidityTimer = ms;
} }
void OpenTxSimulator::sendTelemetry(const QByteArray data) void OpenTxSimulator::sendTelemetry(const QByteArray data)
+2
View File
@@ -20,6 +20,7 @@
*/ */
#include "hal/adc_driver.h" #include "hal/adc_driver.h"
#include "hal/trainer_driver.h"
#include "hal/switch_driver.h" #include "hal/switch_driver.h"
#include "hal/module_port.h" #include "hal/module_port.h"
@@ -137,6 +138,7 @@ void boardInit()
} }
#endif #endif
init_trainer();
// Sets 'hardwareOption.pcbrev' as well // Sets 'hardwareOption.pcbrev' as well
pwrInit(); pwrInit();
boardInitModulePorts(); boardInitModulePorts();
+2 -2
View File
@@ -294,7 +294,7 @@ static void processMultiRxChannels(const uint8_t * data, uint8_t len)
bitsavailable -= MULTI_CHAN_BITS; bitsavailable -= MULTI_CHAN_BITS;
bits >>= MULTI_CHAN_BITS; bits >>= MULTI_CHAN_BITS;
ppmInput[ch] = (value - 1024) * 500 / 800; trainerInput[ch] = (value - 1024) * 500 / 800;
ch++; ch++;
if (byteIdx >= len) if (byteIdx >= len)
@@ -302,7 +302,7 @@ static void processMultiRxChannels(const uint8_t * data, uint8_t len)
} }
if (ch == maxCh) if (ch == maxCh)
ppmInputValidityTimer = PPM_IN_VALID_TIMEOUT; trainerInputValidityTimer = TRAINER_IN_VALID_TIMEOUT;
} }
#endif #endif
+2 -2
View File
@@ -815,8 +815,8 @@ TEST(Trainer, UnpluggedTest)
g_model.mixData[0].weight = 100; g_model.mixData[0].weight = 100;
g_model.mixData[0].delayUp = 50; g_model.mixData[0].delayUp = 50;
g_model.mixData[0].delayDown = 50; g_model.mixData[0].delayDown = 50;
ppmInputValidityTimer = 0; trainerInputValidityTimer = 0;
ppmInput[0] = 1024; trainerInput[0] = 1024;
CHECK_DELAY(0, 5000); CHECK_DELAY(0, 5000);
} }
+12 -12
View File
@@ -23,8 +23,8 @@
#include "hal/trainer_driver.h" #include "hal/trainer_driver.h"
#include "heartbeat_driver.h" #include "heartbeat_driver.h"
int16_t ppmInput[MAX_TRAINER_CHANNELS]; int16_t trainerInput[MAX_TRAINER_CHANNELS];
uint8_t ppmInputValidityTimer; uint8_t trainerInputValidityTimer;
uint8_t currentTrainerMode = 0xff; uint8_t currentTrainerMode = 0xff;
enum { enum {
@@ -38,25 +38,25 @@ uint8_t trainerStatus = TRAINER_NOT_CONNECTED;
void checkTrainerSignalWarning() void checkTrainerSignalWarning()
{ {
enum { enum {
PPM_IN_IS_NOT_USED = 0, TRAINER_IN_IS_NOT_USED = 0,
PPM_IN_IS_VALID, TRAINER_IN_IS_VALID,
PPM_IN_INVALID TRAINER_IN_IS_INVALID
}; };
static uint8_t ppmInputValidState = PPM_IN_IS_NOT_USED; static uint8_t trainerInputValidState = TRAINER_IN_IS_NOT_USED;
if (ppmInputValidityTimer && (ppmInputValidState == PPM_IN_IS_NOT_USED)) { if (trainerInputValidityTimer && (trainerInputValidState == TRAINER_IN_IS_NOT_USED)) {
ppmInputValidState = PPM_IN_IS_VALID; trainerInputValidState = TRAINER_IN_IS_VALID;
trainerStatus = TRAINER_CONNECTED; trainerStatus = TRAINER_CONNECTED;
AUDIO_TRAINER_CONNECTED(); AUDIO_TRAINER_CONNECTED();
} }
else if (!ppmInputValidityTimer && (ppmInputValidState == PPM_IN_IS_VALID)) { else if (!trainerInputValidityTimer && (trainerInputValidState == TRAINER_IN_IS_VALID)) {
ppmInputValidState = PPM_IN_INVALID; trainerInputValidState = TRAINER_IN_IS_INVALID;
trainerStatus = TRAINER_DISCONNECTED; trainerStatus = TRAINER_DISCONNECTED;
AUDIO_TRAINER_LOST(); AUDIO_TRAINER_LOST();
} }
else if (ppmInputValidityTimer && (ppmInputValidState == PPM_IN_INVALID)) { else if (trainerInputValidityTimer && (trainerInputValidState == TRAINER_IN_IS_INVALID)) {
ppmInputValidState = PPM_IN_IS_VALID; trainerInputValidState = TRAINER_IN_IS_VALID;
trainerStatus = TRAINER_RECONNECTED; trainerStatus = TRAINER_RECONNECTED;
AUDIO_TRAINER_BACK(); AUDIO_TRAINER_BACK();
} }
+6 -7
View File
@@ -25,14 +25,13 @@
#include "dataconstants.h" #include "dataconstants.h"
// Trainer input channels // Trainer input channels
extern int16_t ppmInput[MAX_TRAINER_CHANNELS]; extern int16_t trainerInput[MAX_TRAINER_CHANNELS];
// Timer gets decremented in per10ms() // Timer gets decremented in per10ms()
#define PPM_IN_VALID_TIMEOUT 100 // 1s #define TRAINER_IN_VALID_TIMEOUT 100 // 1s
extern uint8_t ppmInputValidityTimer; extern uint8_t trainerInputValidityTimer;
extern uint8_t currentTrainerMode; extern uint8_t currentTrainerMode;
#define IS_TRAINER_INPUT_VALID() (ppmInputValidityTimer != 0)
void checkTrainerSignalWarning(); void checkTrainerSignalWarning();
void checkTrainerSettings(); void checkTrainerSettings();
@@ -48,7 +47,7 @@ inline void captureTrainerPulses(uint16_t capture)
uint16_t val = (uint16_t)(capture - lastCapt) / 2; uint16_t val = (uint16_t)(capture - lastCapt) / 2;
lastCapt = capture; lastCapt = capture;
// We process ppmInput right here to make servo movement as smooth as possible // We process trainerInput right here to make servo movement as smooth as possible
// while under trainee control // while under trainee control
// //
// G: Prioritize reset pulse. (Needed when less than 16 incoming pulses) // G: Prioritize reset pulse. (Needed when less than 16 incoming pulses)
@@ -59,8 +58,8 @@ inline void captureTrainerPulses(uint16_t capture)
else { else {
if (channelNumber >= 0 && channelNumber < MAX_TRAINER_CHANNELS) { if (channelNumber >= 0 && channelNumber < MAX_TRAINER_CHANNELS) {
if (val > 800 && val < 2200) { if (val > 800 && val < 2200) {
ppmInputValidityTimer = PPM_IN_VALID_TIMEOUT; trainerInputValidityTimer = TRAINER_IN_VALID_TIMEOUT;
ppmInput[channelNumber++] = trainerInput[channelNumber++] =
// +-500 != 512, but close enough. // +-500 != 512, but close enough.
(int16_t)(val - 1500) * (g_eeGeneral.PPM_Multiplier+10) / 10; (int16_t)(val - 1500) * (g_eeGeneral.PPM_Multiplier+10) / 10;
} }