refactor: Notify module driver on config changes (#3780)
This commit is contained in:
+56
-22
@@ -37,51 +37,68 @@
|
||||
#define BAD_DATA 0x47
|
||||
|
||||
#define DSM2_BITRATE 125000
|
||||
#define DSM2_FRAME_SIZE 14
|
||||
|
||||
static bool _bind_restart = false;
|
||||
|
||||
static inline void sendByte(uint8_t*& p_buf, uint8_t b)
|
||||
{
|
||||
*p_buf++ = b;
|
||||
}
|
||||
|
||||
static void setupPulsesDSM2(uint8_t module, uint8_t*& p_buf)
|
||||
static void setupPulsesDSM2(uint8_t module, uint8_t type, uint8_t*& p_buf)
|
||||
{
|
||||
uint8_t dsmDat[14];
|
||||
uint8_t dsmDat[DSM2_FRAME_SIZE];
|
||||
|
||||
switch (moduleState[module].protocol) {
|
||||
case PROTOCOL_CHANNELS_DSM2_LP45:
|
||||
switch (type) {
|
||||
case DSM2_PROTO_LP45:
|
||||
dsmDat[0] = 0x00;
|
||||
break;
|
||||
case PROTOCOL_CHANNELS_DSM2_DSM2:
|
||||
case DSM2_PROTO_DSM2:
|
||||
dsmDat[0] = 0x10;
|
||||
break;
|
||||
default: // DSMX
|
||||
default: // DSM2_PROTO_DSMX
|
||||
dsmDat[0] = 0x10 | DSMX_BIT;
|
||||
break;
|
||||
}
|
||||
|
||||
if (moduleState[module].mode == MODULE_MODE_BIND) {
|
||||
|
||||
// The module is set to OFF during one second,
|
||||
// before BIND starts. It will start with bind packets
|
||||
// directly after restarts.
|
||||
if (!_bind_restart) {
|
||||
_bind_restart = true;
|
||||
// approx 1.1s @ 22ms cycle
|
||||
restartModuleAsync(module, 50);
|
||||
}
|
||||
|
||||
dsmDat[0] |= DSM2_SEND_BIND;
|
||||
} else {
|
||||
_bind_restart = false;
|
||||
}
|
||||
else if (moduleState[module].mode == MODULE_MODE_RANGECHECK) {
|
||||
|
||||
if (moduleState[module].mode == MODULE_MODE_RANGECHECK) {
|
||||
dsmDat[0] |= DSM2_SEND_RANGECHECK;
|
||||
}
|
||||
|
||||
dsmDat[1] = g_model.header.modelId[module]; // DSM2 Header second byte for model match
|
||||
|
||||
for (int i=0; i<DSM2_CHANS; i++) {
|
||||
int channel = g_model.moduleData[module].channelsStart+i;
|
||||
int value = channelOutputs[channel] + 2*PPM_CH_CENTER(channel) - 2*PPM_CENTER;
|
||||
uint16_t pulse = limit(0, ((value*13)>>5)+512, 1023);
|
||||
dsmDat[2+2*i] = (i<<2) | ((pulse>>8)&0x03);
|
||||
dsmDat[3+2*i] = pulse & 0xff;
|
||||
for (int i = 0; i < DSM2_CHANS; i++) {
|
||||
int channel = g_model.moduleData[module].channelsStart + i;
|
||||
int value =
|
||||
channelOutputs[channel] + 2 * PPM_CH_CENTER(channel) - 2 * PPM_CENTER;
|
||||
|
||||
uint16_t pulse = limit(0, ((value * 13) >> 5) + 512, 1023);
|
||||
dsmDat[2 + 2 * i] = (i << 2) | ((pulse >> 8) & 0x03);
|
||||
dsmDat[3 + 2 * i] = pulse & 0xff;
|
||||
}
|
||||
|
||||
for (int i=0; i<14; i++) {
|
||||
for (int i = 0; i < DSM2_FRAME_SIZE; i++) {
|
||||
sendByte(p_buf, dsmDat[i]);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
#define DSMP_BITRATE 115200
|
||||
|
||||
static void setupPulsesLemonDSMP(uint8_t module, uint8_t*& p_buf)
|
||||
@@ -178,7 +195,8 @@ const etx_serial_init dsmUartParams = {
|
||||
.polarity = ETX_Pol_Inverted,
|
||||
};
|
||||
|
||||
static void* dsmInit(uint8_t module, uint32_t baudrate, uint16_t period, bool telemetry)
|
||||
static etx_module_state_t* dsmInit(uint8_t module, uint32_t baudrate,
|
||||
uint16_t period, bool telemetry)
|
||||
{
|
||||
// only external module supported
|
||||
if (module == INTERNAL_MODULE) return nullptr;
|
||||
@@ -201,20 +219,21 @@ static void* dsmInit(uint8_t module, uint32_t baudrate, uint16_t period, bool t
|
||||
}
|
||||
|
||||
mixerSchedulerSetPeriod(module, period);
|
||||
return (void*)mod_st;
|
||||
return mod_st;
|
||||
}
|
||||
|
||||
static void* dsm2Init(uint8_t module)
|
||||
{
|
||||
return dsmInit(module, DSM2_BITRATE, DSM2_PERIOD, false);
|
||||
auto mod_st = dsmInit(module, DSM2_BITRATE, DSM2_PERIOD, false);
|
||||
mod_st->user_data = (void*)(uintptr_t)g_model.moduleData[module].subType;
|
||||
return (void*)mod_st;
|
||||
}
|
||||
|
||||
static void* dsmpInit(uint8_t module)
|
||||
{
|
||||
return dsmInit(module, DSMP_BITRATE, 11 * 1000 /* 11ms in us */, true);
|
||||
return (void*)dsmInit(module, DSMP_BITRATE, 11 * 1000 /* 11ms in us */, true);
|
||||
}
|
||||
|
||||
|
||||
static void dsmDeInit(void* ctx)
|
||||
{
|
||||
auto mod_st = (etx_module_state_t*)ctx;
|
||||
@@ -236,12 +255,25 @@ static void dsm2SendPulses(void* ctx, uint8_t* buffer, int16_t* channels, uint8_
|
||||
|
||||
auto mod_st = (etx_module_state_t*)ctx;
|
||||
auto module = modulePortGetModule(mod_st);
|
||||
auto type = (uint8_t)(uintptr_t)mod_st->user_data;
|
||||
|
||||
auto p_data = buffer;
|
||||
setupPulsesDSM2(module, p_data);
|
||||
setupPulsesDSM2(module, type, p_data);
|
||||
_dsm_send(mod_st, buffer, p_data - buffer);
|
||||
}
|
||||
|
||||
static void dsm2ConfigChange(void* ctx)
|
||||
{
|
||||
auto mod_st = (etx_module_state_t*)ctx;
|
||||
auto module = modulePortGetModule(mod_st);
|
||||
auto type = (uint8_t)(uintptr_t)mod_st->user_data;
|
||||
|
||||
if (type != g_model.moduleData[module].subType) {
|
||||
// restart during next mixer cycle
|
||||
restartModuleAsync(module, 0);
|
||||
}
|
||||
}
|
||||
|
||||
static void dsmpSendPulses(void* ctx, uint8_t* buffer, int16_t* channels, uint8_t nChannels)
|
||||
{
|
||||
// TODO:
|
||||
@@ -266,11 +298,12 @@ static void dsmpProcessData(void* ctx, uint8_t data, uint8_t* buffer, uint8_t* l
|
||||
|
||||
// No telemetry
|
||||
const etx_proto_driver_t DSM2Driver = {
|
||||
.protocol = PROTOCOL_CHANNELS_DSM2_LP45,
|
||||
.protocol = PROTOCOL_CHANNELS_DSM2,
|
||||
.init = dsm2Init,
|
||||
.deinit = dsmDeInit,
|
||||
.sendPulses = dsm2SendPulses,
|
||||
.processData = nullptr,
|
||||
.onConfigChange = dsm2ConfigChange,
|
||||
};
|
||||
|
||||
const etx_proto_driver_t DSMPDriver = {
|
||||
@@ -279,4 +312,5 @@ const etx_proto_driver_t DSMPDriver = {
|
||||
.deinit = dsmDeInit,
|
||||
.sendPulses = dsmpSendPulses,
|
||||
.processData = dsmpProcessData,
|
||||
.onConfigChange = nullptr,
|
||||
};
|
||||
|
||||
+69
-35
@@ -98,6 +98,46 @@ static uint32_t setupPulsesPPMModule(uint8_t module, pulse_duration_t*& data)
|
||||
return data - start;
|
||||
}
|
||||
|
||||
typedef void (*ppm_telemetry_fct_t)(uint8_t module, uint8_t data, uint8_t* buffer, uint8_t* len);
|
||||
static ppm_telemetry_fct_t _processTelemetryData;
|
||||
|
||||
static etx_serial_init ppmMLinkSerialParams = {
|
||||
.baudrate = PPM_MSB_BAUDRATE,
|
||||
.encoding = ETX_Encoding_8N1,
|
||||
.direction = ETX_Dir_RX,
|
||||
.polarity = ETX_Pol_Inverted,
|
||||
};
|
||||
|
||||
static bool ppmInitMLinkTelemetry(uint8_t module)
|
||||
{
|
||||
// Try S.PORT hardware USART (requires HW inverters)
|
||||
if (modulePortInitSerial(module, ETX_MOD_PORT_SPORT, &ppmMLinkSerialParams) != nullptr) {
|
||||
return true;
|
||||
}
|
||||
|
||||
// fall-back to softserial
|
||||
if (modulePortInitSerial(module, ETX_MOD_PORT_SPORT_INV, &ppmMLinkSerialParams) != nullptr) {
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
static void _init_telemetry(uint8_t module, uint8_t telemetry_type)
|
||||
{
|
||||
switch (telemetry_type) {
|
||||
case PPM_PROTO_TLM_MLINK:
|
||||
if (ppmInitMLinkTelemetry(module)) {
|
||||
_processTelemetryData = processExternalMLinkSerialData;
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
_processTelemetryData = nullptr;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
static void* ppmInit(uint8_t module)
|
||||
{
|
||||
#if defined(HARDWARE_INTERNAL_MODULE)
|
||||
@@ -116,6 +156,11 @@ static void* ppmInit(uint8_t module)
|
||||
if (!mod_st) return nullptr;
|
||||
|
||||
mixerSchedulerSetPeriod(module, PPM_PERIOD(module));
|
||||
|
||||
uint8_t telemetry_type = g_model.moduleData[module].subType;
|
||||
mod_st->user_data = (void*)(uintptr_t)telemetry_type;
|
||||
|
||||
_init_telemetry(module, telemetry_type);
|
||||
return (void*)mod_st;
|
||||
}
|
||||
|
||||
@@ -123,6 +168,7 @@ static void ppmDeInit(void* ctx)
|
||||
{
|
||||
auto mod_st = (etx_module_state_t*)ctx;
|
||||
modulePortDeInit(mod_st);
|
||||
_processTelemetryData = nullptr;
|
||||
}
|
||||
|
||||
static void ppmSendPulses(void* ctx, uint8_t* buffer, int16_t* channels, uint8_t nChannels)
|
||||
@@ -153,44 +199,32 @@ static void ppmSendPulses(void* ctx, uint8_t* buffer, int16_t* channels, uint8_t
|
||||
mixerSchedulerSetPeriod(module, PPM_PERIOD(module));
|
||||
}
|
||||
|
||||
static void ppmProcessTelemetryData(void* ctx, uint8_t data, uint8_t* buffer, uint8_t* len) {
|
||||
auto mod_st = (etx_module_state_t*)ctx;
|
||||
auto module = modulePortGetModule(mod_st);
|
||||
|
||||
if (_processTelemetryData) {
|
||||
_processTelemetryData(module, data, buffer, len);
|
||||
}
|
||||
}
|
||||
|
||||
static void ppmOnConfigChange(void* ctx)
|
||||
{
|
||||
auto mod_st = (etx_module_state_t*)ctx;
|
||||
auto module = modulePortGetModule(mod_st);
|
||||
|
||||
uint8_t telemetry_type = (uint8_t)(uintptr_t)mod_st->user_data;
|
||||
if (telemetry_type != g_model.moduleData[module].subType) {
|
||||
// restart during next mixer cycle
|
||||
restartModuleAsync(module, 0);
|
||||
}
|
||||
}
|
||||
|
||||
const etx_proto_driver_t PpmDriver = {
|
||||
.protocol = PROTOCOL_CHANNELS_PPM,
|
||||
.init = ppmInit,
|
||||
.deinit = ppmDeInit,
|
||||
.sendPulses = ppmSendPulses,
|
||||
.processData = nullptr,
|
||||
.processData = ppmProcessTelemetryData,
|
||||
.onConfigChange = ppmOnConfigChange,
|
||||
};
|
||||
|
||||
//
|
||||
// additions for PPM with external MLink Module telemetry
|
||||
//
|
||||
|
||||
static void* ppmMLinkInit(uint8_t module);
|
||||
|
||||
const etx_proto_driver_t PpmDriverMLink = {
|
||||
.protocol = PROTOCOL_CHANNELS_PPM_MLINK,
|
||||
.init = ppmMLinkInit,
|
||||
.deinit = ppmDeInit,
|
||||
.sendPulses = ppmSendPulses,
|
||||
.processData = processExternalMLinkSerialData,
|
||||
};
|
||||
|
||||
static etx_serial_init ppmMLinkSerialParams = {
|
||||
.baudrate = PPM_MSB_BAUDRATE,
|
||||
.encoding = ETX_Encoding_8N1,
|
||||
.direction = ETX_Dir_RX,
|
||||
.polarity = ETX_Pol_Inverted,
|
||||
};
|
||||
|
||||
static void* ppmMLinkInit(uint8_t module) {
|
||||
etx_module_state_t *mod_st = (etx_module_state_t *)ppmInit(module);
|
||||
|
||||
if (!mod_st)
|
||||
return nullptr;
|
||||
|
||||
if (!modulePortInitSerial(module, ETX_MOD_PORT_UART, &ppmMLinkSerialParams)) {
|
||||
modulePortInitSerial(module, ETX_MOD_PORT_SPORT_INV, &ppmMLinkSerialParams);
|
||||
}
|
||||
|
||||
return (void*)mod_st;
|
||||
}
|
||||
|
||||
@@ -24,4 +24,3 @@
|
||||
#include "hal/module_driver.h"
|
||||
|
||||
extern const etx_proto_driver_t PpmDriver;
|
||||
extern const etx_proto_driver_t PpmDriverMLink;
|
||||
|
||||
+26
-44
@@ -147,6 +147,11 @@ bool restartModuleAsync(uint8_t module, uint8_t cnt_delay)
|
||||
#endif
|
||||
}
|
||||
|
||||
void pulsesModuleSettingsUpdate(uint8_t module)
|
||||
{
|
||||
moduleState[module].settings_updated = 1;
|
||||
}
|
||||
|
||||
// TODO: this should be moved to PXX2 territory!
|
||||
#if defined(PXX2)
|
||||
// use only for PXX
|
||||
@@ -263,16 +268,7 @@ uint8_t getRequiredProtocol(uint8_t module)
|
||||
|
||||
switch (getModuleType(module)) {
|
||||
case MODULE_TYPE_PPM:
|
||||
switch (g_model.moduleData[module].subType) {
|
||||
case PPM_PROTO_TLM_NONE:
|
||||
protocol = PROTOCOL_CHANNELS_PPM;
|
||||
break;
|
||||
case PPM_PROTO_TLM_MLINK:
|
||||
protocol = PROTOCOL_CHANNELS_PPM_MLINK;
|
||||
break;
|
||||
default:
|
||||
protocol = PROTOCOL_CHANNELS_PPM;
|
||||
}
|
||||
protocol = PROTOCOL_CHANNELS_PPM;
|
||||
break;
|
||||
|
||||
case MODULE_TYPE_XJT_PXX1:
|
||||
@@ -301,25 +297,7 @@ uint8_t getRequiredProtocol(uint8_t module)
|
||||
|
||||
#if defined(DSM2)
|
||||
case MODULE_TYPE_DSM2:
|
||||
protocol = limit<uint8_t>(
|
||||
PROTOCOL_CHANNELS_DSM2_LP45,
|
||||
PROTOCOL_CHANNELS_DSM2_LP45 + g_model.moduleData[module].subType,
|
||||
PROTOCOL_CHANNELS_DSM2_DSMX);
|
||||
// The module is set to OFF during one second before BIND start
|
||||
// TODO: move this to DSM2 driver...
|
||||
{
|
||||
static tmr10ms_t bindStartTime = 0;
|
||||
if (moduleState[module].mode == MODULE_MODE_BIND) {
|
||||
if (bindStartTime == 0) bindStartTime = get_tmr10ms();
|
||||
if ((tmr10ms_t)(get_tmr10ms() - bindStartTime) < 100) {
|
||||
protocol = PROTOCOL_CHANNELS_NONE;
|
||||
break;
|
||||
}
|
||||
}
|
||||
else {
|
||||
bindStartTime = 0;
|
||||
}
|
||||
}
|
||||
protocol = PROTOCOL_CHANNELS_DSM2;
|
||||
break;
|
||||
#endif
|
||||
|
||||
@@ -432,9 +410,7 @@ static void pulsesEnableModule(uint8_t module, uint8_t protocol)
|
||||
#endif
|
||||
|
||||
#if defined(DSM2)
|
||||
case PROTOCOL_CHANNELS_DSM2_LP45:
|
||||
case PROTOCOL_CHANNELS_DSM2_DSM2:
|
||||
case PROTOCOL_CHANNELS_DSM2_DSMX:
|
||||
case PROTOCOL_CHANNELS_DSM2:
|
||||
_init_module(module, &DSM2Driver);
|
||||
break;
|
||||
#endif
|
||||
@@ -473,9 +449,6 @@ static void pulsesEnableModule(uint8_t module, uint8_t protocol)
|
||||
case PROTOCOL_CHANNELS_PPM:
|
||||
_init_module(module, &PpmDriver);
|
||||
break;
|
||||
case PROTOCOL_CHANNELS_PPM_MLINK:
|
||||
_init_module(module, &PpmDriverMLink);
|
||||
break;
|
||||
#endif
|
||||
|
||||
#if defined(INTERNAL_MODULE_AFHDS2A) && defined(AFHDS2)
|
||||
@@ -521,13 +494,15 @@ void pulsesStopModule(uint8_t module)
|
||||
|
||||
static bool _handle_async_restart(uint8_t module)
|
||||
{
|
||||
if (moduleState[module].forced_off) {
|
||||
if (--moduleState[module].counter > 0) {
|
||||
auto& state = moduleState[module];
|
||||
if (state.forced_off) {
|
||||
if (state.counter > 0) {
|
||||
_deinit_module(module);
|
||||
moduleState[module].protocol = PROTOCOL_CHANNELS_NONE;
|
||||
state.protocol = PROTOCOL_CHANNELS_NONE;
|
||||
--state.counter;
|
||||
return true;
|
||||
} else {
|
||||
moduleState[module].forced_off = 0;
|
||||
state.forced_off = 0;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
@@ -539,9 +514,8 @@ void pulsesSendNextFrame(uint8_t module)
|
||||
|
||||
uint8_t protocol = getRequiredProtocol(module);
|
||||
|
||||
if (moduleState[module].protocol != protocol ||
|
||||
moduleState[module].forced_off) {
|
||||
// TODO: error checking!
|
||||
auto& state = moduleState[module];
|
||||
if (state.protocol != protocol || state.forced_off) {
|
||||
|
||||
if (_telemetryIsPolling) {
|
||||
// In case the telemetry timer is currently polling the port,
|
||||
@@ -555,16 +529,24 @@ void pulsesSendNextFrame(uint8_t module)
|
||||
pulsesEnableModule(module, protocol);
|
||||
moduleState[module].protocol = protocol;
|
||||
return;
|
||||
|
||||
}
|
||||
|
||||
auto mod = &(_module_drivers[module]);
|
||||
if (mod->drv) {
|
||||
|
||||
auto drv = mod->drv;
|
||||
auto ctx = mod->ctx;
|
||||
|
||||
if (state.settings_updated) {
|
||||
if (drv->onConfigChange) drv->onConfigChange(ctx);
|
||||
state.settings_updated = 0;
|
||||
}
|
||||
|
||||
uint8_t channelStart = g_model.moduleData[module].channelsStart;
|
||||
int16_t* channels = &channelOutputs[channelStart];
|
||||
uint8_t nChannels = 16; // TODO: MAX_CHANNELS - channelsStart
|
||||
|
||||
auto drv = mod->drv;
|
||||
auto ctx = mod->ctx;
|
||||
auto buffer = _module_buffers[module]._buffer;
|
||||
drv->sendPulses(ctx, buffer, channels, nChannels);
|
||||
}
|
||||
|
||||
@@ -66,7 +66,8 @@ PACK(struct ModuleState {
|
||||
uint8_t protocol;
|
||||
uint8_t mode:4;
|
||||
uint8_t forced_off:1;
|
||||
uint8_t spare:3;
|
||||
uint8_t settings_updated:1;
|
||||
uint8_t spare:2;
|
||||
uint16_t counter;
|
||||
|
||||
#if defined(PXX2)
|
||||
@@ -174,6 +175,8 @@ void pulsesSetModuleDeInitCb(module_deinit_cb_t cb);
|
||||
void restartModule(uint8_t module);
|
||||
bool restartModuleAsync(uint8_t module, uint8_t cnt_delay);
|
||||
|
||||
void pulsesModuleSettingsUpdate(uint8_t module);
|
||||
|
||||
void setupPulsesPPMTrainer();
|
||||
|
||||
void getModuleStatusString(uint8_t moduleIdx, char * statusText);
|
||||
|
||||
Reference in New Issue
Block a user