mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-19 06:15:16 +03:00
First cut of configurable serial port functionality.
Currently port usage is hard-coded to the default port layout, cli commands are coming in a future commit. This decouples all code from the global 'serialPorts' structure which has been removed. Any code that needs to use a serial port can use findOpenSerialPort() and openSerialPort() and maintain it's own reference to the port. Ports can switch between functions. e.g. by default cli/msp/telemetry/gps passthrough all use USART1. Each port maintains it's current function. see begin/endSerialPortFunction. There are only certain combinations of serial port functions that are supported, these are listed in serialPortFunctionScenario_e. This commit also adds a few 'static' keywords to variables that should have been. There a a few other minor fixes and tweaks to various bits of code that this uncovered too.
This commit is contained in:
parent
533a1f9e48
commit
1777d8feda
33 changed files with 787 additions and 394 deletions
71
src/main.c
71
src/main.c
|
@ -44,22 +44,21 @@
|
|||
|
||||
#include "build_config.h"
|
||||
|
||||
//#define USE_SOFTSERIAL_FOR_MAIN_PORT
|
||||
|
||||
extern rcReadRawDataPtr rcReadRawFunc;
|
||||
|
||||
extern uint32_t previousTime;
|
||||
|
||||
failsafe_t *failsafe;
|
||||
|
||||
void initPrintfSupport(void);
|
||||
void timerInit(void);
|
||||
void initTelemetry(serialPorts_t *serialPorts);
|
||||
void initTelemetry(void);
|
||||
void serialInit(serialConfig_t *initialSerialConfig);
|
||||
failsafe_t* failsafeInit(rxConfig_t *intialRxConfig);
|
||||
void pwmInit(drv_pwm_config_t *init);
|
||||
void rxInit(rxConfig_t *rxConfig, failsafe_t *failsafe);
|
||||
void buzzerInit(failsafe_t *initialFailsafe);
|
||||
void gpsInit(uint8_t baudrateIndex, uint8_t initialGpsProvider, gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile);
|
||||
void gpsInit(serialConfig_t *serialConfig, uint8_t baudrateIndex, uint8_t initialGpsProvider, gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile);
|
||||
bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, int16_t magDeclinationFromConfig);
|
||||
void imuInit(void);
|
||||
|
||||
|
@ -98,13 +97,13 @@ int main(void)
|
|||
serialPort_t* loopbackPort2 = NULL;
|
||||
#endif
|
||||
|
||||
initPrintfSupport();
|
||||
|
||||
ensureEEPROMContainsValidData();
|
||||
readEEPROM();
|
||||
|
||||
systemInit(masterConfig.emf_avoidance);
|
||||
|
||||
initPrintfSupport();
|
||||
|
||||
// configure power ADC
|
||||
if (masterConfig.power_adc_channel > 0 && (masterConfig.power_adc_channel == 1 || masterConfig.power_adc_channel == 9))
|
||||
adc_params.powerAdcChannel = masterConfig.power_adc_channel;
|
||||
|
@ -151,6 +150,10 @@ int main(void)
|
|||
imuInit(); // Mag is initialized inside imuInit
|
||||
mixerInit(masterConfig.mixerConfiguration, masterConfig.customMixer);
|
||||
|
||||
if (!canSoftwareSerialBeUsed()) {
|
||||
featureClear(FEATURE_SOFTSERIAL);
|
||||
}
|
||||
|
||||
serialInit(&masterConfig.serialConfig);
|
||||
|
||||
// when using airplane/wing mixer, servo/motor outputs are remapped
|
||||
|
@ -194,10 +197,11 @@ int main(void)
|
|||
|
||||
rxInit(&masterConfig.rxConfig, failsafe);
|
||||
|
||||
if (feature(FEATURE_GPS) && !feature(FEATURE_SERIALRX)) {
|
||||
if (feature(FEATURE_GPS)) {
|
||||
gpsInit(
|
||||
masterConfig.gps_baudrate,
|
||||
masterConfig.gps_type,
|
||||
&masterConfig.serialConfig,
|
||||
masterConfig.gps_initial_baudrate_index,
|
||||
masterConfig.gps_provider,
|
||||
¤tProfile.gpsProfile,
|
||||
¤tProfile.pidProfile
|
||||
);
|
||||
|
@ -211,32 +215,8 @@ int main(void)
|
|||
}
|
||||
#endif
|
||||
|
||||
#ifndef FY90Q
|
||||
if (canSoftwareSerialBeUsed()) {
|
||||
#if defined(USE_SOFTSERIAL_FOR_MAIN_PORT) || (0)
|
||||
masterConfig.serialConfig.softserial_baudrate = 19200;
|
||||
#endif
|
||||
setupSoftSerialPrimary(masterConfig.serialConfig.softserial_baudrate, masterConfig.serialConfig.softserial_1_inverted);
|
||||
setupSoftSerialSecondary(masterConfig.serialConfig.softserial_2_inverted);
|
||||
|
||||
#ifdef SOFTSERIAL_LOOPBACK
|
||||
loopbackPort1 = (serialPort_t*)&(softSerialPorts[0]);
|
||||
serialPrint(loopbackPort1, "SOFTSERIAL 1 - LOOPBACK ENABLED\r\n");
|
||||
|
||||
loopbackPort2 = (serialPort_t*)&(softSerialPorts[1]);
|
||||
#ifndef OLIMEXINO // PB0/D27 and PB1/D28 internally connected so this would result in a continuous stream of data
|
||||
serialPrint(loopbackPort2, "SOFTSERIAL 2 - LOOPBACK ENABLED\r\n");
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#ifdef USE_SOFTSERIAL_FOR_MAIN_PORT
|
||||
serialPorts.mainport = (serialPort_t*)&(softSerialPorts[0]); // Uncomment to switch the main port to use softserial.
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
if (feature(FEATURE_TELEMETRY))
|
||||
initTelemetry(&serialPorts);
|
||||
initTelemetry();
|
||||
|
||||
previousTime = micros();
|
||||
|
||||
|
@ -253,29 +233,6 @@ int main(void)
|
|||
// loopy
|
||||
while (1) {
|
||||
loop();
|
||||
#ifdef SOFTSERIAL_LOOPBACK
|
||||
if (loopbackPort1) {
|
||||
while (serialTotalBytesWaiting(loopbackPort1)) {
|
||||
uint8_t b = serialRead(loopbackPort1);
|
||||
serialWrite(loopbackPort1, b);
|
||||
//serialWrite(core.mainport, 0x01);
|
||||
//serialWrite(core.mainport, b);
|
||||
};
|
||||
}
|
||||
|
||||
if (loopbackPort2) {
|
||||
while (serialTotalBytesWaiting(loopbackPort2)) {
|
||||
#ifndef OLIMEXINO // PB0/D27 and PB1/D28 internally connected so this would result in a continuous stream of data
|
||||
serialRead(loopbackPort2);
|
||||
#else
|
||||
uint8_t b = serialRead(loopbackPort2);
|
||||
serialWrite(loopbackPort2, b);
|
||||
//serialWrite(core.mainport, 0x02);
|
||||
//serialWrite(core.mainport, b);
|
||||
#endif // OLIMEXINO
|
||||
};
|
||||
}
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue