added inflight acc cal as a feature (NOT TESTED NOT GUARANTEED TO WORK ETC)
changed gyro/acc to allow different drivers without recompile, this allowed adding mpu6050 support w/autodetect moved sensor orientation code into driver - each driver provides its own added support for mpu6050 - desolder mpu3050 and adxl345, replace with mpu6050 for instant ??? merged multiwii 2.0pre3 code changes (none that mattered except mag calibration and typo in baro stuff) changes to sensor autodetect for new dynamic drivers more of ledring stuff done (still doesn't work, so dont try) git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@115 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop
parent
0f6a75af4a
commit
7592316c04
130
baseflight.uvopt
130
baseflight.uvopt
|
|
@ -161,9 +161,9 @@
|
|||
<Bp>
|
||||
<Number>0</Number>
|
||||
<Type>0</Type>
|
||||
<LineNumber>35</LineNumber>
|
||||
<LineNumber>58</LineNumber>
|
||||
<EnabledFlag>1</EnabledFlag>
|
||||
<Address>134236980</Address>
|
||||
<Address>134240618</Address>
|
||||
<ByteObject>0</ByteObject>
|
||||
<ManyObjects>0</ManyObjects>
|
||||
<SizeOfObject>0</SizeOfObject>
|
||||
|
|
@ -171,7 +171,7 @@
|
|||
<BreakIfRCount>1</BreakIfRCount>
|
||||
<Filename></Filename>
|
||||
<ExecCommand></ExecCommand>
|
||||
<Expression>\\baseflight\src/drv_mpu3050.c\35</Expression>
|
||||
<Expression>\\baseflight\src/drv_mpu6050.c\58</Expression>
|
||||
</Bp>
|
||||
</Breakpoint>
|
||||
<WatchWindow1>
|
||||
|
|
@ -202,7 +202,7 @@
|
|||
<Mm>
|
||||
<WinNumber>1</WinNumber>
|
||||
<SubType>0</SubType>
|
||||
<ItemText>cmdline</ItemText>
|
||||
<ItemText>buf</ItemText>
|
||||
</Mm>
|
||||
</MemoryWindow1>
|
||||
<MemoryWindow2>
|
||||
|
|
@ -244,7 +244,7 @@
|
|||
</Target>
|
||||
|
||||
<Target>
|
||||
<TargetName>STM32-FreeFlight</TargetName>
|
||||
<TargetName>STM32-Release</TargetName>
|
||||
<ToolsetNumber>0x4</ToolsetNumber>
|
||||
<ToolsetName>ARM-ADS</ToolsetName>
|
||||
<TargetOption>
|
||||
|
|
@ -519,10 +519,10 @@
|
|||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>26</ColumnNumber>
|
||||
<ColumnNumber>15</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>92</TopLine>
|
||||
<CurrentLine>110</CurrentLine>
|
||||
<TopLine>116</TopLine>
|
||||
<CurrentLine>131</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\config.c</PathWithFileName>
|
||||
<FilenameWithoutPath>config.c</FilenameWithoutPath>
|
||||
|
|
@ -535,8 +535,8 @@
|
|||
<Focus>0</Focus>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>229</TopLine>
|
||||
<CurrentLine>238</CurrentLine>
|
||||
<TopLine>320</TopLine>
|
||||
<CurrentLine>340</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\imu.c</PathWithFileName>
|
||||
<FilenameWithoutPath>imu.c</FilenameWithoutPath>
|
||||
|
|
@ -547,10 +547,10 @@
|
|||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<ColumnNumber>13</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>0</TopLine>
|
||||
<CurrentLine>0</CurrentLine>
|
||||
<TopLine>11</TopLine>
|
||||
<CurrentLine>25</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\main.c</PathWithFileName>
|
||||
<FilenameWithoutPath>main.c</FilenameWithoutPath>
|
||||
|
|
@ -563,8 +563,8 @@
|
|||
<Focus>0</Focus>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>268</TopLine>
|
||||
<CurrentLine>268</CurrentLine>
|
||||
<TopLine>37</TopLine>
|
||||
<CurrentLine>68</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\mixer.c</PathWithFileName>
|
||||
<FilenameWithoutPath>mixer.c</FilenameWithoutPath>
|
||||
|
|
@ -575,10 +575,10 @@
|
|||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<ColumnNumber>20</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>124</TopLine>
|
||||
<CurrentLine>129</CurrentLine>
|
||||
<TopLine>359</TopLine>
|
||||
<CurrentLine>376</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\mw.c</PathWithFileName>
|
||||
<FilenameWithoutPath>mw.c</FilenameWithoutPath>
|
||||
|
|
@ -589,10 +589,10 @@
|
|||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<ColumnNumber>41</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>216</TopLine>
|
||||
<CurrentLine>231</CurrentLine>
|
||||
<TopLine>276</TopLine>
|
||||
<CurrentLine>298</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\sensors.c</PathWithFileName>
|
||||
<FilenameWithoutPath>sensors.c</FilenameWithoutPath>
|
||||
|
|
@ -603,10 +603,10 @@
|
|||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<ColumnNumber>18</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>237</TopLine>
|
||||
<CurrentLine>237</CurrentLine>
|
||||
<TopLine>199</TopLine>
|
||||
<CurrentLine>233</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\serial.c</PathWithFileName>
|
||||
<FilenameWithoutPath>serial.c</FilenameWithoutPath>
|
||||
|
|
@ -617,10 +617,10 @@
|
|||
<FileType>5</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<ColumnNumber>20</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>54</TopLine>
|
||||
<CurrentLine>72</CurrentLine>
|
||||
<TopLine>1</TopLine>
|
||||
<CurrentLine>29</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\board.h</PathWithFileName>
|
||||
<FilenameWithoutPath>board.h</FilenameWithoutPath>
|
||||
|
|
@ -631,10 +631,10 @@
|
|||
<FileType>5</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>14</ColumnNumber>
|
||||
<ColumnNumber>32</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>241</TopLine>
|
||||
<CurrentLine>249</CurrentLine>
|
||||
<TopLine>248</TopLine>
|
||||
<CurrentLine>262</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\mw.h</PathWithFileName>
|
||||
<FilenameWithoutPath>mw.h</FilenameWithoutPath>
|
||||
|
|
@ -652,10 +652,10 @@
|
|||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<ColumnNumber>23</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>0</TopLine>
|
||||
<CurrentLine>0</CurrentLine>
|
||||
<TopLine>1</TopLine>
|
||||
<CurrentLine>1</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\drv_adc.c</PathWithFileName>
|
||||
<FilenameWithoutPath>drv_adc.c</FilenameWithoutPath>
|
||||
|
|
@ -666,10 +666,10 @@
|
|||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<ColumnNumber>31</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>54</TopLine>
|
||||
<CurrentLine>72</CurrentLine>
|
||||
<TopLine>11</TopLine>
|
||||
<CurrentLine>15</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\drv_adxl345.c</PathWithFileName>
|
||||
<FilenameWithoutPath>drv_adxl345.c</FilenameWithoutPath>
|
||||
|
|
@ -696,8 +696,8 @@
|
|||
<Focus>0</Focus>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>33</TopLine>
|
||||
<CurrentLine>51</CurrentLine>
|
||||
<TopLine>0</TopLine>
|
||||
<CurrentLine>0</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\drv_hmc5883l.c</PathWithFileName>
|
||||
<FilenameWithoutPath>drv_hmc5883l.c</FilenameWithoutPath>
|
||||
|
|
@ -708,10 +708,10 @@
|
|||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>29</ColumnNumber>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>101</TopLine>
|
||||
<CurrentLine>101</CurrentLine>
|
||||
<TopLine>0</TopLine>
|
||||
<CurrentLine>0</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\drv_i2c.c</PathWithFileName>
|
||||
<FilenameWithoutPath>drv_i2c.c</FilenameWithoutPath>
|
||||
|
|
@ -722,10 +722,10 @@
|
|||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>15</ColumnNumber>
|
||||
<ColumnNumber>32</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>13</TopLine>
|
||||
<CurrentLine>35</CurrentLine>
|
||||
<TopLine>1</TopLine>
|
||||
<CurrentLine>1</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\drv_mpu3050.c</PathWithFileName>
|
||||
<FilenameWithoutPath>drv_mpu3050.c</FilenameWithoutPath>
|
||||
|
|
@ -738,8 +738,8 @@
|
|||
<Focus>0</Focus>
|
||||
<ColumnNumber>5</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>71</TopLine>
|
||||
<CurrentLine>87</CurrentLine>
|
||||
<TopLine>0</TopLine>
|
||||
<CurrentLine>0</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\drv_pwm.c</PathWithFileName>
|
||||
<FilenameWithoutPath>drv_pwm.c</FilenameWithoutPath>
|
||||
|
|
@ -752,8 +752,8 @@
|
|||
<Focus>0</Focus>
|
||||
<ColumnNumber>51</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>1</TopLine>
|
||||
<CurrentLine>4</CurrentLine>
|
||||
<TopLine>0</TopLine>
|
||||
<CurrentLine>0</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\drv_system.c</PathWithFileName>
|
||||
<FilenameWithoutPath>drv_system.c</FilenameWithoutPath>
|
||||
|
|
@ -778,14 +778,42 @@
|
|||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>1</ColumnNumber>
|
||||
<ColumnNumber>17</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>1</TopLine>
|
||||
<CurrentLine>19</CurrentLine>
|
||||
<TopLine>0</TopLine>
|
||||
<CurrentLine>0</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\drv_ledring.c</PathWithFileName>
|
||||
<FilenameWithoutPath>drv_ledring.c</FilenameWithoutPath>
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>2</GroupNumber>
|
||||
<FileNumber>0</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>21</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>3</TopLine>
|
||||
<CurrentLine>3</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\drv_mpu6050.c</PathWithFileName>
|
||||
<FilenameWithoutPath>drv_mpu6050.c</FilenameWithoutPath>
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>2</GroupNumber>
|
||||
<FileNumber>0</FileNumber>
|
||||
<FileType>5</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>1</TopLine>
|
||||
<CurrentLine>4</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\drv_mpu6050.h</PathWithFileName>
|
||||
<FilenameWithoutPath>drv_mpu6050.h</FilenameWithoutPath>
|
||||
</File>
|
||||
</Group>
|
||||
|
||||
<Group>
|
||||
|
|
|
|||
|
|
@ -491,10 +491,74 @@
|
|||
<FileType>1</FileType>
|
||||
<FilePath>.\src\drv_ledring.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>drv_mpu6050.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>.\src\drv_mpu6050.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>drv_mpu6050.h</FileName>
|
||||
<FileType>5</FileType>
|
||||
<FilePath>.\src\drv_mpu6050.h</FilePath>
|
||||
</File>
|
||||
</Files>
|
||||
</Group>
|
||||
<Group>
|
||||
<GroupName>System</GroupName>
|
||||
<GroupOption>
|
||||
<CommonProperty>
|
||||
<UseCPPCompiler>0</UseCPPCompiler>
|
||||
<RVCTCodeConst>0</RVCTCodeConst>
|
||||
<RVCTZI>0</RVCTZI>
|
||||
<RVCTOtherData>0</RVCTOtherData>
|
||||
<ModuleSelection>0</ModuleSelection>
|
||||
<IncludeInBuild>2</IncludeInBuild>
|
||||
<AlwaysBuild>2</AlwaysBuild>
|
||||
<GenerateAssemblyFile>2</GenerateAssemblyFile>
|
||||
<AssembleAssemblyFile>2</AssembleAssemblyFile>
|
||||
<PublicsOnly>2</PublicsOnly>
|
||||
<StopOnExitCode>11</StopOnExitCode>
|
||||
<CustomArgument></CustomArgument>
|
||||
<IncludeLibraryModules></IncludeLibraryModules>
|
||||
</CommonProperty>
|
||||
<GroupArmAds>
|
||||
<Cads>
|
||||
<interw>2</interw>
|
||||
<Optim>4</Optim>
|
||||
<oTime>2</oTime>
|
||||
<SplitLS>2</SplitLS>
|
||||
<OneElfS>2</OneElfS>
|
||||
<Strict>2</Strict>
|
||||
<EnumInt>2</EnumInt>
|
||||
<PlainCh>2</PlainCh>
|
||||
<Ropi>2</Ropi>
|
||||
<Rwpi>2</Rwpi>
|
||||
<wLevel>0</wLevel>
|
||||
<uThumb>2</uThumb>
|
||||
<VariousControls>
|
||||
<MiscControls></MiscControls>
|
||||
<Define></Define>
|
||||
<Undefine></Undefine>
|
||||
<IncludePath></IncludePath>
|
||||
</VariousControls>
|
||||
</Cads>
|
||||
<Aads>
|
||||
<interw>2</interw>
|
||||
<Ropi>2</Ropi>
|
||||
<Rwpi>2</Rwpi>
|
||||
<thumb>2</thumb>
|
||||
<SplitLS>2</SplitLS>
|
||||
<SwStkChk>2</SwStkChk>
|
||||
<NoWarn>2</NoWarn>
|
||||
<VariousControls>
|
||||
<MiscControls></MiscControls>
|
||||
<Define></Define>
|
||||
<Undefine></Undefine>
|
||||
<IncludePath></IncludePath>
|
||||
</VariousControls>
|
||||
</Aads>
|
||||
</GroupArmAds>
|
||||
</GroupOption>
|
||||
<Files>
|
||||
<File>
|
||||
<FileName>core_cm3.c</FileName>
|
||||
|
|
@ -566,7 +630,7 @@
|
|||
</Groups>
|
||||
</Target>
|
||||
<Target>
|
||||
<TargetName>STM32-FreeFlight</TargetName>
|
||||
<TargetName>STM32-Release</TargetName>
|
||||
<ToolsetNumber>0x4</ToolsetNumber>
|
||||
<ToolsetName>ARM-ADS</ToolsetName>
|
||||
<TargetOption>
|
||||
|
|
@ -767,7 +831,7 @@
|
|||
<hadIRAM2>0</hadIRAM2>
|
||||
<hadIROM2>0</hadIROM2>
|
||||
<StupSel>8</StupSel>
|
||||
<useUlib>0</useUlib>
|
||||
<useUlib>1</useUlib>
|
||||
<EndSel>0</EndSel>
|
||||
<uLtcg>0</uLtcg>
|
||||
<RoSelD>3</RoSelD>
|
||||
|
|
@ -1050,6 +1114,16 @@
|
|||
<FileType>1</FileType>
|
||||
<FilePath>.\src\drv_ledring.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>drv_mpu6050.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>.\src\drv_mpu6050.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>drv_mpu6050.h</FileName>
|
||||
<FileType>5</FileType>
|
||||
<FilePath>.\src\drv_mpu6050.h</FilePath>
|
||||
</File>
|
||||
</Files>
|
||||
</Group>
|
||||
<Group>
|
||||
|
|
|
|||
4812
obj/baseflight.hex
4812
obj/baseflight.hex
File diff suppressed because it is too large
Load Diff
34
src/board.h
34
src/board.h
|
|
@ -9,17 +9,6 @@
|
|||
#include "stm32f10x_conf.h"
|
||||
#include "core_cm3.h"
|
||||
|
||||
#include "drv_system.h" // timers, delays, etc
|
||||
#include "drv_adc.h"
|
||||
#include "drv_adxl345.h"
|
||||
#include "drv_bmp085.h"
|
||||
#include "drv_hmc5883l.h"
|
||||
#include "drv_i2c.h"
|
||||
#include "drv_ledring.h"
|
||||
#include "drv_mpu3050.h"
|
||||
#include "drv_pwm.h"
|
||||
#include "drv_uart.h"
|
||||
|
||||
typedef enum {
|
||||
SENSOR_ACC = 1 << 0,
|
||||
SENSOR_BARO = 1 << 1,
|
||||
|
|
@ -38,8 +27,19 @@ typedef enum {
|
|||
FEATURE_CAMTRIG = 1 << 6,
|
||||
FEATURE_GYRO_SMOOTHING = 1 << 7,
|
||||
FEATURE_LED_RING = 1 << 8,
|
||||
FEATURE_INFLIGHT_ACC_CAL = 1 << 9,
|
||||
} AvailableFeatures;
|
||||
|
||||
typedef void (* sensorInitFuncPtr)(void);
|
||||
typedef void (* sensorReadFuncPtr)(int16_t *data);
|
||||
|
||||
typedef struct sensor_t
|
||||
{
|
||||
sensorInitFuncPtr init;
|
||||
sensorReadFuncPtr read;
|
||||
sensorReadFuncPtr orient;
|
||||
} sensor_t;
|
||||
|
||||
#define digitalHi(p, i) { p->BSRR = i; }
|
||||
#define digitalLo(p, i) { p->BRR = i; }
|
||||
#define digitalToggle(p, i) { p->ODR ^= i; }
|
||||
|
|
@ -69,3 +69,15 @@ typedef enum {
|
|||
|
||||
#define BARO_OFF digitalLo(BARO_GPIO, BARO_PIN);
|
||||
#define BARO_ON digitalHi(BARO_GPIO, BARO_PIN);
|
||||
|
||||
#include "drv_system.h" // timers, delays, etc
|
||||
#include "drv_adc.h"
|
||||
#include "drv_adxl345.h"
|
||||
#include "drv_bmp085.h"
|
||||
#include "drv_hmc5883l.h"
|
||||
#include "drv_i2c.h"
|
||||
#include "drv_ledring.h"
|
||||
#include "drv_mpu3050.h"
|
||||
#include "drv_mpu6050.h"
|
||||
#include "drv_pwm.h"
|
||||
#include "drv_uart.h"
|
||||
|
|
|
|||
|
|
@ -15,7 +15,11 @@
|
|||
#define ADXL345_FULL_RANGE 0x08
|
||||
#define ADXL345_RANGE_16G 0x03
|
||||
|
||||
bool adxl345Detect(void)
|
||||
static void adxl345Init(void);
|
||||
static void adxl345Read(int16_t *accelData);
|
||||
static void adxl345Align(int16_t *accelData);
|
||||
|
||||
bool adxl345Detect(sensor_t *acc)
|
||||
{
|
||||
bool ack = false;
|
||||
uint8_t sig = 0;
|
||||
|
|
@ -23,6 +27,10 @@ bool adxl345Detect(void)
|
|||
ack = i2cRead(ADXL345_ADDRESS, 0x00, 1, &sig);
|
||||
if (!ack || sig != 0xE5)
|
||||
return false;
|
||||
|
||||
acc->init = adxl345Init;
|
||||
acc->read = adxl345Read;
|
||||
acc->orient = adxl345Align;
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
@ -38,7 +46,7 @@ bool adxl345Detect(void)
|
|||
#define ADXL_RANGE_8G 0x02
|
||||
#define ADXL_RANGE_16G 0x03
|
||||
|
||||
void adxl345Init(void)
|
||||
static void adxl345Init(void)
|
||||
{
|
||||
#ifdef FREEFLIGHT
|
||||
i2cWrite(ADXL345_ADDRESS, ADXL345_BW_RATE, ADXL345_BW_RATE_200);
|
||||
|
|
@ -60,7 +68,7 @@ void adxl345Init(void)
|
|||
#endif /* FreeFlight */
|
||||
}
|
||||
|
||||
void adxl345Read(int16_t *accelData)
|
||||
static void adxl345Read(int16_t *accelData)
|
||||
{
|
||||
static uint8_t buf[6];
|
||||
|
||||
|
|
@ -69,3 +77,8 @@ void adxl345Read(int16_t *accelData)
|
|||
accelData[1] = buf[2] + (buf[3] << 8);
|
||||
accelData[2] = buf[4] + (buf[5] << 8);
|
||||
}
|
||||
|
||||
static void adxl345Align(int16_t *accData)
|
||||
{
|
||||
// official direction is RPY, nothing to change here.
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,5 +1,3 @@
|
|||
#pragma once
|
||||
|
||||
bool adxl345Detect(void);
|
||||
void adxl345Init(void);
|
||||
void adxl345Read(int16_t *accelData);
|
||||
bool adxl345Detect(sensor_t *acc);
|
||||
|
|
|
|||
|
|
@ -25,7 +25,28 @@
|
|||
|
||||
static uint8_t mpuLowPassFliter = MPU3050_DLPF_42HZ;
|
||||
|
||||
void mpu3050Init(void)
|
||||
static void mpu3050Init(void);
|
||||
static void mpu3050Read(int16_t *gyroData);
|
||||
static void mpu3050Align(int16_t *gyroData);
|
||||
|
||||
bool mpu3050Detect(sensor_t *gyro)
|
||||
{
|
||||
bool ack;
|
||||
|
||||
delay(25); // datasheet page 13 says 20ms. other stuff could have been running meanwhile. but we'll be safe
|
||||
|
||||
ack = i2cWrite(MPU3050_ADDRESS, MPU3050_SMPLRT_DIV, 0);
|
||||
if (!ack)
|
||||
return false;
|
||||
|
||||
gyro->init = mpu3050Init;
|
||||
gyro->read = mpu3050Read;
|
||||
gyro->orient = mpu3050Align;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static void mpu3050Init(void)
|
||||
{
|
||||
bool ack;
|
||||
|
||||
|
|
@ -41,17 +62,25 @@ void mpu3050Init(void)
|
|||
i2cWrite(MPU3050_ADDRESS, MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
|
||||
}
|
||||
|
||||
static void mpu3050Align(int16_t *gyroData)
|
||||
{
|
||||
// official direction is RPY
|
||||
gyroData[0] = gyroData[0] / 4;
|
||||
gyroData[1] = gyroData[1] / 4;
|
||||
gyroData[2] = -gyroData[2] / 4;
|
||||
}
|
||||
|
||||
// Read 3 gyro values into user-provided buffer. No overrun checking is done.
|
||||
void mpu3050Read(int16_t *gyroData)
|
||||
static void mpu3050Read(int16_t *gyroData)
|
||||
{
|
||||
uint8_t buf[6];
|
||||
i2cRead(MPU3050_ADDRESS, MPU3050_GYRO_OUT, 6, buf);
|
||||
gyroData[0] = buf[0] << 8 | buf[1];
|
||||
gyroData[1] = buf[2] << 8 | buf[3];
|
||||
gyroData[2] = buf[4] << 8 | buf[5];
|
||||
gyroData[0] = (buf[0] << 8) | buf[1];
|
||||
gyroData[1] = (buf[2] << 8) | buf[3];
|
||||
gyroData[2] = (buf[4] << 8) | buf[5];
|
||||
}
|
||||
|
||||
int16_t mpu3050ReadTemp(void)
|
||||
static int16_t mpu3050ReadTemp(void)
|
||||
{
|
||||
uint8_t buf[2];
|
||||
i2cRead(MPU3050_ADDRESS, MPU3050_TEMP_OUT, 2, buf);
|
||||
|
|
|
|||
|
|
@ -1,5 +1,3 @@
|
|||
#pragma once
|
||||
|
||||
void mpu3050Init(void);
|
||||
void mpu3050Read(int16_t *gyroData);
|
||||
int16_t mpu3050ReadTemp(void);
|
||||
bool mpu3050Detect(sensor_t *gyro);
|
||||
|
|
|
|||
|
|
@ -0,0 +1,100 @@
|
|||
#include "board.h"
|
||||
|
||||
// MPU6050, Standard address 0x68
|
||||
#define MPU6050_ADDRESS 0x68
|
||||
#define MPU6050_WHO_AM_I 0x75
|
||||
#define MPU6050_SMPLRT_DIV 0 //8000Hz
|
||||
#define MPU6050_DLPF_CFG 0
|
||||
#define MPU6050_GYRO_OUT 0x43
|
||||
#define MPU6050_ACC_OUT 0x3B
|
||||
|
||||
static void mpu6050AccInit(void);
|
||||
static void mpu6050AccRead(int16_t *accData);
|
||||
static void mpu6050AccAlign(int16_t *accData);
|
||||
static void mpu6050GyroInit(void);
|
||||
static void mpu6050GyroRead(int16_t *gyroData);
|
||||
static void mpu6050GyroAlign(int16_t *gyroData);
|
||||
|
||||
extern uint16_t acc_1G;
|
||||
|
||||
bool mpu6050Detect(sensor_t *acc, sensor_t *gyro)
|
||||
{
|
||||
bool ack;
|
||||
uint8_t sig;
|
||||
|
||||
delay(35); // datasheet page 13 says 30ms. other stuff could have been running meanwhile. but we'll be safe
|
||||
|
||||
ack = i2cRead(MPU6050_ADDRESS, MPU6050_WHO_AM_I, 1, &sig);
|
||||
if (!ack)
|
||||
return false;
|
||||
|
||||
if (sig != 0x68)
|
||||
return false;
|
||||
|
||||
acc->init = mpu6050AccInit;
|
||||
acc->read = mpu6050AccRead;
|
||||
acc->orient = mpu6050AccAlign;
|
||||
gyro->init = mpu6050GyroInit;
|
||||
gyro->read = mpu6050GyroRead;
|
||||
gyro->orient = mpu6050GyroAlign;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static void mpu6050AccInit(void)
|
||||
{
|
||||
i2cWrite(MPU6050_ADDRESS, 0x1C, 0x10); // ACCEL_CONFIG -- AFS_SEL=2 (Full Scale = +/-8G) ; ACCELL_HPF=0 //note something is wrong in the spec.
|
||||
// note: something seems to be wrong in the spec here. With AFS=2 1G = 4096 but according to my measurement: 1G=2048 (and 2048/8 = 256)
|
||||
// confirmed here: http://www.multiwii.com/forum/viewtopic.php?f=8&t=1080&start=10#p7480
|
||||
// seems to be not a problem on MPU6000 lol
|
||||
acc_1G = 1023;
|
||||
}
|
||||
|
||||
static void mpu6050AccRead(int16_t *accData)
|
||||
{
|
||||
uint8_t buf[6];
|
||||
|
||||
i2cRead(MPU6050_ADDRESS, MPU6050_ACC_OUT, 6, buf);
|
||||
accData[0] = (buf[0] << 8) | buf[1];
|
||||
accData[1] = (buf[2] << 8) | buf[3];
|
||||
accData[2] = (buf[4] << 8) | buf[5];
|
||||
}
|
||||
|
||||
static void mpu6050AccAlign(int16_t *accData)
|
||||
{
|
||||
int16_t temp[2];
|
||||
temp[0] = accData[0];
|
||||
temp[1] = accData[1];
|
||||
|
||||
// official direction is RPY
|
||||
accData[0] = temp[1] / 8;
|
||||
accData[1] = -temp[0] / 8;
|
||||
accData[2] = accData[2] / 8;
|
||||
}
|
||||
|
||||
static void mpu6050GyroInit(void)
|
||||
{
|
||||
i2cWrite(MPU6050_ADDRESS, 0x6B, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
|
||||
delay(5);
|
||||
i2cWrite(MPU6050_ADDRESS, 0x19, 0x00); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
|
||||
i2cWrite(MPU6050_ADDRESS, 0x1A, MPU6050_DLPF_CFG); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
|
||||
i2cWrite(MPU6050_ADDRESS, 0x6B, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
|
||||
i2cWrite(MPU6050_ADDRESS, 0x1B, 0x18); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
|
||||
}
|
||||
|
||||
static void mpu6050GyroRead(int16_t *gyroData)
|
||||
{
|
||||
uint8_t buf[6];
|
||||
i2cRead(MPU6050_ADDRESS, MPU6050_GYRO_OUT, 6, buf);
|
||||
gyroData[0] = (buf[0] << 8) | buf[1];
|
||||
gyroData[1] = (buf[2] << 8) | buf[3];
|
||||
gyroData[2] = (buf[4] << 8) | buf[5];
|
||||
}
|
||||
|
||||
static void mpu6050GyroAlign(int16_t *gyroData)
|
||||
{
|
||||
// official direction is RPY
|
||||
gyroData[0] = gyroData[0] / 4;
|
||||
gyroData[1] = gyroData[1] / 4;
|
||||
gyroData[2] = -gyroData[2] / 4;
|
||||
}
|
||||
|
|
@ -0,0 +1,3 @@
|
|||
#pragma once
|
||||
|
||||
bool mpu6050Detect(sensor_t *acc, sensor_t *gyro);
|
||||
|
|
@ -336,8 +336,8 @@ void getEstimatedAltitude(void)
|
|||
EstAlt = BaroHigh * 10 / (BARO_TAB_SIZE / 2);
|
||||
|
||||
temp32 = AltHold - EstAlt;
|
||||
if (abs(temp32) < 10 && BaroPID < 10)
|
||||
BaroPID = 0; // remove small D parametr to reduce noise near zoro position
|
||||
if (abs(temp32) < 10 && abs(BaroPID) < 10)
|
||||
BaroPID = 0; // remove small D parameter to reduce noise near zero position
|
||||
// P
|
||||
BaroPID += cfg.P8[PIDALT] * constrain(temp32, (-2) * cfg.P8[PIDALT], 2 * cfg.P8[PIDALT]) / 100;
|
||||
BaroPID = constrain(BaroPID, -150, +150); // sum of P and D should be in range 150
|
||||
|
|
|
|||
13
src/main.c
13
src/main.c
|
|
@ -31,24 +31,15 @@ int main(void)
|
|||
LED1_TOGGLE;
|
||||
LED0_TOGGLE;
|
||||
delay(25);
|
||||
BEEP_ON
|
||||
BEEP_ON;
|
||||
delay(25);
|
||||
BEEP_OFF;
|
||||
}
|
||||
LED0_OFF;
|
||||
LED1_OFF;
|
||||
|
||||
// drop out any sensors that don't seem to work
|
||||
// drop out any sensors that don't seem to work, init all the others. halt if gyro is dead.
|
||||
sensorsAutodetect();
|
||||
|
||||
// Init sensors
|
||||
if (sensors(SENSOR_BARO))
|
||||
bmp085Init();
|
||||
if (sensors(SENSOR_ACC))
|
||||
adxl345Init();
|
||||
// if this fails, we get a beep + blink pattern. we're doomed.
|
||||
mpu3050Init();
|
||||
|
||||
imuInit(); // Mag is initialized inside imuInit
|
||||
|
||||
previousTime = micros();
|
||||
|
|
|
|||
61
src/mw.c
61
src/mw.c
|
|
@ -1,7 +1,7 @@
|
|||
#include "board.h"
|
||||
#include "mw.h"
|
||||
|
||||
// March 2012 V2.0_pre_version_1
|
||||
// March 2012 V2.0_pre_version_3
|
||||
|
||||
#define CHECKBOXITEMS 11
|
||||
#define PIDITEMS 8
|
||||
|
|
@ -56,6 +56,7 @@ uint16_t GPS_distanceToHome; // in meters
|
|||
int16_t GPS_directionToHome = 0; // in degrees
|
||||
uint16_t GPS_distanceToHome, GPS_distanceToHold; // distance to home or hold point in meters
|
||||
int16_t GPS_directionToHome, GPS_directionToHold; // direction to home or hol point in degrees
|
||||
uint16_t GPS_altitude, GPS_speed; // altitude in 0.1m and speed in 0.1m/s - Added by Mis
|
||||
uint8_t GPS_update = 0; // it's a binary toogle to distinct a GPS position update
|
||||
int16_t GPS_angle[2]; // it's the angles that must be applied for GPS correction
|
||||
|
||||
|
|
@ -94,8 +95,9 @@ void blinkLED(uint8_t num, uint8_t wait, uint8_t repeat)
|
|||
}
|
||||
}
|
||||
|
||||
// this code is executed at each loop and won't interfere with control loop if it lasts less than 650 microseconds
|
||||
void annexCode(void)
|
||||
{ //this code is excetuted at each loop and won't interfere with control loop if it lasts less than 650 microseconds
|
||||
{
|
||||
static uint32_t buzzerTime, calibratedAccTime;
|
||||
#if defined(LCD_TELEMETRY)
|
||||
static uint16_t telemetryTimer = 0, telemetryAutoTimer = 0, psensorTimer = 0;
|
||||
|
|
@ -173,19 +175,12 @@ void annexCode(void)
|
|||
}
|
||||
#endif
|
||||
|
||||
#define ADC_REF_VOLTAGE 3.3f
|
||||
#define ADC_TO_VOLTAGE (ADC_REF_VOLTAGE / (1<<12)) // 12 bit ADC resolution
|
||||
#define ADC_VOLTS_PRECISION 12
|
||||
#define ADC_VOLTS_SLOPE ((10.0f + 1.0f) / 1.0f) // Rtop = 10K, Rbot = 1.0K
|
||||
#define ADC_TO_VOLTS ((ADC_TO_VOLTAGE / ((1<<(ADC_VOLTS_PRECISION))+1)) * ADC_VOLTS_SLOPE)
|
||||
|
||||
if (feature(FEATURE_VBAT)) {
|
||||
if (!(++vbatTimer % VBATFREQ)) {
|
||||
// avgVolts = adcAvgVolts * ADC_TO_VOLTS;
|
||||
vbatRawArray[(ind++) % 8] = adcGetBattery();
|
||||
for (i = 0; i < 8; i++)
|
||||
vbatRaw += vbatRawArray[i];
|
||||
vbat = vbatRaw / (VBATSCALE / 2); // result is Vbatt in 0.1V steps
|
||||
vbat = (((vbatRaw / 8) * 3.3f) / 4095) * 110; // result is Vbatt in 0.1V steps
|
||||
}
|
||||
if (rcOptions[BOXBEEPERON]) { // unconditional beeper on via AUXn switch
|
||||
buzzerFreq = 7;
|
||||
|
|
@ -232,13 +227,13 @@ void annexCode(void)
|
|||
}
|
||||
}
|
||||
|
||||
#if defined(LED_RING)
|
||||
if (feature(FEATURE_LED_RING)) {
|
||||
static uint32_t LEDTime;
|
||||
if (currentTime > LEDTime) {
|
||||
LEDTime = currentTime + 50000;
|
||||
i2CLedRingState();
|
||||
ledringState();
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
if (currentTime > calibratedAccTime) {
|
||||
if (smallAngle25 == 0) {
|
||||
|
|
@ -369,16 +364,20 @@ void loop(void)
|
|||
calibratingG = 400;
|
||||
} else if (rcData[YAW] > MAXCHECK && rcData[PITCH] > MAXCHECK && armed == 0) {
|
||||
if (rcDelayCommand == 20) {
|
||||
servo[0] = 1500; //we center the yaw gyro in conf mode
|
||||
if (cfg.mixerConfiguration == MULTITYPE_TRI) {
|
||||
servo[5] = 1500; // we center the yaw servo in conf mode
|
||||
writeServos();
|
||||
} else if (cfg.mixerConfiguration == MULTITYPE_FLYING_WING) {
|
||||
servo[0] = cfg.wing_left_mid;
|
||||
servo[1] = cfg.wing_right_mid;
|
||||
writeServos();
|
||||
}
|
||||
#if defined(LCD_CONF)
|
||||
configurationLoop(); //beginning LCD configuration
|
||||
#endif
|
||||
previousTime = micros();
|
||||
}
|
||||
}
|
||||
#if defined(InflightAccCalibration)
|
||||
else if (armed == 0 && rcData[YAW] < MINCHECK && rcData[PITCH] > MAXCHECK && rcData[ROLL] > MAXCHECK) {
|
||||
} else if (feature(FEATURE_INFLIGHT_ACC_CAL) && (armed == 0 && rcData[YAW] < MINCHECK && rcData[PITCH] > MAXCHECK && rcData[ROLL] > MAXCHECK)) {
|
||||
if (rcDelayCommand == 20) {
|
||||
if (AccInflightCalibrationMeasurementDone) { //trigger saving into eeprom after landing
|
||||
AccInflightCalibrationMeasurementDone = 0;
|
||||
|
|
@ -392,9 +391,7 @@ void loop(void)
|
|||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
else if ((cfg.activate1[BOXARM] > 0) || (cfg.activate2[BOXARM] > 0)) {
|
||||
} else if ((cfg.activate1[BOXARM] > 0) || (cfg.activate2[BOXARM] > 0)) {
|
||||
if (rcOptions[BOXARM] && okToArm) {
|
||||
armed = 1;
|
||||
headFreeModeHold = heading;
|
||||
|
|
@ -435,27 +432,23 @@ void loop(void)
|
|||
} else if (rcData[PITCH] > MAXCHECK) {
|
||||
cfg.accTrim[PITCH] += 2;
|
||||
writeParams();
|
||||
#if defined(LED_RING)
|
||||
blinkLedRing();
|
||||
#endif
|
||||
if (feature(FEATURE_LED_RING))
|
||||
ledringBlink();
|
||||
} else if (rcData[PITCH] < MINCHECK) {
|
||||
cfg.accTrim[PITCH] -= 2;
|
||||
writeParams();
|
||||
#if defined(LED_RING)
|
||||
blinkLedRing();
|
||||
#endif
|
||||
if (feature(FEATURE_LED_RING))
|
||||
ledringBlink();
|
||||
} else if (rcData[ROLL] > MAXCHECK) {
|
||||
cfg.accTrim[ROLL] += 2;
|
||||
writeParams();
|
||||
#if defined(LED_RING)
|
||||
blinkLedRing();
|
||||
#endif
|
||||
if (feature(FEATURE_LED_RING))
|
||||
ledringBlink();
|
||||
} else if (rcData[ROLL] < MINCHECK) {
|
||||
cfg.accTrim[ROLL] -= 2;
|
||||
writeParams();
|
||||
#if defined(LED_RING)
|
||||
blinkLedRing();
|
||||
#endif
|
||||
if (feature(FEATURE_LED_RING))
|
||||
ledringBlink();
|
||||
} else {
|
||||
rcDelayCommand = 0;
|
||||
}
|
||||
|
|
@ -467,7 +460,7 @@ void loop(void)
|
|||
cycleTimeMin = cycleTime; // remember lowscore
|
||||
#endif
|
||||
|
||||
#if defined(InflightAccCalibration)
|
||||
if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
|
||||
if (AccInflightCalibrationArmed && armed == 1 && rcData[THROTTLE] > MINCHECK && !rcOptions[BOXARM]) { // Copter is airborne and you are turning it off via boxarm : start measurement
|
||||
InflightcalibratingA = 50;
|
||||
AccInflightCalibrationArmed = 0;
|
||||
|
|
@ -482,7 +475,7 @@ void loop(void)
|
|||
AccInflightCalibrationMeasurementDone = 0;
|
||||
AccInflightCalibrationSavetoEEProm = 1;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
for(i = 0; i < CHECKBOXITEMS; i++) {
|
||||
rcOptions[i] = (((rcData[AUX1] < 1300) | (1300 < rcData[AUX1] && rcData[AUX1] < 1700) << 1 | (rcData[AUX1] > 1700) << 2 | (rcData[AUX2] < 1300) << 3 | (1300 < rcData[AUX2] && rcData[AUX2] < 1700) << 4 | (rcData[AUX2] > 1700) << 5) & cfg.activate1[i]) || (((rcData[AUX3] < 1300) | (1300 < rcData[AUX3] && rcData[AUX3] < 1700) << 1 | (rcData[AUX3] > 1700) << 2 | (rcData[AUX4] < 1300) << 3 | (1300 < rcData[AUX4] && rcData[AUX4] < 1700) << 4 | (rcData[AUX4] > 1700) << 5) & cfg.activate2[i]);
|
||||
|
|
|
|||
9
src/mw.h
9
src/mw.h
|
|
@ -62,7 +62,7 @@
|
|||
with R1=33k and R2=51k
|
||||
vbat = [0;1023]*16/VBATSCALE */
|
||||
#define VBATFREQ 6 // to read battery voltage - keep equal to PSENSORFREQ (6) unless you know what you are doing
|
||||
#define VBATSCALE 151 // change this value if readed Battery voltage is different than real voltage
|
||||
#define VBATSCALE 160 // change this value if readed Battery voltage is different than real voltage
|
||||
#define VBATLEVEL1_3S 107 // 10,7V
|
||||
#define VBATLEVEL2_3S 103 // 10,3V
|
||||
#define VBATLEVEL3_3S 99 // 9.9V
|
||||
|
|
@ -75,7 +75,7 @@
|
|||
//#define MMSERVOGIMBAL // Active Output Moving Average Function for Servos Gimbal
|
||||
//#define MMSERVOGIMBALVECTORLENGHT 32 // Lenght of Moving Average Vector
|
||||
|
||||
#define VERSION 201
|
||||
#define VERSION 203
|
||||
|
||||
// Syncronized with GUI. Only exception is mixer > 11, which is always returned as 11 during serialization.
|
||||
typedef enum MultiType
|
||||
|
|
@ -230,10 +230,14 @@ extern int16_t GPS_directionToHome;
|
|||
extern uint8_t GPS_update;
|
||||
extern uint8_t GPSModeHome;
|
||||
extern uint8_t GPSModeHold;
|
||||
extern uint16_t GPS_altitude;
|
||||
extern uint16_t GPS_speed; // altitude in 0.1m and speed in 0.1m/s - Added by Mis
|
||||
extern uint8_t vbat;
|
||||
extern int16_t lookupRX[7]; // lookup table for expo & RC rate
|
||||
|
||||
extern config_t cfg;
|
||||
extern sensor_t acc;
|
||||
extern sensor_t gyro;
|
||||
|
||||
// main
|
||||
void loop(void);
|
||||
|
|
@ -257,6 +261,7 @@ void Mag_getADC(void);
|
|||
void mixerInit(void);
|
||||
void writeServos(void);
|
||||
void writeMotors(void);
|
||||
void writeAllMotors(int16_t mc);
|
||||
void mixTable(void);
|
||||
|
||||
// Serial
|
||||
|
|
|
|||
101
src/sensors.c
101
src/sensors.c
|
|
@ -8,14 +8,41 @@ uint8_t calibratingM = 0;
|
|||
uint16_t acc_1G = 256; // this is the 1G measured acceleration
|
||||
int16_t heading, magHold;
|
||||
|
||||
extern uint16_t InflightcalibratingA;
|
||||
extern int16_t AccInflightCalibrationArmed;
|
||||
extern uint16_t AccInflightCalibrationMeasurementDone;
|
||||
extern uint16_t AccInflightCalibrationSavetoEEProm;
|
||||
extern uint16_t AccInflightCalibrationActive;
|
||||
|
||||
sensor_t acc; // acc access functions
|
||||
sensor_t gyro; // gyro access functions
|
||||
|
||||
void sensorsAutodetect(void)
|
||||
{
|
||||
if (!adxl345Detect())
|
||||
// Detect what's available
|
||||
if (!adxl345Detect(&acc))
|
||||
sensorsClear(SENSOR_ACC);
|
||||
if (!bmp085Init())
|
||||
sensorsClear(SENSOR_BARO);
|
||||
if (!hmc5883lDetect())
|
||||
sensorsClear(SENSOR_MAG);
|
||||
|
||||
// Init sensors
|
||||
if (sensors(SENSOR_ACC))
|
||||
acc.init();
|
||||
if (sensors(SENSOR_BARO))
|
||||
bmp085Init();
|
||||
|
||||
// special case for supported gyros - MPU3050 and MPU6050
|
||||
if (mpu6050Detect(&acc, &gyro)) { // first, try MPU6050, and re-enable acc (if ADXL345 is missing) since this chip has it built in
|
||||
sensorsSet(SENSOR_ACC);
|
||||
acc.init();
|
||||
} else if (!mpu3050Detect(&gyro)) {
|
||||
// if this fails, we get a beep + blink pattern. we're doomed, no gyro or i2c error.
|
||||
failureMode(3);
|
||||
}
|
||||
// this is safe because either mpu6050 or mpu3050 sets it, and in case of fail, none do.
|
||||
gyro.init();
|
||||
}
|
||||
|
||||
static void ACC_Common(void)
|
||||
|
|
@ -45,20 +72,22 @@ static void ACC_Common(void)
|
|||
}
|
||||
calibratingA--;
|
||||
}
|
||||
#if defined(InflightAccCalibration)
|
||||
|
||||
if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
|
||||
static int32_t b[3];
|
||||
static int16_t accZero_saved[3] = { 0, 0, 0 };
|
||||
static int16_t accTrim_saved[2] = { 0, 0 };
|
||||
//Saving old zeropoints before measurement
|
||||
if (InflightcalibratingA == 50) {
|
||||
accZero_saved[ROLL] = accZero[ROLL];
|
||||
accZero_saved[PITCH] = accZero[PITCH];
|
||||
accZero_saved[YAW] = accZero[YAW];
|
||||
accTrim_saved[ROLL] = accTrim[ROLL];
|
||||
accTrim_saved[PITCH] = accTrim[PITCH];
|
||||
accZero_saved[ROLL] = cfg.accZero[ROLL];
|
||||
accZero_saved[PITCH] = cfg.accZero[PITCH];
|
||||
accZero_saved[YAW] = cfg.accZero[YAW];
|
||||
accTrim_saved[ROLL] = cfg.accTrim[ROLL];
|
||||
accTrim_saved[PITCH] = cfg.accTrim[PITCH];
|
||||
}
|
||||
if (InflightcalibratingA > 0) {
|
||||
for (uint8_t axis = 0; axis < 3; axis++) {
|
||||
uint8_t axis;
|
||||
for (axis = 0; axis < 3; axis++) {
|
||||
// Reset a[axis] at start of calibration
|
||||
if (InflightcalibratingA == 50)
|
||||
b[axis] = 0;
|
||||
|
|
@ -66,7 +95,7 @@ static void ACC_Common(void)
|
|||
b[axis] += accADC[axis];
|
||||
// Clear global variables for next reading
|
||||
accADC[axis] = 0;
|
||||
accZero[axis] = 0;
|
||||
cfg.accZero[axis] = 0;
|
||||
}
|
||||
//all values are measured
|
||||
if (InflightcalibratingA == 1) {
|
||||
|
|
@ -74,25 +103,26 @@ static void ACC_Common(void)
|
|||
AccInflightCalibrationMeasurementDone = 1;
|
||||
blinkLED(10, 10, 2); //buzzer for indicatiing the start inflight
|
||||
// recover saved values to maintain current flight behavior until new values are transferred
|
||||
accZero[ROLL] = accZero_saved[ROLL];
|
||||
accZero[PITCH] = accZero_saved[PITCH];
|
||||
accZero[YAW] = accZero_saved[YAW];
|
||||
accTrim[ROLL] = accTrim_saved[ROLL];
|
||||
accTrim[PITCH] = accTrim_saved[PITCH];
|
||||
cfg.accZero[ROLL] = accZero_saved[ROLL];
|
||||
cfg.accZero[PITCH] = accZero_saved[PITCH];
|
||||
cfg.accZero[YAW] = accZero_saved[YAW];
|
||||
cfg.accTrim[ROLL] = accTrim_saved[ROLL];
|
||||
cfg.accTrim[PITCH] = accTrim_saved[PITCH];
|
||||
}
|
||||
InflightcalibratingA--;
|
||||
}
|
||||
// Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
|
||||
if (AccInflightCalibrationSavetoEEProm == 1) { //the copter is landed, disarmed and the combo has been done again
|
||||
AccInflightCalibrationSavetoEEProm = 0;
|
||||
accZero[ROLL] = b[ROLL] / 50;
|
||||
accZero[PITCH] = b[PITCH] / 50;
|
||||
accZero[YAW] = b[YAW] / 50 - acc_1G; // for nunchuk 200=1G
|
||||
accTrim[ROLL] = 0;
|
||||
accTrim[PITCH] = 0;
|
||||
cfg.accZero[ROLL] = b[ROLL] / 50;
|
||||
cfg.accZero[PITCH] = b[PITCH] / 50;
|
||||
cfg.accZero[YAW] = b[YAW] / 50 - acc_1G; // for nunchuk 200=1G
|
||||
cfg.accTrim[ROLL] = 0;
|
||||
cfg.accTrim[PITCH] = 0;
|
||||
writeParams(); // write accZero in EEPROM
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
accADC[ROLL] -= cfg.accZero[ROLL];
|
||||
accADC[PITCH] -= cfg.accZero[PITCH];
|
||||
accADC[YAW] -= cfg.accZero[YAW];
|
||||
|
|
@ -101,11 +131,9 @@ static void ACC_Common(void)
|
|||
|
||||
void ACC_getADC(void)
|
||||
{
|
||||
int16_t rawADC[3];
|
||||
acc.read(accADC);
|
||||
acc.orient(accADC);
|
||||
|
||||
adxl345Read(rawADC);
|
||||
|
||||
ACC_ORIENTATION(-(rawADC[1]), (rawADC[0]), (rawADC[2]));
|
||||
ACC_Common();
|
||||
}
|
||||
|
||||
|
|
@ -207,15 +235,9 @@ static void GYRO_Common(void)
|
|||
|
||||
void Gyro_getADC(void)
|
||||
{
|
||||
int16_t rawADC[3];
|
||||
|
||||
mpu3050Read(rawADC);
|
||||
|
||||
// range: +/- 8192; +/- 2000 deg/sec
|
||||
GYRO_ORIENTATION(+((rawADC[1]) / 4), -((rawADC[0]) / 4), -((rawADC[2]) / 4));
|
||||
gyroADC[ROLL] = rawADC[0] / 4;
|
||||
gyroADC[PITCH] = rawADC[1] / 4;
|
||||
gyroADC[YAW] = -rawADC[2] / 4;
|
||||
gyro.read(gyroADC);
|
||||
gyro.orient(gyroADC);
|
||||
|
||||
GYRO_Common();
|
||||
}
|
||||
|
|
@ -228,17 +250,10 @@ static void Mag_getRawADC(void)
|
|||
static int16_t rawADC[3];
|
||||
hmc5883lRead(rawADC);
|
||||
|
||||
#if 0
|
||||
// no way? is this finally the proper orientation?? (seems -180 swapped)
|
||||
magADC[ROLL] = -rawADC[2]; // X
|
||||
magADC[PITCH] = rawADC[0]; // Y
|
||||
magADC[YAW] = rawADC[1]; // Z
|
||||
#else
|
||||
// no way? is THIS finally the proper orientation?? (by GrootWitBaas)
|
||||
magADC[ROLL] = rawADC[2]; // X
|
||||
magADC[PITCH] = -rawADC[0]; // Y
|
||||
magADC[YAW] = -rawADC[1]; // Z
|
||||
#endif
|
||||
}
|
||||
|
||||
void Mag_init(void)
|
||||
|
|
@ -271,12 +286,16 @@ void Mag_getADC(void)
|
|||
// Read mag sensor
|
||||
Mag_getRawADC();
|
||||
|
||||
magADC[ROLL] = magADC[ROLL] * magCal[ROLL];
|
||||
magADC[PITCH] = magADC[PITCH] * magCal[PITCH];
|
||||
magADC[YAW] = magADC[YAW] * magCal[YAW];
|
||||
|
||||
if (calibratingM == 1) {
|
||||
tCal = t;
|
||||
for (axis = 0; axis < 3; axis++) {
|
||||
cfg.magZero[axis] = 0;
|
||||
magZeroTempMin[axis] = 0;
|
||||
magZeroTempMax[axis] = 0;
|
||||
magZeroTempMin[axis] = magADC[axis];
|
||||
magZeroTempMax[axis] = magADC[axis];
|
||||
}
|
||||
calibratingM = 0;
|
||||
}
|
||||
|
|
|
|||
17
src/serial.c
17
src/serial.c
|
|
@ -185,7 +185,7 @@ void serialCom(void)
|
|||
|
||||
}
|
||||
serialize16(GPS_distanceToHome);
|
||||
serialize16(GPS_directionToHome);
|
||||
serialize16(GPS_directionToHome + 180);
|
||||
serialize8(GPS_numSat);
|
||||
serialize8(GPS_fix);
|
||||
serialize8(GPS_update);
|
||||
|
|
@ -214,12 +214,19 @@ void serialCom(void)
|
|||
for (i = 0; i < 6; i++) {
|
||||
serialize16(rcData[i]);
|
||||
} //44
|
||||
serialize8(sensors(SENSOR_ACC) << 1 | sensors(SENSOR_BARO) << 2 | sensors(SENSOR_MAG) << 3);
|
||||
serialize8(accMode | baroMode << 1 | magMode << 2);
|
||||
serialize8(sensors(SENSOR_ACC) << 1 | sensors(SENSOR_BARO) << 2 | sensors(SENSOR_MAG) << 3 | sensors(SENSOR_GPS) << 4);
|
||||
serialize8(accMode | baroMode << 1 | magMode << 2 | GPSModeHome << 3 | GPSModeHold << 4 | armed << 5);
|
||||
serialize8(vbat); // Vbatt 47
|
||||
serialize8(VERSION); // MultiWii Firmware version
|
||||
serialize8('O'); //49
|
||||
// UartSendData();
|
||||
serialize8(GPS_fix); // Fix indicator for OSD
|
||||
serialize8(GPS_numSat);
|
||||
serialize16(GPS_latitude);
|
||||
serialize16(GPS_latitude >> 16);
|
||||
serialize16(GPS_longitude);
|
||||
serialize16(GPS_longitude >> 16);
|
||||
serialize16(GPS_altitude);
|
||||
serialize16(GPS_speed); // Speed for OSD
|
||||
serialize8('O'); // NOT 49 anymore
|
||||
break;
|
||||
case 'R': // reboot to bootloader (oops, apparently this w as used for other trash, fix later)
|
||||
systemReset(true);
|
||||
|
|
|
|||
Loading…
Reference in New Issue