Merge remote-tracking branch 'multiwii/master'

Conflicts:
	src/board.h
	src/drivers/adc_common.c
	src/drivers/pwm_common.c
	src/drivers/pwm_common.h
	src/main.c
	src/mw.c
	src/mw.h
	src/sensors.c
	src/utils.h

src/boardalignment.c

@@ -125,3 +125,24 @@ void alignSensors(int16_t *src, int16_t *dest, uint8_t rotation)
     if (!standardBoardAlignment)
         alignBoard(dest);
 }
+
+#ifdef PROD_DEBUG
+void productionDebug(void)
+{
+    gpio_config_t gpio;
+
+    // remap PB6 to USART1_TX
+    gpio.pin = Pin_6;
+    gpio.mode = Mode_AF_PP;
+    gpio.speed = Speed_2MHz;
+    gpioInit(GPIOB, &gpio);
+    gpioPinRemapConfig(AFIO_MAPR_USART1_REMAP, true);
+    serialInit(mcfg.serial_baudrate);
+    delay(25);
+    serialPrint(core.mainport, "DBG ");
+    printf("%08x%08x%08x OK\n", U_ID_0, U_ID_1, U_ID_2);
+    serialPrint(core.mainport, "EOF");
+    delay(25);
+    gpioPinRemapConfig(AFIO_MAPR_USART1_REMAP, false);
+}
+#endif

src/drivers/adc_common.c

@@ -2,6 +2,7 @@
 #include <stdint.h>
 
 #include "platform.h"
+#include "system_common.h"
 
 #include "sensors_common.h" // FIXME dependency into the main code
 
@@ -9,45 +10,73 @@
 
 #include "adc_common.h"
 
-// static volatile uint16_t adc1Ch4Value = 0;
+// Driver for STM32F103CB onboard ADC
-static volatile uint16_t adcValues[2];
+// VBAT is connected to PA4 (ADC1_IN4) with 10k:1k divider
+// rev.5 hardware has PA5 (ADC1_IN5) on breakout pad on bottom of board
+// Additional channel can be stolen from RC_CH2 (PA1, ADC1_IN1) or 
+// RC_CH8 (PB1, ADC1_IN9) by using set power_adc_channel=1|9
+
+typedef struct adc_config_t {
+    uint8_t adcChannel;         // ADC1_INxx channel number
+    uint8_t dmaIndex;           // index into DMA buffer in case of sparse channels
+} adc_config_t;
+
+static adc_config_t adcConfig[ADC_CHANNEL_MAX];
+static volatile uint16_t adcValues[ADC_CHANNEL_MAX];
 
 void adcInit(drv_adc_config_t *init)
 {
 #ifndef STM32F3DISCOVERY
+    ADC_InitTypeDef adc;
+    DMA_InitTypeDef dma;
+    int numChannels = 1, i;
 
-    ADC_InitTypeDef ADC_InitStructure;
+    // configure always-present battery index (ADC4)
-    DMA_InitTypeDef DMA_InitStructure;
+    adcConfig[ADC_BATTERY].adcChannel = ADC_Channel_4;
-    bool multiChannel = init->powerAdcChannel > 0;
+    adcConfig[ADC_BATTERY].dmaIndex = numChannels - 1;
 
-    // ADC assumes all the GPIO was already placed in 'AIN' mode
+    // optional ADC5 input on rev.5 hardware
+    if (hse_value == 12000000) {
+        numChannels++;
+        adcConfig[ADC_EXTERNAL1].adcChannel = ADC_Channel_5;
+        adcConfig[ADC_EXTERNAL1].dmaIndex = numChannels - 1;
+    }
+    // another channel can be stolen from PWM for current measurement or other things
+    if (init->powerAdcChannel > 0) {
+        numChannels++;
+        adcConfig[ADC_EXTERNAL2].adcChannel = init->powerAdcChannel;
+        adcConfig[ADC_EXTERNAL2].dmaIndex = numChannels - 1;
+    }
+
+    // ADC driver assumes all the GPIO was already placed in 'AIN' mode
     DMA_DeInit(DMA1_Channel1);
-    DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR;
+    dma.DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR;
-    DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)adcValues;
+    dma.DMA_MemoryBaseAddr = (uint32_t)adcValues;
-    DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC;
+    dma.DMA_DIR = DMA_DIR_PeripheralSRC;
-    DMA_InitStructure.DMA_BufferSize = multiChannel ? 2 : 1;
+    dma.DMA_BufferSize = numChannels;
-    DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
+    dma.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
-    DMA_InitStructure.DMA_MemoryInc = multiChannel ? DMA_MemoryInc_Enable : DMA_MemoryInc_Disable;
+    dma.DMA_MemoryInc = numChannels > 1 ? DMA_MemoryInc_Enable : DMA_MemoryInc_Disable;
-    DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord;
+    dma.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord;
-    DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord;
+    dma.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord;
-    DMA_InitStructure.DMA_Mode = DMA_Mode_Circular;
+    dma.DMA_Mode = DMA_Mode_Circular;
-    DMA_InitStructure.DMA_Priority = DMA_Priority_High;
+    dma.DMA_Priority = DMA_Priority_High;
-    DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;
+    dma.DMA_M2M = DMA_M2M_Disable;
-    DMA_Init(DMA1_Channel1, &DMA_InitStructure);
+    DMA_Init(DMA1_Channel1, &dma);
-    /* Enable DMA1 channel1 */
     DMA_Cmd(DMA1_Channel1, ENABLE);
 
-    ADC_InitStructure.ADC_Mode = ADC_Mode_Independent;
+    adc.ADC_Mode = ADC_Mode_Independent;
-    ADC_InitStructure.ADC_ScanConvMode = multiChannel ? ENABLE : DISABLE;
+    adc.ADC_ScanConvMode = numChannels > 1 ? ENABLE : DISABLE;
-    ADC_InitStructure.ADC_ContinuousConvMode = ENABLE;
+    adc.ADC_ContinuousConvMode = ENABLE;
-    ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None;
+    adc.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None;
-    ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;
+    adc.ADC_DataAlign = ADC_DataAlign_Right;
-    ADC_InitStructure.ADC_NbrOfChannel = multiChannel ? 2 : 1;
+    adc.ADC_NbrOfChannel = numChannels;
-    ADC_Init(ADC1, &ADC_InitStructure);
+    ADC_Init(ADC1, &adc);
 
+    // fixed ADC4
     ADC_RegularChannelConfig(ADC1, ADC_Channel_4, 1, ADC_SampleTime_28Cycles5);
-    if (multiChannel)
+    // configure any additional ADC channels (2 + n)
-        ADC_RegularChannelConfig(ADC1, init->powerAdcChannel, 2, ADC_SampleTime_28Cycles5);
+    for (i = 1; i < numChannels; i++)
+        ADC_RegularChannelConfig(ADC1, adcConfig[i].adcChannel, i + 1, ADC_SampleTime_28Cycles5);
     ADC_DMACmd(ADC1, ENABLE);
 
     ADC_Cmd(ADC1, ENABLE);
@@ -65,5 +94,5 @@ void adcInit(drv_adc_config_t *init)
 
 uint16_t adcGetChannel(uint8_t channel)
 {
-    return adcValues[channel];
+    return adcValues[adcConfig[channel].dmaIndex];
 }

src/drivers/adc_common.h

@@ -1,12 +1,15 @@
 #pragma once
 
-#define ADC_BATTERY     0
+typedef enum {
-#define ADC_CURRENT     1
+    ADC_BATTERY = 0,
+    ADC_EXTERNAL1 = 1,
+    ADC_EXTERNAL2 = 2,
+    ADC_CHANNEL_MAX = 3
+} AdcChannel;
 
 typedef struct drv_adc_config_t {
     uint8_t powerAdcChannel;     // which channel used for current monitor, allowed PA1, PB1 (ADC_Channel_1, ADC_Channel_9)
 } drv_adc_config_t;
 
-
 void adcInit(drv_adc_config_t *init);
 uint16_t adcGetChannel(uint8_t channel);

src/drivers/pwm_common.c

@@ -261,6 +261,18 @@ static pwmPortData_t *pwmInConfig(uint8_t port, timerCCCallbackPtr callback, uin
     return p;
 }
 
+static void failsafeCheck(uint8_t channel, uint16_t pulse)
+{
+    static uint8_t goodPulses;
+    
+    if (channel < 4 && pulse > failsafeThreshold)
+        goodPulses |= (1 << channel);       // if signal is valid - mark channel as OK
+    if (goodPulses == 0x0F) {               // If first four chanells have good pulses, clear FailSafe counter
+        goodPulses = 0;
+        failsafe->vTable->validDataReceived();
+    }
+}
+
 static void ppmCallback(uint8_t port, captureCompare_t capture)
 {
     captureCompare_t diff;
@@ -268,7 +280,6 @@ static void ppmCallback(uint8_t port, captureCompare_t capture)
     static captureCompare_t last = 0;
 
     static uint8_t chan = 0;
-    static uint8_t GoodPulses;
 
     last = now;
     now = capture;
@@ -277,17 +288,11 @@ static void ppmCallback(uint8_t port, captureCompare_t capture)
     if (diff > 2700) { // Per http://www.rcgroups.com/forums/showpost.php?p=21996147&postcount=3960 "So, if you use 2.5ms or higher as being the reset for the PPM stream start, you will be fine. I use 2.7ms just to be safe."
         chan = 0;
     } else {
-        if (diff > 750 && diff < 2250 && chan < MAX_PPM_AND_PWM_INPUTS) {   // 750 to 2250 ms is our 'valid' channel range
+        if (diff > PULSE_MIN && diff < PULSE_MAX && chan < MAX_PPM_AND_PWM_INPUTS) {   // 750 to 2250 ms is our 'valid' channel range
             captures[chan] = diff;
-            if (chan < 4 && diff > failsafeThreshold)
+            failsafeCheck(chan, diff);
-                GoodPulses |= (1 << chan);      // if signal is valid - mark channel as OK
-            if (GoodPulses == 0x0F) {           // If first four channels have good pulses, clear FailSafe counter
-                GoodPulses = 0;
-                failsafe->vTable->validDataReceived();
-            }
         }
         chan++;
-        failsafe->vTable->reset();
     }
 }
 
@@ -301,12 +306,13 @@ static void pwmCallback(uint8_t port, captureCompare_t capture)
         pwmPorts[port].fall = capture;
         // compute capture
         pwmPorts[port].capture = pwmPorts[port].fall - pwmPorts[port].rise;
+        if (pwmPorts[port].capture > PULSE_MIN && pwmPorts[port].capture < PULSE_MAX) { // valid pulse width
             captures[pwmPorts[port].channel] = pwmPorts[port].capture;
+            failsafeCheck(pwmPorts[port].channel, pwmPorts[port].capture);
+        }
         // switch state
         pwmPorts[port].state = 0;
         pwmICConfig(timerHardware[port].tim, timerHardware[port].channel, TIM_ICPolarity_Rising);
-        // reset failsafe
-        failsafe->vTable->reset();
     }
 }
 

src/drivers/pwm_common.h

@@ -5,6 +5,12 @@
 #define MAX_PPM_AND_PWM_INPUTS  8
 
 #define PULSE_1MS   (1000)      // 1ms pulse width
+#define PULSE_MIN   (750)       // minimum PWM pulse width which is considered valid
+#define PULSE_MAX   (2250)      // maximum PWM pulse width which is considered valid
+
+#define MAX_MOTORS  12
+#define MAX_SERVOS  8
+#define MAX_INPUTS  8
 
 typedef struct drv_pwm_config_t {
     bool enableInput;

src/drivers/pwm_fy90q.c

@@ -93,7 +93,7 @@ static void ppmIRQHandler(TIM_TypeDef *tim)
     if (diff > 4000) {
         chan = 0;
     } else {
-        if (diff > 750 && diff < 2250 && chan < 8) {   // 750 to 2250 ms is our 'valid' channel range
+        if (diff > PULSE_MIN && diff < PULSE_MAX && chan < 8) {   // 750 to 2250 ms is our 'valid' channel range
             Inputs[chan].capture = diff;
             if (chan < 4 && diff > failsafeThreshold)
                 GoodPulses |= (1 << chan);      // if signal is valid - mark channel as OK

src/main.c

@@ -60,26 +60,51 @@ void pwmInit(drv_pwm_config_t *init, failsafe_t *initialFailsafe);
 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 sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, int16_t magDeclinationFromConfig);
+bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, int16_t magDeclinationFromConfig);
 void imuInit(void);
 
 void loop(void);
 
+// FIXME bad naming - this appears to be for some new board that hasn't been made available yet.
+#ifdef PROD_DEBUG
+void productionDebug(void)
+{
+    gpio_config_t gpio;
+
+    // remap PB6 to USART1_TX
+    gpio.pin = Pin_6;
+    gpio.mode = Mode_AF_PP;
+    gpio.speed = Speed_2MHz;
+    gpioInit(GPIOB, &gpio);
+    gpioPinRemapConfig(AFIO_MAPR_USART1_REMAP, true);
+    serialInit(mcfg.serial_baudrate);
+    delay(25);
+    serialPrint(core.mainport, "DBG ");
+    printf("%08x%08x%08x OK\n", U_ID_0, U_ID_1, U_ID_2);
+    serialPrint(core.mainport, "EOF");
+    delay(25);
+    gpioPinRemapConfig(AFIO_MAPR_USART1_REMAP, false);
+}
+#endif
+
 int main(void)
 {
     uint8_t i;
     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
-    systemInit(masterConfig.emf_avoidance);
-    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;
@@ -89,12 +114,45 @@ int main(void)
     }
 
     adcInit(&adc_params);
+
+    // Check battery type/voltage
+    if (feature(FEATURE_VBAT))
+        batteryInit(&masterConfig.batteryConfig);
+
     initBoardAlignment(&masterConfig.boardAlignment);
 
     // We have these sensors; SENSORS_SET defined in board.h depending on hardware platform
     sensorsSet(SENSORS_SET);
+    // drop out any sensors that don't seem to work, init all the others. halt if gyro is dead.
+    sensorsOK = sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, currentProfile.mag_declination);
 
+    // production debug output
+#ifdef PROD_DEBUG
+    productionDebug();
+#endif
+
+    // if gyro was not detected due to whatever reason, we give up now.
+    if (!sensorsOK)
+        failureMode(3);
+
+    LED1_ON;
+    LED0_OFF;
+    for (i = 0; i < 10; i++) {
+        LED1_TOGGLE;
+        LED0_TOGGLE;
+        delay(25);
+        BEEP_ON;
+        delay(25);
+        BEEP_OFF;
+    }
+    LED0_OFF;
+    LED1_OFF;
+
+    imuInit(); // Mag is initialized inside imuInit
     mixerInit(masterConfig.mixerConfiguration, masterConfig.customMixer);
+
+    serialInit(&masterConfig.serialConfig);
+
     // when using airplane/wing mixer, servo/motor outputs are remapped
     if (masterConfig.mixerConfiguration == MULTITYPE_AIRPLANE || masterConfig.mixerConfiguration == MULTITYPE_FLYING_WING)
         pwm_params.airplane = true;
@@ -154,32 +212,8 @@ int main(void)
     }
 #endif
 
-    LED1_ON;
-    LED0_OFF;
-    for (i = 0; i < 10; i++) {
-        LED1_TOGGLE;
-        LED0_TOGGLE;
-        delay(25);
-        BEEP_ON;
-        delay(25);
-        BEEP_OFF;
-    }
-    LED0_OFF;
-    LED1_OFF;
-
-    // drop out any sensors that don't seem to work, init all the others. halt if gyro is dead.
-    sensorsAutodetect(&masterConfig.sensorAlignmentConfig, masterConfig.gyro_lpf, masterConfig.acc_hardware, currentProfile.mag_declination);
-    imuInit(); // Mag is initialized inside imuInit
-
-    // Check battery type/voltage
-    if (feature(FEATURE_VBAT))
-        batteryInit(&masterConfig.batteryConfig);
-
-    serialInit(&masterConfig.serialConfig);
-
 #ifndef FY90Q
     if (canSoftwareSerialBeUsed()) {
-
 #if defined(USE_SOFTSERIAL_FOR_MAIN_PORT) || (0)
         masterConfig.serialConfig.softserial_baudrate = 19200;
 #endif

src/mw.c

@@ -206,7 +206,6 @@ void annexCode(void)
     }
 }
 
-
 static void mwArm(void)
 {
     if (isGyroCalibrationComplete() && f.ACC_CALIBRATED) {

src/rx_pwm.c

@@ -17,13 +17,7 @@
 
 static uint16_t pwmReadRawRC(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
 {
-    uint16_t data;
+    return pwmRead(rxConfig->rcmap[chan]);
-
-    data = pwmRead(rxConfig->rcmap[chan]);
-    if (data < 750 || data > 2250)
-        data = rxConfig->midrc;
-
-    return data;
 }
 
 void pwmRxInit(rxRuntimeConfig_t *rxRuntimeConfig, failsafe_t *initialFailsafe, rcReadRawDataPtr *callback)

src/sensors_initialisation.c

@@ -80,14 +80,15 @@ bool fakeAccDetect(acc_t *acc)
 
 #ifdef FY90Q
 // FY90Q analog gyro/acc
-void sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, int16_t magDeclinationFromConfig)
+bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, int16_t magDeclinationFromConfig)
 {
     memset(&acc, sizeof(acc), 0);
     memset(&gyro, sizeof(gyro), 0);
     adcSensorInit(&acc, &gyro);
+    return true;
 }
 #else
-void sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, int16_t magDeclinationFromConfig)
+bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, int16_t magDeclinationFromConfig)
 {
     int16_t deg, min;
     drv_adxl345_config_t acc_params;
@@ -115,8 +116,7 @@ void sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t
 #endif
 #endif
     } else {
-        // if this fails, we get a beep + blink pattern. we're doomed, no gyro or i2c error.
+        return false;
-        failureMode(3);
     }
 
     // Accelerometer. Fuck it. Let user break shit.
@@ -242,6 +242,8 @@ retry:
     } else {
         magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used.
     }
+
+    return true;
 }
 #endif