1
0
Fork 0
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:
Dominic Clifton 2014-05-08 21:46:09 +01:00
parent 533a1f9e48
commit 1777d8feda
33 changed files with 787 additions and 394 deletions

View file

@ -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,
&currentProfile.gpsProfile,
&currentProfile.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
}
}