From e179218caf67adab6e1fced774836fad636ac960 Mon Sep 17 00:00:00 2001 From: Petr Ledvina Date: Thu, 23 Oct 2014 15:08:57 +0200 Subject: [PATCH] Trivial changes - use inline functions for gpio (typesafe, no speed penalty) - fix sortSerialPortFunctions (original was IMO broken) - allow softserial port on sonar pin when FEATURE_SONAR is not enabled - minor style changes and comments --- src/main/drivers/bus_i2c_stm32f10x.c | 2 ++ src/main/drivers/gpio.h | 8 ++++---- src/main/io/serial.c | 21 +++++++++------------ src/main/io/serial_msp.c | 6 ++++-- src/main/mw.c | 3 +-- 5 files changed, 20 insertions(+), 20 deletions(-) diff --git a/src/main/drivers/bus_i2c_stm32f10x.c b/src/main/drivers/bus_i2c_stm32f10x.c index 55949cafb..7031f3ebe 100755 --- a/src/main/drivers/bus_i2c_stm32f10x.c +++ b/src/main/drivers/bus_i2c_stm32f10x.c @@ -195,6 +195,7 @@ static void i2c_er_handler(void) I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); // disable the RXNE/TXE interrupt - prevent the ISR tailchaining onto the ER (hopefully) if (!(SR1Register & 0x0200) && !(I2Cx->CR1 & 0x0200)) { // if we dont have an ARLO error, ensure sending of a stop if (I2Cx->CR1 & 0x0100) { // We are currently trying to send a start, this is very bad as start, stop will hang the peripheral + // TODO - busy waiting in highest priority IRQ. Maybe only set flag and handle it from main loop while (I2Cx->CR1 & 0x0100) { ; } // wait for any start to finish sending I2C_GenerateSTOP(I2Cx, ENABLE); // send stop to finalise bus transaction while (I2Cx->CR1 & 0x0200) { ; } // wait for stop to finish sending @@ -281,6 +282,7 @@ void i2c_ev_handler(void) subaddress_sent = 1; // this is set back to zero upon completion of the current task } } + // TODO - busy waiting in ISR // we must wait for the start to clear, otherwise we get constant BTF while (I2Cx->CR1 & 0x0100) { ; } } else if (SReg_1 & 0x0040) { // Byte received - EV7 diff --git a/src/main/drivers/gpio.h b/src/main/drivers/gpio.h index c4eff23d4..011087583 100644 --- a/src/main/drivers/gpio.h +++ b/src/main/drivers/gpio.h @@ -106,10 +106,10 @@ typedef struct GPIO_Speed speed; } gpio_config_t; -#define digitalHi(p, i) { p->BSRR = i; } -#define digitalLo(p, i) { p->BRR = i; } -#define digitalToggle(p, i) { p->ODR ^= i; } -#define digitalIn(p, i) (p->IDR & i) +static inline void digitalHi(GPIO_TypeDef *p, uint16_t i) { p->BSRR = i; } +static inline void digitalLo(GPIO_TypeDef *p, uint16_t i) { p->BRR = i; } +static inline void digitalToggle(GPIO_TypeDef *p, uint16_t i) { p->ODR ^= i; } +static inline uint16_t digitalIn(GPIO_TypeDef *p, uint16_t i) {return p->IDR & i; } void gpioInit(GPIO_TypeDef *gpio, gpio_config_t *config); void gpioExtiLineConfig(uint8_t portsrc, uint8_t pinsrc); diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 03426d495..09b3d877f 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -218,19 +218,16 @@ static void sortSerialPortFunctions(serialPortFunction_t *serialPortFunctions, u { serialPortFunction_t swap; - int8_t index1; - int8_t index2; - int result; - + int index1; + int index2; + + // bubble-sort array (TODO - port selection can be implemented as repeated minimum search with bitmask marking used elements) for (index1 = 0; index1 < (elements - 1); index1++) { for (index2 = 0; index2 < elements - index1 - 1; index2++) { - - result = serialPortFunctionMostSpecificFirstComparator(&serialPortFunctions[index2], &serialPortFunctions[index2 + 1]); - - if (result > 0) { - memcpy(&swap, &serialPortFunctions[index1], sizeof(serialPortFunction_t)); - memcpy(&serialPortFunctions[index1], &serialPortFunctions[index2 + 1], sizeof(serialPortFunction_t)); - memcpy(&serialPortFunctions[index2 + 1], &swap, sizeof(serialPortFunction_t)); + if(serialPortFunctionMostSpecificFirstComparator(&serialPortFunctions[index2], &serialPortFunctions[index2 + 1]) > 0) { + swap=serialPortFunctions[index2]; + serialPortFunctions[index2] = serialPortFunctions[index2 + 1]; + serialPortFunctions[index2 + 1] = swap; } } } @@ -259,7 +256,7 @@ serialPortSearchResult_t *findNextSerialPort(serialPortFunction_e function, cons } #if (defined(NAZE) || defined(OLIMEXINO)) && defined(SONAR) - if (!feature(FEATURE_RX_PARALLEL_PWM) && (serialPortConstraint->identifier == SERIAL_PORT_SOFTSERIAL2)) { + if (feature(FEATURE_SONAR) && !feature(FEATURE_RX_PARALLEL_PWM) && (serialPortConstraint->identifier == SERIAL_PORT_SOFTSERIAL2)) { continue; } #endif diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 8bd7ae906..0efb1b29d 100755 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -1391,19 +1391,21 @@ void mspSetTelemetryPort(serialPort_t *serialPort) mspPort_t *matchedPort = NULL; // find existing telemetry port - for (portIndex = 0; portIndex < MAX_MSP_PORT_COUNT && !matchedPort; portIndex++) { + for (portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) { candidatePort = &mspPorts[portIndex]; if (candidatePort->mspPortUsage == FOR_TELEMETRY) { matchedPort = candidatePort; + break; } } if (!matchedPort) { // find unused port - for (portIndex = 0; portIndex < MAX_MSP_PORT_COUNT && !matchedPort; portIndex++) { + for (portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) { candidatePort = &mspPorts[portIndex]; if (candidatePort->mspPortUsage == UNUSED_PORT) { matchedPort = candidatePort; + break; } } } diff --git a/src/main/mw.c b/src/main/mw.c index a415cf37e..53710b007 100755 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -190,8 +190,7 @@ void annexCode(void) rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100; prop1 = 100 - (uint16_t)currentProfile->controlRateConfig.rollPitchRate * tmp / 500; prop1 = (uint16_t)prop1 * prop2 / 100; - } - if (axis == YAW) { + } else if (axis == YAW) { if (currentProfile->yaw_deadband) { if (tmp > currentProfile->yaw_deadband) { tmp -= currentProfile->yaw_deadband;