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

@@ -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

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);

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

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;
             }
         }
     }

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;