added dynamic mixer stuff
moved around features/sensors setting stuff into config instead of drv_system saving features to eeprom now to avoid stupid added gcc crap for __nop and __dmb added digital servo feature to pwm driver, used for tri or whatever added just plain reboot option in addition to reboot to bootloader git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@93 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
parent
e1afcdca09
commit
13173a30dc
File diff suppressed because it is too large
Load Diff
215
baseflight.uvopt
215
baseflight.uvopt
|
@ -112,7 +112,7 @@
|
||||||
<tRbox>1</tRbox>
|
<tRbox>1</tRbox>
|
||||||
<sRunDeb>0</sRunDeb>
|
<sRunDeb>0</sRunDeb>
|
||||||
<sLrtime>0</sLrtime>
|
<sLrtime>0</sLrtime>
|
||||||
<nTsel>7</nTsel>
|
<nTsel>1</nTsel>
|
||||||
<sDll></sDll>
|
<sDll></sDll>
|
||||||
<sDllPa></sDllPa>
|
<sDllPa></sDllPa>
|
||||||
<sDlgDll></sDlgDll>
|
<sDlgDll></sDlgDll>
|
||||||
|
@ -123,7 +123,7 @@
|
||||||
<tDlgDll></tDlgDll>
|
<tDlgDll></tDlgDll>
|
||||||
<tDlgPa></tDlgPa>
|
<tDlgPa></tDlgPa>
|
||||||
<tIfile></tIfile>
|
<tIfile></tIfile>
|
||||||
<pMon>Segger\JL2CM3.dll</pMon>
|
<pMon>BIN\UL2CM3.DLL</pMon>
|
||||||
</DebugOpt>
|
</DebugOpt>
|
||||||
<TargetDriverDllRegistry>
|
<TargetDriverDllRegistry>
|
||||||
<SetRegEntry>
|
<SetRegEntry>
|
||||||
|
@ -144,7 +144,7 @@
|
||||||
<SetRegEntry>
|
<SetRegEntry>
|
||||||
<Number>0</Number>
|
<Number>0</Number>
|
||||||
<Key>DLGUARM</Key>
|
<Key>DLGUARM</Key>
|
||||||
<Name>(106=-1,-1,-1,-1,0)(107=-1,-1,-1,-1,0)</Name>
|
<Name>(105=-1,-1,-1,-1,0)(106=-1,-1,-1,-1,0)(107=-1,-1,-1,-1,0)</Name>
|
||||||
</SetRegEntry>
|
</SetRegEntry>
|
||||||
<SetRegEntry>
|
<SetRegEntry>
|
||||||
<Number>0</Number>
|
<Number>0</Number>
|
||||||
|
@ -154,77 +154,9 @@
|
||||||
<SetRegEntry>
|
<SetRegEntry>
|
||||||
<Number>0</Number>
|
<Number>0</Number>
|
||||||
<Key>UL2CM3</Key>
|
<Key>UL2CM3</Key>
|
||||||
<Name>-O14 -S0 -C0 -N00("ARM Cortex-M3") -D00(1BA00477) -L00(4) -FO7 -FD20000000 -FC800 -FN1 -FF0STM32F10x_128 -FS08000000 -FL010000)</Name>
|
<Name>-UV0168AVR -O238 -S8 -C0 -N00("ARM CoreSight SW-DP") -D00(1BA01477) -L00(0) -TO18 -TC10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO7 -FD20000000 -FC800 -FN1 -FF0STM32F10x_128 -FS08000000 -FL010000</Name>
|
||||||
</SetRegEntry>
|
</SetRegEntry>
|
||||||
</TargetDriverDllRegistry>
|
</TargetDriverDllRegistry>
|
||||||
<WatchWindow1>
|
|
||||||
<Ww>
|
|
||||||
<count>0</count>
|
|
||||||
<WinNumber>1</WinNumber>
|
|
||||||
<ItemText>gyroADC,0x0A</ItemText>
|
|
||||||
</Ww>
|
|
||||||
<Ww>
|
|
||||||
<count>1</count>
|
|
||||||
<WinNumber>1</WinNumber>
|
|
||||||
<ItemText>accADC,0x0A</ItemText>
|
|
||||||
</Ww>
|
|
||||||
<Ww>
|
|
||||||
<count>2</count>
|
|
||||||
<WinNumber>1</WinNumber>
|
|
||||||
<ItemText>currentTime,0x0A</ItemText>
|
|
||||||
</Ww>
|
|
||||||
<Ww>
|
|
||||||
<count>3</count>
|
|
||||||
<WinNumber>1</WinNumber>
|
|
||||||
<ItemText>previousTime,0x0A</ItemText>
|
|
||||||
</Ww>
|
|
||||||
<Ww>
|
|
||||||
<count>4</count>
|
|
||||||
<WinNumber>1</WinNumber>
|
|
||||||
<ItemText>cycleTime,0x0A</ItemText>
|
|
||||||
</Ww>
|
|
||||||
<Ww>
|
|
||||||
<count>5</count>
|
|
||||||
<WinNumber>1</WinNumber>
|
|
||||||
<ItemText>eep_entry</ItemText>
|
|
||||||
</Ww>
|
|
||||||
<Ww>
|
|
||||||
<count>6</count>
|
|
||||||
<WinNumber>1</WinNumber>
|
|
||||||
<ItemText>gyroData,0x0A</ItemText>
|
|
||||||
</Ww>
|
|
||||||
<Ww>
|
|
||||||
<count>7</count>
|
|
||||||
<WinNumber>1</WinNumber>
|
|
||||||
<ItemText>magADC,0x0A</ItemText>
|
|
||||||
</Ww>
|
|
||||||
<Ww>
|
|
||||||
<count>8</count>
|
|
||||||
<WinNumber>1</WinNumber>
|
|
||||||
<ItemText>maginv</ItemText>
|
|
||||||
</Ww>
|
|
||||||
<Ww>
|
|
||||||
<count>9</count>
|
|
||||||
<WinNumber>1</WinNumber>
|
|
||||||
<ItemText>magaxis</ItemText>
|
|
||||||
</Ww>
|
|
||||||
<Ww>
|
|
||||||
<count>10</count>
|
|
||||||
<WinNumber>1</WinNumber>
|
|
||||||
<ItemText>rawMagADC,0x0A</ItemText>
|
|
||||||
</Ww>
|
|
||||||
<Ww>
|
|
||||||
<count>11</count>
|
|
||||||
<WinNumber>1</WinNumber>
|
|
||||||
<ItemText>param_b5
|
|
||||||
</ItemText>
|
|
||||||
</Ww>
|
|
||||||
<Ww>
|
|
||||||
<count>12</count>
|
|
||||||
<WinNumber>1</WinNumber>
|
|
||||||
<ItemText>rawADC,0x0A</ItemText>
|
|
||||||
</Ww>
|
|
||||||
</WatchWindow1>
|
|
||||||
<MemoryWindow1>
|
<MemoryWindow1>
|
||||||
<Mm>
|
<Mm>
|
||||||
<WinNumber>1</WinNumber>
|
<WinNumber>1</WinNumber>
|
||||||
|
@ -267,19 +199,6 @@
|
||||||
</DebugFlag>
|
</DebugFlag>
|
||||||
<LintExecutable></LintExecutable>
|
<LintExecutable></LintExecutable>
|
||||||
<LintConfigFile></LintConfigFile>
|
<LintConfigFile></LintConfigFile>
|
||||||
<LogicAnalyzers>
|
|
||||||
<Wi>
|
|
||||||
<IntNumber>0</IntNumber>
|
|
||||||
<FirstString>cycleTime</FirstString>
|
|
||||||
<SecondString>00800000000000000000000000000000E0FFEF40000000000000000000000000000000006379636C6554696D650000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000010000000000000000000000000000000000000000000000000000000000000000000000CC010008</SecondString>
|
|
||||||
</Wi>
|
|
||||||
</LogicAnalyzers>
|
|
||||||
<SystemViewers>
|
|
||||||
<Entry>
|
|
||||||
<Name>System Viewer\I2C2</Name>
|
|
||||||
<WinId>35905</WinId>
|
|
||||||
</Entry>
|
|
||||||
</SystemViewers>
|
|
||||||
</TargetOption>
|
</TargetOption>
|
||||||
</Target>
|
</Target>
|
||||||
|
|
||||||
|
@ -545,10 +464,10 @@
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<Focus>0</Focus>
|
<Focus>0</Focus>
|
||||||
<ColumnNumber>32</ColumnNumber>
|
<ColumnNumber>37</ColumnNumber>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
<TopLine>1</TopLine>
|
<TopLine>1</TopLine>
|
||||||
<CurrentLine>23</CurrentLine>
|
<CurrentLine>26</CurrentLine>
|
||||||
<bDave2>0</bDave2>
|
<bDave2>0</bDave2>
|
||||||
<PathWithFileName>.\main.c</PathWithFileName>
|
<PathWithFileName>.\main.c</PathWithFileName>
|
||||||
<FilenameWithoutPath>main.c</FilenameWithoutPath>
|
<FilenameWithoutPath>main.c</FilenameWithoutPath>
|
||||||
|
@ -559,10 +478,10 @@
|
||||||
<FileType>5</FileType>
|
<FileType>5</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<Focus>0</Focus>
|
<Focus>0</Focus>
|
||||||
<ColumnNumber>14</ColumnNumber>
|
<ColumnNumber>25</ColumnNumber>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
<TopLine>22</TopLine>
|
<TopLine>0</TopLine>
|
||||||
<CurrentLine>28</CurrentLine>
|
<CurrentLine>0</CurrentLine>
|
||||||
<bDave2>0</bDave2>
|
<bDave2>0</bDave2>
|
||||||
<PathWithFileName>.\board.h</PathWithFileName>
|
<PathWithFileName>.\board.h</PathWithFileName>
|
||||||
<FilenameWithoutPath>board.h</FilenameWithoutPath>
|
<FilenameWithoutPath>board.h</FilenameWithoutPath>
|
||||||
|
@ -573,10 +492,10 @@
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<Focus>0</Focus>
|
<Focus>0</Focus>
|
||||||
<ColumnNumber>0</ColumnNumber>
|
<ColumnNumber>26</ColumnNumber>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
<TopLine>27</TopLine>
|
<TopLine>0</TopLine>
|
||||||
<CurrentLine>27</CurrentLine>
|
<CurrentLine>0</CurrentLine>
|
||||||
<bDave2>0</bDave2>
|
<bDave2>0</bDave2>
|
||||||
<PathWithFileName>.\imu.c</PathWithFileName>
|
<PathWithFileName>.\imu.c</PathWithFileName>
|
||||||
<FilenameWithoutPath>imu.c</FilenameWithoutPath>
|
<FilenameWithoutPath>imu.c</FilenameWithoutPath>
|
||||||
|
@ -589,8 +508,8 @@
|
||||||
<Focus>0</Focus>
|
<Focus>0</Focus>
|
||||||
<ColumnNumber>0</ColumnNumber>
|
<ColumnNumber>0</ColumnNumber>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
<TopLine>624</TopLine>
|
<TopLine>0</TopLine>
|
||||||
<CurrentLine>631</CurrentLine>
|
<CurrentLine>0</CurrentLine>
|
||||||
<bDave2>0</bDave2>
|
<bDave2>0</bDave2>
|
||||||
<PathWithFileName>.\mw.c</PathWithFileName>
|
<PathWithFileName>.\mw.c</PathWithFileName>
|
||||||
<FilenameWithoutPath>mw.c</FilenameWithoutPath>
|
<FilenameWithoutPath>mw.c</FilenameWithoutPath>
|
||||||
|
@ -601,10 +520,10 @@
|
||||||
<FileType>5</FileType>
|
<FileType>5</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<Focus>0</Focus>
|
<Focus>0</Focus>
|
||||||
<ColumnNumber>0</ColumnNumber>
|
<ColumnNumber>27</ColumnNumber>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
<TopLine>154</TopLine>
|
<TopLine>115</TopLine>
|
||||||
<CurrentLine>167</CurrentLine>
|
<CurrentLine>124</CurrentLine>
|
||||||
<bDave2>0</bDave2>
|
<bDave2>0</bDave2>
|
||||||
<PathWithFileName>.\mw.h</PathWithFileName>
|
<PathWithFileName>.\mw.h</PathWithFileName>
|
||||||
<FilenameWithoutPath>mw.h</FilenameWithoutPath>
|
<FilenameWithoutPath>mw.h</FilenameWithoutPath>
|
||||||
|
@ -617,8 +536,8 @@
|
||||||
<Focus>0</Focus>
|
<Focus>0</Focus>
|
||||||
<ColumnNumber>0</ColumnNumber>
|
<ColumnNumber>0</ColumnNumber>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
<TopLine>187</TopLine>
|
<TopLine>0</TopLine>
|
||||||
<CurrentLine>212</CurrentLine>
|
<CurrentLine>0</CurrentLine>
|
||||||
<bDave2>0</bDave2>
|
<bDave2>0</bDave2>
|
||||||
<PathWithFileName>.\sensors.c</PathWithFileName>
|
<PathWithFileName>.\sensors.c</PathWithFileName>
|
||||||
<FilenameWithoutPath>sensors.c</FilenameWithoutPath>
|
<FilenameWithoutPath>sensors.c</FilenameWithoutPath>
|
||||||
|
@ -629,10 +548,10 @@
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<Focus>0</Focus>
|
<Focus>0</Focus>
|
||||||
<ColumnNumber>26</ColumnNumber>
|
<ColumnNumber>0</ColumnNumber>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
<TopLine>0</TopLine>
|
<TopLine>1</TopLine>
|
||||||
<CurrentLine>0</CurrentLine>
|
<CurrentLine>11</CurrentLine>
|
||||||
<bDave2>0</bDave2>
|
<bDave2>0</bDave2>
|
||||||
<PathWithFileName>.\mixer.c</PathWithFileName>
|
<PathWithFileName>.\mixer.c</PathWithFileName>
|
||||||
<FilenameWithoutPath>mixer.c</FilenameWithoutPath>
|
<FilenameWithoutPath>mixer.c</FilenameWithoutPath>
|
||||||
|
@ -643,10 +562,10 @@
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<Focus>0</Focus>
|
<Focus>0</Focus>
|
||||||
<ColumnNumber>12</ColumnNumber>
|
<ColumnNumber>26</ColumnNumber>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
<TopLine>162</TopLine>
|
<TopLine>200</TopLine>
|
||||||
<CurrentLine>181</CurrentLine>
|
<CurrentLine>215</CurrentLine>
|
||||||
<bDave2>0</bDave2>
|
<bDave2>0</bDave2>
|
||||||
<PathWithFileName>.\serial.c</PathWithFileName>
|
<PathWithFileName>.\serial.c</PathWithFileName>
|
||||||
<FilenameWithoutPath>serial.c</FilenameWithoutPath>
|
<FilenameWithoutPath>serial.c</FilenameWithoutPath>
|
||||||
|
@ -657,10 +576,10 @@
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<Focus>0</Focus>
|
<Focus>0</Focus>
|
||||||
<ColumnNumber>0</ColumnNumber>
|
<ColumnNumber>4</ColumnNumber>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
<TopLine>0</TopLine>
|
<TopLine>107</TopLine>
|
||||||
<CurrentLine>0</CurrentLine>
|
<CurrentLine>120</CurrentLine>
|
||||||
<bDave2>0</bDave2>
|
<bDave2>0</bDave2>
|
||||||
<PathWithFileName>.\config.c</PathWithFileName>
|
<PathWithFileName>.\config.c</PathWithFileName>
|
||||||
<FilenameWithoutPath>config.c</FilenameWithoutPath>
|
<FilenameWithoutPath>config.c</FilenameWithoutPath>
|
||||||
|
@ -680,8 +599,8 @@
|
||||||
<Focus>0</Focus>
|
<Focus>0</Focus>
|
||||||
<ColumnNumber>0</ColumnNumber>
|
<ColumnNumber>0</ColumnNumber>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
<TopLine>283</TopLine>
|
<TopLine>0</TopLine>
|
||||||
<CurrentLine>300</CurrentLine>
|
<CurrentLine>0</CurrentLine>
|
||||||
<bDave2>0</bDave2>
|
<bDave2>0</bDave2>
|
||||||
<PathWithFileName>.\drv_i2c.c</PathWithFileName>
|
<PathWithFileName>.\drv_i2c.c</PathWithFileName>
|
||||||
<FilenameWithoutPath>drv_i2c.c</FilenameWithoutPath>
|
<FilenameWithoutPath>drv_i2c.c</FilenameWithoutPath>
|
||||||
|
@ -694,8 +613,8 @@
|
||||||
<Focus>0</Focus>
|
<Focus>0</Focus>
|
||||||
<ColumnNumber>0</ColumnNumber>
|
<ColumnNumber>0</ColumnNumber>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
<TopLine>1</TopLine>
|
<TopLine>0</TopLine>
|
||||||
<CurrentLine>1</CurrentLine>
|
<CurrentLine>0</CurrentLine>
|
||||||
<bDave2>0</bDave2>
|
<bDave2>0</bDave2>
|
||||||
<PathWithFileName>.\drv_pwm.c</PathWithFileName>
|
<PathWithFileName>.\drv_pwm.c</PathWithFileName>
|
||||||
<FilenameWithoutPath>drv_pwm.c</FilenameWithoutPath>
|
<FilenameWithoutPath>drv_pwm.c</FilenameWithoutPath>
|
||||||
|
@ -708,8 +627,8 @@
|
||||||
<Focus>0</Focus>
|
<Focus>0</Focus>
|
||||||
<ColumnNumber>6</ColumnNumber>
|
<ColumnNumber>6</ColumnNumber>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
<TopLine>18</TopLine>
|
<TopLine>0</TopLine>
|
||||||
<CurrentLine>18</CurrentLine>
|
<CurrentLine>0</CurrentLine>
|
||||||
<bDave2>0</bDave2>
|
<bDave2>0</bDave2>
|
||||||
<PathWithFileName>.\drv_adc.c</PathWithFileName>
|
<PathWithFileName>.\drv_adc.c</PathWithFileName>
|
||||||
<FilenameWithoutPath>drv_adc.c</FilenameWithoutPath>
|
<FilenameWithoutPath>drv_adc.c</FilenameWithoutPath>
|
||||||
|
@ -720,10 +639,10 @@
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<Focus>0</Focus>
|
<Focus>0</Focus>
|
||||||
<ColumnNumber>0</ColumnNumber>
|
<ColumnNumber>8</ColumnNumber>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
<TopLine>213</TopLine>
|
<TopLine>0</TopLine>
|
||||||
<CurrentLine>232</CurrentLine>
|
<CurrentLine>0</CurrentLine>
|
||||||
<bDave2>0</bDave2>
|
<bDave2>0</bDave2>
|
||||||
<PathWithFileName>.\drv_bmp085.c</PathWithFileName>
|
<PathWithFileName>.\drv_bmp085.c</PathWithFileName>
|
||||||
<FilenameWithoutPath>drv_bmp085.c</FilenameWithoutPath>
|
<FilenameWithoutPath>drv_bmp085.c</FilenameWithoutPath>
|
||||||
|
@ -736,8 +655,8 @@
|
||||||
<Focus>0</Focus>
|
<Focus>0</Focus>
|
||||||
<ColumnNumber>0</ColumnNumber>
|
<ColumnNumber>0</ColumnNumber>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
<TopLine>32</TopLine>
|
<TopLine>0</TopLine>
|
||||||
<CurrentLine>56</CurrentLine>
|
<CurrentLine>0</CurrentLine>
|
||||||
<bDave2>0</bDave2>
|
<bDave2>0</bDave2>
|
||||||
<PathWithFileName>.\drv_adxl345.c</PathWithFileName>
|
<PathWithFileName>.\drv_adxl345.c</PathWithFileName>
|
||||||
<FilenameWithoutPath>drv_adxl345.c</FilenameWithoutPath>
|
<FilenameWithoutPath>drv_adxl345.c</FilenameWithoutPath>
|
||||||
|
@ -750,8 +669,8 @@
|
||||||
<Focus>0</Focus>
|
<Focus>0</Focus>
|
||||||
<ColumnNumber>13</ColumnNumber>
|
<ColumnNumber>13</ColumnNumber>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
<TopLine>13</TopLine>
|
<TopLine>0</TopLine>
|
||||||
<CurrentLine>28</CurrentLine>
|
<CurrentLine>0</CurrentLine>
|
||||||
<bDave2>0</bDave2>
|
<bDave2>0</bDave2>
|
||||||
<PathWithFileName>.\drv_mpu3050.c</PathWithFileName>
|
<PathWithFileName>.\drv_mpu3050.c</PathWithFileName>
|
||||||
<FilenameWithoutPath>drv_mpu3050.c</FilenameWithoutPath>
|
<FilenameWithoutPath>drv_mpu3050.c</FilenameWithoutPath>
|
||||||
|
@ -774,12 +693,12 @@
|
||||||
<GroupNumber>2</GroupNumber>
|
<GroupNumber>2</GroupNumber>
|
||||||
<FileNumber>17</FileNumber>
|
<FileNumber>17</FileNumber>
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>1</tvExp>
|
||||||
<Focus>0</Focus>
|
<Focus>0</Focus>
|
||||||
<ColumnNumber>115</ColumnNumber>
|
<ColumnNumber>22</ColumnNumber>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
<TopLine>55</TopLine>
|
<TopLine>139</TopLine>
|
||||||
<CurrentLine>60</CurrentLine>
|
<CurrentLine>169</CurrentLine>
|
||||||
<bDave2>0</bDave2>
|
<bDave2>0</bDave2>
|
||||||
<PathWithFileName>.\drv_system.c</PathWithFileName>
|
<PathWithFileName>.\drv_system.c</PathWithFileName>
|
||||||
<FilenameWithoutPath>drv_system.c</FilenameWithoutPath>
|
<FilenameWithoutPath>drv_system.c</FilenameWithoutPath>
|
||||||
|
@ -792,8 +711,8 @@
|
||||||
<Focus>0</Focus>
|
<Focus>0</Focus>
|
||||||
<ColumnNumber>42</ColumnNumber>
|
<ColumnNumber>42</ColumnNumber>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
<TopLine>20</TopLine>
|
<TopLine>0</TopLine>
|
||||||
<CurrentLine>45</CurrentLine>
|
<CurrentLine>0</CurrentLine>
|
||||||
<bDave2>0</bDave2>
|
<bDave2>0</bDave2>
|
||||||
<PathWithFileName>.\drv_hmc5883l.c</PathWithFileName>
|
<PathWithFileName>.\drv_hmc5883l.c</PathWithFileName>
|
||||||
<FilenameWithoutPath>drv_hmc5883l.c</FilenameWithoutPath>
|
<FilenameWithoutPath>drv_hmc5883l.c</FilenameWithoutPath>
|
||||||
|
@ -835,7 +754,7 @@
|
||||||
</File>
|
</File>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>3</GroupNumber>
|
<GroupNumber>3</GroupNumber>
|
||||||
<FileNumber>22</FileNumber>
|
<FileNumber>21</FileNumber>
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<Focus>0</Focus>
|
<Focus>0</Focus>
|
||||||
|
@ -849,7 +768,7 @@
|
||||||
</File>
|
</File>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>3</GroupNumber>
|
<GroupNumber>3</GroupNumber>
|
||||||
<FileNumber>23</FileNumber>
|
<FileNumber>22</FileNumber>
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<Focus>0</Focus>
|
<Focus>0</Focus>
|
||||||
|
@ -863,11 +782,11 @@
|
||||||
</File>
|
</File>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>3</GroupNumber>
|
<GroupNumber>3</GroupNumber>
|
||||||
<FileNumber>24</FileNumber>
|
<FileNumber>23</FileNumber>
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<Focus>0</Focus>
|
<Focus>0</Focus>
|
||||||
<ColumnNumber>26</ColumnNumber>
|
<ColumnNumber>0</ColumnNumber>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
<TopLine>0</TopLine>
|
<TopLine>0</TopLine>
|
||||||
<CurrentLine>0</CurrentLine>
|
<CurrentLine>0</CurrentLine>
|
||||||
|
@ -877,7 +796,7 @@
|
||||||
</File>
|
</File>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>3</GroupNumber>
|
<GroupNumber>3</GroupNumber>
|
||||||
<FileNumber>25</FileNumber>
|
<FileNumber>24</FileNumber>
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<Focus>0</Focus>
|
<Focus>0</Focus>
|
||||||
|
@ -891,11 +810,11 @@
|
||||||
</File>
|
</File>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>3</GroupNumber>
|
<GroupNumber>3</GroupNumber>
|
||||||
<FileNumber>26</FileNumber>
|
<FileNumber>25</FileNumber>
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<Focus>0</Focus>
|
<Focus>0</Focus>
|
||||||
<ColumnNumber>16</ColumnNumber>
|
<ColumnNumber>0</ColumnNumber>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
<TopLine>0</TopLine>
|
<TopLine>0</TopLine>
|
||||||
<CurrentLine>0</CurrentLine>
|
<CurrentLine>0</CurrentLine>
|
||||||
|
@ -905,21 +824,21 @@
|
||||||
</File>
|
</File>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>3</GroupNumber>
|
<GroupNumber>3</GroupNumber>
|
||||||
<FileNumber>27</FileNumber>
|
<FileNumber>26</FileNumber>
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<Focus>0</Focus>
|
<Focus>0</Focus>
|
||||||
<ColumnNumber>34</ColumnNumber>
|
<ColumnNumber>34</ColumnNumber>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
<TopLine>1095</TopLine>
|
<TopLine>0</TopLine>
|
||||||
<CurrentLine>1102</CurrentLine>
|
<CurrentLine>0</CurrentLine>
|
||||||
<bDave2>0</bDave2>
|
<bDave2>0</bDave2>
|
||||||
<PathWithFileName>.\lib\STM32F10x_StdPeriph_Driver\src\stm32f10x_rcc.c</PathWithFileName>
|
<PathWithFileName>.\lib\STM32F10x_StdPeriph_Driver\src\stm32f10x_rcc.c</PathWithFileName>
|
||||||
<FilenameWithoutPath>stm32f10x_rcc.c</FilenameWithoutPath>
|
<FilenameWithoutPath>stm32f10x_rcc.c</FilenameWithoutPath>
|
||||||
</File>
|
</File>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>3</GroupNumber>
|
<GroupNumber>3</GroupNumber>
|
||||||
<FileNumber>28</FileNumber>
|
<FileNumber>27</FileNumber>
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<Focus>0</Focus>
|
<Focus>0</Focus>
|
||||||
|
@ -933,7 +852,7 @@
|
||||||
</File>
|
</File>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>3</GroupNumber>
|
<GroupNumber>3</GroupNumber>
|
||||||
<FileNumber>29</FileNumber>
|
<FileNumber>28</FileNumber>
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<Focus>0</Focus>
|
<Focus>0</Focus>
|
||||||
|
@ -947,21 +866,21 @@
|
||||||
</File>
|
</File>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>3</GroupNumber>
|
<GroupNumber>3</GroupNumber>
|
||||||
<FileNumber>30</FileNumber>
|
<FileNumber>29</FileNumber>
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<Focus>0</Focus>
|
<Focus>0</Focus>
|
||||||
<ColumnNumber>0</ColumnNumber>
|
<ColumnNumber>0</ColumnNumber>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
<TopLine>576</TopLine>
|
<TopLine>0</TopLine>
|
||||||
<CurrentLine>591</CurrentLine>
|
<CurrentLine>0</CurrentLine>
|
||||||
<bDave2>0</bDave2>
|
<bDave2>0</bDave2>
|
||||||
<PathWithFileName>.\lib\STM32F10x_StdPeriph_Driver\src\stm32f10x_gpio.c</PathWithFileName>
|
<PathWithFileName>.\lib\STM32F10x_StdPeriph_Driver\src\stm32f10x_gpio.c</PathWithFileName>
|
||||||
<FilenameWithoutPath>stm32f10x_gpio.c</FilenameWithoutPath>
|
<FilenameWithoutPath>stm32f10x_gpio.c</FilenameWithoutPath>
|
||||||
</File>
|
</File>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>3</GroupNumber>
|
<GroupNumber>3</GroupNumber>
|
||||||
<FileNumber>31</FileNumber>
|
<FileNumber>30</FileNumber>
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<Focus>0</Focus>
|
<Focus>0</Focus>
|
||||||
|
@ -975,14 +894,14 @@
|
||||||
</File>
|
</File>
|
||||||
<File>
|
<File>
|
||||||
<GroupNumber>3</GroupNumber>
|
<GroupNumber>3</GroupNumber>
|
||||||
<FileNumber>0</FileNumber>
|
<FileNumber>31</FileNumber>
|
||||||
<FileType>2</FileType>
|
<FileType>2</FileType>
|
||||||
<tvExp>0</tvExp>
|
<tvExp>0</tvExp>
|
||||||
<Focus>0</Focus>
|
<Focus>0</Focus>
|
||||||
<ColumnNumber>21</ColumnNumber>
|
<ColumnNumber>0</ColumnNumber>
|
||||||
<tvExpOptDlg>0</tvExpOptDlg>
|
<tvExpOptDlg>0</tvExpOptDlg>
|
||||||
<TopLine>133</TopLine>
|
<TopLine>0</TopLine>
|
||||||
<CurrentLine>143</CurrentLine>
|
<CurrentLine>0</CurrentLine>
|
||||||
<bDave2>0</bDave2>
|
<bDave2>0</bDave2>
|
||||||
<PathWithFileName>.\startup_stm32f10x_md.s</PathWithFileName>
|
<PathWithFileName>.\startup_stm32f10x_md.s</PathWithFileName>
|
||||||
<FilenameWithoutPath>startup_stm32f10x_md.s</FilenameWithoutPath>
|
<FilenameWithoutPath>startup_stm32f10x_md.s</FilenameWithoutPath>
|
||||||
|
|
|
@ -136,7 +136,7 @@
|
||||||
<RestoreToolbox>1</RestoreToolbox>
|
<RestoreToolbox>1</RestoreToolbox>
|
||||||
</Target>
|
</Target>
|
||||||
<RunDebugAfterBuild>0</RunDebugAfterBuild>
|
<RunDebugAfterBuild>0</RunDebugAfterBuild>
|
||||||
<TargetSelection>7</TargetSelection>
|
<TargetSelection>1</TargetSelection>
|
||||||
<SimDlls>
|
<SimDlls>
|
||||||
<CpuDll></CpuDll>
|
<CpuDll></CpuDll>
|
||||||
<CpuDllArguments></CpuDllArguments>
|
<CpuDllArguments></CpuDllArguments>
|
||||||
|
@ -150,7 +150,7 @@
|
||||||
<PeripheralDll></PeripheralDll>
|
<PeripheralDll></PeripheralDll>
|
||||||
<PeripheralDllArguments></PeripheralDllArguments>
|
<PeripheralDllArguments></PeripheralDllArguments>
|
||||||
<InitializationFile></InitializationFile>
|
<InitializationFile></InitializationFile>
|
||||||
<Driver>Segger\JL2CM3.dll</Driver>
|
<Driver>BIN\UL2CM3.DLL</Driver>
|
||||||
</TargetDlls>
|
</TargetDlls>
|
||||||
</DebugOption>
|
</DebugOption>
|
||||||
<Utilities>
|
<Utilities>
|
||||||
|
@ -160,9 +160,9 @@
|
||||||
<RunIndependent>0</RunIndependent>
|
<RunIndependent>0</RunIndependent>
|
||||||
<UpdateFlashBeforeDebugging>1</UpdateFlashBeforeDebugging>
|
<UpdateFlashBeforeDebugging>1</UpdateFlashBeforeDebugging>
|
||||||
<Capability>1</Capability>
|
<Capability>1</Capability>
|
||||||
<DriverSelection>4099</DriverSelection>
|
<DriverSelection>4096</DriverSelection>
|
||||||
</Flash1>
|
</Flash1>
|
||||||
<Flash2>Segger\JL2CM3.dll</Flash2>
|
<Flash2>BIN\UL2CM3.DLL</Flash2>
|
||||||
<Flash3>"" ()</Flash3>
|
<Flash3>"" ()</Flash3>
|
||||||
<Flash4></Flash4>
|
<Flash4></Flash4>
|
||||||
</Utilities>
|
</Utilities>
|
||||||
|
@ -331,7 +331,7 @@
|
||||||
</ArmAdsMisc>
|
</ArmAdsMisc>
|
||||||
<Cads>
|
<Cads>
|
||||||
<interw>1</interw>
|
<interw>1</interw>
|
||||||
<Optim>1</Optim>
|
<Optim>4</Optim>
|
||||||
<oTime>0</oTime>
|
<oTime>0</oTime>
|
||||||
<SplitLS>0</SplitLS>
|
<SplitLS>0</SplitLS>
|
||||||
<OneElfS>0</OneElfS>
|
<OneElfS>0</OneElfS>
|
||||||
|
@ -390,45 +390,6 @@
|
||||||
<FileName>main.c</FileName>
|
<FileName>main.c</FileName>
|
||||||
<FileType>1</FileType>
|
<FileType>1</FileType>
|
||||||
<FilePath>.\main.c</FilePath>
|
<FilePath>.\main.c</FilePath>
|
||||||
<FileOption>
|
|
||||||
<CommonProperty>
|
|
||||||
<UseCPPCompiler>2</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>
|
|
||||||
<FileArmAds>
|
|
||||||
<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>
|
|
||||||
</FileArmAds>
|
|
||||||
</FileOption>
|
|
||||||
</File>
|
</File>
|
||||||
<File>
|
<File>
|
||||||
<FileName>board.h</FileName>
|
<FileName>board.h</FileName>
|
||||||
|
|
2
board.h
2
board.h
|
@ -28,6 +28,8 @@ typedef enum {
|
||||||
typedef enum {
|
typedef enum {
|
||||||
FEATURE_PPM = 1 << 0,
|
FEATURE_PPM = 1 << 0,
|
||||||
FEATURE_VBAT = 1 << 1,
|
FEATURE_VBAT = 1 << 1,
|
||||||
|
FEATURE_SERVO = 1 << 2,
|
||||||
|
FEATURE_DIGITAL_SERVO = 1 << 3,
|
||||||
} AvailableFeatures;
|
} AvailableFeatures;
|
||||||
|
|
||||||
#define digitalHi(p, i) { p->BSRR = i; }
|
#define digitalHi(p, i) { p->BSRR = i; }
|
||||||
|
|
85
config.c
85
config.c
|
@ -5,7 +5,10 @@
|
||||||
#define FLASH_PAGE_SIZE ((uint16_t)0x400)
|
#define FLASH_PAGE_SIZE ((uint16_t)0x400)
|
||||||
#define FLASH_WRITE_ADDR (0x08000000 + (uint32_t)FLASH_PAGE_SIZE * 63) // use the last KB for storage
|
#define FLASH_WRITE_ADDR (0x08000000 + (uint32_t)FLASH_PAGE_SIZE * 63) // use the last KB for storage
|
||||||
|
|
||||||
static uint8_t checkNewConf = 151;
|
static uint32_t enabledSensors = 0;
|
||||||
|
static uint32_t enabledFeatures = 0;
|
||||||
|
|
||||||
|
static uint8_t checkNewConf = 152;
|
||||||
|
|
||||||
typedef struct eep_entry_t {
|
typedef struct eep_entry_t {
|
||||||
void *var;
|
void *var;
|
||||||
|
@ -17,6 +20,8 @@ typedef struct eep_entry_t {
|
||||||
// ************************************************************************************************************
|
// ************************************************************************************************************
|
||||||
volatile eep_entry_t eep_entry[] = {
|
volatile eep_entry_t eep_entry[] = {
|
||||||
{&checkNewConf, sizeof(checkNewConf)}
|
{&checkNewConf, sizeof(checkNewConf)}
|
||||||
|
, {&enabledFeatures, sizeof(enabledFeatures)}
|
||||||
|
, {&mixerConfiguration, sizeof(mixerConfiguration)}
|
||||||
, {&P8, sizeof(P8)}
|
, {&P8, sizeof(P8)}
|
||||||
, {&I8, sizeof(I8)}
|
, {&I8, sizeof(I8)}
|
||||||
, {&D8, sizeof(D8)}
|
, {&D8, sizeof(D8)}
|
||||||
|
@ -31,14 +36,9 @@ volatile eep_entry_t eep_entry[] = {
|
||||||
, {&activate1, sizeof(activate1)}
|
, {&activate1, sizeof(activate1)}
|
||||||
, {&activate2, sizeof(activate2)}
|
, {&activate2, sizeof(activate2)}
|
||||||
, {&powerTrigger1, sizeof(powerTrigger1)}
|
, {&powerTrigger1, sizeof(powerTrigger1)}
|
||||||
#ifdef FLYING_WING
|
|
||||||
, {&wing_left_mid, sizeof(wing_left_mid)}
|
, {&wing_left_mid, sizeof(wing_left_mid)}
|
||||||
, {&wing_right_mid, sizeof(wing_right_mid)}
|
, {&wing_right_mid, sizeof(wing_right_mid)}
|
||||||
#endif
|
|
||||||
#ifdef TRI
|
|
||||||
, {&tri_yaw_middle, sizeof(tri_yaw_middle)}
|
, {&tri_yaw_middle, sizeof(tri_yaw_middle)}
|
||||||
#endif
|
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
#define EEBLOCK_SIZE sizeof(eep_entry) / sizeof(eep_entry_t)
|
#define EEBLOCK_SIZE sizeof(eep_entry) / sizeof(eep_entry_t)
|
||||||
|
@ -56,15 +56,20 @@ void readEEPROM(void)
|
||||||
#if defined(POWERMETER)
|
#if defined(POWERMETER)
|
||||||
pAlarm = (uint32_t) powerTrigger1 *(uint32_t) PLEVELSCALE *(uint32_t) PLEVELDIV; // need to cast before multiplying
|
pAlarm = (uint32_t) powerTrigger1 *(uint32_t) PLEVELSCALE *(uint32_t) PLEVELDIV; // need to cast before multiplying
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
for (i = 0; i < 7; i++)
|
for (i = 0; i < 7; i++)
|
||||||
lookupRX[i] = (2500 + rcExpo8 * (i * i - 25)) * i * (int32_t) rcRate8 / 1250;
|
lookupRX[i] = (2500 + rcExpo8 * (i * i - 25)) * i * (int32_t) rcRate8 / 1250;
|
||||||
#ifdef FLYING_WING
|
|
||||||
wing_left_mid = constrain(wing_left_mid, WING_LEFT_MIN, WING_LEFT_MAX); //LEFT
|
switch (mixerConfiguration) {
|
||||||
wing_right_mid = constrain(wing_right_mid, WING_RIGHT_MIN, WING_RIGHT_MAX); //RIGHT
|
case MULTITYPE_FLYING_WING:
|
||||||
#endif
|
wing_left_mid = constrain(wing_left_mid, WING_LEFT_MIN, WING_LEFT_MAX); //LEFT
|
||||||
#ifdef TRI
|
wing_right_mid = constrain(wing_right_mid, WING_RIGHT_MIN, WING_RIGHT_MAX); //RIGHT
|
||||||
tri_yaw_middle = constrain(tri_yaw_middle, TRI_YAW_CONSTRAINT_MIN, TRI_YAW_CONSTRAINT_MAX); //REAR
|
break;
|
||||||
#endif
|
|
||||||
|
case MULTITYPE_TRI:
|
||||||
|
tri_yaw_middle = constrain(tri_yaw_middle, TRI_YAW_CONSTRAINT_MIN, TRI_YAW_CONSTRAINT_MAX); //REAR
|
||||||
|
break;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void writeParams(void)
|
void writeParams(void)
|
||||||
|
@ -80,7 +85,7 @@ void writeParams(void)
|
||||||
memcpy(p, eep_entry[i].var, eep_entry[i].size);
|
memcpy(p, eep_entry[i].var, eep_entry[i].size);
|
||||||
p += eep_entry[i].size;
|
p += eep_entry[i].size;
|
||||||
}
|
}
|
||||||
|
|
||||||
p = conf;
|
p = conf;
|
||||||
|
|
||||||
FLASH_Unlock();
|
FLASH_Unlock();
|
||||||
|
@ -112,6 +117,9 @@ void checkFirstTime(void)
|
||||||
return;
|
return;
|
||||||
|
|
||||||
// Default settings
|
// Default settings
|
||||||
|
mixerConfiguration = MULTITYPE_QUADX;
|
||||||
|
featureSet(FEATURE_VBAT | FEATURE_PPM);
|
||||||
|
|
||||||
P8[ROLL] = 40;
|
P8[ROLL] = 40;
|
||||||
I8[ROLL] = 30;
|
I8[ROLL] = 30;
|
||||||
D8[ROLL] = 23;
|
D8[ROLL] = 23;
|
||||||
|
@ -146,12 +154,47 @@ void checkFirstTime(void)
|
||||||
accTrim[0] = 0;
|
accTrim[0] = 0;
|
||||||
accTrim[1] = 0;
|
accTrim[1] = 0;
|
||||||
powerTrigger1 = 0;
|
powerTrigger1 = 0;
|
||||||
#ifdef FLYING_WING
|
|
||||||
wing_left_mid = WING_LEFT_MID;
|
switch (mixerConfiguration) {
|
||||||
wing_right_mid = WING_RIGHT_MID;
|
case MULTITYPE_FLYING_WING:
|
||||||
#endif
|
wing_left_mid = WING_LEFT_MID;
|
||||||
#ifdef TRI
|
wing_right_mid = WING_RIGHT_MID;
|
||||||
tri_yaw_middle = TRI_YAW_MIDDLE;
|
break;
|
||||||
#endif
|
|
||||||
|
case MULTITYPE_TRI:
|
||||||
|
tri_yaw_middle = TRI_YAW_MIDDLE;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
writeParams();
|
writeParams();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool sensors(uint32_t mask)
|
||||||
|
{
|
||||||
|
return enabledSensors & mask;
|
||||||
|
}
|
||||||
|
|
||||||
|
void sensorsSet(uint32_t mask)
|
||||||
|
{
|
||||||
|
enabledSensors |= mask;
|
||||||
|
}
|
||||||
|
|
||||||
|
void sensorsClear(uint32_t mask)
|
||||||
|
{
|
||||||
|
enabledSensors &= ~(mask);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool feature(uint32_t mask)
|
||||||
|
{
|
||||||
|
return enabledFeatures & mask;
|
||||||
|
}
|
||||||
|
|
||||||
|
void featureSet(uint32_t mask)
|
||||||
|
{
|
||||||
|
enabledFeatures |= mask;
|
||||||
|
}
|
||||||
|
|
||||||
|
void featureClear(uint32_t mask)
|
||||||
|
{
|
||||||
|
enabledFeatures &= ~(mask);
|
||||||
|
}
|
||||||
|
|
|
@ -224,6 +224,10 @@ void bmp085_start_ut(void)
|
||||||
i2cWrite(p_bmp085->dev_addr, BMP085_CTRL_MEAS_REG, BMP085_T_MEASURE);
|
i2cWrite(p_bmp085->dev_addr, BMP085_CTRL_MEAS_REG, BMP085_T_MEASURE);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#ifdef __GNUC__
|
||||||
|
#define __nop() asm("mov r0,r0");
|
||||||
|
#endif
|
||||||
|
|
||||||
uint16_t bmp085_get_ut(void)
|
uint16_t bmp085_get_ut(void)
|
||||||
{
|
{
|
||||||
uint16_t ut;
|
uint16_t ut;
|
||||||
|
|
12
drv_i2c.c
12
drv_i2c.c
|
@ -4,10 +4,8 @@
|
||||||
// SCL PB10
|
// SCL PB10
|
||||||
// SDA PB11
|
// SDA PB11
|
||||||
|
|
||||||
#ifdef __gnuc__ // TODO check this
|
#ifdef __GNUC__ // TODO check this
|
||||||
#define DMB() asm volatile ("dmb":::"memory")
|
#define __DMB() asm volatile ("dmb":::"memory")
|
||||||
#else
|
|
||||||
#define DMB() __DMB()
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
static I2C_TypeDef *I2Cx;
|
static I2C_TypeDef *I2Cx;
|
||||||
|
@ -168,17 +166,17 @@ void i2c_ev_handler(void)
|
||||||
} else if (SReg_1 & 0x0002) { //we just sent the address - EV6 in ref manual
|
} else if (SReg_1 & 0x0002) { //we just sent the address - EV6 in ref manual
|
||||||
//Read SR1,2 to clear ADDR
|
//Read SR1,2 to clear ADDR
|
||||||
volatile uint8_t a;
|
volatile uint8_t a;
|
||||||
DMB(); // memory fence to control hardware
|
__DMB(); // memory fence to control hardware
|
||||||
if (bytes == 1 && reading && subaddress_sent) { //we are receiving 1 byte - EV6_3
|
if (bytes == 1 && reading && subaddress_sent) { //we are receiving 1 byte - EV6_3
|
||||||
I2C_AcknowledgeConfig(I2Cx, DISABLE); //turn off ACK
|
I2C_AcknowledgeConfig(I2Cx, DISABLE); //turn off ACK
|
||||||
DMB();
|
__DMB();
|
||||||
a = I2Cx->SR2; //clear ADDR after ACK is turned off
|
a = I2Cx->SR2; //clear ADDR after ACK is turned off
|
||||||
I2C_GenerateSTOP(I2Cx, ENABLE); //program the stop
|
I2C_GenerateSTOP(I2Cx, ENABLE); //program the stop
|
||||||
final_stop = 1;
|
final_stop = 1;
|
||||||
I2C_ITConfig(I2Cx, I2C_IT_BUF, ENABLE); //allow us to have an EV7
|
I2C_ITConfig(I2Cx, I2C_IT_BUF, ENABLE); //allow us to have an EV7
|
||||||
} else { //EV6 and EV6_1
|
} else { //EV6 and EV6_1
|
||||||
a = I2Cx->SR2; //clear the ADDR here
|
a = I2Cx->SR2; //clear the ADDR here
|
||||||
DMB();
|
__DMB();
|
||||||
if (bytes == 2 && reading && subaddress_sent) { //rx 2 bytes - EV6_1
|
if (bytes == 2 && reading && subaddress_sent) { //rx 2 bytes - EV6_1
|
||||||
I2C_AcknowledgeConfig(I2Cx, DISABLE); //turn off ACK
|
I2C_AcknowledgeConfig(I2Cx, DISABLE); //turn off ACK
|
||||||
I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); //disable TXE to allow the buffer to fill
|
I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); //disable TXE to allow the buffer to fill
|
||||||
|
|
11
drv_pwm.c
11
drv_pwm.c
|
@ -144,7 +144,7 @@ static void pwmIRQHandler(TIM_TypeDef *tim)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void pwmInit(bool usePPM, bool useServos)
|
void pwmInit(bool usePPM, bool useServos, bool useDigitalServos)
|
||||||
{
|
{
|
||||||
uint8_t i;
|
uint8_t i;
|
||||||
GPIO_InitTypeDef GPIO_InitStructure = { 0, };
|
GPIO_InitTypeDef GPIO_InitStructure = { 0, };
|
||||||
|
@ -268,8 +268,11 @@ void pwmInit(bool usePPM, bool useServos)
|
||||||
|
|
||||||
// Output timers
|
// Output timers
|
||||||
if (useServos) {
|
if (useServos) {
|
||||||
// 50Hz period on ch1, 2 for servo
|
// 50Hz/200Hz period on ch1, 2 for servo
|
||||||
TIM_TimeBaseStructure.TIM_Period = PULSE_PERIOD_SERVO_ANALOG - 1;
|
if (useDigitalServos)
|
||||||
|
TIM_TimeBaseStructure.TIM_Period = PULSE_PERIOD_SERVO_DIGITAL - 1;
|
||||||
|
else
|
||||||
|
TIM_TimeBaseStructure.TIM_Period = PULSE_PERIOD_SERVO_ANALOG - 1;
|
||||||
TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure);
|
TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure);
|
||||||
TIM_TimeBaseStructure.TIM_Period = PULSE_PERIOD - 1;
|
TIM_TimeBaseStructure.TIM_Period = PULSE_PERIOD - 1;
|
||||||
TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
|
TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
|
||||||
|
@ -278,7 +281,7 @@ void pwmInit(bool usePPM, bool useServos)
|
||||||
TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure);
|
TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure);
|
||||||
TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
|
TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
|
||||||
}
|
}
|
||||||
|
|
||||||
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2;
|
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM2;
|
||||||
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
|
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
|
||||||
TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable;
|
TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Disable;
|
||||||
|
|
|
@ -1,5 +1,5 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
void pwmInit(bool usePPM, bool useServos);
|
void pwmInit(bool usePPM, bool useServos, bool useDigitalServos);
|
||||||
void pwmWrite(uint8_t channel, uint16_t value);
|
void pwmWrite(uint8_t channel, uint16_t value);
|
||||||
uint16_t pwmRead(uint8_t channel);
|
uint16_t pwmRead(uint8_t channel);
|
||||||
|
|
43
drv_system.c
43
drv_system.c
|
@ -1,8 +1,5 @@
|
||||||
#include "board.h"
|
#include "board.h"
|
||||||
|
|
||||||
static uint32_t enabledSensors = 0;
|
|
||||||
static uint32_t enabledFeatures = 0;
|
|
||||||
|
|
||||||
// Cycle counter stuff - these should be defined by CMSIS, but they aren't
|
// Cycle counter stuff - these should be defined by CMSIS, but they aren't
|
||||||
#define DWT_CTRL (*(volatile uint32_t *)0xE0001000)
|
#define DWT_CTRL (*(volatile uint32_t *)0xE0001000)
|
||||||
#define DWT_CYCCNT ((volatile uint32_t *)0xE0001004)
|
#define DWT_CYCCNT ((volatile uint32_t *)0xE0001004)
|
||||||
|
@ -143,36 +140,6 @@ void delay(uint32_t ms)
|
||||||
delayMicroseconds(1000);
|
delayMicroseconds(1000);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool sensors(uint32_t mask)
|
|
||||||
{
|
|
||||||
return enabledSensors & mask;
|
|
||||||
}
|
|
||||||
|
|
||||||
void sensorsSet(uint32_t mask)
|
|
||||||
{
|
|
||||||
enabledSensors |= mask;
|
|
||||||
}
|
|
||||||
|
|
||||||
void sensorsClear(uint32_t mask)
|
|
||||||
{
|
|
||||||
enabledSensors &= ~(mask);
|
|
||||||
}
|
|
||||||
|
|
||||||
bool feature(uint32_t mask)
|
|
||||||
{
|
|
||||||
return enabledFeatures & mask;
|
|
||||||
}
|
|
||||||
|
|
||||||
void featureSet(uint32_t mask)
|
|
||||||
{
|
|
||||||
enabledFeatures |= mask;
|
|
||||||
}
|
|
||||||
|
|
||||||
void featureClear(uint32_t mask)
|
|
||||||
{
|
|
||||||
enabledFeatures &= ~(mask);
|
|
||||||
}
|
|
||||||
|
|
||||||
void failureMode(uint8_t mode)
|
void failureMode(uint8_t mode)
|
||||||
{
|
{
|
||||||
LED1_ON;
|
LED1_ON;
|
||||||
|
@ -189,11 +156,13 @@ void failureMode(uint8_t mode)
|
||||||
|
|
||||||
#define AIRCR_VECTKEY_MASK ((uint32_t)0x05FA0000)
|
#define AIRCR_VECTKEY_MASK ((uint32_t)0x05FA0000)
|
||||||
|
|
||||||
void systemResetToBootloader(void)
|
void systemReset(bool toBootloader)
|
||||||
{
|
{
|
||||||
// 1FFFF000 -> 20000200 -> SP
|
if (toBootloader) {
|
||||||
// 1FFFF004 -> 1FFFF021 -> PC
|
// 1FFFF000 -> 20000200 -> SP
|
||||||
*((uint32_t *)0x20004FF0) = 0xDEADBEEF; // 20KB STM32F103
|
// 1FFFF004 -> 1FFFF021 -> PC
|
||||||
|
*((uint32_t *)0x20004FF0) = 0xDEADBEEF; // 20KB STM32F103
|
||||||
|
}
|
||||||
|
|
||||||
// Generate system reset
|
// Generate system reset
|
||||||
SCB->AIRCR = AIRCR_VECTKEY_MASK | (uint32_t)0x04;
|
SCB->AIRCR = AIRCR_VECTKEY_MASK | (uint32_t)0x04;
|
||||||
|
|
11
drv_system.h
11
drv_system.h
|
@ -7,17 +7,8 @@ void delay(uint32_t ms);
|
||||||
uint32_t micros(void);
|
uint32_t micros(void);
|
||||||
uint32_t millis(void);
|
uint32_t millis(void);
|
||||||
|
|
||||||
// features
|
|
||||||
bool sensors(uint32_t mask);
|
|
||||||
void sensorsSet(uint32_t mask);
|
|
||||||
void sensorsClear(uint32_t mask);
|
|
||||||
|
|
||||||
bool feature(uint32_t mask);
|
|
||||||
void featureSet(uint32_t mask);
|
|
||||||
void featureClear(uint32_t mask);
|
|
||||||
|
|
||||||
// failure
|
// failure
|
||||||
void failureMode(uint8_t mode);
|
void failureMode(uint8_t mode);
|
||||||
|
|
||||||
// bootloader/IAP
|
// bootloader/IAP
|
||||||
void systemResetToBootloader(void);
|
void systemReset(bool toBootloader);
|
||||||
|
|
40
imu.c
40
imu.c
|
@ -70,11 +70,11 @@ void computeIMU(void)
|
||||||
if (!sensors(SENSOR_ACC))
|
if (!sensors(SENSOR_ACC))
|
||||||
accADC[axis] = 0;
|
accADC[axis] = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
#if defined(TRI)
|
if (mixerConfiguration == MULTITYPE_TRI) {
|
||||||
gyroData[YAW] = (gyroYawSmooth * 2 + gyroData[YAW] + 1) / 3;
|
gyroData[YAW] = (gyroYawSmooth * 2 + gyroData[YAW] + 1) / 3;
|
||||||
gyroYawSmooth = gyroData[YAW];
|
gyroYawSmooth = gyroData[YAW];
|
||||||
#endif
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// **************************************************
|
// **************************************************
|
||||||
|
@ -152,37 +152,11 @@ void rotateV(struct fp_vector *v, float *delta)
|
||||||
v->Y += delta[PITCH] * v_tmp.Z + delta[YAW] * v_tmp.X;
|
v->Y += delta[PITCH] * v_tmp.Z + delta[YAW] * v_tmp.X;
|
||||||
}
|
}
|
||||||
|
|
||||||
#if 1
|
|
||||||
static int16_t _atan2f(float y, float x)
|
static int16_t _atan2f(float y, float x)
|
||||||
{
|
{
|
||||||
#define fp_is_neg(val) (val < 0 ? 1 : 0)
|
// no need for aidsy inaccurate shortcuts on a proper platform
|
||||||
|
return (int16_t)(atan2f(y, x) * (180.0f / M_PI * 10.0f));
|
||||||
float z = y / x;
|
|
||||||
int16_t zi = abs((int16_t)(z * 100));
|
|
||||||
int8_t y_neg = fp_is_neg(y);
|
|
||||||
if (zi < 100) {
|
|
||||||
if (zi > 10)
|
|
||||||
z = z / (1.0f + 0.28f * z * z);
|
|
||||||
if (fp_is_neg(x)) {
|
|
||||||
if (y_neg)
|
|
||||||
z -= M_PI;
|
|
||||||
else
|
|
||||||
z += M_PI;
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
z = (M_PI / 2.0f) - z / (z * z + 0.28f);
|
|
||||||
if (y_neg)
|
|
||||||
z -= M_PI;
|
|
||||||
}
|
|
||||||
z *= (180.0f / M_PI * 10);
|
|
||||||
return z;
|
|
||||||
}
|
}
|
||||||
#else
|
|
||||||
static int16_t _atan2f(float y, float x)
|
|
||||||
{
|
|
||||||
return (int16_t)atan2f(y, x) * (180.0f / M_PI * 10.0f);
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
static void getEstimatedAttitude(void)
|
static void getEstimatedAttitude(void)
|
||||||
{
|
{
|
||||||
|
|
6
main.c
6
main.c
|
@ -6,6 +6,7 @@ int main(void)
|
||||||
uint8_t i;
|
uint8_t i;
|
||||||
|
|
||||||
#if 0
|
#if 0
|
||||||
|
// using this to write asm for bootloader :)
|
||||||
RCC->APB2ENR |= RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO; // GPIOB + AFIO
|
RCC->APB2ENR |= RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO; // GPIOB + AFIO
|
||||||
AFIO->MAPR &= 0xF0FFFFFF;
|
AFIO->MAPR &= 0xF0FFFFFF;
|
||||||
AFIO->MAPR = 0x02000000;
|
AFIO->MAPR = 0x02000000;
|
||||||
|
@ -22,8 +23,9 @@ int main(void)
|
||||||
featureSet(FEATURE_VBAT | FEATURE_PPM);
|
featureSet(FEATURE_VBAT | FEATURE_PPM);
|
||||||
sensorsSet(SENSOR_ACC | SENSOR_BARO | SENSOR_MAG);
|
sensorsSet(SENSOR_ACC | SENSOR_BARO | SENSOR_MAG);
|
||||||
|
|
||||||
pwmInit(feature(FEATURE_PPM), false);
|
mixerInit(); // this will configure FEATURE_SERVO depending on mixer type
|
||||||
|
pwmInit(feature(FEATURE_PPM), feature(FEATURE_SERVO), feature(FEATURE_DIGITAL_SERVO));
|
||||||
|
|
||||||
LED1_ON;
|
LED1_ON;
|
||||||
LED0_OFF;
|
LED0_OFF;
|
||||||
for (i = 0; i < 10; i++) {
|
for (i = 0; i < 10; i++) {
|
||||||
|
|
339
mixer.c
339
mixer.c
|
@ -1,59 +1,96 @@
|
||||||
#include "board.h"
|
#include "board.h"
|
||||||
#include "mw.h"
|
#include "mw.h"
|
||||||
|
|
||||||
#if defined(BI) || defined(TRI) || defined(SERVO_TILT) || defined(GIMBAL) || defined(FLYING_WING) || defined(CAMTRIG)
|
static uint8_t numberMotor = 4;
|
||||||
#define SERVO
|
int16_t motor[8];
|
||||||
#endif
|
int16_t servo[8] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 };
|
||||||
|
uint8_t mixerConfiguration = MULTITYPE_TRI;
|
||||||
|
uint16_t wing_left_mid = WING_LEFT_MID;
|
||||||
|
uint16_t wing_right_mid = WING_RIGHT_MID;
|
||||||
|
uint16_t tri_yaw_middle = TRI_YAW_MIDDLE;
|
||||||
|
|
||||||
#if defined(GIMBAL)
|
void mixerInit(void)
|
||||||
#define NUMBER_MOTOR 0
|
{
|
||||||
#define PRI_SERVO_FROM 1 // use servo from 1 to 2
|
if (mixerConfiguration == MULTITYPE_BI || mixerConfiguration == MULTITYPE_TRI || mixerConfiguration == MULTITYPE_GIMBAL || mixerConfiguration == MULTITYPE_FLYING_WING)
|
||||||
#define PRI_SERVO_TO 2
|
featureSet(FEATURE_SERVO);
|
||||||
#elif defined(FLYING_WING)
|
|
||||||
#define NUMBER_MOTOR 1
|
switch (mixerConfiguration) {
|
||||||
#define PRI_SERVO_FROM 1 // use servo from 1 to 2
|
case MULTITYPE_GIMBAL:
|
||||||
#define PRI_SERVO_TO 2
|
numberMotor = 0;
|
||||||
#elif defined(BI)
|
break;
|
||||||
#define NUMBER_MOTOR 2
|
case MULTITYPE_FLYING_WING:
|
||||||
#define PRI_SERVO_FROM 5 // use servo from 5 to 6
|
numberMotor = 1;
|
||||||
#define PRI_SERVO_TO 6
|
break;
|
||||||
#elif defined(TRI)
|
case MULTITYPE_BI:
|
||||||
#define NUMBER_MOTOR 3
|
numberMotor = 2;
|
||||||
#define PRI_SERVO_FROM 6 // use only servo 5
|
break;
|
||||||
#define PRI_SERVO_TO 6
|
case MULTITYPE_TRI:
|
||||||
#elif defined(QUADP) || defined(QUADX) || defined(Y4)
|
numberMotor = 3;
|
||||||
#define NUMBER_MOTOR 4
|
break;
|
||||||
#elif defined(Y6) || defined(HEX6) || defined(HEX6X)
|
|
||||||
#define NUMBER_MOTOR 6
|
case MULTITYPE_QUADP:
|
||||||
#elif defined(OCTOX8) || defined(OCTOFLATP) || defined(OCTOFLATX)
|
case MULTITYPE_QUADX:
|
||||||
#define NUMBER_MOTOR 8
|
case MULTITYPE_Y4:
|
||||||
#endif
|
case MULTITYPE_VTAIL4:
|
||||||
|
numberMotor = 4;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MULTITYPE_Y6:
|
||||||
|
case MULTITYPE_HEX6:
|
||||||
|
case MULTITYPE_HEX6X:
|
||||||
|
numberMotor = 6;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MULTITYPE_OCTOX8:
|
||||||
|
case MULTITYPE_OCTOFLATP:
|
||||||
|
case MULTITYPE_OCTOFLATX:
|
||||||
|
numberMotor = 8;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
void writeServos(void)
|
void writeServos(void)
|
||||||
{
|
{
|
||||||
#if defined(SERVO)
|
if (!feature(FEATURE_SERVO))
|
||||||
|
return;
|
||||||
|
|
||||||
#endif
|
if (mixerConfiguration == MULTITYPE_TRI || mixerConfiguration == MULTITYPE_BI) {
|
||||||
|
/* One servo on Motor #4 */
|
||||||
|
pwmWrite(0, servo[4]);
|
||||||
|
if (mixerConfiguration == MULTITYPE_BI)
|
||||||
|
pwmWrite(1, servo[5]);
|
||||||
|
} else {
|
||||||
|
/* Two servos for camstab or FLYING_WING */
|
||||||
|
pwmWrite(0, servo[0]);
|
||||||
|
pwmWrite(1, servo[1]);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void writeMotors(void)
|
void writeMotors(void)
|
||||||
{
|
{
|
||||||
uint8_t i;
|
uint8_t i;
|
||||||
|
uint8_t offset = 0;
|
||||||
|
|
||||||
for (i = 0; i < NUMBER_MOTOR; i++)
|
// when servos are enabled, motor outputs 1..2 are for servos only
|
||||||
pwmWrite(i, motor[i]);
|
if (feature(FEATURE_SERVO))
|
||||||
|
offset = 2;
|
||||||
|
|
||||||
|
for (i = 0; i < numberMotor; i++)
|
||||||
|
pwmWrite(i + offset, motor[i]);
|
||||||
}
|
}
|
||||||
|
|
||||||
void writeAllMotors(int16_t mc)
|
void writeAllMotors(int16_t mc)
|
||||||
{
|
{
|
||||||
uint8_t i;
|
uint8_t i;
|
||||||
|
|
||||||
// Sends commands to all motors
|
// Sends commands to all motors
|
||||||
for (i = 0; i < NUMBER_MOTOR; i++)
|
for (i = 0; i < numberMotor; i++)
|
||||||
motor[i] = mc;
|
motor[i] = mc;
|
||||||
writeMotors();
|
writeMotors();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#define PIDMIX(X,Y,Z) rcCommand[THROTTLE] + axisPID[ROLL] * X + axisPID[PITCH] * Y + YAW_DIRECTION * axisPID[YAW] * Z
|
||||||
|
|
||||||
void mixTable(void)
|
void mixTable(void)
|
||||||
{
|
{
|
||||||
int16_t maxMotor;
|
int16_t maxMotor;
|
||||||
|
@ -61,105 +98,133 @@ void mixTable(void)
|
||||||
static uint8_t camCycle = 0;
|
static uint8_t camCycle = 0;
|
||||||
static uint8_t camState = 0;
|
static uint8_t camState = 0;
|
||||||
static uint32_t camTime = 0;
|
static uint32_t camTime = 0;
|
||||||
|
|
||||||
|
if (numberMotor > 3) {
|
||||||
|
//prevent "yaw jump" during yaw correction
|
||||||
|
axisPID[YAW] = constrain(axisPID[YAW], -100 - abs(rcCommand[YAW]), +100 + abs(rcCommand[YAW]));
|
||||||
|
}
|
||||||
|
|
||||||
#define PIDMIX(X,Y,Z) rcCommand[THROTTLE] + axisPID[ROLL] * X + axisPID[PITCH] * Y + YAW_DIRECTION * axisPID[YAW] * Z
|
switch (mixerConfiguration) {
|
||||||
|
case MULTITYPE_BI:
|
||||||
|
motor[0] = PIDMIX(+1, 0, 0); //LEFT
|
||||||
|
motor[1] = PIDMIX(-1, 0, 0); //RIGHT
|
||||||
|
servo[4] = constrain(1500 + YAW_DIRECTION * (axisPID[YAW] + axisPID[PITCH]), 1020, 2000); //LEFT
|
||||||
|
servo[5] = constrain(1500 + YAW_DIRECTION * (axisPID[YAW] - axisPID[PITCH]), 1020, 2000); //RIGHT
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MULTITYPE_TRI:
|
||||||
|
motor[0] = PIDMIX(0, +4 / 3, 0); //REAR
|
||||||
|
motor[1] = PIDMIX(-1, -2 / 3, 0); //RIGHT
|
||||||
|
motor[2] = PIDMIX(+1, -2 / 3, 0); //LEFT
|
||||||
|
servo[4] = constrain(tri_yaw_middle + YAW_DIRECTION * axisPID[YAW], TRI_YAW_CONSTRAINT_MIN, TRI_YAW_CONSTRAINT_MAX); //REAR
|
||||||
|
break;
|
||||||
|
|
||||||
#if NUMBER_MOTOR > 3
|
case MULTITYPE_QUADP:
|
||||||
//prevent "yaw jump" during yaw correction
|
motor[0] = PIDMIX(0, +1, -1); //REAR
|
||||||
axisPID[YAW] = constrain(axisPID[YAW], -100 - abs(rcCommand[YAW]), +100 + abs(rcCommand[YAW]));
|
motor[1] = PIDMIX(-1, 0, +1); //RIGHT
|
||||||
#endif
|
motor[2] = PIDMIX(+1, 0, +1); //LEFT
|
||||||
#ifdef BI
|
motor[3] = PIDMIX(0, -1, -1); //FRONT
|
||||||
motor[0] = PIDMIX(+1, 0, 0); //LEFT
|
break;
|
||||||
motor[1] = PIDMIX(-1, 0, 0); //RIGHT
|
|
||||||
servo[4] = constrain(1500 + YAW_DIRECTION * (axisPID[YAW] + axisPID[PITCH]), 1020, 2000); //LEFT
|
|
||||||
servo[5] = constrain(1500 + YAW_DIRECTION * (axisPID[YAW] - axisPID[PITCH]), 1020, 2000); //RIGHT
|
|
||||||
#endif
|
|
||||||
#ifdef TRI
|
|
||||||
motor[0] = PIDMIX(0, +4 / 3, 0); //REAR
|
|
||||||
motor[1] = PIDMIX(-1, -2 / 3, 0); //RIGHT
|
|
||||||
motor[2] = PIDMIX(+1, -2 / 3, 0); //LEFT
|
|
||||||
servo[5] = constrain(tri_yaw_middle + YAW_DIRECTION * axisPID[YAW], TRI_YAW_CONSTRAINT_MIN, TRI_YAW_CONSTRAINT_MAX); //REAR
|
|
||||||
|
|
||||||
#endif
|
case MULTITYPE_QUADX:
|
||||||
#ifdef QUADP
|
motor[0] = PIDMIX(-1, +1, -1); //REAR_R
|
||||||
motor[0] = PIDMIX(0, +1, -1); //REAR
|
motor[1] = PIDMIX(-1, -1, +1); //FRONT_R
|
||||||
motor[1] = PIDMIX(-1, 0, +1); //RIGHT
|
motor[2] = PIDMIX(+1, +1, +1); //REAR_L
|
||||||
motor[2] = PIDMIX(+1, 0, +1); //LEFT
|
motor[3] = PIDMIX(+1, -1, -1); //FRONT_L
|
||||||
motor[3] = PIDMIX(0, -1, -1); //FRONT
|
break;
|
||||||
#endif
|
|
||||||
#ifdef QUADX
|
|
||||||
motor[0] = PIDMIX(-1, +1, -1); //REAR_R
|
|
||||||
motor[1] = PIDMIX(-1, -1, +1); //FRONT_R
|
|
||||||
motor[2] = PIDMIX(+1, +1, +1); //REAR_L
|
|
||||||
motor[3] = PIDMIX(+1, -1, -1); //FRONT_L
|
|
||||||
#endif
|
|
||||||
#ifdef Y4
|
|
||||||
motor[0] = PIDMIX(+0, +1, -1); //REAR_1 CW
|
|
||||||
motor[1] = PIDMIX(-1, -1, 0); //FRONT_R CCW
|
|
||||||
motor[2] = PIDMIX(+0, +1, +1); //REAR_2 CCW
|
|
||||||
motor[3] = PIDMIX(+1, -1, 0); //FRONT_L CW
|
|
||||||
#endif
|
|
||||||
#ifdef Y6
|
|
||||||
motor[0] = PIDMIX(+0, +4 / 3, +1); //REAR
|
|
||||||
motor[1] = PIDMIX(-1, -2 / 3, -1); //RIGHT
|
|
||||||
motor[2] = PIDMIX(+1, -2 / 3, -1); //LEFT
|
|
||||||
motor[3] = PIDMIX(+0, +4 / 3, -1); //UNDER_REAR
|
|
||||||
motor[4] = PIDMIX(-1, -2 / 3, +1); //UNDER_RIGHT
|
|
||||||
motor[5] = PIDMIX(+1, -2 / 3, +1); //UNDER_LEFT
|
|
||||||
#endif
|
|
||||||
#ifdef HEX6
|
|
||||||
motor[0] = PIDMIX(-1 / 2, +1 / 2, +1); //REAR_R
|
|
||||||
motor[1] = PIDMIX(-1 / 2, -1 / 2, -1); //FRONT_R
|
|
||||||
motor[2] = PIDMIX(+1 / 2, +1 / 2, +1); //REAR_L
|
|
||||||
motor[3] = PIDMIX(+1 / 2, -1 / 2, -1); //FRONT_L
|
|
||||||
motor[4] = PIDMIX(+0, -1, +1); //FRONT
|
|
||||||
motor[5] = PIDMIX(+0, +1, -1); //REAR
|
|
||||||
#endif
|
|
||||||
#ifdef HEX6X
|
|
||||||
motor[0] = PIDMIX(-1 / 2, +1 / 2, +1); //REAR_R
|
|
||||||
motor[1] = PIDMIX(-1 / 2, -1 / 2, +1); //FRONT_R
|
|
||||||
motor[2] = PIDMIX(+1 / 2, +1 / 2, -1); //REAR_L
|
|
||||||
motor[3] = PIDMIX(+1 / 2, -1 / 2, -1); //FRONT_L
|
|
||||||
motor[4] = PIDMIX(-1, +0, -1); //RIGHT
|
|
||||||
motor[5] = PIDMIX(+1, +0, +1); //LEFT
|
|
||||||
#endif
|
|
||||||
#ifdef OCTOX8
|
|
||||||
motor[0] = PIDMIX(-1, +1, -1); //REAR_R
|
|
||||||
motor[1] = PIDMIX(-1, -1, +1); //FRONT_R
|
|
||||||
motor[2] = PIDMIX(+1, +1, +1); //REAR_L
|
|
||||||
motor[3] = PIDMIX(+1, -1, -1); //FRONT_L
|
|
||||||
motor[4] = PIDMIX(-1, +1, +1); //UNDER_REAR_R
|
|
||||||
motor[5] = PIDMIX(-1, -1, -1); //UNDER_FRONT_R
|
|
||||||
motor[6] = PIDMIX(+1, +1, -1); //UNDER_REAR_L
|
|
||||||
motor[7] = PIDMIX(+1, -1, +1); //UNDER_FRONT_L
|
|
||||||
#endif
|
|
||||||
#ifdef OCTOFLATP
|
|
||||||
motor[0] = PIDMIX(+7 / 10, -7 / 10, +1); //FRONT_L
|
|
||||||
motor[1] = PIDMIX(-7 / 10, -7 / 10, +1); //FRONT_R
|
|
||||||
motor[2] = PIDMIX(-7 / 10, +7 / 10, +1); //REAR_R
|
|
||||||
motor[3] = PIDMIX(+7 / 10, +7 / 10, +1); //REAR_L
|
|
||||||
motor[4] = PIDMIX(+0, -1, -1); //FRONT
|
|
||||||
motor[5] = PIDMIX(-1, +0, -1); //RIGHT
|
|
||||||
motor[6] = PIDMIX(+0, +1, -1); //REAR
|
|
||||||
motor[7] = PIDMIX(+1, +0, -1); //LEFT
|
|
||||||
#endif
|
|
||||||
#ifdef OCTOFLATX
|
|
||||||
motor[0] = PIDMIX(+1, -1 / 2, +1); //MIDFRONT_L
|
|
||||||
motor[1] = PIDMIX(-1 / 2, -1, +1); //FRONT_R
|
|
||||||
motor[2] = PIDMIX(-1, +1 / 2, +1); //MIDREAR_R
|
|
||||||
motor[3] = PIDMIX(+1 / 2, +1, +1); //REAR_L
|
|
||||||
motor[4] = PIDMIX(+1 / 2, -1, -1); //FRONT_L
|
|
||||||
motor[5] = PIDMIX(-1, -1 / 2, -1); //MIDFRONT_R
|
|
||||||
motor[6] = PIDMIX(-1 / 2, +1, -1); //REAR_R
|
|
||||||
motor[7] = PIDMIX(+1, +1 / 2, -1); //MIDREAR_L
|
|
||||||
#endif
|
|
||||||
#ifdef VTAIL4
|
|
||||||
motor[0] = PIDMIX(+0, +1, -1 / 2); //REAR_R
|
|
||||||
motor[1] = PIDMIX(-1, -1, +2 / 10); //FRONT_R
|
|
||||||
motor[2] = PIDMIX(+0, +1, +1 / 2); //REAR_L
|
|
||||||
motor[3] = PIDMIX(+1, -1, -2 / 10); //FRONT_L
|
|
||||||
#endif
|
|
||||||
|
|
||||||
|
case MULTITYPE_Y4:
|
||||||
|
motor[0] = PIDMIX(+0, +1, -1); //REAR_1 CW
|
||||||
|
motor[1] = PIDMIX(-1, -1, 0); //FRONT_R CCW
|
||||||
|
motor[2] = PIDMIX(+0, +1, +1); //REAR_2 CCW
|
||||||
|
motor[3] = PIDMIX(+1, -1, 0); //FRONT_L CW
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MULTITYPE_Y6:
|
||||||
|
motor[0] = PIDMIX(+0, +4 / 3, +1); //REAR
|
||||||
|
motor[1] = PIDMIX(-1, -2 / 3, -1); //RIGHT
|
||||||
|
motor[2] = PIDMIX(+1, -2 / 3, -1); //LEFT
|
||||||
|
motor[3] = PIDMIX(+0, +4 / 3, -1); //UNDER_REAR
|
||||||
|
motor[4] = PIDMIX(-1, -2 / 3, +1); //UNDER_RIGHT
|
||||||
|
motor[5] = PIDMIX(+1, -2 / 3, +1); //UNDER_LEFT
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MULTITYPE_HEX6:
|
||||||
|
motor[0] = PIDMIX(-1 / 2, +1 / 2, +1); //REAR_R
|
||||||
|
motor[1] = PIDMIX(-1 / 2, -1 / 2, -1); //FRONT_R
|
||||||
|
motor[2] = PIDMIX(+1 / 2, +1 / 2, +1); //REAR_L
|
||||||
|
motor[3] = PIDMIX(+1 / 2, -1 / 2, -1); //FRONT_L
|
||||||
|
motor[4] = PIDMIX(+0, -1, +1); //FRONT
|
||||||
|
motor[5] = PIDMIX(+0, +1, -1); //REAR
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MULTITYPE_HEX6X:
|
||||||
|
motor[0] = PIDMIX(-1 / 2, +1 / 2, +1); //REAR_R
|
||||||
|
motor[1] = PIDMIX(-1 / 2, -1 / 2, +1); //FRONT_R
|
||||||
|
motor[2] = PIDMIX(+1 / 2, +1 / 2, -1); //REAR_L
|
||||||
|
motor[3] = PIDMIX(+1 / 2, -1 / 2, -1); //FRONT_L
|
||||||
|
motor[4] = PIDMIX(-1, +0, -1); //RIGHT
|
||||||
|
motor[5] = PIDMIX(+1, +0, +1); //LEFT
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MULTITYPE_OCTOX8:
|
||||||
|
motor[0] = PIDMIX(-1, +1, -1); //REAR_R
|
||||||
|
motor[1] = PIDMIX(-1, -1, +1); //FRONT_R
|
||||||
|
motor[2] = PIDMIX(+1, +1, +1); //REAR_L
|
||||||
|
motor[3] = PIDMIX(+1, -1, -1); //FRONT_L
|
||||||
|
motor[4] = PIDMIX(-1, +1, +1); //UNDER_REAR_R
|
||||||
|
motor[5] = PIDMIX(-1, -1, -1); //UNDER_FRONT_R
|
||||||
|
motor[6] = PIDMIX(+1, +1, -1); //UNDER_REAR_L
|
||||||
|
motor[7] = PIDMIX(+1, -1, +1); //UNDER_FRONT_L
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MULTITYPE_OCTOFLATP:
|
||||||
|
motor[0] = PIDMIX(+7 / 10, -7 / 10, +1); //FRONT_L
|
||||||
|
motor[1] = PIDMIX(-7 / 10, -7 / 10, +1); //FRONT_R
|
||||||
|
motor[2] = PIDMIX(-7 / 10, +7 / 10, +1); //REAR_R
|
||||||
|
motor[3] = PIDMIX(+7 / 10, +7 / 10, +1); //REAR_L
|
||||||
|
motor[4] = PIDMIX(+0, -1, -1); //FRONT
|
||||||
|
motor[5] = PIDMIX(-1, +0, -1); //RIGHT
|
||||||
|
motor[6] = PIDMIX(+0, +1, -1); //REAR
|
||||||
|
motor[7] = PIDMIX(+1, +0, -1); //LEFT
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MULTITYPE_OCTOFLATX:
|
||||||
|
motor[0] = PIDMIX(+1, -1 / 2, +1); //MIDFRONT_L
|
||||||
|
motor[1] = PIDMIX(-1 / 2, -1, +1); //FRONT_R
|
||||||
|
motor[2] = PIDMIX(-1, +1 / 2, +1); //MIDREAR_R
|
||||||
|
motor[3] = PIDMIX(+1 / 2, +1, +1); //REAR_L
|
||||||
|
motor[4] = PIDMIX(+1 / 2, -1, -1); //FRONT_L
|
||||||
|
motor[5] = PIDMIX(-1, -1 / 2, -1); //MIDFRONT_R
|
||||||
|
motor[6] = PIDMIX(-1 / 2, +1, -1); //REAR_R
|
||||||
|
motor[7] = PIDMIX(+1, +1 / 2, -1); //MIDREAR_L
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MULTITYPE_VTAIL4:
|
||||||
|
motor[0] = PIDMIX(+0, +1, -1 / 2); //REAR_R
|
||||||
|
motor[1] = PIDMIX(-1, -1, +2 / 10); //FRONT_R
|
||||||
|
motor[2] = PIDMIX(+0, +1, +1 / 2); //REAR_L
|
||||||
|
motor[3] = PIDMIX(+1, -1, -2 / 10); //FRONT_L
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MULTITYPE_GIMBAL:
|
||||||
|
servo[0] = constrain(TILT_PITCH_MIDDLE + TILT_PITCH_PROP * angle[PITCH] / 16 + rcCommand[PITCH], TILT_PITCH_MIN, TILT_PITCH_MAX);
|
||||||
|
servo[1] = constrain(TILT_ROLL_MIDDLE + TILT_ROLL_PROP * angle[ROLL] / 16 + rcCommand[ROLL], TILT_ROLL_MIN, TILT_ROLL_MAX);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MULTITYPE_FLYING_WING:
|
||||||
|
motor[0] = rcCommand[THROTTLE];
|
||||||
|
if (passThruMode) { // do not use sensors for correction, simple 2 channel mixing
|
||||||
|
servo[0] = PITCH_DIRECTION_L * (rcData[PITCH] - MIDRC) + ROLL_DIRECTION_L * (rcData[ROLL] - MIDRC);
|
||||||
|
servo[1] = PITCH_DIRECTION_R * (rcData[PITCH] - MIDRC) + ROLL_DIRECTION_R * (rcData[ROLL] - MIDRC);
|
||||||
|
} else { // use sensors to correct (gyro only or gyro+acc according to aux1/aux2 configuration
|
||||||
|
servo[0] = PITCH_DIRECTION_L * axisPID[PITCH] + ROLL_DIRECTION_L * axisPID[ROLL];
|
||||||
|
servo[1] = PITCH_DIRECTION_R * axisPID[PITCH] + ROLL_DIRECTION_R * axisPID[ROLL];
|
||||||
|
}
|
||||||
|
servo[0] = constrain(servo[0] + wing_left_mid , WING_LEFT_MIN, WING_LEFT_MAX);
|
||||||
|
servo[1] = constrain(servo[1] + wing_right_mid, WING_RIGHT_MIN, WING_RIGHT_MAX);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
#ifdef SERVO_TILT
|
#ifdef SERVO_TILT
|
||||||
servo[0] = TILT_PITCH_MIDDLE + rcData[AUX3] - 1500;
|
servo[0] = TILT_PITCH_MIDDLE + rcData[AUX3] - 1500;
|
||||||
|
@ -173,22 +238,6 @@ void mixTable(void)
|
||||||
servo[0] = constrain(servo[0], TILT_PITCH_MIN, TILT_PITCH_MAX);
|
servo[0] = constrain(servo[0], TILT_PITCH_MIN, TILT_PITCH_MAX);
|
||||||
servo[1] = constrain(servo[1], TILT_ROLL_MIN, TILT_ROLL_MAX);
|
servo[1] = constrain(servo[1], TILT_ROLL_MIN, TILT_ROLL_MAX);
|
||||||
#endif
|
#endif
|
||||||
#ifdef GIMBAL
|
|
||||||
servo[0] = constrain(TILT_PITCH_MIDDLE + TILT_PITCH_PROP * angle[PITCH] / 16 + rcCommand[PITCH], TILT_PITCH_MIN, TILT_PITCH_MAX);
|
|
||||||
servo[1] = constrain(TILT_ROLL_MIDDLE + TILT_ROLL_PROP * angle[ROLL] / 16 + rcCommand[ROLL], TILT_ROLL_MIN, TILT_ROLL_MAX);
|
|
||||||
#endif
|
|
||||||
#ifdef FLYING_WING
|
|
||||||
motor[0] = rcCommand[THROTTLE];
|
|
||||||
if (passThruMode) {// do not use sensors for correction, simple 2 channel mixing
|
|
||||||
servo[0] = PITCH_DIRECTION_L * (rcData[PITCH] - MIDRC) + ROLL_DIRECTION_L * (rcData[ROLL] - MIDRC);
|
|
||||||
servo[1] = PITCH_DIRECTION_R * (rcData[PITCH] - MIDRC) + ROLL_DIRECTION_R * (rcData[ROLL] - MIDRC);
|
|
||||||
} else { // use sensors to correct (gyro only or gyro+acc according to aux1/aux2 configuration
|
|
||||||
servo[0] = PITCH_DIRECTION_L * axisPID[PITCH] + ROLL_DIRECTION_L * axisPID[ROLL];
|
|
||||||
servo[1] = PITCH_DIRECTION_R * axisPID[PITCH] + ROLL_DIRECTION_R * axisPID[ROLL];
|
|
||||||
}
|
|
||||||
servo[0] = constrain(servo[0] + wing_left_mid , WING_LEFT_MIN, WING_LEFT_MAX);
|
|
||||||
servo[1] = constrain(servo[1] + wing_right_mid, WING_RIGHT_MIN, WING_RIGHT_MAX);
|
|
||||||
#endif
|
|
||||||
#if defined(CAMTRIG)
|
#if defined(CAMTRIG)
|
||||||
if (camCycle == 1) {
|
if (camCycle == 1) {
|
||||||
if (camState == 0) {
|
if (camState == 0) {
|
||||||
|
@ -213,10 +262,10 @@ void mixTable(void)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
maxMotor = motor[0];
|
maxMotor = motor[0];
|
||||||
for (i = 1; i < NUMBER_MOTOR; i++)
|
for (i = 1; i < numberMotor; i++)
|
||||||
if (motor[i] > maxMotor)
|
if (motor[i] > maxMotor)
|
||||||
maxMotor = motor[i];
|
maxMotor = motor[i];
|
||||||
for (i = 0; i < NUMBER_MOTOR; i++) {
|
for (i = 0; i < numberMotor; i++) {
|
||||||
if (maxMotor > MAXTHROTTLE) // this is a way to still have good gyro corrections if at least one motor reaches its max.
|
if (maxMotor > MAXTHROTTLE) // this is a way to still have good gyro corrections if at least one motor reaches its max.
|
||||||
motor[i] -= maxMotor - MAXTHROTTLE;
|
motor[i] -= maxMotor - MAXTHROTTLE;
|
||||||
motor[i] = constrain(motor[i], MINTHROTTLE, MAXTHROTTLE);
|
motor[i] = constrain(motor[i], MINTHROTTLE, MAXTHROTTLE);
|
||||||
|
|
6
mw.c
6
mw.c
|
@ -42,12 +42,6 @@ uint8_t magMode = 0; // if compass heading hold is a activated
|
||||||
uint8_t baroMode = 0; // if altitude hold is activated
|
uint8_t baroMode = 0; // if altitude hold is activated
|
||||||
|
|
||||||
int16_t axisPID[3];
|
int16_t axisPID[3];
|
||||||
int16_t motor[8];
|
|
||||||
int16_t servo[8] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 };
|
|
||||||
uint16_t wing_left_mid = WING_LEFT_MID;
|
|
||||||
uint16_t wing_right_mid = WING_RIGHT_MID;
|
|
||||||
uint16_t tri_yaw_middle = TRI_YAW_MIDDLE;
|
|
||||||
|
|
||||||
volatile uint16_t rcValue[18] = { 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502 }; // interval [1000;2000]
|
volatile uint16_t rcValue[18] = { 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502 }; // interval [1000;2000]
|
||||||
uint8_t rcChannel[8] = { ROLL, PITCH, THROTTLE, YAW, AUX1, AUX2, AUX3, AUX4 };
|
uint8_t rcChannel[8] = { ROLL, PITCH, THROTTLE, YAW, AUX1, AUX2, AUX3, AUX4 };
|
||||||
|
|
||||||
|
|
77
mw.h
77
mw.h
|
@ -1,7 +1,5 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#define QUADX
|
|
||||||
|
|
||||||
#define MINCHECK 1100
|
#define MINCHECK 1100
|
||||||
#define MAXCHECK 1900
|
#define MAXCHECK 1900
|
||||||
|
|
||||||
|
@ -59,6 +57,19 @@
|
||||||
#define WING_RIGHT_MIN 1020 // limit servo travel range must be inside [1020;2000]
|
#define WING_RIGHT_MIN 1020 // limit servo travel range must be inside [1020;2000]
|
||||||
#define WING_RIGHT_MAX 2000 // limit servo travel range must be inside [1020;2000]
|
#define WING_RIGHT_MAX 2000 // limit servo travel range must be inside [1020;2000]
|
||||||
|
|
||||||
|
/* The following lines apply only for a pitch/roll tilt stabilization system
|
||||||
|
On promini board, it is not compatible with config with 6 motors or more
|
||||||
|
Uncomment the first line to activate it */
|
||||||
|
//#define SERVO_TILT
|
||||||
|
#define TILT_PITCH_MIN 1020 //servo travel min, don't set it below 1020
|
||||||
|
#define TILT_PITCH_MAX 2000 //servo travel max, max value=2000
|
||||||
|
#define TILT_PITCH_MIDDLE 1500 //servo neutral value
|
||||||
|
#define TILT_PITCH_PROP 10 //servo proportional (tied to angle) ; can be negative to invert movement
|
||||||
|
#define TILT_ROLL_MIN 1020
|
||||||
|
#define TILT_ROLL_MAX 2000
|
||||||
|
#define TILT_ROLL_MIDDLE 1500
|
||||||
|
#define TILT_ROLL_PROP 10
|
||||||
|
|
||||||
/* for V BAT monitoring
|
/* for V BAT monitoring
|
||||||
after the resistor divisor we should get [0V;5V]->[0;1023] on analog V_BATPIN
|
after the resistor divisor we should get [0V;5V]->[0;1023] on analog V_BATPIN
|
||||||
with R1=33k and R2=51k
|
with R1=33k and R2=51k
|
||||||
|
@ -70,38 +81,28 @@
|
||||||
#define VBATLEVEL3_3S 99 // 9.9V
|
#define VBATLEVEL3_3S 99 // 9.9V
|
||||||
#define NO_VBAT 16 // Avoid beeping without any battery
|
#define NO_VBAT 16 // Avoid beeping without any battery
|
||||||
|
|
||||||
|
|
||||||
#define VERSION 19
|
#define VERSION 19
|
||||||
|
|
||||||
#if defined(TRI)
|
// Syncronized with GUI. Only exception is mixer > 11, which is always returned as 11 during serialization.
|
||||||
#define MULTITYPE 1
|
typedef enum MultiType
|
||||||
#elif defined(QUADP)
|
{
|
||||||
#define MULTITYPE 2
|
MULTITYPE_TRI = 1, // XA
|
||||||
#elif defined(QUADX)
|
MULTITYPE_QUADP = 2, // XB
|
||||||
#define MULTITYPE 3
|
MULTITYPE_QUADX = 3, // XC
|
||||||
#elif defined(BI)
|
MULTITYPE_BI = 4, // XD
|
||||||
#define MULTITYPE 4
|
MULTITYPE_GIMBAL = 5, // XE
|
||||||
#elif defined(GIMBAL)
|
MULTITYPE_Y6 = 6, // XF
|
||||||
#define MULTITYPE 5
|
MULTITYPE_HEX6 = 7, // XG
|
||||||
#elif defined(Y6)
|
MULTITYPE_FLYING_WING = 8, // XH
|
||||||
#define MULTITYPE 6
|
MULTITYPE_Y4 = 9, // XI
|
||||||
#elif defined(HEX6)
|
MULTITYPE_HEX6X = 10, // XJ
|
||||||
#define MULTITYPE 7
|
MULTITYPE_OCTOX8 = 11, // XK
|
||||||
#elif defined(FLYING_WING)
|
MULTITYPE_OCTOFLATP = 12, // XL the GUI is the same for all 8 motor configs
|
||||||
#define MULTITYPE 8
|
MULTITYPE_OCTOFLATX = 13, // XM the GUI is the same for all 8 motor configs
|
||||||
#elif defined(Y4)
|
// XN missing for some reason??
|
||||||
#define MULTITYPE 9
|
MULTITYPE_VTAIL4 = 15, // XO
|
||||||
#elif defined(HEX6X)
|
MULTITYPE_LAST = 16
|
||||||
#define MULTITYPE 10
|
} MultiType;
|
||||||
#elif defined(OCTOX8)
|
|
||||||
#define MULTITYPE 11
|
|
||||||
#elif defined(OCTOFLATP)
|
|
||||||
#define MULTITYPE 11 //the GUI is the same for all 8 motor configs
|
|
||||||
#elif defined(OCTOFLATX)
|
|
||||||
#define MULTITYPE 11 //the GUI is the same for all 8 motor configs
|
|
||||||
#elif defined(VTAIL4)
|
|
||||||
#define MULTITYPE 15
|
|
||||||
#endif
|
|
||||||
|
|
||||||
/*********** RC alias *****************/
|
/*********** RC alias *****************/
|
||||||
#define ROLL 0
|
#define ROLL 0
|
||||||
|
@ -174,6 +175,7 @@ extern int32_t EstVelocity;
|
||||||
extern int16_t BaroPID;
|
extern int16_t BaroPID;
|
||||||
extern uint8_t headFreeMode;
|
extern uint8_t headFreeMode;
|
||||||
extern int16_t headFreeModeHold;
|
extern int16_t headFreeModeHold;
|
||||||
|
extern uint8_t passThruMode;
|
||||||
extern int8_t smallAngle25;
|
extern int8_t smallAngle25;
|
||||||
extern int16_t zVelocity;
|
extern int16_t zVelocity;
|
||||||
extern int16_t heading, magHold;
|
extern int16_t heading, magHold;
|
||||||
|
@ -205,6 +207,10 @@ extern uint8_t GPSModeHold;
|
||||||
extern uint8_t vbat;
|
extern uint8_t vbat;
|
||||||
extern uint8_t powerTrigger1;
|
extern uint8_t powerTrigger1;
|
||||||
extern int16_t lookupRX[7]; // lookup table for expo & RC rate
|
extern int16_t lookupRX[7]; // lookup table for expo & RC rate
|
||||||
|
extern uint8_t mixerConfiguration;
|
||||||
|
extern uint16_t wing_left_mid;
|
||||||
|
extern uint16_t wing_right_mid;
|
||||||
|
extern uint16_t tri_yaw_middle;
|
||||||
|
|
||||||
// main
|
// main
|
||||||
void loop(void);
|
void loop(void);
|
||||||
|
@ -225,6 +231,7 @@ void Mag_init(void);
|
||||||
void Mag_getADC(void);
|
void Mag_getADC(void);
|
||||||
|
|
||||||
// Output
|
// Output
|
||||||
|
void mixerInit(void);
|
||||||
void writeServos(void);
|
void writeServos(void);
|
||||||
void writeMotors(void);
|
void writeMotors(void);
|
||||||
void mixTable(void);
|
void mixTable(void);
|
||||||
|
@ -236,3 +243,9 @@ void serialCom(void);
|
||||||
void readEEPROM(void);
|
void readEEPROM(void);
|
||||||
void writeParams(void);
|
void writeParams(void);
|
||||||
void checkFirstTime(void);
|
void checkFirstTime(void);
|
||||||
|
bool sensors(uint32_t mask);
|
||||||
|
void sensorsSet(uint32_t mask);
|
||||||
|
void sensorsClear(uint32_t mask);
|
||||||
|
bool feature(uint32_t mask);
|
||||||
|
void featureSet(uint32_t mask);
|
||||||
|
void featureClear(uint32_t mask);
|
||||||
|
|
20
serial.c
20
serial.c
|
@ -155,7 +155,7 @@ void serialCom(void)
|
||||||
serialize16(i2cGetErrorCounter());
|
serialize16(i2cGetErrorCounter());
|
||||||
for (i = 0; i < 2; i++)
|
for (i = 0; i < 2; i++)
|
||||||
serialize16(angle[i]);
|
serialize16(angle[i]);
|
||||||
serialize8(MULTITYPE);
|
serialize8(mixerConfiguration);
|
||||||
for (i = 0; i < PIDITEMS; i++) {
|
for (i = 0; i < PIDITEMS; i++) {
|
||||||
serialize8(P8[i]);
|
serialize8(P8[i]);
|
||||||
serialize8(I8[i]);
|
serialize8(I8[i]);
|
||||||
|
@ -207,8 +207,22 @@ void serialCom(void)
|
||||||
serialize8('O'); //49
|
serialize8('O'); //49
|
||||||
// UartSendData();
|
// UartSendData();
|
||||||
break;
|
break;
|
||||||
case 'R': // reboot to bootloader
|
case 'R': // reboot to bootloader (oops, apparently this w as used for other trash, fix later)
|
||||||
systemResetToBootloader();
|
systemReset(true);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 'X': // dynamic mixer
|
||||||
|
i = uartReadPoll();
|
||||||
|
if (i > 64 && i < 64 + MULTITYPE_LAST) {
|
||||||
|
serialize8('O');
|
||||||
|
serialize8('K');
|
||||||
|
mixerConfiguration = i - '@'; // A..B..C.. index
|
||||||
|
writeParams();
|
||||||
|
systemReset(false);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
serialize8('N');
|
||||||
|
serialize8('G');
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case 'W': //GUI write params to eeprom @ arduino
|
case 'W': //GUI write params to eeprom @ arduino
|
||||||
|
|
Loading…
Reference in New Issue