From e56f46a7561625449ae47599c132a75090ddf044 Mon Sep 17 00:00:00 2001 From: Petr Ledvina Date: Thu, 29 Jan 2015 11:59:10 +0100 Subject: [PATCH 01/17] Move utility macros to common/utils.h --- src/main/blackbox/blackbox.c | 6 +----- src/main/common/utils.h | 6 ++++++ 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 0ee1234768..a8db32f82d 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -26,6 +26,7 @@ #include "common/maths.h" #include "common/axis.h" #include "common/color.h" +#include "common/utils.h" #include "drivers/gpio.h" #include "drivers/sensor.h" @@ -86,11 +87,6 @@ typedef char assert_failed_ ## name [(condition) ? 1 : -1 ] // Some macros to make writing FLIGHT_LOG_FIELD_* constants shorter: -#define STR_HELPER(x) #x -#define STR(x) STR_HELPER(x) - -#define CONCAT_HELPER(x,y) x ## y -#define CONCAT(x,y) CONCAT_HELPER(x, y) #define PREDICT(x) CONCAT(FLIGHT_LOG_FIELD_PREDICTOR_, x) #define ENCODING(x) CONCAT(FLIGHT_LOG_FIELD_ENCODING_, x) diff --git a/src/main/common/utils.h b/src/main/common/utils.h index d9f22c01e9..90e12ac759 100644 --- a/src/main/common/utils.h +++ b/src/main/common/utils.h @@ -21,6 +21,12 @@ #define ARRAYLEN(x) (sizeof(x) / sizeof((x)[0])) +#define CONCAT_HELPER(x,y) x ## y +#define CONCAT(x,y) CONCAT_HELPER(x, y) + +#define STR_HELPER(x) #x +#define STR(x) STR_HELPER(x) + /* http://resnet.uoregon.edu/~gurney_j/jmpc/bitwise.html */ From 6b0fea5952da024c1417e60cfb03cbcbabee73f4 Mon Sep 17 00:00:00 2001 From: Petr Ledvina Date: Thu, 29 Jan 2015 14:40:14 +0100 Subject: [PATCH 02/17] simplify `#ifdef`s --- src/main/config/config.c | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index b489ff3bd7..03ade62284 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -723,16 +723,12 @@ void validateAndFixConfig(void) #if defined(LED_STRIP) && (defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2)) if (feature(FEATURE_SOFTSERIAL) && ( + 0 #ifdef USE_SOFTSERIAL1 - (LED_STRIP_TIMER == SOFTSERIAL_1_TIMER) -#else - 0 + || (LED_STRIP_TIMER == SOFTSERIAL_1_TIMER) #endif - || #ifdef USE_SOFTSERIAL2 - (LED_STRIP_TIMER == SOFTSERIAL_2_TIMER) -#else - 0 + || (LED_STRIP_TIMER == SOFTSERIAL_2_TIMER) #endif )) { // led strip needs the same timer as softserial From d3324a9f0dbf7856edee8f4d68edc0941522639e Mon Sep 17 00:00:00 2001 From: Petr Ledvina Date: Thu, 29 Jan 2015 14:43:40 +0100 Subject: [PATCH 03/17] Move ADC internal interface into new header file --- src/main/drivers/adc.c | 1 + src/main/drivers/adc_impl.h | 21 +++++++++++++++++++++ src/main/drivers/adc_stm32f10x.c | 3 +-- src/main/drivers/adc_stm32f30x.c | 4 +--- 4 files changed, 24 insertions(+), 5 deletions(-) create mode 100644 src/main/drivers/adc_impl.h diff --git a/src/main/drivers/adc.c b/src/main/drivers/adc.c index 958a831b46..bdc7a67748 100644 --- a/src/main/drivers/adc.c +++ b/src/main/drivers/adc.c @@ -24,6 +24,7 @@ #include "system.h" #include "adc.h" +#include "adc_impl.h" //#define DEBUG_ADC_CHANNELS diff --git a/src/main/drivers/adc_impl.h b/src/main/drivers/adc_impl.h new file mode 100644 index 0000000000..8007308251 --- /dev/null +++ b/src/main/drivers/adc_impl.h @@ -0,0 +1,21 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +extern adc_config_t adcConfig[ADC_CHANNEL_COUNT]; +extern volatile uint16_t adcValues[ADC_CHANNEL_COUNT]; diff --git a/src/main/drivers/adc_stm32f10x.c b/src/main/drivers/adc_stm32f10x.c index c6c97c2f45..2becc32df0 100644 --- a/src/main/drivers/adc_stm32f10x.c +++ b/src/main/drivers/adc_stm32f10x.c @@ -31,6 +31,7 @@ #include "accgyro.h" #include "adc.h" +#include "adc_impl.h" // Driver for STM32F103CB onboard ADC // @@ -43,8 +44,6 @@ // // CC3D Only one ADC channel supported currently, for battery on S5_IN/PA0 -extern adc_config_t adcConfig[ADC_CHANNEL_COUNT]; -extern volatile uint16_t adcValues[ADC_CHANNEL_COUNT]; void adcInit(drv_adc_config_t *init) { diff --git a/src/main/drivers/adc_stm32f30x.c b/src/main/drivers/adc_stm32f30x.c index c18b37417b..3b2fa18592 100644 --- a/src/main/drivers/adc_stm32f30x.c +++ b/src/main/drivers/adc_stm32f30x.c @@ -28,9 +28,7 @@ #include "accgyro.h" #include "adc.h" - -extern adc_config_t adcConfig[ADC_CHANNEL_COUNT]; -extern volatile uint16_t adcValues[ADC_CHANNEL_COUNT]; +#include "adc_impl.h" #ifndef ADC_INSTANCE #define ADC_INSTANCE ADC1 From 5129403c83269b06b4339019d316a5e59d3279bd Mon Sep 17 00:00:00 2001 From: Petr Ledvina Date: Thu, 29 Jan 2015 14:45:22 +0100 Subject: [PATCH 04/17] minor I2C cleanup --- src/main/drivers/bus_i2c_stm32f10x.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/bus_i2c_stm32f10x.c b/src/main/drivers/bus_i2c_stm32f10x.c index fa0e27c439..20e1396985 100644 --- a/src/main/drivers/bus_i2c_stm32f10x.c +++ b/src/main/drivers/bus_i2c_stm32f10x.c @@ -184,7 +184,7 @@ bool i2cRead(uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf) static void i2c_er_handler(void) { // Read the I2Cx status register - volatile uint32_t SR1Register = I2Cx->SR1; + uint32_t SR1Register = I2Cx->SR1; if (SR1Register & 0x0F00) { // an error error = true; @@ -333,7 +333,7 @@ void i2cInit(I2CDevice index) I2C_DeInit(I2Cx); I2C_StructInit(&i2c); - I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, DISABLE); // Enable EVT and ERR interrupts - they are enabled by the first request + I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, DISABLE); // Disable EVT and ERR interrupts - they are enabled by the first request i2c.I2C_Mode = I2C_Mode_I2C; i2c.I2C_DutyCycle = I2C_DutyCycle_2; i2c.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; From f9f40fb98fa499a557e3b0acf27326e3fc0858b3 Mon Sep 17 00:00:00 2001 From: Petr Ledvina Date: Thu, 29 Jan 2015 14:48:21 +0100 Subject: [PATCH 05/17] Handle possible problem with unintentional I2C interrupt handler triggering Error handler may be called repeatedly during I2C unstucking. Not sure if this change is necessary, but it is safe --- src/main/drivers/bus_i2c_stm32f10x.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/drivers/bus_i2c_stm32f10x.c b/src/main/drivers/bus_i2c_stm32f10x.c index 20e1396985..026dccf507 100644 --- a/src/main/drivers/bus_i2c_stm32f10x.c +++ b/src/main/drivers/bus_i2c_stm32f10x.c @@ -325,6 +325,9 @@ void i2cInit(I2CDevice index) I2Cx_index = index; RCC_APB1PeriphClockCmd(i2cHardwareMap[index].peripheral, ENABLE); + // diable I2C interrrupts first to avoid ER handler triggering + I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, DISABLE); + // clock out stuff to make sure slaves arent stuck // This will also configure GPIO as AF_OD at the end i2cUnstick(); From fd355caca6b5118102af7bf676cc15620dba03c9 Mon Sep 17 00:00:00 2001 From: Petr Ledvina Date: Thu, 29 Jan 2015 14:56:21 +0100 Subject: [PATCH 06/17] Modify some macros so they evaluate to single statement Require macros to be followed with semicolon, empty version expands to dummy statement. Fixes dangling-else problem: ``` if(1) INVERTER_ON; else INVERTER_OFF; ``` --- src/main/drivers/inverter.h | 8 +++--- src/main/drivers/light_led.h | 48 ++++++++++++++++++------------------ 2 files changed, 28 insertions(+), 28 deletions(-) diff --git a/src/main/drivers/inverter.h b/src/main/drivers/inverter.h index 81bd432956..017df613b0 100644 --- a/src/main/drivers/inverter.h +++ b/src/main/drivers/inverter.h @@ -18,11 +18,11 @@ #pragma once #ifdef INVERTER -#define INVERTER_OFF digitalLo(INVERTER_GPIO, INVERTER_PIN); -#define INVERTER_ON digitalHi(INVERTER_GPIO, INVERTER_PIN); +#define INVERTER_OFF digitalLo(INVERTER_GPIO, INVERTER_PIN) +#define INVERTER_ON digitalHi(INVERTER_GPIO, INVERTER_PIN) #else -#define INVERTER_OFF ; -#define INVERTER_ON ; +#define INVERTER_OFF do {} while(0) +#define INVERTER_ON do {} while(0) #endif void initInverter(void); diff --git a/src/main/drivers/light_led.h b/src/main/drivers/light_led.h index 8f62bd5ab6..cf6a2bea97 100644 --- a/src/main/drivers/light_led.h +++ b/src/main/drivers/light_led.h @@ -19,49 +19,49 @@ // Helpful macros #ifdef LED0 -#define LED0_TOGGLE digitalToggle(LED0_GPIO, LED0_PIN); +#define LED0_TOGGLE digitalToggle(LED0_GPIO, LED0_PIN) #ifndef LED0_INVERTED -#define LED0_OFF digitalHi(LED0_GPIO, LED0_PIN); -#define LED0_ON digitalLo(LED0_GPIO, LED0_PIN); +#define LED0_OFF digitalHi(LED0_GPIO, LED0_PIN) +#define LED0_ON digitalLo(LED0_GPIO, LED0_PIN) #else -#define LED0_OFF digitalLo(LED0_GPIO, LED0_PIN); -#define LED0_ON digitalHi(LED0_GPIO, LED0_PIN); +#define LED0_OFF digitalLo(LED0_GPIO, LED0_PIN) +#define LED0_ON digitalHi(LED0_GPIO, LED0_PIN) #endif // inverted #else -#define LED0_TOGGLE -#define LED0_OFF -#define LED0_ON +#define LED0_TOGGLE do {} while(0) +#define LED0_OFF do {} while(0) +#define LED0_ON do {} while(0) #endif #ifdef LED1 -#define LED1_TOGGLE digitalToggle(LED1_GPIO, LED1_PIN); +#define LED1_TOGGLE digitalToggle(LED1_GPIO, LED1_PIN) #ifndef LED1_INVERTED -#define LED1_OFF digitalHi(LED1_GPIO, LED1_PIN); -#define LED1_ON digitalLo(LED1_GPIO, LED1_PIN); +#define LED1_OFF digitalHi(LED1_GPIO, LED1_PIN) +#define LED1_ON digitalLo(LED1_GPIO, LED1_PIN) #else -#define LED1_OFF digitalLo(LED1_GPIO, LED1_PIN); -#define LED1_ON digitalHi(LED1_GPIO, LED1_PIN); +#define LED1_OFF digitalLo(LED1_GPIO, LED1_PIN) +#define LED1_ON digitalHi(LED1_GPIO, LED1_PIN) #endif // inverted #else -#define LED1_TOGGLE -#define LED1_OFF -#define LED1_ON +#define LED1_TOGGLE do {} while(0) +#define LED1_OFF do {} while(0) +#define LED1_ON do {} while(0) #endif #ifdef LED2 -#define LED2_TOGGLE digitalToggle(LED2_GPIO, LED2_PIN); +#define LED2_TOGGLE digitalToggle(LED2_GPIO, LED2_PIN) #ifndef LED2_INVERTED -#define LED2_OFF digitalHi(LED2_GPIO, LED2_PIN); -#define LED2_ON digitalLo(LED2_GPIO, LED2_PIN); +#define LED2_OFF digitalHi(LED2_GPIO, LED2_PIN) +#define LED2_ON digitalLo(LED2_GPIO, LED2_PIN) #else -#define LED2_OFF digitalLo(LED2_GPIO, LED2_PIN); -#define LED2_ON digitalHi(LED2_GPIO, LED2_PIN); +#define LED2_OFF digitalLo(LED2_GPIO, LED2_PIN) +#define LED2_ON digitalHi(LED2_GPIO, LED2_PIN) #endif // inverted #else -#define LED2_TOGGLE -#define LED2_OFF -#define LED2_ON +#define LED2_TOGGLE do {} while(0) +#define LED2_OFF do {} while(0) +#define LED2_ON do {} while(0) #endif void ledInit(void); From 7c263254b2cfaeeae4e2e99f55b682209e60cd89 Mon Sep 17 00:00:00 2001 From: Petr Ledvina Date: Thu, 29 Jan 2015 14:56:59 +0100 Subject: [PATCH 07/17] improve loop readability --- src/main/drivers/pwm_mapping.c | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 6ac1e78475..533d980746 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -351,13 +351,10 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) setup = hardwareMaps[i]; - for (i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { + for (i = 0; i < USABLE_TIMER_CHANNEL_COUNT && setup[i] != 0xFFFF; i++) { uint8_t timerIndex = setup[i] & 0x00FF; uint8_t type = (setup[i] & 0xFF00) >> 8; - if (setup[i] == 0xFFFF) // terminator - break; - const timerHardware_t *timerHardwarePtr = &timerHardware[timerIndex]; #ifdef OLIMEXINO_UNCUT_LED2_E_JUMPER From 7c62ec975534326b2b8cec4be76fc2d671eb27fd Mon Sep 17 00:00:00 2001 From: Petr Ledvina Date: Thu, 29 Jan 2015 15:05:28 +0100 Subject: [PATCH 08/17] Move internal serial interface into separate header file Declaring function from other file is quite dangerous - there is no warning if interfaces get out of sync --- src/main/drivers/serial_uart.c | 5 +--- src/main/drivers/serial_uart.h | 2 -- src/main/drivers/serial_uart_impl.h | 29 ++++++++++++++++++++++++ src/main/drivers/serial_uart_stm32f10x.c | 3 +-- src/main/drivers/serial_uart_stm32f30x.c | 3 +-- 5 files changed, 32 insertions(+), 10 deletions(-) create mode 100644 src/main/drivers/serial_uart_impl.h diff --git a/src/main/drivers/serial_uart.c b/src/main/drivers/serial_uart.c index 0057b7ba92..90b7648f3a 100644 --- a/src/main/drivers/serial_uart.c +++ b/src/main/drivers/serial_uart.c @@ -34,10 +34,7 @@ #include "serial.h" #include "serial_uart.h" - -uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode); -uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode); -uartPort_t *serialUSART3(uint32_t baudRate, portMode_t mode); +#include "serial_uart_impl.h" static void usartConfigurePinInversion(uartPort_t *uartPort) { #if !defined(INVERTER) && !defined(STM32F303xC) diff --git a/src/main/drivers/serial_uart.h b/src/main/drivers/serial_uart.h index 817929f288..6a7357d26d 100644 --- a/src/main/drivers/serial_uart.h +++ b/src/main/drivers/serial_uart.h @@ -44,8 +44,6 @@ typedef struct { USART_TypeDef *USARTx; } uartPort_t; -extern const struct serialPortVTable uartVTable[]; - serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode, serialInversion_e inversion); // serialPort API diff --git a/src/main/drivers/serial_uart_impl.h b/src/main/drivers/serial_uart_impl.h new file mode 100644 index 0000000000..713405bc09 --- /dev/null +++ b/src/main/drivers/serial_uart_impl.h @@ -0,0 +1,29 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +// device specific uart implementation is defined here + +extern const struct serialPortVTable uartVTable[]; + +void uartStartTxDMA(uartPort_t *s); + +uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode); +uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode); +uartPort_t *serialUSART3(uint32_t baudRate, portMode_t mode); + diff --git a/src/main/drivers/serial_uart_stm32f10x.c b/src/main/drivers/serial_uart_stm32f10x.c index 5369206dc3..02a99b3f66 100644 --- a/src/main/drivers/serial_uart_stm32f10x.c +++ b/src/main/drivers/serial_uart_stm32f10x.c @@ -34,6 +34,7 @@ #include "serial.h" #include "serial_uart.h" +#include "serial_uart_impl.h" #ifdef USE_USART1 static uartPort_t uartPort1; @@ -54,8 +55,6 @@ static uartPort_t uartPort3; #undef USE_USART1_RX_DMA #endif -void uartStartTxDMA(uartPort_t *s); - void usartIrqCallback(uartPort_t *s) { uint16_t SR = s->USARTx->SR; diff --git a/src/main/drivers/serial_uart_stm32f30x.c b/src/main/drivers/serial_uart_stm32f30x.c index f7bc087e08..4436fc291b 100644 --- a/src/main/drivers/serial_uart_stm32f30x.c +++ b/src/main/drivers/serial_uart_stm32f30x.c @@ -35,6 +35,7 @@ #include "serial.h" #include "serial_uart.h" +#include "serial_uart_impl.h" // Using RX DMA disables the use of receive callbacks #define USE_USART1_RX_DMA @@ -74,8 +75,6 @@ static uartPort_t uartPort1; static uartPort_t uartPort2; static uartPort_t uartPort3; -void uartStartTxDMA(uartPort_t *s); - uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode) { uartPort_t *s; From 7875b97aae9689b9dce72373f817245d472cc766 Mon Sep 17 00:00:00 2001 From: Petr Ledvina Date: Thu, 29 Jan 2015 15:11:27 +0100 Subject: [PATCH 09/17] Compile serial code conditionally on stm32f303 --- src/main/drivers/serial_uart_stm32f30x.c | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/src/main/drivers/serial_uart_stm32f30x.c b/src/main/drivers/serial_uart_stm32f30x.c index 4436fc291b..10d8cdd9eb 100644 --- a/src/main/drivers/serial_uart_stm32f30x.c +++ b/src/main/drivers/serial_uart_stm32f30x.c @@ -71,10 +71,17 @@ #define UART3_RX_PINSOURCE GPIO_PinSource11 #endif +#ifdef USE_USART1 static uartPort_t uartPort1; +#endif +#ifdef USE_USART2 static uartPort_t uartPort2; +#endif +#ifdef USE_USART3 static uartPort_t uartPort3; +#endif +#ifdef USE_USART1 uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode) { uartPort_t *s; @@ -147,7 +154,9 @@ uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode) return s; } +#endif +#ifdef USE_USART2 uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode) { uartPort_t *s; @@ -226,7 +235,9 @@ uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode) return s; } +#endif +#ifdef USE_USART3 uartPort_t *serialUSART3(uint32_t baudRate, portMode_t mode) { uartPort_t *s; @@ -305,6 +316,7 @@ uartPort_t *serialUSART3(uint32_t baudRate, portMode_t mode) return s; } +#endif static void handleUsartTxDma(uartPort_t *s) { @@ -380,24 +392,29 @@ void usartIrqHandler(uartPort_t *s) } } +#ifdef USE_USART1 void USART1_IRQHandler(void) { uartPort_t *s = &uartPort1; usartIrqHandler(s); } +#endif +#ifdef USE_USART2 void USART2_IRQHandler(void) { uartPort_t *s = &uartPort2; usartIrqHandler(s); } +#endif +#ifdef USE_USART3 void USART3_IRQHandler(void) { uartPort_t *s = &uartPort3; usartIrqHandler(s); } - +#endif From 9dd7faeefafb999555ec404e212c5293dc4c4a59 Mon Sep 17 00:00:00 2001 From: Petr Ledvina Date: Thu, 29 Jan 2015 15:05:56 +0100 Subject: [PATCH 10/17] Remove unneeded volatile --- src/main/drivers/system.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/drivers/system.c b/src/main/drivers/system.c index d5a4860fd8..2de34f3072 100644 --- a/src/main/drivers/system.c +++ b/src/main/drivers/system.c @@ -31,7 +31,7 @@ #include "system.h" // cycles per microsecond -static volatile uint32_t usTicks = 0; +static uint32_t usTicks = 0; // current uptime for 1kHz systick timer. will rollover after 49 days. hopefully we won't care. static volatile uint32_t sysTickUptime = 0; From 6b37b960481a9c20150802d5abea19d0961463e2 Mon Sep 17 00:00:00 2001 From: Petr Ledvina Date: Thu, 29 Jan 2015 15:11:27 +0100 Subject: [PATCH 11/17] Simplify #ifdef nesting --- src/main/io/serial.c | 22 +++++++--------------- 1 file changed, 7 insertions(+), 15 deletions(-) diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 4c9b5c4d75..a7e8495a1b 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -101,9 +101,8 @@ const serialPortConstraint_t serialPortConstraints[SERIAL_PORT_COUNT] = { #endif }; -#else +#elif defined(CC3D) -#ifdef CC3D static serialPortFunction_t serialPortFunctions[SERIAL_PORT_COUNT] = { #ifdef USE_VCP {SERIAL_PORT_USB_VCP, NULL, SCENARIO_UNUSED, FUNCTION_NONE}, @@ -140,7 +139,7 @@ const serialPortConstraint_t serialPortConstraints[SERIAL_PORT_COUNT] = { {SERIAL_PORT_SOFTSERIAL2, 9600, 19200, SPF_SUPPORTS_CALLBACK | SPF_IS_SOFTWARE_INVERTABLE} #endif }; -#endif + #endif const functionConstraint_t functionConstraints[] = { @@ -271,25 +270,18 @@ serialPortSearchResult_t *findNextSerialPort(serialPortFunction_e function, cons uint8_t serialPortIndex = lookupSerialPortIndexByIdentifier(serialPortFunction->identifier); const serialPortConstraint_t *serialPortConstraint = &serialPortConstraints[serialPortIndex]; -#if defined(CC3D) - if (!feature(FEATURE_SOFTSERIAL) && ( - serialPortConstraint->identifier == SERIAL_PORT_SOFTSERIAL1)) { +#if defined(USE_SOFTSERIAL1) + if (!feature(FEATURE_SOFTSERIAL) && serialPortConstraint->identifier == SERIAL_PORT_SOFTSERIAL1) continue; - } -#else -#if defined(USE_SOFTSERIAL1) ||(defined(USE_SOFTSERIAL2)) - if (!feature(FEATURE_SOFTSERIAL) && ( - serialPortConstraint->identifier == SERIAL_PORT_SOFTSERIAL1 || - serialPortConstraint->identifier == SERIAL_PORT_SOFTSERIAL2 - )) { +#endif +#if defined(USE_SOFTSERIAL2) + if (!feature(FEATURE_SOFTSERIAL) && serialPortConstraint->identifier == SERIAL_PORT_SOFTSERIAL2) continue; - } #endif #if (defined(NAZE) || defined(OLIMEXINO)) && defined(SONAR) if (feature(FEATURE_SONAR) && !feature(FEATURE_RX_PARALLEL_PWM) && (serialPortConstraint->identifier == SERIAL_PORT_SOFTSERIAL2)) { continue; } -#endif #endif if ((serialPortConstraint->feature & functionConstraint->requiredSerialPortFeatures) != functionConstraint->requiredSerialPortFeatures) { From 1ecbdf3daeef4b73941a0e0fb05bc20ab909c902 Mon Sep 17 00:00:00 2001 From: Petr Ledvina Date: Thu, 29 Jan 2015 15:11:27 +0100 Subject: [PATCH 12/17] Minor code cleanup --- src/main/io/serial_cli.c | 8 ++++---- src/main/sensors/battery.c | 15 ++++++--------- src/main/telemetry/smartport.c | 5 ++--- 3 files changed, 12 insertions(+), 16 deletions(-) diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 29c2f5497c..4ee885942f 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -1437,16 +1437,16 @@ static void cliSetVar(const clivalue_t *var, const int_float_value_t value) switch (var->type & VALUE_TYPE_MASK) { case VAR_UINT8: case VAR_INT8: - *(char *)ptr = (char)value.int_value; + *(int8_t *)ptr = value.int_value; break; case VAR_UINT16: case VAR_INT16: - *(short *)ptr = (short)value.int_value; + *(int16_t *)ptr = value.int_value; break; case VAR_UINT32: - *(int *)ptr = (int)value.int_value; + *(uint32_t *)ptr = value.int_value; break; case VAR_FLOAT: @@ -1639,7 +1639,7 @@ void cliProcess(void) } for (; i < bufferIndex; i++) cliWrite(cliBuffer[i]); - } else if (!bufferIndex && c == 4) { + } else if (!bufferIndex && c == 4) { // CTRL-D cliExit(cliBuffer); return; } else if (c == 12) { diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index a367cb8f85..83fb29d82b 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -92,23 +92,20 @@ void batteryInit(batteryConfig_t *initialBatteryConfig) delay((32 / BATTERY_SAMPLE_COUNT) * 10); } - // autodetect cell count, going from 1S..8S - for (i = 1; i < 8; i++) { - if (vbat < i * batteryConfig->vbatmaxcellvoltage) - break; - } - - batteryCellCount = i; + unsigned cells = (vbat / batteryConfig->vbatmaxcellvoltage) + 1; + if(cells > 8) // something is wrong, we expect 8 cells maximum (and autodetection will be problematic at 6+ cells) + cells = 8; + batteryCellCount = cells; batteryWarningVoltage = batteryCellCount * batteryConfig->vbatwarningcellvoltage; batteryCriticalVoltage = batteryCellCount * batteryConfig->vbatmincellvoltage; } -#define ADCVREF 33L +#define ADCVREF 3300 // in mV int32_t currentSensorToCentiamps(uint16_t src) { int32_t millivolts; - millivolts = ((uint32_t)src * ADCVREF * 100) / 4095; + millivolts = ((uint32_t)src * ADCVREF) / 4096; millivolts -= batteryConfig->currentMeterOffset; return (millivolts * 1000) / (int32_t)batteryConfig->currentMeterScale; // current in 0.01A steps diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 01c8d74a6f..c17eec1c17 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -322,13 +322,12 @@ void handleSmartPortTelemetry(void) smartPortIdCnt++; int32_t tmpi; - uint32_t tmpui; static uint8_t t1Cnt = 0; switch(id) { case FSSP_DATAID_SPEED : if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { - tmpui = (GPS_speed * 36 + 36 / 2) / 100; + uint32_t tmpui = (GPS_speed * 36 + 36 / 2) / 100; smartPortSendPackage(id, tmpui); // given in 0.1 m/s, provide in KM/H smartPortHasRequest = 0; } @@ -355,7 +354,7 @@ void handleSmartPortTelemetry(void) //case FSSP_DATAID_ADC2 : case FSSP_DATAID_LATLONG : if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { - tmpui = 0; + uint32_t tmpui = 0; // the same ID is sent twice, one for longitude, one for latitude // the MSB of the sent uint32_t helps FrSky keep track // the even/odd bit of our counter helps us keep track From f1ac4f8461b389a2e04e8a4ac557c7257a2d73a6 Mon Sep 17 00:00:00 2001 From: Petr Ledvina Date: Thu, 29 Jan 2015 15:11:27 +0100 Subject: [PATCH 13/17] fix #ifdef typo --- src/main/drivers/serial_uart_stm32f30x.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/drivers/serial_uart_stm32f30x.c b/src/main/drivers/serial_uart_stm32f30x.c index 10d8cdd9eb..6565e27fb0 100644 --- a/src/main/drivers/serial_uart_stm32f30x.c +++ b/src/main/drivers/serial_uart_stm32f30x.c @@ -349,7 +349,7 @@ void DMA1_Channel7_IRQHandler(void) #endif // USART3 Tx DMA Handler -#ifdef USE_USART2_TX_DMA +#ifdef USE_USART3_TX_DMA void DMA1_Channel2_IRQHandler(void) { uartPort_t *s = &uartPort3; From a67d2f8a9e30c54a6a1c38e307d5547471ebfd28 Mon Sep 17 00:00:00 2001 From: Petr Ledvina Date: Thu, 29 Jan 2015 15:11:27 +0100 Subject: [PATCH 14/17] Whitespace fixes --- src/main/main.c | 2 +- src/main/telemetry/hott.c | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/main.c b/src/main/main.c index 7e530b3f32..5c48aee3c9 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -162,7 +162,7 @@ void init(void) ledInit(); - #ifdef SPEKTRUM_BIND +#ifdef SPEKTRUM_BIND if (feature(FEATURE_RX_SERIAL)) { switch (masterConfig.rxConfig.serialrx_provider) { case SERIALRX_SPEKTRUM1024: diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index d0bfd56bfd..e81359d293 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -219,15 +219,15 @@ static inline void hottEAMUpdateBattery(HOTT_EAM_MSG_t *hottEAMMessage) static inline void hottEAMUpdateCurrentMeter(HOTT_EAM_MSG_t *hottEAMMessage) { - int32_t amp = amperage / 10; - hottEAMMessage->current_L = amp & 0xFF; + int32_t amp = amperage / 10; + hottEAMMessage->current_L = amp & 0xFF; hottEAMMessage->current_H = amp >> 8; } static inline void hottEAMUpdateBatteryDrawnCapacity(HOTT_EAM_MSG_t *hottEAMMessage) { - int32_t mAh = mAhDrawn / 10; - hottEAMMessage->batt_cap_L = mAh & 0xFF; + int32_t mAh = mAhDrawn / 10; + hottEAMMessage->batt_cap_L = mAh & 0xFF; hottEAMMessage->batt_cap_H = mAh >> 8; } From a96a12bd479f2b3403a7772dd841fd2df54c6c12 Mon Sep 17 00:00:00 2001 From: Petr Ledvina Date: Thu, 29 Jan 2015 15:10:25 +0100 Subject: [PATCH 15/17] compile some GPS code conditionally --- src/main/telemetry/frsky.c | 5 ++++- src/main/telemetry/smartport.c | 8 ++++++++ 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index 25691fb47d..e88bacaadb 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -172,6 +172,7 @@ static void sendBaro(void) serialize16(ABS(BaroAlt % 100)); } +#ifdef GPS static void sendGpsAltitude(void) { uint16_t altitude = GPS_altitude; @@ -184,7 +185,7 @@ static void sendGpsAltitude(void) sendDataHead(ID_GPS_ALTIDUTE_AP); serialize16(0); } - +#endif static void sendThrottleOrBatterySizeAsRpm(void) { @@ -207,6 +208,7 @@ static void sendTemperature1(void) #endif } +#ifdef GPS static void sendSatalliteSignalQualityAsTemperature2(void) { uint16_t satellite = GPS_numSat; @@ -236,6 +238,7 @@ static void sendSpeed(void) sendDataHead(ID_GPS_SPEED_AP); serialize16(0); //Not dipslayed } +#endif static void sendTime(void) { diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index c17eec1c17..f82b3d5273 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -325,6 +325,7 @@ void handleSmartPortTelemetry(void) static uint8_t t1Cnt = 0; switch(id) { +#ifdef GPS case FSSP_DATAID_SPEED : if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { uint32_t tmpui = (GPS_speed * 36 + 36 / 2) / 100; @@ -332,6 +333,7 @@ void handleSmartPortTelemetry(void) smartPortHasRequest = 0; } break; +#endif case FSSP_DATAID_VFAS : smartPortSendPackage(id, vbat * 83); // supposedly given in 0.1V, unknown requested unit // multiplying by 83 seems to make Taranis read correctly @@ -352,6 +354,7 @@ void handleSmartPortTelemetry(void) break; //case FSSP_DATAID_ADC1 : //case FSSP_DATAID_ADC2 : +#ifdef GPS case FSSP_DATAID_LATLONG : if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { uint32_t tmpui = 0; @@ -377,6 +380,7 @@ void handleSmartPortTelemetry(void) smartPortHasRequest = 0; } break; +#endif //case FSSP_DATAID_CAP_USED : case FSSP_DATAID_VARIO : smartPortSendPackage(id, vario); // unknown given unit but requested in 100 = 1m/s @@ -448,21 +452,25 @@ void handleSmartPortTelemetry(void) break; case FSSP_DATAID_T2 : if (sensors(SENSOR_GPS)) { +#ifdef GPS // provide GPS lock status smartPortSendPackage(id, (STATE(GPS_FIX) ? 1000 : 0) + (STATE(GPS_FIX_HOME) ? 2000 : 0) + GPS_numSat); smartPortHasRequest = 0; +#endif } else { smartPortSendPackage(id, 0); smartPortHasRequest = 0; } break; +#ifdef GPS case FSSP_DATAID_GPS_ALT : if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { smartPortSendPackage(id, GPS_altitude * 1000); // given in 0.1m , requested in 100 = 1m smartPortHasRequest = 0; } break; +#endif default: break; // if nothing is sent, smartPortHasRequest isn't cleared, we already incremented the counter, just wait for the next loop From 3abcbace72645de22140de8e1e5661cc70dcd488 Mon Sep 17 00:00:00 2001 From: Petr Ledvina Date: Thu, 29 Jan 2015 15:11:27 +0100 Subject: [PATCH 16/17] Generate phony targets in auto-dependencies This will fix problem with missing dependencies where #include changes --- Makefile | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Makefile b/Makefile index f3f29edd4d..a13ddb07f0 100644 --- a/Makefile +++ b/Makefile @@ -555,12 +555,12 @@ CFLAGS = $(ARCH_FLAGS) \ -D'__TARGET__="$(TARGET)"' \ -D'__REVISION__="$(REVISION)"' \ -save-temps=obj \ - -MMD + -MMD -MP ASFLAGS = $(ARCH_FLAGS) \ -x assembler-with-cpp \ $(addprefix -I,$(INCLUDE_DIRS)) \ - -MMD + -MMD -MP LDFLAGS = -lm \ -nostartfiles \ From 737fbe02ed716e1c292b803a1fead8efbe27079f Mon Sep 17 00:00:00 2001 From: Petr Ledvina Date: Fri, 30 Jan 2015 12:35:01 +0100 Subject: [PATCH 17/17] Fix battery unittest New code uses divition to get cell count, so nonzero vbatmaxcellvoltage is neccessary. Also added remaining fields to avoid future problems (and g++ doesn't support non-trivial initializers) --- src/test/unit/battery_unittest.cc | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/test/unit/battery_unittest.cc b/src/test/unit/battery_unittest.cc index 2a67017439..8eccb32e54 100644 --- a/src/test/unit/battery_unittest.cc +++ b/src/test/unit/battery_unittest.cc @@ -37,7 +37,17 @@ TEST(BatteryTest, BatteryADCToVoltage) { // given - batteryConfig_t batteryConfig; + batteryConfig_t batteryConfig = { + .vbatscale = 110, + .vbatmaxcellvoltage = 43, + .vbatmincellvoltage = 33, + .vbatwarningcellvoltage = 35, + .currentMeterScale = 400, + .currentMeterOffset = 0, + .currentMeterType = CURRENT_SENSOR_NONE, + .multiwiiCurrentMeterOutput = 0, + .batteryCapacity = 2200, + }; // batteryInit() reads a bunch of fields including vbatscale, so set up the config with useful initial values: memset(&batteryConfig, 0, sizeof(batteryConfig));