took out cycle counter stuff for timing, now using systick + counter strictly. ... seems improved loop precision a bit.

put gyro interleave under define. this needs to be cleaned sometime.
took out "gyro glitch" stuff that was leftover from  nintendo days.

git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@162 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61

baseflight.uvproj

@@ -958,7 +958,7 @@
             <AdsLsun>1</AdsLsun>
             <AdsLven>1</AdsLven>
             <AdsLsxf>1</AdsLsxf>
-            <RvctClst>0</RvctClst>
+            <RvctClst>1</RvctClst>
             <GenPPlst>0</GenPPlst>
             <AdsCpuType>"Cortex-M3"</AdsCpuType>
             <RvctDeviceName></RvctDeviceName>
@@ -979,7 +979,7 @@
             <RoSelD>3</RoSelD>
             <RwSelD>3</RwSelD>
             <CodeSel>0</CodeSel>
-            <OptFeed>0</OptFeed>
+            <OptFeed>1</OptFeed>
             <NoZi1>0</NoZi1>
             <NoZi2>0</NoZi2>
             <NoZi3>0</NoZi3>
@@ -1105,7 +1105,7 @@
             <PlainCh>0</PlainCh>
             <Ropi>0</Ropi>
             <Rwpi>0</Rwpi>
-            <wLevel>0</wLevel>
+            <wLevel>2</wLevel>
             <uThumb>0</uThumb>
             <VariousControls>
               <MiscControls></MiscControls>

src/drv_bmp085.c

@@ -2,6 +2,7 @@
 
 // BMP085, Standard address 0x77
 static bool convDone = false;
+static uint16_t convOverrun = 0;
 
 #define BARO_OFF                 digitalLo(BARO_GPIO, BARO_PIN);
 #define BARO_ON                  digitalHi(BARO_GPIO, BARO_PIN);
@@ -144,7 +145,8 @@ int16_t bmp085_read_temperature(void)
 {
     convDone = false;
     bmp085_start_ut();
-    while (!convDone);
+    if (!convDone)
+        convOverrun++;
     return bmp085_get_temperature(bmp085_get_ut());
 }
 
@@ -152,7 +154,8 @@ int32_t bmp085_read_pressure(void)
 {
     convDone = false;
     bmp085_start_up();
-    while (!convDone);
+    if (!convDone)
+        convOverrun++;
     return bmp085_get_pressure(bmp085_get_up());
 }
 
@@ -236,10 +239,13 @@ uint16_t bmp085_get_ut(void)
     uint16_t timeout = 10000;
 
     // wait in case of cockup
+    if (!convDone)
+        convOverrun++;
+#if 0
     while (!convDone && timeout-- > 0) {
         __NOP();
     }
-
+#endif
     i2cRead(p_bmp085->dev_addr, BMP085_ADC_OUT_MSB_REG, 2, data);
     ut = (data[0] << 8) | data[1];
     return ut;
@@ -265,10 +271,13 @@ uint32_t bmp085_get_up(void)
     uint16_t timeout = 10000;
     
     // wait in case of cockup
+    if (!convDone)
+        convOverrun++;
+#if 0
     while (!convDone && timeout-- > 0) {
         __NOP();
     }
-
+#endif
     i2cRead(p_bmp085->dev_addr, BMP085_ADC_OUT_MSB_REG, 3, data);
     up = (((uint32_t) data[0] << 16) | ((uint32_t) data[1] << 8) | (uint32_t) data[2]) >> (8 - p_bmp085->oversampling_setting);
 

src/drv_system.c

@@ -1,10 +1,5 @@
 #include "board.h"
 
-// Cycle counter stuff - these should be defined by CMSIS, but they aren't
-#define DWT_CTRL    (*(volatile uint32_t *)0xE0001000)
-#define DWT_CYCCNT  ((volatile uint32_t *)0xE0001004)
-#define CYCCNTENA   (1 << 0)
-
 typedef struct gpio_config_t
 {
     GPIO_TypeDef *gpio;
@@ -16,38 +11,29 @@ typedef struct gpio_config_t
 static volatile 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;
-static volatile uint32_t sysTickCycleCounter = 0;
 
 static void cycleCounterInit(void)
 {
     RCC_ClocksTypeDef clocks;
     RCC_GetClocksFreq(&clocks);
     usTicks = clocks.SYSCLK_Frequency / 1000000;
-
-    // enable DWT access
-    CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
-    // enable the CPU cycle counter
-    DWT_CTRL |= CYCCNTENA;
 }
 
-
 // SysTick
 void SysTick_Handler(void)
 {
-    sysTickCycleCounter = *DWT_CYCCNT;
     sysTickUptime++;
 }
 
 // Return system uptime in microseconds (rollover in 70minutes)
 uint32_t micros(void)
 {
-    register uint32_t oldCycle, cycle, timeMs;
+    register uint32_t ms, cycle_cnt;
-    __disable_irq();
+    do {
-    cycle = *DWT_CYCCNT;
+        ms = sysTickUptime;
-    oldCycle = sysTickCycleCounter;
+        cycle_cnt = SysTick->VAL;
-    timeMs = sysTickUptime;
+    } while (ms != sysTickUptime);
-    __enable_irq();
+    return (ms * 1000) + (72000 - cycle_cnt) / 72;
-    return (timeMs * 1000) + (cycle - oldCycle) / usTicks;
 }
 
 // Return system uptime in milliseconds (rollover in 49 days)
@@ -71,16 +57,9 @@ void systemInit(void)
     uint8_t gpio_count = sizeof(gpio_cfg) / sizeof(gpio_cfg[0]);
 
     // Turn on clocks for stuff we use
-    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB | RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO, ENABLE);
+    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4 | RCC_APB1Periph_I2C2, ENABLE);
-    RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
+    RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO | RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB | RCC_APB2Periph_GPIOC | RCC_APB2Periph_TIM1 | RCC_APB2Periph_ADC1 | RCC_APB2Periph_USART1, ENABLE);
-    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
-    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
-    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
-    RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE);
-    RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE);
-    RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE);
     RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
-    RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA2, ENABLE);
     RCC_ClearFlag();
 
     // Make all GPIO in by default to save power and reduce noise
@@ -121,13 +100,20 @@ void systemInit(void)
     delay(100);
 }
 
+#if 1
+void delayMicroseconds(uint32_t us)
+{
+    uint32_t now = micros();
+    while (micros() - now < us);
+}
+#else
 void delayMicroseconds(uint32_t us)
 {
     uint32_t elapsed = 0;
-    uint32_t lastCount = *DWT_CYCCNT;
+    uint32_t lastCount = SysTick->VAL;
 
     for (;;) {
-        register uint32_t current_count = *DWT_CYCCNT;
+        register uint32_t current_count = SysTick->VAL;
         uint32_t elapsed_us;
 
         // measure the time elapsed since the last time we checked
@@ -146,6 +132,7 @@ void delayMicroseconds(uint32_t us)
         elapsed %= usTicks;
     }
 }
+#endif
 
 void delay(uint32_t ms)
 {

src/imu.c

@@ -32,6 +32,7 @@ void imuInit(void)
 #endif
 }
 
+
 void computeIMU(void)
 {
     uint8_t axis;
@@ -41,6 +42,8 @@ void computeIMU(void)
     static uint32_t timeInterleave = 0;
     static int16_t gyroYawSmooth = 0;
 
+#define GYRO_INTERLEAVE
+
     if (sensors(SENSOR_ACC)) {
         ACC_getADC();
         getEstimatedAttitude();
@@ -49,9 +52,16 @@ void computeIMU(void)
     Gyro_getADC();
 
     for (axis = 0; axis < 3; axis++)
+#ifdef GYRO_INTERLEAVE
         gyroADCp[axis] = gyroADC[axis];
+#else
+        gyroData[axis] = gyroADC[axis];
+        if (!sensors(SENSOR_ACC))
+            accADC[axis] = 0;
+#endif
     timeInterleave = micros();
     annexCode();
+#ifdef GYRO_INTERLEAVE
     if ((micros() - timeInterleave) > 650) {
         annex650_overrun_count++;
     } else {
@@ -59,7 +69,6 @@ void computeIMU(void)
     }
 
     Gyro_getADC();
-
     for (axis = 0; axis < 3; axis++) {
         gyroADCinter[axis] = gyroADC[axis] + gyroADCp[axis];
         // empirical, we take a weighted value of the current and the previous values
@@ -68,6 +77,7 @@ void computeIMU(void)
         if (!sensors(SENSOR_ACC))
             accADC[axis] = 0;
     }
+#endif
 
     if (feature(FEATURE_GYRO_SMOOTHING)) {
         static uint8_t Smoothing[3] = { 0, 0, 0 };

src/mw.c

@@ -144,7 +144,6 @@ void annexCode(void)
     tmp = (uint32_t) (tmp - cfg.mincheck) * 1000 / (2000 - cfg.mincheck);       // [MINCHECK;2000] -> [0;1000]
     tmp2 = tmp / 100;
     rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100;    // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
-    // rcCommand[THROTTLE] = cfg.minthrottle + (int32_t)(cfg.maxthrottle - cfg.minthrottle) * (rcData[THROTTLE] - cfg.mincheck) / (2000 - cfg.mincheck);
 
     if (headFreeMode) {
         float radDiff = (heading - headFreeModeHold) * M_PI / 180.0f;
@@ -247,7 +246,9 @@ void computeRC(void)
     }
 }
 
-#if 0
+// #define TIMINGDEBUG
+
+#ifdef TIMINGDEBUG
 uint32_t trollTime = 0;
 uint16_t cn = 0xffff, cx = 0x0;
 #endif
@@ -266,6 +267,10 @@ void loop(void)
     static uint32_t rcTime = 0;
     static int16_t initialThrottleHold;
 
+#ifdef TIMINGDEBUG
+    trollTime = micros();
+#endif
+
     // this will return false if spektrum is disabled. shrug.
     if (spektrumFrameComplete())
         computeRC();
@@ -600,14 +605,17 @@ void loop(void)
     writeServos();
     writeMotors();
 
-#if 0
+#ifdef TIMINGDEBUG
-    while (micros() < trollTime + 2000);
+    while (micros() < trollTime + 1750);
-    LED0_TOGGLE;
+    // LED0_TOGGLE;
     {
         if (cycleTime < cn)
             cn = cycleTime;
         if (cycleTime > cx)
             cx = cycleTime;
+            
+        debug1 = cn;
+        debug2 = cx;
     }
 #endif
 }

src/sensors.c

@@ -272,7 +272,6 @@ void Baro_update(void)
 
 static void GYRO_Common(void)
 {
-    static int16_t previousGyroADC[3] = { 0, 0, 0 };
     static int32_t g[3];
     uint8_t axis;
 
@@ -293,13 +292,6 @@ static void GYRO_Common(void)
         }
         calibratingG--;
     }
-
-    for (axis = 0; axis < 3; axis++) {
-        gyroADC[axis] -= gyroZero[axis];
-        //anti gyro glitch, limit the variation between two consecutive readings
-        gyroADC[axis] = constrain(gyroADC[axis], previousGyroADC[axis] - 800, previousGyroADC[axis] + 800);
-        previousGyroADC[axis] = gyroADC[axis];
-    }
 }
 
 void Gyro_getADC(void)