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

slight improvement to motion threshold calculation 

git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@234 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop@gmail.com 2012-10-27 07:34:45 +00:00
parent 23acf529d7
commit 12dfa8915d
3 changed files with 3205 additions and 3160 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
baseflight.uvproj
View File

@ -12,14 +12,14 @@
<ToolsetName>ARM-ADS</ToolsetName> <ToolsetName>ARM-ADS</ToolsetName>
<TargetOption> <TargetOption>
<TargetCommonOption> <TargetCommonOption>
<Device>STM32F103C8</Device> <Device>STM32F103CB</Device>
<Vendor>STMicroelectronics</Vendor> <Vendor>STMicroelectronics</Vendor>
<Cpu>IRAM(0x20000000-0x20004FFF) IROM(0x8000000-0x800FFFF) CLOCK(8000000) CPUTYPE("Cortex-M3")</Cpu> <Cpu>IRAM(0x20000000-0x20004FFF) IROM(0x8000000-0x801FFFF) CLOCK(8000000) CPUTYPE("Cortex-M3")</Cpu>
<FlashUtilSpec></FlashUtilSpec> <FlashUtilSpec></FlashUtilSpec>
<StartupFile>"STARTUP\ST\STM32F10x\startup_stm32f10x_md.s" ("STM32 Medium Density Line Startup Code")</StartupFile> <StartupFile>"STARTUP\ST\STM32F10x\startup_stm32f10x_md.s" ("STM32 Medium Density Line Startup Code")</StartupFile>
<FlashDriverDll>UL2CM3(-O14 -S0 -C0 -N00("ARM Cortex-M3") -D00(1BA00477) -L00(4) -FO7 -FD20000000 -FC800 -FN1 -FF0STM32F10x_128 -FS08000000 -FL010000)</FlashDriverDll> <FlashDriverDll>UL2CM3(-O14 -S0 -C0 -N00("ARM Cortex-M3") -D00(1BA00477) -L00(4) -FO7 -FD20000000 -FC800 -FN1 -FF0STM32F10x_128 -FS08000000 -FL020000)</FlashDriverDll>
<DeviceId>4235</DeviceId> <DeviceId>4401</DeviceId>
<RegisterFile>stm32f10x_md.h</RegisterFile> <RegisterFile>stm32f10x.h</RegisterFile>
<MemoryEnv></MemoryEnv> <MemoryEnv></MemoryEnv>
<Cmp></Cmp> <Cmp></Cmp>
<Asm></Asm> <Asm></Asm>
@ -29,7 +29,7 @@
<SLE66CMisc></SLE66CMisc> <SLE66CMisc></SLE66CMisc>
<SLE66AMisc></SLE66AMisc> <SLE66AMisc></SLE66AMisc>
<SLE66LinkerMisc></SLE66LinkerMisc> <SLE66LinkerMisc></SLE66LinkerMisc>
<SFDFile>SFD\ST\STM32F10xx\STM32F10xxB.sfr</SFDFile> <SFDFile>SFD\ST\STM32F1xx\STM32F103xx.sfr</SFDFile>
<UseEnv>0</UseEnv> <UseEnv>0</UseEnv>
<BinPath></BinPath> <BinPath></BinPath>
<IncludePath></IncludePath> <IncludePath></IncludePath>
@ -102,11 +102,11 @@
<SimDllName>SARMCM3.DLL</SimDllName> <SimDllName>SARMCM3.DLL</SimDllName>
<SimDllArguments></SimDllArguments> <SimDllArguments></SimDllArguments>
<SimDlgDll>DARMSTM.DLL</SimDlgDll> <SimDlgDll>DARMSTM.DLL</SimDlgDll>
<SimDlgDllArguments>-pSTM32F103C8</SimDlgDllArguments> <SimDlgDllArguments>-pSTM32F103CB</SimDlgDllArguments>
<TargetDllName>SARMCM3.DLL</TargetDllName> <TargetDllName>SARMCM3.DLL</TargetDllName>
<TargetDllArguments></TargetDllArguments> <TargetDllArguments></TargetDllArguments>
<TargetDlgDll>TARMSTM.DLL</TargetDlgDll> <TargetDlgDll>TARMSTM.DLL</TargetDlgDll>
<TargetDlgDllArguments>-pSTM32F103C8</TargetDlgDllArguments> <TargetDlgDllArguments>-pSTM32F103CB</TargetDlgDllArguments>
</DllOption> </DllOption>
<DebugOption> <DebugOption>
<OPTHX> <OPTHX>
@ -136,6 +136,7 @@
<RestoreMemoryDisplay>1</RestoreMemoryDisplay> <RestoreMemoryDisplay>1</RestoreMemoryDisplay>
<RestoreFunctions>0</RestoreFunctions> <RestoreFunctions>0</RestoreFunctions>
<RestoreToolbox>1</RestoreToolbox> <RestoreToolbox>1</RestoreToolbox>
<RestoreTracepoints>0</RestoreTracepoints>
</Target> </Target>
<RunDebugAfterBuild>0</RunDebugAfterBuild> <RunDebugAfterBuild>0</RunDebugAfterBuild>
<TargetSelection>1</TargetSelection> <TargetSelection>1</TargetSelection>
@ -165,7 +166,7 @@
<DriverSelection>4096</DriverSelection> <DriverSelection>4096</DriverSelection>
</Flash1> </Flash1>
<Flash2>BIN\UL2CM3.DLL</Flash2> <Flash2>BIN\UL2CM3.DLL</Flash2>
<Flash3>"" ()</Flash3> <Flash3></Flash3>
<Flash4></Flash4> <Flash4></Flash4>
</Utilities> </Utilities>
<TargetArmAds> <TargetArmAds>
@ -271,7 +272,7 @@
<IROM> <IROM>
<Type>1</Type> <Type>1</Type>
<StartAddress>0x8000000</StartAddress> <StartAddress>0x8000000</StartAddress>
<Size>0x10000</Size> <Size>0x20000</Size>
</IROM> </IROM>
<XRAM> <XRAM>
<Type>0</Type> <Type>0</Type>
@ -296,7 +297,7 @@
<OCR_RVCT4> <OCR_RVCT4>
<Type>1</Type> <Type>1</Type>
<StartAddress>0x8000000</StartAddress> <StartAddress>0x8000000</StartAddress>
<Size>0x10000</Size> <Size>0x20000</Size>
</OCR_RVCT4> </OCR_RVCT4>
<OCR_RVCT5> <OCR_RVCT5>
<Type>1</Type> <Type>1</Type>
@ -823,7 +824,7 @@
<StartupFile>"STARTUP\ST\STM32F10x\startup_stm32f10x_md.s" ("STM32 Medium Density Line Startup Code")</StartupFile> <StartupFile>"STARTUP\ST\STM32F10x\startup_stm32f10x_md.s" ("STM32 Medium Density Line Startup Code")</StartupFile>
<FlashDriverDll>UL2CM3(-O14 -S0 -C0 -N00("ARM Cortex-M3") -D00(1BA00477) -L00(4) -FO7 -FD20000000 -FC800 -FN1 -FF0STM32F10x_128 -FS08000000 -FL010000)</FlashDriverDll> <FlashDriverDll>UL2CM3(-O14 -S0 -C0 -N00("ARM Cortex-M3") -D00(1BA00477) -L00(4) -FO7 -FD20000000 -FC800 -FN1 -FF0STM32F10x_128 -FS08000000 -FL010000)</FlashDriverDll>
<DeviceId>4235</DeviceId> <DeviceId>4235</DeviceId>
<RegisterFile>stm32f10x_md.h</RegisterFile> <RegisterFile>stm32f10x.h</RegisterFile>
<MemoryEnv></MemoryEnv> <MemoryEnv></MemoryEnv>
<Cmp></Cmp> <Cmp></Cmp>
<Asm></Asm> <Asm></Asm>
@ -833,7 +834,7 @@
<SLE66CMisc></SLE66CMisc> <SLE66CMisc></SLE66CMisc>
<SLE66AMisc></SLE66AMisc> <SLE66AMisc></SLE66AMisc>
<SLE66LinkerMisc></SLE66LinkerMisc> <SLE66LinkerMisc></SLE66LinkerMisc>
<SFDFile>SFD\ST\STM32F10xx\STM32F10xxB.sfr</SFDFile> <SFDFile>SFD\ST\STM32F1xx\STM32F103xx.sfrSVD=SFD\ST\STM32F1xx\STM32F103xx.sfr</SFDFile>
<UseEnv>0</UseEnv> <UseEnv>0</UseEnv>
<BinPath></BinPath> <BinPath></BinPath>
<IncludePath></IncludePath> <IncludePath></IncludePath>
@ -940,6 +941,7 @@
<RestoreMemoryDisplay>1</RestoreMemoryDisplay> <RestoreMemoryDisplay>1</RestoreMemoryDisplay>
<RestoreFunctions>0</RestoreFunctions> <RestoreFunctions>0</RestoreFunctions>
<RestoreToolbox>1</RestoreToolbox> <RestoreToolbox>1</RestoreToolbox>
<RestoreTracepoints>0</RestoreTracepoints>
</Target> </Target>
<RunDebugAfterBuild>0</RunDebugAfterBuild> <RunDebugAfterBuild>0</RunDebugAfterBuild>
<TargetSelection>1</TargetSelection> <TargetSelection>1</TargetSelection>
@ -969,7 +971,7 @@
<DriverSelection>4096</DriverSelection> <DriverSelection>4096</DriverSelection>
</Flash1> </Flash1>
<Flash2>BIN\UL2CM3.DLL</Flash2> <Flash2>BIN\UL2CM3.DLL</Flash2>
<Flash3>"" ()</Flash3> <Flash3></Flash3>
<Flash4></Flash4> <Flash4></Flash4>
</Utilities> </Utilities>
<TargetArmAds> <TargetArmAds>
@ -1688,6 +1690,7 @@
<RestoreMemoryDisplay>1</RestoreMemoryDisplay> <RestoreMemoryDisplay>1</RestoreMemoryDisplay>
<RestoreFunctions>0</RestoreFunctions> <RestoreFunctions>0</RestoreFunctions>
<RestoreToolbox>1</RestoreToolbox> <RestoreToolbox>1</RestoreToolbox>
<RestoreTracepoints>0</RestoreTracepoints>
</Target> </Target>
<RunDebugAfterBuild>0</RunDebugAfterBuild> <RunDebugAfterBuild>0</RunDebugAfterBuild>
<TargetSelection>1</TargetSelection> <TargetSelection>1</TargetSelection>

6276
obj/baseflight.hex
View File

File diff suppressed because it is too large Load Diff

58
src/sensors.c
View File

@ -306,35 +306,69 @@ void Baro_update(void)
} }
#endif /* BARO */ #endif /* BARO */
typedef struct stdev_t
{
float m_oldM, m_newM, m_oldS, m_newS;
int m_n;
} stdev_t;
static void devClear(stdev_t *dev)
{
dev->m_n = 0;
}
static void devPush(stdev_t *dev, float x)
{
dev->m_n++;
if (dev->m_n == 1) {
dev->m_oldM = dev->m_newM = x;
dev->m_oldS = 0.0f;
} else {
dev->m_newM = dev->m_oldM + (x - dev->m_oldM) / dev->m_n;
dev->m_newS = dev->m_oldS + (x - dev->m_oldM) * (x - dev->m_newM);
dev->m_oldM = dev->m_newM;
dev->m_oldS = dev->m_newS;
}
}
static float devVariance(stdev_t *dev)
{
return ((dev->m_n > 1) ? dev->m_newS / (dev->m_n - 1) : 0.0f);
}
static float devStandardDeviation(stdev_t *dev)
{
return sqrtf(devVariance(dev));
}
static void GYRO_Common(void) static void GYRO_Common(void)
{ {
int axis; int axis;
static int16_t previousGyroADC[3] = { 0, 0, 0 }; static int16_t previousGyroADC[3] = { 0, 0, 0 };
static int32_t g[3]; static int32_t g[3];
static int16_t gyroMin[3]; static stdev_t var[3];
static int16_t gyroMax[3];
if (calibratingG > 0) { if (calibratingG > 0) {
for (axis = 0; axis < 3; axis++) { for (axis = 0; axis < 3; axis++) {
// Reset g[axis] at start of calibration // Reset g[axis] at start of calibration
if (calibratingG == 1000) if (calibratingG == 1000) {
g[axis] = 0; g[axis] = 0;
devClear(&var[axis]);
}
// Sum up 1000 readings // Sum up 1000 readings
g[axis] += gyroADC[axis]; g[axis] += gyroADC[axis];
if (gyroMin[axis] > gyroADC[axis]) devPush(&var[axis], gyroADC[axis]);
gyroMin[axis] = gyroADC[axis];
if (gyroMax[axis] < gyroADC[axis])
gyroMax[axis] = gyroADC[axis];
// Clear global variables for next reading // Clear global variables for next reading
gyroADC[axis] = 0; gyroADC[axis] = 0;
gyroZero[axis] = 0; gyroZero[axis] = 0;
if (calibratingG == 1) { if (calibratingG == 1) {
int16_t gyroDiff = gyroMax[axis] - gyroMin[axis]; float dev = devStandardDeviation(&var[axis]);
// check variance and startover if idiot was moving the model // check deviation and startover if idiot was moving the model
if (cfg.moron_threshold && gyroDiff > cfg.moron_threshold) { if (cfg.moron_threshold && dev > cfg.moron_threshold) {
calibratingG = 1000; calibratingG = 1000;
gyroMin[0] = gyroMin[1] = gyroMin[2] = 0; devClear(&var[0]);
gyroMax[0] = gyroMax[1] = gyroMax[2] = 0; devClear(&var[1]);
devClear(&var[2]);
g[0] = g[1] = g[2] = 0; g[0] = g[1] = g[2] = 0;
continue; continue;
} }