diff --git a/docs/Configuration.md b/docs/Configuration.md new file mode 100644 index 0000000000..b2f1a21580 --- /dev/null +++ b/docs/Configuration.md @@ -0,0 +1,85 @@ + +## Serial port functions and scenarios + +### Serial port scenarios + +``` +0 UNUSED +1 MSP, CLI, TELEMETRY, GPS-PASTHROUGH +2 GPS ONLY +3 SERIAL-RX ONLY +4 TELEMETRY ONLY +5 MSP, CLI, GPS-PASTHROUGH +6 CLI ONLY +7 GPS-PASSTHROUGH ONLY +8 MSP ONLY +``` + +### Contraints + +* There must always be a port available to use for MSP +* There must always be a port available to use for CLI +* To use a port for a function, the function's corresponding feature must be enabled first. +e.g. to use GPS enable the GPS feature. +* If the configuration is invalid the serial port configuration will reset to it's defaults and features may be disabled. + +### Examples + +All examples assume default configuration (via cli `defaults` command) + +a) GPS and TELEMETRY (when armed) + +- TELEMETRY,MSP,CLI,GPS PASSTHROUGH on UART1 +- GPS on UART2 + +``` +feature TELEMETRY +feature GPS +save +``` + +b) SERIAL_RX and TELEMETRY (when armed) + +- TELEMETRY,MSP,CLI,GPS PASSTHROUGH on UART1 +- SERIAL_RX on UART2 + +``` +feature TELEMETRY +feature SERIAL_RX +set serial_port_2_scenario = 3 +save +``` + +c) GPS and TELEMETRY via softserial + +- TELEMETRY,MSP,CLI,GPS PASSTHROUGH on UART1 +- GPS on UART2 + +``` +feature TELEMETRY +feature GPS +feature SOFTSERIAL +set serial_port_3_scenario = 4 +save +``` +d) SERIAL_RX, GPS and TELEMETRY (when armed) MSP/CLI via softserial + +- GPS on UART1 +- SERIAL RX on UART2 +- TELEMETRY,MSP,CLI,GPS PASSTHROUGH on SOFTSERIAL1 +Note: PPM needed to disable parallel PWM which in turn allows SOFTSERIAL to be used. + +``` +feature PPM +feature TELEMETRY +feature GPS +feature SERIALRX +feature SOFTSERIAL +set serial_port_1_scenario = 2 +set serial_port_2_scenario = 3 +set serial_port_3_scenario = 1 +set msp_baudrate = 19200 +set cli_baudrate = 19200 +set gps_passthrough_baudrate = 19200 +save +``` diff --git a/src/config.c b/src/config.c index 8db296a0bc..39281893ae 100755 --- a/src/config.c +++ b/src/config.c @@ -50,7 +50,7 @@ void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DCon master_t masterConfig; // master config struct with data independent from profiles profile_t currentProfile; // profile config struct -static const uint8_t EEPROM_CONF_VERSION = 65; +static const uint8_t EEPROM_CONF_VERSION = 66; static void resetConf(void); static uint8_t calculateChecksum(const uint8_t *data, uint32_t length) @@ -282,6 +282,18 @@ void resetTelemetryConfig(telemetryConfig_t *telemetryConfig) telemetryConfig->telemetry_switch = 0; } +void resetSerialConfig(serialConfig_t *serialConfig) +{ + serialConfig->serial_port_1_scenario = lookupScenarioIndex(SCENARIO_MSP_CLI_TELEMETRY_GPS_PASTHROUGH); + serialConfig->serial_port_2_scenario = lookupScenarioIndex(SCENARIO_GPS_ONLY); + serialConfig->serial_port_3_scenario = lookupScenarioIndex(SCENARIO_UNUSED); + serialConfig->serial_port_4_scenario = lookupScenarioIndex(SCENARIO_UNUSED); + serialConfig->msp_baudrate = 115200; + serialConfig->cli_baudrate = 115200; + serialConfig->gps_passthrough_baudrate = 115200; + serialConfig->reboot_character = 'R'; +} + // Default settings static void resetConf(void) { @@ -338,10 +350,7 @@ static void resetConf(void) masterConfig.gps_provider = GPS_NMEA; masterConfig.gps_initial_baudrate_index = GPS_BAUDRATE_115200; - masterConfig.serialConfig.msp_baudrate = 115200; - masterConfig.serialConfig.cli_baudrate = 115200; - masterConfig.serialConfig.gps_passthrough_baudrate = 115200; - masterConfig.serialConfig.reboot_character = 'R'; + resetSerialConfig(&masterConfig.serialConfig); masterConfig.looptime = 3500; masterConfig.emf_avoidance = 0; diff --git a/src/config.h b/src/config.h index 8e8b968a8d..811b64851b 100644 --- a/src/config.h +++ b/src/config.h @@ -34,3 +34,4 @@ void ensureEEPROMContainsValidData(void); void saveAndReloadCurrentProfileToCurrentProfileSlot(void); bool canSoftwareSerialBeUsed(void); + diff --git a/src/drivers/serial_softserial.c b/src/drivers/serial_softserial.c index 88e8e56fb1..765ff2baab 100644 --- a/src/drivers/serial_softserial.c +++ b/src/drivers/serial_softserial.c @@ -155,6 +155,7 @@ void resetBuffers(softSerial_t *softSerial) void initialiseSoftSerial(softSerial_t *softSerial, uint8_t portIndex, uint32_t baud, serialInversion_e inversion) { softSerial->port.vTable = softSerialVTable; + softSerial->port.baudRate = baud; softSerial->port.mode = MODE_RXTX; softSerial->port.inversion = inversion; diff --git a/src/main.c b/src/main.c index c7cb55970f..3e636fadf6 100755 --- a/src/main.c +++ b/src/main.c @@ -92,10 +92,6 @@ int main(void) drv_pwm_config_t pwm_params; drv_adc_config_t adc_params; bool sensorsOK = false; -#ifdef SOFTSERIAL_LOOPBACK - serialPort_t* loopbackPort1 = NULL; - serialPort_t* loopbackPort2 = NULL; -#endif initPrintfSupport(); @@ -154,6 +150,10 @@ int main(void) featureClear(FEATURE_SOFTSERIAL); } +#ifndef FY90Q + timerInit(); +#endif + serialInit(&masterConfig.serialConfig); // when using airplane/wing mixer, servo/motor outputs are remapped @@ -187,14 +187,11 @@ int main(void) pwm_params.adcChannel = 0; // FIXME this is the same as PWM1 break; } - - failsafe = failsafeInit(&masterConfig.rxConfig); - buzzerInit(failsafe); -#ifndef FY90Q - timerInit(); -#endif + pwmInit(&pwm_params); + failsafe = failsafeInit(&masterConfig.rxConfig); + buzzerInit(failsafe); rxInit(&masterConfig.rxConfig, failsafe); if (feature(FEATURE_GPS)) { @@ -230,9 +227,28 @@ int main(void) f.SMALL_ANGLE = 1; +#ifdef SOFTSERIAL_LOOPBACK + // FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly + serialPort_t *loopbackPort = (serialPort_t*)&(softSerialPorts[0]); + if (!loopbackPort->vTable) { + openSoftSerial1(19200, false); + } + serialPrint(loopbackPort, "LOOPBACK\r\n"); +#endif + // loopy while (1) { loop(); + +#ifdef SOFTSERIAL_LOOPBACK + if (loopbackPort) { + while (serialTotalBytesWaiting(loopbackPort)) { + uint8_t b = serialRead(loopbackPort); + serialWrite(loopbackPort, b); + }; + } +#endif + } } diff --git a/src/serial_cli.c b/src/serial_cli.c index 9db8cf4799..370cb5d5a1 100644 --- a/src/serial_cli.c +++ b/src/serial_cli.c @@ -168,6 +168,11 @@ const clivalue_t valueTable[] = { { "fixedwing_althold_dir", VAR_INT8, &masterConfig.fixedwing_althold_dir, -1, 1 }, + { "serial_port_1_scenario", VAR_UINT8, &masterConfig.serialConfig.serial_port_1_scenario, 0, SERIAL_PORT_SCENARIO_MAX }, + { "serial_port_2_scenario", VAR_UINT8, &masterConfig.serialConfig.serial_port_2_scenario, 0, SERIAL_PORT_SCENARIO_MAX }, + { "serial_port_3_scenario", VAR_UINT8, &masterConfig.serialConfig.serial_port_3_scenario, 0, SERIAL_PORT_SCENARIO_MAX }, + { "serial_port_4_scenario", VAR_UINT8, &masterConfig.serialConfig.serial_port_4_scenario, 0, SERIAL_PORT_SCENARIO_MAX }, + { "reboot_character", VAR_UINT8, &masterConfig.serialConfig.reboot_character, 48, 126 }, { "msp_baudrate", VAR_UINT32, &masterConfig.serialConfig.msp_baudrate, 1200, 115200 }, { "cli_baudrate", VAR_UINT32, &masterConfig.serialConfig.cli_baudrate, 1200, 115200 }, diff --git a/src/serial_common.c b/src/serial_common.c index 761e9648c0..e28978d308 100644 --- a/src/serial_common.c +++ b/src/serial_common.c @@ -15,28 +15,38 @@ #include "serial_msp.h" #include "serial_common.h" +#include "config.h" void mspInit(serialConfig_t *serialConfig); void cliInit(serialConfig_t *serialConfig); +void resetSerialConfig(serialConfig_t *serialConfig); + +// this exists so the user can reference scenarios by a number in the CLI instead of an unuser-friendly bitmask. +const serialPortFunctionScenario_e serialPortScenarios[SERIAL_PORT_SCENARIO_COUNT] = { + SCENARIO_UNUSED, + + // common scenarios in order of importance + SCENARIO_MSP_CLI_TELEMETRY_GPS_PASTHROUGH, + SCENARIO_GPS_ONLY, + SCENARIO_SERIAL_RX_ONLY, + SCENARIO_TELEMETRY_ONLY, + + // other scenarios + SCENARIO_MSP_CLI_GPS_PASTHROUGH, + SCENARIO_CLI_ONLY, + SCENARIO_GPS_PASSTHROUGH_ONLY, + SCENARIO_MSP_ONLY +}; static serialConfig_t *serialConfig; static serialPort_t *serialPorts[SERIAL_PORT_COUNT]; static serialPortFunction_t serialPortFunctions[SERIAL_PORT_COUNT] = { - {SERIAL_PORT_USART1, NULL, SCENARIO_MAIN_PORT, FUNCTION_NONE}, - {SERIAL_PORT_USART2, NULL, SCENARIO_GPS_AND_TELEMETRY, FUNCTION_NONE}, - {SERIAL_PORT_SOFTSERIAL1, NULL, SCENARIO_UNUSED, FUNCTION_NONE}, - {SERIAL_PORT_SOFTSERIAL2, NULL, SCENARIO_UNUSED, FUNCTION_NONE} + {SERIAL_PORT_USART1, NULL, SCENARIO_UNUSED, FUNCTION_NONE}, + {SERIAL_PORT_USART2, NULL, SCENARIO_UNUSED, FUNCTION_NONE}, + {SERIAL_PORT_SOFTSERIAL1, NULL, SCENARIO_UNUSED, FUNCTION_NONE}, + {SERIAL_PORT_SOFTSERIAL2, NULL, SCENARIO_UNUSED, FUNCTION_NONE} }; -#if 0 // example using softserial for telemetry, note that it is used because the scenario is more specific than telemetry on usart1 -static serialPortFunction_t serialPortFunctions[SERIAL_PORT_COUNT] = { - {SERIAL_PORT_USART1, NULL, SCENARIO_MAIN_PORT, FUNCTION_NONE}, - {SERIAL_PORT_USART2, NULL, SCENARIO_GPS_ONLY, FUNCTION_NONE}, - {SERIAL_PORT_SOFTSERIAL1, NULL, SCENARIO_TELEMETRY_ONLY, FUNCTION_NONE}, - {SERIAL_PORT_SOFTSERIAL2, NULL, SCENARIO_UNUSED, FUNCTION_NONE} -}; -#endif - const static serialPortConstraint_t serialPortConstraints[SERIAL_PORT_COUNT] = { {SERIAL_PORT_USART1, 9600, 115200, SPF_NONE | SPF_SUPPORTS_SBUS_MODE }, {SERIAL_PORT_USART2, 9600, 115200, SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE}, @@ -61,23 +71,42 @@ const functionConstraint_t functionConstraints[] = { #define FUNCTION_CONSTRAINT_COUNT (sizeof(functionConstraints) / sizeof(functionConstraint_t)) typedef struct serialPortSearchResult_s { - bool found; serialPortIndex_e portIndex; serialPortFunction_t *portFunction; const serialPortConstraint_t *portConstraint; const functionConstraint_t *functionConstraint; } serialPortSearchResult_t; -static serialPortIndex_e findSerialPortIndexByIdentifier(serialPortIdentifier_e identifier) +uint8_t lookupScenarioIndex(serialPortFunctionScenario_e scenario) { + uint8_t index; + for (index = 0; index < SERIAL_PORT_SCENARIO_COUNT; index++) { + if (serialPortScenarios[index] == scenario) { + break; + } + } + return index; +} + +static serialPortIndex_e lookupSerialPortIndexByIdentifier(serialPortIdentifier_e identifier) { serialPortIndex_e portIndex; for (portIndex = 0; portIndex < SERIAL_PORT_COUNT; portIndex++) { if (serialPortConstraints[portIndex].identifier == identifier) { - return portIndex; + break; } } + return portIndex; +} - return SERIAL_PORT_1; // FIXME use failureMode() ? - invalid identifier used. +static uint8_t lookupSerialPortFunctionIndexByIdentifier(serialPortIdentifier_e identifier) +{ + uint8_t index; + for (index = 0; index < SERIAL_PORT_COUNT; index++) { + if (serialPortFunctions[index].identifier == identifier) { + break; + } + } + return index; } static const functionConstraint_t *findFunctionConstraint(serialPortFunction_e function) @@ -113,7 +142,6 @@ static void sortSerialPortFunctions(serialPortFunction_t *serialPortFunctions, u qsort(serialPortFunctions, elements, sizeof(serialPortFunction_t), serialPortFunctionMostSpecificFirstComparator); } - /* * since this method, and other methods that use it, use a single instance of * searchPortSearchResult be sure to copy the data out of it before it gets overwritten by another caller. @@ -122,7 +150,6 @@ static void sortSerialPortFunctions(serialPortFunction_t *serialPortFunctions, u static serialPortSearchResult_t *findSerialPort(serialPortFunction_e function) { static serialPortSearchResult_t serialPortSearchResult; - serialPortSearchResult.found = false; const functionConstraint_t *functionConstraint = findFunctionConstraint(function); if (!functionConstraint) { @@ -140,9 +167,16 @@ static serialPortSearchResult_t *findSerialPort(serialPortFunction_e function) continue; } - uint8_t serialPortIndex = findSerialPortIndexByIdentifier(serialPortFunction->identifier); + uint8_t serialPortIndex = lookupSerialPortIndexByIdentifier(serialPortFunction->identifier); const serialPortConstraint_t *serialPortConstraint = &serialPortConstraints[serialPortIndex]; + if (!canSoftwareSerialBeUsed() && ( + serialPortConstraint->identifier == SERIAL_PORT_SOFTSERIAL1 || + serialPortConstraint->identifier == SERIAL_PORT_SOFTSERIAL2 + )) { + continue; + } + if (functionConstraint->requiredSerialPortFeatures != SPF_NONE) { if (!(serialPortConstraint->feature & functionConstraint->requiredSerialPortFeatures)) { continue; @@ -155,17 +189,16 @@ static serialPortSearchResult_t *findSerialPort(serialPortFunction_e function) serialPortSearchResult.portConstraint = serialPortConstraint; serialPortSearchResult.portFunction = serialPortFunction; serialPortSearchResult.functionConstraint = functionConstraint; - serialPortSearchResult.found = true; - break; + return &serialPortSearchResult; } - return &serialPortSearchResult; + return NULL; } bool canOpenSerialPort(uint16_t functionMask) { serialPortSearchResult_t *result = findSerialPort(functionMask); - return result->found; + return result != NULL; } serialPortFunction_t *findSerialPortFunction(uint16_t functionMask) @@ -212,7 +245,7 @@ serialPort_t *openSerialPort(serialPortFunction_e function, serialReceiveCallbac serialPortSearchResult_t *searchResult = findSerialPort(function); - if (!searchResult->found) { + if (!searchResult) { return NULL; } serialPortIndex_e portIndex = searchResult->portIndex; @@ -284,9 +317,56 @@ void endSerialPortFunction(serialPort_t *port, serialPortFunction_e function) serialPortFunction->currentFunction = FUNCTION_NONE; } +bool isSerialConfigValid(serialConfig_t *serialConfig) +{ + serialPortSearchResult_t *searchResult; + + searchResult = findSerialPort(FUNCTION_MSP); + if (!searchResult) { + return false; + } + + searchResult = findSerialPort(FUNCTION_CLI); + if (!searchResult) { + return false; + } + + searchResult = findSerialPort(FUNCTION_GPS); + if (feature(FEATURE_GPS) && !searchResult) { + return false; + } + + searchResult = findSerialPort(FUNCTION_SERIAL_RX); + if (feature(FEATURE_SERIALRX) && !searchResult) { + return false; + } + + searchResult = findSerialPort(FUNCTION_TELEMETRY); + if (feature(FEATURE_TELEMETRY) && !searchResult) { + return false; + } + + return true; +} + +void applySerialConfigToPortFunctions(serialConfig_t *serialConfig) +{ + serialPortFunctions[lookupSerialPortFunctionIndexByIdentifier(SERIAL_PORT_USART1)].scenario = serialPortScenarios[serialConfig->serial_port_1_scenario]; + serialPortFunctions[lookupSerialPortFunctionIndexByIdentifier(SERIAL_PORT_USART2)].scenario = serialPortScenarios[serialConfig->serial_port_2_scenario]; + serialPortFunctions[lookupSerialPortFunctionIndexByIdentifier(SERIAL_PORT_SOFTSERIAL1)].scenario = serialPortScenarios[serialConfig->serial_port_3_scenario]; + serialPortFunctions[lookupSerialPortFunctionIndexByIdentifier(SERIAL_PORT_SOFTSERIAL2)].scenario = serialPortScenarios[serialConfig->serial_port_4_scenario]; +} + + void serialInit(serialConfig_t *initialSerialConfig) { serialConfig = initialSerialConfig; + applySerialConfigToPortFunctions(serialConfig); + + if (!isSerialConfigValid(serialConfig)) { + resetSerialConfig(serialConfig); + applySerialConfigToPortFunctions(serialConfig); + } mspInit(serialConfig); cliInit(serialConfig); diff --git a/src/serial_common.h b/src/serial_common.h index 2b71af4db5..e810a1c00a 100644 --- a/src/serial_common.h +++ b/src/serial_common.h @@ -11,18 +11,22 @@ typedef enum { } serialPortFunction_e; typedef enum { - SCENARIO_MAIN_PORT = FUNCTION_MSP | FUNCTION_CLI | FUNCTION_TELEMETRY | FUNCTION_GPS_PASSTHROUGH, - SCENARIO_CLI_ONLY = FUNCTION_CLI, - SCENARIO_GPS_AND_TELEMETRY = FUNCTION_GPS | FUNCTION_TELEMETRY, - SCENARIO_GPS_PASSTHROUGH_ONLY = FUNCTION_GPS_PASSTHROUGH, - SCENARIO_GPS_ONLY = FUNCTION_GPS, - SCENARIO_MSP_ONLY = FUNCTION_MSP, - SCENARIO_SERIAL_RX_ONLY = FUNCTION_SERIAL_RX, - SCENARIO_TELEMETRY_ONLY = FUNCTION_TELEMETRY, - SCENARIO_MSP_CLI_GPS_PASTHROUGH = FUNCTION_CLI | FUNCTION_MSP | FUNCTION_GPS_PASSTHROUGH, - SCENARIO_UNUSED = FUNCTION_NONE + SCENARIO_UNUSED = FUNCTION_NONE, + + SCENARIO_CLI_ONLY = FUNCTION_CLI, + SCENARIO_GPS_ONLY = FUNCTION_GPS, + SCENARIO_GPS_PASSTHROUGH_ONLY = FUNCTION_GPS_PASSTHROUGH, + SCENARIO_MSP_ONLY = FUNCTION_MSP, + SCENARIO_MSP_CLI_GPS_PASTHROUGH = FUNCTION_CLI | FUNCTION_MSP | FUNCTION_GPS_PASSTHROUGH, + SCENARIO_MSP_CLI_TELEMETRY_GPS_PASTHROUGH = FUNCTION_MSP | FUNCTION_CLI | FUNCTION_TELEMETRY | FUNCTION_GPS_PASSTHROUGH, + SCENARIO_SERIAL_RX_ONLY = FUNCTION_SERIAL_RX, + SCENARIO_TELEMETRY_ONLY = FUNCTION_TELEMETRY, } serialPortFunctionScenario_e; +#define SERIAL_PORT_SCENARIO_COUNT 9 +#define SERIAL_PORT_SCENARIO_MAX (SERIAL_PORT_SCENARIO_COUNT - 1) +extern const serialPortFunctionScenario_e serialPortScenarios[SERIAL_PORT_SCENARIO_COUNT]; + #define SERIAL_PORT_COUNT 4 typedef enum { @@ -62,6 +66,11 @@ typedef struct serialPortFunction_s { } serialPortFunction_t; typedef struct serialConfig_s { + uint8_t serial_port_1_scenario; + uint8_t serial_port_2_scenario; + uint8_t serial_port_3_scenario; + uint8_t serial_port_4_scenario; + uint32_t msp_baudrate; uint32_t cli_baudrate; uint32_t gps_passthrough_baudrate; @@ -71,6 +80,8 @@ typedef struct serialConfig_s { uint8_t reboot_character; // which byte is used to reboot. Default 'R', could be changed carefully to something else. } serialConfig_t; +uint8_t lookupScenarioIndex(serialPortFunctionScenario_e scenario); + serialPort_t *findOpenSerialPort(uint16_t functionMask); serialPort_t *openSerialPort(serialPortFunction_e functionMask, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode, serialInversion_e inversion);