rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
timecop 2012-06-07 15:29:37 +00:00
parent 19ca85963b
commit 572d5827cc
7 changed files with 2478 additions and 2794 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
baseflight.uvproj
View File

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

5162
obj/baseflight.hex
View File

File diff suppressed because it is too large Load Diff

17
src/drv_bmp085.c
View File

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

49
src/drv_system.c
View File

@ -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;
__disable_irq();
cycle = *DWT_CYCCNT;
oldCycle = sysTickCycleCounter;
timeMs = sysTickUptime;
__enable_irq();
return (timeMs * 1000) + (cycle - oldCycle) / usTicks;
register uint32_t ms, cycle_cnt;
do {
ms = sysTickUptime;
cycle_cnt = SysTick->VAL;
} while (ms != sysTickUptime);
return (ms * 1000) + (72000 - cycle_cnt) / 72;
}
// 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_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, 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_APB1PeriphClockCmd(RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4 | RCC_APB1Periph_I2C2, 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_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)
{

12
src/imu.c
View File

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

18
src/mw.c
View File

@ -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
while (micros() < trollTime + 2000);
LED0_TOGGLE;
#ifdef TIMINGDEBUG
while (micros() < trollTime + 1750);
// LED0_TOGGLE;
{
if (cycleTime < cn)
cn = cycleTime;
if (cycleTime > cx)
cx = cycleTime;
debug1 = cn;
debug2 = cx;
}
#endif
}

8
src/sensors.c
View File

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