synced code with multiwii 2.0 release
split uart2 initialization inside drv_uart. added receive data callback to use either with GPS or spektrum satellite added spektrum satellite support, also freeing up 4 motor outputs for hexa/octo/camstab configurable acc lpf and gyro lpf via cli configurable (build-time) temperature lpf on baro. seems mostly useless. fixed a nice boner bug in mag code which ended up multiplying magADC twice with magCal data. fixed mpu3050 driver to allow configurable lpf, also broke other stuff in the process. considering moving this sort of stuff to "init" struct for sensor. pwm driver rewritten to fully disable pwm/ppm inputs (such as using spektrum satellite case) cleaned up double math in gps.c to use sinf/cosf etc removed TRUSTED_ACCZ since its useless anyway whitespace cleanup git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@130 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop
parent
fd9d986169
commit
96829b7306
132
baseflight.uvopt
132
baseflight.uvopt
|
|
@ -73,7 +73,7 @@
|
|||
<OPTFL>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<IsCurrentTarget>1</IsCurrentTarget>
|
||||
<IsCurrentTarget>0</IsCurrentTarget>
|
||||
</OPTFL>
|
||||
<CpuCode>255</CpuCode>
|
||||
<Books>
|
||||
|
|
@ -317,7 +317,7 @@
|
|||
<OPTFL>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<IsCurrentTarget>0</IsCurrentTarget>
|
||||
<IsCurrentTarget>1</IsCurrentTarget>
|
||||
</OPTFL>
|
||||
<CpuCode>255</CpuCode>
|
||||
<Books>
|
||||
|
|
@ -383,7 +383,7 @@
|
|||
<SetRegEntry>
|
||||
<Number>0</Number>
|
||||
<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>
|
||||
<Number>0</Number>
|
||||
|
|
@ -400,9 +400,9 @@
|
|||
<Bp>
|
||||
<Number>0</Number>
|
||||
<Type>0</Type>
|
||||
<LineNumber>254</LineNumber>
|
||||
<LineNumber>71</LineNumber>
|
||||
<EnabledFlag>1</EnabledFlag>
|
||||
<Address>134227724</Address>
|
||||
<Address>134232388</Address>
|
||||
<ByteObject>0</ByteObject>
|
||||
<ManyObjects>0</ManyObjects>
|
||||
<SizeOfObject>0</SizeOfObject>
|
||||
|
|
@ -410,7 +410,7 @@
|
|||
<BreakIfRCount>1</BreakIfRCount>
|
||||
<Filename></Filename>
|
||||
<ExecCommand></ExecCommand>
|
||||
<Expression>\\baseflight\drv_bmp085.c\254</Expression>
|
||||
<Expression>\\baseflight\src/spektrum.c\71</Expression>
|
||||
</Bp>
|
||||
</Breakpoint>
|
||||
<WatchWindow1>
|
||||
|
|
@ -469,6 +469,12 @@
|
|||
<WinNumber>1</WinNumber>
|
||||
<ItemText>rawMagADC,0x0A</ItemText>
|
||||
</Ww>
|
||||
<Ww>
|
||||
<count>11</count>
|
||||
<WinNumber>1</WinNumber>
|
||||
<ItemText>spekChannelData
|
||||
</ItemText>
|
||||
</Ww>
|
||||
</WatchWindow1>
|
||||
<MemoryWindow1>
|
||||
<Mm>
|
||||
|
|
@ -526,10 +532,10 @@
|
|||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>37</ColumnNumber>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>523</TopLine>
|
||||
<CurrentLine>523</CurrentLine>
|
||||
<TopLine>1</TopLine>
|
||||
<CurrentLine>1</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\cli.c</PathWithFileName>
|
||||
<FilenameWithoutPath>cli.c</FilenameWithoutPath>
|
||||
|
|
@ -540,10 +546,10 @@
|
|||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>20</ColumnNumber>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>149</TopLine>
|
||||
<CurrentLine>162</CurrentLine>
|
||||
<TopLine>102</TopLine>
|
||||
<CurrentLine>125</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\config.c</PathWithFileName>
|
||||
<FilenameWithoutPath>config.c</FilenameWithoutPath>
|
||||
|
|
@ -556,8 +562,8 @@
|
|||
<Focus>0</Focus>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>50</TopLine>
|
||||
<CurrentLine>62</CurrentLine>
|
||||
<TopLine>0</TopLine>
|
||||
<CurrentLine>0</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\imu.c</PathWithFileName>
|
||||
<FilenameWithoutPath>imu.c</FilenameWithoutPath>
|
||||
|
|
@ -568,10 +574,10 @@
|
|||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>43</ColumnNumber>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>35</TopLine>
|
||||
<CurrentLine>51</CurrentLine>
|
||||
<TopLine>43</TopLine>
|
||||
<CurrentLine>50</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\main.c</PathWithFileName>
|
||||
<FilenameWithoutPath>main.c</FilenameWithoutPath>
|
||||
|
|
@ -582,10 +588,10 @@
|
|||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<ColumnNumber>8</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>284</TopLine>
|
||||
<CurrentLine>302</CurrentLine>
|
||||
<TopLine>0</TopLine>
|
||||
<CurrentLine>0</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\mixer.c</PathWithFileName>
|
||||
<FilenameWithoutPath>mixer.c</FilenameWithoutPath>
|
||||
|
|
@ -596,10 +602,10 @@
|
|||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>21</ColumnNumber>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>560</TopLine>
|
||||
<CurrentLine>587</CurrentLine>
|
||||
<TopLine>15</TopLine>
|
||||
<CurrentLine>26</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\mw.c</PathWithFileName>
|
||||
<FilenameWithoutPath>mw.c</FilenameWithoutPath>
|
||||
|
|
@ -610,10 +616,10 @@
|
|||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>31</ColumnNumber>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>281</TopLine>
|
||||
<CurrentLine>299</CurrentLine>
|
||||
<TopLine>0</TopLine>
|
||||
<CurrentLine>0</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\sensors.c</PathWithFileName>
|
||||
<FilenameWithoutPath>sensors.c</FilenameWithoutPath>
|
||||
|
|
@ -626,8 +632,8 @@
|
|||
<Focus>0</Focus>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>205</TopLine>
|
||||
<CurrentLine>209</CurrentLine>
|
||||
<TopLine>0</TopLine>
|
||||
<CurrentLine>0</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\serial.c</PathWithFileName>
|
||||
<FilenameWithoutPath>serial.c</FilenameWithoutPath>
|
||||
|
|
@ -638,10 +644,10 @@
|
|||
<FileType>5</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>18</ColumnNumber>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>1</TopLine>
|
||||
<CurrentLine>20</CurrentLine>
|
||||
<TopLine>19</TopLine>
|
||||
<CurrentLine>43</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\board.h</PathWithFileName>
|
||||
<FilenameWithoutPath>board.h</FilenameWithoutPath>
|
||||
|
|
@ -654,8 +660,8 @@
|
|||
<Focus>0</Focus>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>248</TopLine>
|
||||
<CurrentLine>280</CurrentLine>
|
||||
<TopLine>252</TopLine>
|
||||
<CurrentLine>288</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\mw.h</PathWithFileName>
|
||||
<FilenameWithoutPath>mw.h</FilenameWithoutPath>
|
||||
|
|
@ -666,19 +672,33 @@
|
|||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>1</ColumnNumber>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>35</TopLine>
|
||||
<CurrentLine>71</CurrentLine>
|
||||
<TopLine>1</TopLine>
|
||||
<CurrentLine>4</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\gps.c</PathWithFileName>
|
||||
<FilenameWithoutPath>gps.c</FilenameWithoutPath>
|
||||
</File>
|
||||
<File>
|
||||
<GroupNumber>1</GroupNumber>
|
||||
<FileNumber>0</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>14</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>53</TopLine>
|
||||
<CurrentLine>71</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\spektrum.c</PathWithFileName>
|
||||
<FilenameWithoutPath>spektrum.c</FilenameWithoutPath>
|
||||
</File>
|
||||
</Group>
|
||||
|
||||
<Group>
|
||||
<GroupName>Drivers</GroupName>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<cbSel>0</cbSel>
|
||||
<File>
|
||||
|
|
@ -701,7 +721,7 @@
|
|||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>65</ColumnNumber>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>0</TopLine>
|
||||
<CurrentLine>0</CurrentLine>
|
||||
|
|
@ -729,10 +749,10 @@
|
|||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>42</ColumnNumber>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>1</TopLine>
|
||||
<CurrentLine>29</CurrentLine>
|
||||
<TopLine>0</TopLine>
|
||||
<CurrentLine>0</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\drv_hmc5883l.c</PathWithFileName>
|
||||
<FilenameWithoutPath>drv_hmc5883l.c</FilenameWithoutPath>
|
||||
|
|
@ -745,8 +765,8 @@
|
|||
<Focus>0</Focus>
|
||||
<ColumnNumber>27</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>167</TopLine>
|
||||
<CurrentLine>167</CurrentLine>
|
||||
<TopLine>0</TopLine>
|
||||
<CurrentLine>0</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\drv_i2c.c</PathWithFileName>
|
||||
<FilenameWithoutPath>drv_i2c.c</FilenameWithoutPath>
|
||||
|
|
@ -771,10 +791,10 @@
|
|||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>1</ColumnNumber>
|
||||
<ColumnNumber>47</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>77</TopLine>
|
||||
<CurrentLine>99</CurrentLine>
|
||||
<TopLine>0</TopLine>
|
||||
<CurrentLine>0</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\drv_pwm.c</PathWithFileName>
|
||||
<FilenameWithoutPath>drv_pwm.c</FilenameWithoutPath>
|
||||
|
|
@ -797,12 +817,12 @@
|
|||
<GroupNumber>2</GroupNumber>
|
||||
<FileNumber>19</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<tvExp>1</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>0</TopLine>
|
||||
<CurrentLine>0</CurrentLine>
|
||||
<TopLine>153</TopLine>
|
||||
<CurrentLine>190</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\drv_uart.c</PathWithFileName>
|
||||
<FilenameWithoutPath>drv_uart.c</FilenameWithoutPath>
|
||||
|
|
@ -825,12 +845,12 @@
|
|||
<GroupNumber>2</GroupNumber>
|
||||
<FileNumber>21</FileNumber>
|
||||
<FileType>1</FileType>
|
||||
<tvExp>1</tvExp>
|
||||
<tvExp>0</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>13</ColumnNumber>
|
||||
<ColumnNumber>14</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>421</TopLine>
|
||||
<CurrentLine>451</CurrentLine>
|
||||
<TopLine>0</TopLine>
|
||||
<CurrentLine>0</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\src\drv_mpu6050.c</PathWithFileName>
|
||||
<FilenameWithoutPath>drv_mpu6050.c</FilenameWithoutPath>
|
||||
|
|
@ -974,10 +994,10 @@
|
|||
<FileType>1</FileType>
|
||||
<tvExp>0</tvExp>
|
||||
<Focus>0</Focus>
|
||||
<ColumnNumber>0</ColumnNumber>
|
||||
<ColumnNumber>9</ColumnNumber>
|
||||
<tvExpOptDlg>0</tvExpOptDlg>
|
||||
<TopLine>0</TopLine>
|
||||
<CurrentLine>0</CurrentLine>
|
||||
<TopLine>609</TopLine>
|
||||
<CurrentLine>609</CurrentLine>
|
||||
<bDave2>0</bDave2>
|
||||
<PathWithFileName>.\lib\STM32F10x_StdPeriph_Driver\src\stm32f10x_usart.c</PathWithFileName>
|
||||
<FilenameWithoutPath>stm32f10x_usart.c</FilenameWithoutPath>
|
||||
|
|
|
|||
|
|
@ -441,6 +441,11 @@
|
|||
<FileType>1</FileType>
|
||||
<FilePath>.\src\gps.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>spektrum.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>.\src\spektrum.c</FilePath>
|
||||
</File>
|
||||
</Files>
|
||||
</Group>
|
||||
<Group>
|
||||
|
|
@ -1064,6 +1069,11 @@
|
|||
<FileType>1</FileType>
|
||||
<FilePath>.\src\gps.c</FilePath>
|
||||
</File>
|
||||
<File>
|
||||
<FileName>spektrum.c</FileName>
|
||||
<FileType>1</FileType>
|
||||
<FilePath>.\src\spektrum.c</FilePath>
|
||||
</File>
|
||||
</Files>
|
||||
</Group>
|
||||
<Group>
|
||||
|
|
|
|||
4958
obj/baseflight.hex
4958
obj/baseflight.hex
File diff suppressed because it is too large
Load Diff
11
src/board.h
11
src/board.h
|
|
@ -11,6 +11,10 @@
|
|||
#include "stm32f10x_conf.h"
|
||||
#include "core_cm3.h"
|
||||
|
||||
#ifndef M_PI
|
||||
#define M_PI 3.14159265358979323846
|
||||
#endif /* M_PI */
|
||||
|
||||
typedef enum {
|
||||
SENSOR_ACC = 1 << 0,
|
||||
SENSOR_BARO = 1 << 1,
|
||||
|
|
@ -30,10 +34,13 @@ typedef enum {
|
|||
FEATURE_GYRO_SMOOTHING = 1 << 7,
|
||||
FEATURE_LED_RING = 1 << 8,
|
||||
FEATURE_GPS = 1 << 9,
|
||||
FEATURE_SPEKTRUM = 1 << 10,
|
||||
} AvailableFeatures;
|
||||
|
||||
typedef void (* sensorInitFuncPtr)(void);
|
||||
typedef void (* sensorReadFuncPtr)(int16_t *data);
|
||||
typedef void (* sensorInitFuncPtr)(void); // sensor init prototype
|
||||
typedef void (* sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype
|
||||
typedef void (* uartReceiveCallbackPtr)(uint16_t data); // used by uart2 driver to return frames to app
|
||||
typedef uint16_t (* rcReadRawDataPtr)(uint8_t chan); // used by receiver driver to return channel data
|
||||
|
||||
typedef struct sensor_t
|
||||
{
|
||||
|
|
|
|||
|
|
@ -36,6 +36,7 @@ const char *mixerNames[] = {
|
|||
const char *featureNames[] = {
|
||||
"PPM", "VBAT", "INFLIGHT_ACC_CAL", "DIGITAL_SERVO", "MOTOR_STOP",
|
||||
"SERVO_TILT", "CAMTRIG", "GYRO_SMOOTHING", "LED_RING", "GPS",
|
||||
"SPEKTRUM",
|
||||
NULL
|
||||
};
|
||||
|
||||
|
|
@ -88,6 +89,7 @@ const clivalue_t valueTable[] = {
|
|||
{ "mincommand", VAR_UINT16, &cfg.mincommand, 0, 2000 },
|
||||
{ "mincheck", VAR_UINT16, &cfg.mincheck, 0, 2000 },
|
||||
{ "maxcheck", VAR_UINT16, &cfg.maxcheck, 0, 2000 },
|
||||
{ "spektrum_hires", VAR_UINT8, &cfg.spektrum_hires, 0, 1 },
|
||||
{ "vbatscale", VAR_UINT8, &cfg.vbatscale, 10, 200 },
|
||||
{ "vbatmaxcellvoltage", VAR_UINT8, &cfg.vbatmaxcellvoltage, 10, 50 },
|
||||
{ "vbatmincellvoltage", VAR_UINT8, &cfg.vbatmincellvoltage, 10, 50 },
|
||||
|
|
@ -99,6 +101,8 @@ const clivalue_t valueTable[] = {
|
|||
{ "tri_yaw_max", VAR_UINT16, &cfg.tri_yaw_max, 0, 2000 },
|
||||
{ "tilt_pitch_prop", VAR_INT8, &cfg.tilt_pitch_prop, -100, 100 },
|
||||
{ "tilt_roll_prop", VAR_INT8, &cfg.tilt_roll_prop, -100, 100 },
|
||||
{ "acc_lpf_factor", VAR_UINT8, &cfg.acc_lpf_factor, 0, 32 },
|
||||
{ "gyro_lpf", VAR_UINT16, &cfg.gyro_lpf, 0, 256 },
|
||||
{ "p_pitch", VAR_UINT8, &cfg.P8[PITCH], 0, 200},
|
||||
{ "i_pitch", VAR_UINT8, &cfg.I8[PITCH], 0, 200},
|
||||
{ "d_pitch", VAR_UINT8, &cfg.D8[PITCH], 0, 200},
|
||||
|
|
|
|||
|
|
@ -9,7 +9,7 @@ config_t cfg;
|
|||
const char rcChannelLetters[] = "AERT1234";
|
||||
|
||||
static uint32_t enabledSensors = 0;
|
||||
static uint8_t checkNewConf = 6;
|
||||
static uint8_t checkNewConf = 7;
|
||||
|
||||
void parseRcChannels(const char *input)
|
||||
{
|
||||
|
|
@ -112,6 +112,8 @@ void checkFirstTime(bool reset)
|
|||
}
|
||||
cfg.accTrim[0] = 0;
|
||||
cfg.accTrim[1] = 0;
|
||||
cfg.acc_lpf_factor = 4;
|
||||
cfg.gyro_lpf = 42;
|
||||
cfg.gyro_smoothing_factor = 0x00141403; // default factors of 20, 20, 3 for R/P/Y
|
||||
cfg.powerTrigger1 = 0;
|
||||
cfg.vbatscale = 110;
|
||||
|
|
@ -120,8 +122,8 @@ void checkFirstTime(bool reset)
|
|||
|
||||
// Radio/ESC
|
||||
parseRcChannels("AETR1234");
|
||||
// parseRcChannels("ATER1234");
|
||||
cfg.deadband = 0;
|
||||
cfg.spektrum_hires = 0;
|
||||
cfg.midrc = 1500;
|
||||
cfg.mincheck = 1100;
|
||||
cfg.maxcheck = 1900;
|
||||
|
|
|
|||
|
|
@ -57,14 +57,8 @@ static void adxl345Init(void)
|
|||
#else
|
||||
// MWC defaults
|
||||
i2cWrite(ADXL345_ADDRESS, ADXL345_POWER_CTL, 1 << 3); // register: Power CTRL -- value: Set measure bit 3 on
|
||||
#if 1
|
||||
i2cWrite(ADXL345_ADDRESS, ADXL345_DATA_FORMAT, 0x0B); // register: DATA_FORMAT -- value: Set bits 3(full range) and 1 0 on (+/- 16g-range)
|
||||
i2cWrite(ADXL345_ADDRESS, ADXL345_BW_RATE, 0x09); // register: BW_RATE -- value: rate=50hz, bw=20hz
|
||||
#else
|
||||
// testing
|
||||
i2cWrite(ADXL345_ADDRESS, ADXL345_DATA_FORMAT, (ADXL_RANGE_8G & 0x03) | ADXL_FULL_RES); // register: DATA_FORMAT -- value: Set bits 3(full range) and 1 0 on (+/- 16g-range)
|
||||
i2cWrite(ADXL345_ADDRESS, ADXL345_BW_RATE, ADXL_RATE_800); // register: BW_RATE -- value: rate=50hz, bw=20hz
|
||||
#endif
|
||||
#endif /* FreeFlight */
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -35,7 +35,6 @@ typedef struct {
|
|||
uint8_t sensortype;
|
||||
|
||||
int32_t param_b5;
|
||||
int32_t number_of_samples;
|
||||
int16_t oversampling_setting;
|
||||
int16_t smd500_t_resolution, smd500_masterclock;
|
||||
|
||||
|
|
@ -129,7 +128,6 @@ bool bmp085Init(void)
|
|||
p_bmp085->dev_addr = BMP085_I2C_ADDR; /* preset BMP085 I2C_addr */
|
||||
i2cRead(p_bmp085->dev_addr, BMP085_CHIP_ID__REG, 1, &data); /* read Chip Id */
|
||||
p_bmp085->chip_id = BMP085_GET_BITSLICE(data, BMP085_CHIP_ID);
|
||||
p_bmp085->number_of_samples = 1;
|
||||
p_bmp085->oversampling_setting = 2;
|
||||
|
||||
if (p_bmp085->chip_id == BMP085_CHIP_ID) { /* get bitslice */
|
||||
|
|
@ -162,11 +160,15 @@ int32_t bmp085_read_pressure(void)
|
|||
return bmp085_get_pressure(bmp085_get_up());
|
||||
}
|
||||
|
||||
// #define BMP_TEMP_OSS 4
|
||||
|
||||
int16_t bmp085_get_temperature(uint32_t ut)
|
||||
{
|
||||
int16_t temperature;
|
||||
int32_t x1, x2;
|
||||
#ifdef BMP_TEMP_OSS
|
||||
static uint32_t temp;
|
||||
#endif
|
||||
|
||||
if (p_bmp085->sensortype == BOSCH_PRESSURE_BMP085) {
|
||||
x1 = (((int32_t) ut - (int32_t) p_bmp085->cal_param.ac6) * (int32_t) p_bmp085->cal_param.ac5) >> 15;
|
||||
|
|
@ -175,7 +177,14 @@ int16_t bmp085_get_temperature(uint32_t ut)
|
|||
}
|
||||
temperature = ((p_bmp085->param_b5 + 8) >> 4); // temperature in 0.1°C
|
||||
|
||||
#ifdef BMP_TEMP_OSS
|
||||
temp *= (1 << BMP_TEMP_OSS) - 1; // multiply the temperature variable by 3 - we have tau == 1/4
|
||||
temp += ((uint32_t)temperature) << 8; // add on the buffer
|
||||
temp >>= BMP_TEMP_OSS; // divide by 4
|
||||
return (int16_t)temp;
|
||||
#else
|
||||
return temperature;
|
||||
#endif
|
||||
}
|
||||
|
||||
int32_t bmp085_get_pressure(uint32_t up)
|
||||
|
|
@ -266,7 +275,6 @@ uint32_t bmp085_get_up(void)
|
|||
|
||||
i2cRead(p_bmp085->dev_addr, BMP085_ADC_OUT_MSB_REG, 3, data);
|
||||
up = (((uint32_t) data[0] << 16) | ((uint32_t) data[1] << 8) | (uint32_t) data[2]) >> (8 - p_bmp085->oversampling_setting);
|
||||
p_bmp085->number_of_samples = 1;
|
||||
|
||||
return up;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -14,7 +14,6 @@ bool hmc5883lDetect(void)
|
|||
if (!ack || sig != 'H')
|
||||
return false;
|
||||
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
@ -22,20 +21,20 @@ void hmc5883lInit(void)
|
|||
{
|
||||
delay(100);
|
||||
// force positiveBias
|
||||
i2cWrite(MAG_ADDRESS, 0x00, 0x71); //Configuration Register A -- 0 11 100 01 num samples: 8 ; output rate: 15Hz ; positive bias
|
||||
i2cWrite(MAG_ADDRESS, 0x00, 0x71); // Configuration Register A -- 0 11 100 01 num samples: 8 ; output rate: 15Hz ; positive bias
|
||||
delay(50);
|
||||
// set gains for calibration
|
||||
i2cWrite(MAG_ADDRESS, 0x01, 0x60); //Configuration Register B -- 011 00000 configuration gain 2.5Ga
|
||||
i2cWrite(MAG_ADDRESS, 0x02, 0x01); //Mode register -- 000000 01 single Conversion Mode
|
||||
i2cWrite(MAG_ADDRESS, 0x01, 0x60); // Configuration Register B -- 011 00000 configuration gain 2.5Ga
|
||||
i2cWrite(MAG_ADDRESS, 0x02, 0x01); // Mode register -- 000000 01 single Conversion Mode
|
||||
// this enters test mode
|
||||
}
|
||||
|
||||
void hmc5883lFinishCal(void)
|
||||
{
|
||||
// leave test mode
|
||||
i2cWrite(MAG_ADDRESS, 0x00, 0x70); //Configuration Register A -- 0 11 100 00 num samples: 8 ; output rate: 15Hz ; normal measurement mode
|
||||
i2cWrite(MAG_ADDRESS, 0x01, 0x20); //Configuration Register B -- 001 00000 configuration gain 1.3Ga
|
||||
i2cWrite(MAG_ADDRESS, 0x02, 0x00); //Mode register -- 000000 00 continuous Conversion Mode
|
||||
i2cWrite(MAG_ADDRESS, 0x00, 0x70); // Configuration Register A -- 0 11 100 00 num samples: 8 ; output rate: 15Hz ; normal measurement mode
|
||||
i2cWrite(MAG_ADDRESS, 0x01, 0x20); // Configuration Register B -- 001 00000 configuration gain 1.3Ga
|
||||
i2cWrite(MAG_ADDRESS, 0x02, 0x00); // Mode register -- 000000 00 continuous Conversion Mode
|
||||
}
|
||||
|
||||
void hmc5883lRead(int16_t *magData)
|
||||
|
|
|
|||
|
|
@ -14,6 +14,7 @@
|
|||
|
||||
// Bits
|
||||
#define MPU3050_FS_SEL_2000DPS 0x18
|
||||
#define MPU3050_DLPF_10HZ 0x05
|
||||
#define MPU3050_DLPF_20HZ 0x04
|
||||
#define MPU3050_DLPF_42HZ 0x03
|
||||
#define MPU3050_DLPF_98HZ 0x02
|
||||
|
|
@ -23,7 +24,7 @@
|
|||
#define MPU3050_USER_RESET 0x01
|
||||
#define MPU3050_CLK_SEL_PLL_GX 0x01
|
||||
|
||||
static uint8_t mpuLowPassFliter = MPU3050_DLPF_42HZ;
|
||||
static uint8_t mpuLowPassFilter = MPU3050_DLPF_42HZ;
|
||||
|
||||
static void mpu3050Init(void);
|
||||
static void mpu3050Read(int16_t *gyroData);
|
||||
|
|
@ -46,6 +47,32 @@ bool mpu3050Detect(sensor_t *gyro)
|
|||
return true;
|
||||
}
|
||||
|
||||
void mpu3050Config(uint16_t lpf)
|
||||
{
|
||||
switch (lpf) {
|
||||
case 256:
|
||||
mpuLowPassFilter = MPU3050_DLPF_256HZ;
|
||||
break;
|
||||
case 188:
|
||||
mpuLowPassFilter = MPU3050_DLPF_188HZ;
|
||||
break;
|
||||
case 98:
|
||||
mpuLowPassFilter = MPU3050_DLPF_98HZ;
|
||||
break;
|
||||
case 42:
|
||||
mpuLowPassFilter = MPU3050_DLPF_42HZ;
|
||||
break;
|
||||
case 20:
|
||||
mpuLowPassFilter = MPU3050_DLPF_20HZ;
|
||||
break;
|
||||
case 10:
|
||||
mpuLowPassFilter = MPU3050_DLPF_10HZ;
|
||||
break;
|
||||
}
|
||||
|
||||
i2cWrite(MPU3050_ADDRESS, MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | mpuLowPassFilter);
|
||||
}
|
||||
|
||||
static void mpu3050Init(void)
|
||||
{
|
||||
bool ack;
|
||||
|
|
@ -56,7 +83,7 @@ static void mpu3050Init(void)
|
|||
if (!ack)
|
||||
failureMode(3);
|
||||
|
||||
i2cWrite(MPU3050_ADDRESS, MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | mpuLowPassFliter);
|
||||
i2cWrite(MPU3050_ADDRESS, MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | mpuLowPassFilter);
|
||||
i2cWrite(MPU3050_ADDRESS, MPU3050_INT_CFG, 0);
|
||||
i2cWrite(MPU3050_ADDRESS, MPU3050_USER_CTRL, MPU3050_USER_RESET);
|
||||
i2cWrite(MPU3050_ADDRESS, MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
|
||||
|
|
|
|||
|
|
@ -1,3 +1,4 @@
|
|||
#pragma once
|
||||
|
||||
bool mpu3050Detect(sensor_t *gyro);
|
||||
void mpu3050Config(uint16_t lpf);
|
||||
|
|
|
|||
122
src/drv_pwm.c
122
src/drv_pwm.c
|
|
@ -154,65 +154,12 @@ static void pwmIRQHandler(TIM_TypeDef *tim)
|
|||
}
|
||||
}
|
||||
|
||||
bool pwmInit(bool usePPM, bool useServos, bool useDigitalServos)
|
||||
static void pwmInitializeInput(bool usePPM)
|
||||
{
|
||||
GPIO_InitTypeDef GPIO_InitStructure;
|
||||
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
|
||||
TIM_OCInitTypeDef TIM_OCInitStructure;
|
||||
NVIC_InitTypeDef NVIC_InitStructure;
|
||||
|
||||
uint8_t i, val;
|
||||
uint16_t c;
|
||||
bool throttleCal = false;
|
||||
|
||||
// Inputs
|
||||
|
||||
// RX1 TIM2_CH1 PA0 [also PPM] [also used for throttle calibration]
|
||||
// RX2 TIM2_CH2 PA1
|
||||
// RX3 TIM2_CH3 PA2 [also UART2_TX]
|
||||
// RX4 TIM2_CH4 PA3 [also UART2_RX]
|
||||
// RX5 TIM3_CH1 PA6 [also ADC_IN6]
|
||||
// RX6 TIM3_CH2 PA7 [also ADC_IN7]
|
||||
// RX7 TIM3_CH3 PB0 [also ADC_IN8]
|
||||
// RX8 TIM3_CH4 PB1 [also ADC_IN9]
|
||||
|
||||
// Outputs
|
||||
// PWM1 TIM1_CH1 PA8
|
||||
// PWM2 TIM1_CH4 PA11
|
||||
// PWM3 TIM4_CH1 PB6 [also I2C1_SCL]
|
||||
// PWM4 TIM4_CH2 PB7 [also I2C1_SDA]
|
||||
// PWM5 TIM4_CH3 PB8
|
||||
// PWM6 TIM4_CH4 PB9
|
||||
|
||||
// automatic throttle calibration detection: PA0 to ground via bindplug
|
||||
// Configure TIM2_CH1 for input
|
||||
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;
|
||||
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
|
||||
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
|
||||
GPIO_Init(GPIOA, &GPIO_InitStructure);
|
||||
|
||||
#if 0
|
||||
// wait a while
|
||||
delay(100);
|
||||
|
||||
for (c = 0; c < 50000; c++) {
|
||||
val = GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_0);
|
||||
if (val) {
|
||||
throttleCal = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
// use PPM or PWM input
|
||||
usePPMFlag = usePPM;
|
||||
|
||||
// preset channels to center
|
||||
for (i = 0; i < 8; i++)
|
||||
Inputs[i].capture = 1500;
|
||||
|
||||
// Timers run at 1mhz.
|
||||
// TODO: clean this shit up. Make it all dynamic etc.
|
||||
uint8_t i;
|
||||
|
||||
// Input pins
|
||||
if (usePPM) {
|
||||
|
|
@ -295,6 +242,68 @@ bool pwmInit(bool usePPM, bool useServos, bool useDigitalServos)
|
|||
// In PWM input mode, all 8 channels are wasted
|
||||
numOutputChannels = 6;
|
||||
}
|
||||
}
|
||||
|
||||
bool pwmInit(bool usePPM, bool pwmppmInput, bool useServos, bool useDigitalServos)
|
||||
{
|
||||
GPIO_InitTypeDef GPIO_InitStructure;
|
||||
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
|
||||
TIM_OCInitTypeDef TIM_OCInitStructure;
|
||||
|
||||
uint8_t i, val;
|
||||
uint16_t c;
|
||||
bool throttleCal = false;
|
||||
|
||||
// Inputs
|
||||
|
||||
// RX1 TIM2_CH1 PA0 [also PPM] [also used for throttle calibration]
|
||||
// RX2 TIM2_CH2 PA1
|
||||
// RX3 TIM2_CH3 PA2 [also UART2_TX]
|
||||
// RX4 TIM2_CH4 PA3 [also UART2_RX]
|
||||
// RX5 TIM3_CH1 PA6 [also ADC_IN6]
|
||||
// RX6 TIM3_CH2 PA7 [also ADC_IN7]
|
||||
// RX7 TIM3_CH3 PB0 [also ADC_IN8]
|
||||
// RX8 TIM3_CH4 PB1 [also ADC_IN9]
|
||||
|
||||
// Outputs
|
||||
// PWM1 TIM1_CH1 PA8
|
||||
// PWM2 TIM1_CH4 PA11
|
||||
// PWM3 TIM4_CH1 PB6 [also I2C1_SCL]
|
||||
// PWM4 TIM4_CH2 PB7 [also I2C1_SDA]
|
||||
// PWM5 TIM4_CH3 PB8
|
||||
// PWM6 TIM4_CH4 PB9
|
||||
|
||||
// automatic throttle calibration detection: PA0 to ground via bindplug
|
||||
// Configure TIM2_CH1 for input
|
||||
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;
|
||||
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
|
||||
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
|
||||
GPIO_Init(GPIOA, &GPIO_InitStructure);
|
||||
|
||||
#if 0
|
||||
// wait a while
|
||||
delay(100);
|
||||
|
||||
for (c = 0; c < 50000; c++) {
|
||||
val = GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_0);
|
||||
if (val) {
|
||||
throttleCal = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
// use PPM or PWM input
|
||||
usePPMFlag = usePPM;
|
||||
|
||||
// preset channels to center
|
||||
for (i = 0; i < 8; i++)
|
||||
Inputs[i].capture = 1500;
|
||||
|
||||
// Timers run at 1mhz.
|
||||
// TODO: clean this shit up. Make it all dynamic etc.
|
||||
if (pwmppmInput)
|
||||
pwmInitializeInput(usePPMFlag);
|
||||
|
||||
// Output pins
|
||||
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8 | GPIO_Pin_11;
|
||||
|
|
@ -341,7 +350,8 @@ bool pwmInit(bool usePPM, bool useServos, bool useDigitalServos)
|
|||
TIM_CtrlPWMOutputs(TIM1, ENABLE);
|
||||
TIM_CtrlPWMOutputs(TIM4, ENABLE);
|
||||
|
||||
if (usePPM) {
|
||||
// turn on more motor outputs if we're using ppm / not using pwm input
|
||||
if (!pwmppmInput || usePPM) {
|
||||
// PWM 7,8,9,10
|
||||
TIM_TimeBaseStructure.TIM_Period = PULSE_PERIOD - 1;
|
||||
TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
#pragma once
|
||||
|
||||
bool pwmInit(bool usePPM, bool useServos, bool useDigitalServos); // returns whether driver is asking to calibrate throttle or not
|
||||
bool pwmInit(bool usePPM, bool pwmppmInput, bool useServos, bool useDigitalServos); // returns whether driver is asking to calibrate throttle or not
|
||||
void pwmWrite(uint8_t channel, uint16_t value);
|
||||
uint16_t pwmRead(uint8_t channel);
|
||||
uint8_t pwmGetNumOutputChannels(void);
|
||||
|
|
|
|||
|
|
@ -145,3 +145,45 @@ void uartPrint(char *str)
|
|||
while (*str)
|
||||
uartWrite(*(str++));
|
||||
}
|
||||
|
||||
/* -------------------------- UART2 (Spektrum, GPS) ----------------------------- */
|
||||
uartReceiveCallbackPtr uart2Callback = NULL;
|
||||
|
||||
void uart2Init(uint32_t speed, uartReceiveCallbackPtr func)
|
||||
{
|
||||
NVIC_InitTypeDef NVIC_InitStructure;
|
||||
GPIO_InitTypeDef GPIO_InitStructure;
|
||||
USART_InitTypeDef USART_InitStructure;
|
||||
|
||||
RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE);
|
||||
|
||||
NVIC_InitStructure.NVIC_IRQChannel = USART2_IRQn;
|
||||
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
|
||||
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
|
||||
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
|
||||
NVIC_Init(&NVIC_InitStructure);
|
||||
|
||||
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3;
|
||||
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
|
||||
GPIO_Init(GPIOA, &GPIO_InitStructure);
|
||||
|
||||
USART_InitStructure.USART_BaudRate = speed;
|
||||
USART_InitStructure.USART_WordLength = USART_WordLength_8b;
|
||||
USART_InitStructure.USART_StopBits = USART_StopBits_1;
|
||||
USART_InitStructure.USART_Parity = USART_Parity_No;
|
||||
USART_InitStructure.USART_Mode = USART_Mode_Rx;
|
||||
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
|
||||
USART_Init(USART2, &USART_InitStructure);
|
||||
|
||||
USART_ITConfig(USART2, USART_IT_RXNE, ENABLE);
|
||||
USART_Cmd(USART2, ENABLE);
|
||||
uart2Callback = func;
|
||||
}
|
||||
|
||||
void USART2_IRQHandler(void)
|
||||
{
|
||||
if (USART_GetITStatus(USART2, USART_IT_RXNE) == SET) {
|
||||
if (uart2Callback)
|
||||
uart2Callback(USART_ReceiveData(USART2));
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -6,3 +6,4 @@ uint8_t uartRead(void);
|
|||
uint8_t uartReadPoll(void);
|
||||
void uartWrite(uint8_t ch);
|
||||
void uartPrint(char *str);
|
||||
void uart2Init(uint32_t speed, uartReceiveCallbackPtr func);
|
||||
|
|
|
|||
57
src/gps.c
57
src/gps.c
|
|
@ -1,10 +1,6 @@
|
|||
#include "board.h"
|
||||
#include "mw.h"
|
||||
|
||||
#ifndef PI
|
||||
#define PI 3.14159265358979323846
|
||||
#endif
|
||||
|
||||
#ifndef sq
|
||||
#define sq(x) ((x)*(x))
|
||||
#endif
|
||||
|
|
@ -13,52 +9,9 @@ static void GPS_NewData(uint16_t c);
|
|||
static bool GPS_newFrame(char c);
|
||||
static void GPS_distance(int32_t lat1, int32_t lon1, int32_t lat2, int32_t lon2, uint16_t * dist, int16_t * bearing);
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
*
|
||||
* GPS low level routines
|
||||
*
|
||||
*-----------------------------------------------------------*/
|
||||
|
||||
void USART2_IRQHandler(void)
|
||||
{
|
||||
if (USART_GetITStatus(USART2, USART_IT_RXNE) == SET) {
|
||||
GPS_NewData(USART_ReceiveData(USART2));
|
||||
}
|
||||
}
|
||||
|
||||
static void uart2Init(void)
|
||||
{
|
||||
NVIC_InitTypeDef NVIC_InitStructure;
|
||||
GPIO_InitTypeDef GPIO_InitStructure;
|
||||
USART_InitTypeDef USART_InitStructure;
|
||||
|
||||
RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE);
|
||||
|
||||
NVIC_InitStructure.NVIC_IRQChannel = USART2_IRQn;
|
||||
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
|
||||
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
|
||||
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
|
||||
NVIC_Init(&NVIC_InitStructure);
|
||||
|
||||
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3;
|
||||
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
|
||||
GPIO_Init(GPIOA, &GPIO_InitStructure);
|
||||
|
||||
USART_InitStructure.USART_BaudRate = 9600;
|
||||
USART_InitStructure.USART_WordLength = USART_WordLength_8b;
|
||||
USART_InitStructure.USART_StopBits = USART_StopBits_1;
|
||||
USART_InitStructure.USART_Parity = USART_Parity_No;
|
||||
USART_InitStructure.USART_Mode = USART_Mode_Rx;
|
||||
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
|
||||
USART_Init(USART2, &USART_InitStructure);
|
||||
|
||||
USART_ITConfig(USART2, USART_IT_RXNE, ENABLE);
|
||||
USART_Cmd(USART2, ENABLE);
|
||||
}
|
||||
|
||||
void gpsInit(void)
|
||||
{
|
||||
uart2Init();
|
||||
uart2Init(9600, GPS_NewData);
|
||||
sensorsSet(SENSOR_GPS);
|
||||
}
|
||||
|
||||
|
|
@ -98,9 +51,9 @@ static void GPS_NewData(uint16_t c)
|
|||
static void GPS_distance(int32_t lat1, int32_t lon1, int32_t lat2, int32_t lon2, uint16_t * dist, int16_t * bearing)
|
||||
{
|
||||
float dLat = (lat2 - lat1); // difference of latitude in 1/100000 degrees
|
||||
float dLon = (lon2 - lon1) * cos(lat1 * (PI / 180 / 100000.0)); // difference of longitude in 1/100000 degrees
|
||||
*dist = 6372795 / 100000.0 * PI / 180 * (sqrt(sq(dLat) + sq(dLon)));
|
||||
*bearing = 180 / PI * (atan2(dLon, dLat));
|
||||
float dLon = (lon2 - lon1) * cosf(lat1 * (M_PI / 180 / 100000.0)); // difference of longitude in 1/100000 degrees
|
||||
*dist = 6372795.0 / 100000.0 * M_PI / 180.0 * (sqrtf(sq(dLat) + sq(dLon)));
|
||||
*bearing = 180.0 / M_PI * (atan2f(dLon, dLat));
|
||||
}
|
||||
|
||||
/* The latitude or longitude is coded this way in NMEA frames
|
||||
|
|
@ -231,7 +184,7 @@ static bool GPS_newFrame(char c)
|
|||
else
|
||||
parity ^= c;
|
||||
} else if (c == '\r' || c == '\n') {
|
||||
if (checksum_param) { //parity checksum
|
||||
if (checksum_param) { // parity checksum
|
||||
uint8_t checksum = hex_c(string[0]);
|
||||
checksum <<= 4;
|
||||
checksum += hex_c(string[1]);
|
||||
|
|
|
|||
87
src/imu.c
87
src/imu.c
|
|
@ -1,8 +1,6 @@
|
|||
#include "board.h"
|
||||
#include "mw.h"
|
||||
|
||||
#define M_PI 3.14159265358979323846
|
||||
|
||||
int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3];
|
||||
int16_t acc_25deg = 0;
|
||||
int32_t BaroAlt;
|
||||
|
|
@ -23,7 +21,7 @@ static void getEstimatedAttitude(void);
|
|||
|
||||
void imuInit(void)
|
||||
{
|
||||
acc_25deg = acc_1G * 0.423;
|
||||
acc_25deg = acc_1G * 0.423f;
|
||||
|
||||
// if mag sensor is enabled, use it
|
||||
if (sensors(SENSOR_MAG))
|
||||
|
|
@ -53,7 +51,7 @@ void computeIMU(void)
|
|||
if ((micros() - timeInterleave) > 650) {
|
||||
annex650_overrun_count++;
|
||||
} else {
|
||||
while ((micros() - timeInterleave) < 650); //empirical, interleaving delay between 2 consecutive reads
|
||||
while ((micros() - timeInterleave) < 650); // empirical, interleaving delay between 2 consecutive reads
|
||||
}
|
||||
|
||||
Gyro_getADC();
|
||||
|
|
@ -108,11 +106,6 @@ void computeIMU(void)
|
|||
// **************************************************
|
||||
|
||||
//****** advanced users settings *******************
|
||||
/* Set the Low Pass Filter factor for ACC */
|
||||
/* Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time*/
|
||||
/* Comment this if you do not want filter at all.*/
|
||||
/* Default WMC value: 8*/
|
||||
#define ACC_LPF_FACTOR 4
|
||||
|
||||
/* Set the Low Pass Filter factor for Magnetometer */
|
||||
/* Increasing this value would reduce Magnetometer noise (not visible in GUI), but would increase Magnetometer lag time*/
|
||||
|
|
@ -176,9 +169,7 @@ static void getEstimatedAttitude(void)
|
|||
#if defined(MG_LPF_FACTOR)
|
||||
static int16_t mgSmooth[3];
|
||||
#endif
|
||||
#if defined(ACC_LPF_FACTOR)
|
||||
static int16_t accTemp[3]; //projection of smoothed and normalized magnetic vector on x/y/z axis, as measured by magnetometer
|
||||
#endif
|
||||
static uint32_t previousT;
|
||||
uint32_t currentT = micros();
|
||||
float scale, deltaGyroAngle[3];
|
||||
|
|
@ -189,15 +180,14 @@ static void getEstimatedAttitude(void)
|
|||
// Initialization
|
||||
for (axis = 0; axis < 3; axis++) {
|
||||
deltaGyroAngle[axis] = gyroADC[axis] * scale;
|
||||
#if defined(ACC_LPF_FACTOR)
|
||||
accTemp[axis] = (accTemp[axis] - (accTemp[axis] >> ACC_LPF_FACTOR)) + accADC[axis];
|
||||
accSmooth[axis] = accTemp[axis] >> ACC_LPF_FACTOR;
|
||||
#define ACC_VALUE accSmooth[axis]
|
||||
#else
|
||||
|
||||
if (cfg.acc_lpf_factor > 0) {
|
||||
accTemp[axis] = (accTemp[axis] - (accTemp[axis] >> cfg.acc_lpf_factor)) + accADC[axis];
|
||||
accSmooth[axis] = accTemp[axis] >> cfg.acc_lpf_factor;
|
||||
} else {
|
||||
accSmooth[axis] = accADC[axis];
|
||||
#define ACC_VALUE accADC[axis]
|
||||
#endif
|
||||
accMag += (int32_t) ACC_VALUE * ACC_VALUE;
|
||||
}
|
||||
accMag += (int32_t)accSmooth[axis] * accSmooth[axis];
|
||||
|
||||
if (sensors(SENSOR_MAG)) {
|
||||
#if defined(MG_LPF_FACTOR)
|
||||
|
|
@ -208,7 +198,7 @@ static void getEstimatedAttitude(void)
|
|||
#endif
|
||||
}
|
||||
}
|
||||
accMag = accMag * 100 / ((int32_t) acc_1G * acc_1G);
|
||||
accMag = accMag * 100 / ((int32_t)acc_1G * acc_1G);
|
||||
|
||||
rotateV(&EstG.V, deltaGyroAngle);
|
||||
if (sensors(SENSOR_MAG)) {
|
||||
|
|
@ -223,18 +213,12 @@ static void getEstimatedAttitude(void)
|
|||
// Apply complimentary filter (Gyro drift correction)
|
||||
// If accel magnitude >1.4G or <0.6G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
|
||||
// To do that, we just skip filter, as EstV already rotated by Gyro
|
||||
if ((36 < accMag && accMag < 196) || smallAngle25)
|
||||
if ((36 < accMag && accMag < 196) || smallAngle25) {
|
||||
for (axis = 0; axis < 3; axis++) {
|
||||
int16_t acc = ACC_VALUE;
|
||||
#if !defined(TRUSTED_ACCZ)
|
||||
if (smallAngle25 && axis == YAW)
|
||||
//We consider ACCZ = acc_1G when the acc on other axis is small.
|
||||
//It's a tweak to deal with some configs where ACC_Z tends to a value < acc_1G when high throttle is applied.
|
||||
//This tweak applies only when the multi is not in inverted position
|
||||
acc = acc_1G;
|
||||
#endif
|
||||
int16_t acc = accSmooth[axis];
|
||||
EstG.A[axis] = (EstG.A[axis] * GYR_CMPF_FACTOR + acc) * INV_GYR_CMPF_FACTOR;
|
||||
}
|
||||
}
|
||||
|
||||
if (sensors(SENSOR_MAG)) {
|
||||
for (axis = 0; axis < 3; axis++)
|
||||
|
|
@ -246,53 +230,8 @@ static void getEstimatedAttitude(void)
|
|||
angle[PITCH] = _atan2f(EstG.V.Y, EstG.V.Z);
|
||||
|
||||
if (sensors(SENSOR_MAG)) {
|
||||
#define GHETTO
|
||||
|
||||
#ifdef GHETTO
|
||||
// Attitude of the cross product vector GxM
|
||||
heading = _atan2f(EstG.V.X * EstM.V.Z - EstG.V.Z * EstM.V.X, EstG.V.Z * EstM.V.Y - EstG.V.Y * EstM.V.Z) / 10;
|
||||
#else
|
||||
static float Cos_Roll, Sin_Roll, Cos_Pitch, Sin_Pitch;
|
||||
static float Mx1, My1, Mz1, xh, yh;
|
||||
static float rollRadians;
|
||||
static float pitchRadians;
|
||||
|
||||
// proper tilt compensation
|
||||
// Get pitch and roll in radians
|
||||
rollRadians = angle[ROLL] / 1800.0 * M_PI;
|
||||
pitchRadians = angle[PITCH] /1800.0 * M_PI;
|
||||
|
||||
//rollRadians = _atan2f(accADC[ROLL], accADC[YAW])/1800.0*M_PI;
|
||||
//pitchRadians = _atan2f(accADC[PITCH], accADC[YAW])/1800.0*M_PI;
|
||||
|
||||
// Mx2 and My2 are the corrected values
|
||||
// Mx1, My1 and Mz1 are the floating point values from the mag sensor
|
||||
//Mx1 = magADC[ROLL];
|
||||
//My1 = magADC[PITCH];
|
||||
//Mz1 = magADC[YAW];
|
||||
|
||||
Mx1 = EstM.V.X;
|
||||
My1 = EstM.V.Y;
|
||||
Mz1 = EstM.V.Z;
|
||||
|
||||
// These are used more than once, so pre-calculate for efficiency
|
||||
Cos_Roll = cosf(rollRadians);
|
||||
Cos_Pitch = cosf(pitchRadians);
|
||||
Sin_Roll = sinf(rollRadians);
|
||||
Sin_Pitch = sinf(pitchRadians);
|
||||
|
||||
// The tilt-compensation equations are as follows
|
||||
//X_h=X*cos(pitch)+Y*sin(roll)sin(pitch)-Z*cos(roll)*sin(pitch)
|
||||
//Y_h=Y*cos(roll)+Z*sin(roll)
|
||||
xh = (Mx1 * Cos_Pitch) + (My1 * Sin_Roll * Sin_Pitch) - (Mz1 * Sin_Pitch * Cos_Roll); // Correct x axis
|
||||
yh = (My1 * Cos_Roll) + (Mz1 * Sin_Roll); // Correct y axis
|
||||
|
||||
// Tilt-adjusted heading in degrees
|
||||
heading = _atan2f(yh, xh) / 10;
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
16
src/main.c
16
src/main.c
|
|
@ -2,6 +2,11 @@
|
|||
#include "mw.h"
|
||||
|
||||
extern uint8_t useServo;
|
||||
extern rcReadRawDataPtr rcReadRawFunc;
|
||||
|
||||
// two receiver read functions
|
||||
extern uint16_t pwmReadRawRC(uint8_t chan);
|
||||
extern uint16_t spektrumReadRawRC(uint8_t chan);
|
||||
|
||||
void throttleCalibration(void)
|
||||
{
|
||||
|
|
@ -48,9 +53,12 @@ int main(void)
|
|||
|
||||
mixerInit(); // this will set useServo var depending on mixer type
|
||||
// pwmInit returns true if throttle calibration is requested. if so, do it here. throttleCalibration() does NOT return - for safety.
|
||||
if (pwmInit(feature(FEATURE_PPM), useServo, feature(FEATURE_DIGITAL_SERVO)))
|
||||
if (pwmInit(feature(FEATURE_PPM), !feature(FEATURE_SPEKTRUM), useServo, feature(FEATURE_DIGITAL_SERVO)))
|
||||
throttleCalibration(); // noreturn
|
||||
|
||||
// configure PWM/CPPM read function. spektrum will override that
|
||||
rcReadRawFunc = pwmReadRawRC;
|
||||
|
||||
LED1_ON;
|
||||
LED0_OFF;
|
||||
for (i = 0; i < 10; i++) {
|
||||
|
|
@ -72,9 +80,15 @@ int main(void)
|
|||
if (feature(FEATURE_VBAT))
|
||||
batteryInit();
|
||||
|
||||
if (feature(FEATURE_SPEKTRUM)) {
|
||||
spektrumInit();
|
||||
rcReadRawFunc = spektrumReadRawRC;
|
||||
} else {
|
||||
// spektrum and GPS are mutually exclusive
|
||||
// Optional GPS - available only when using PPM, otherwise required pins won't be usable
|
||||
if (feature(FEATURE_PPM) && feature(FEATURE_GPS))
|
||||
gpsInit();
|
||||
}
|
||||
|
||||
previousTime = micros();
|
||||
calibratingG = 400;
|
||||
|
|
|
|||
95
src/mw.c
95
src/mw.c
|
|
@ -1,7 +1,7 @@
|
|||
#include "board.h"
|
||||
#include "mw.h"
|
||||
|
||||
// March 2012 V2.0_pre_version_3
|
||||
// March 2012 V2.0
|
||||
|
||||
#define CHECKBOXITEMS 11
|
||||
#define PIDITEMS 8
|
||||
|
|
@ -22,19 +22,12 @@ uint8_t vbat; // battery voltage in 0.1V steps
|
|||
|
||||
volatile int16_t failsafeCnt = 0;
|
||||
int16_t failsafeEvents = 0;
|
||||
int16_t rcData[8]; // interval [1000;2000]
|
||||
int16_t rcData[8] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 }; // interval [1000;2000]
|
||||
int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
|
||||
//uint8_t rcRate8;
|
||||
//uint8_t rcExpo8;
|
||||
int16_t lookupRX[7]; // lookup table for expo & RC rate
|
||||
rcReadRawDataPtr rcReadRawFunc = NULL; // receive data from default (pwm/ppm) or additional (spek/sbus/?? receiver drivers)
|
||||
|
||||
// uint8_t P8[8], I8[8], D8[8]; //8 bits is much faster and the code is much shorter
|
||||
uint8_t dynP8[3], dynI8[3], dynD8[3];
|
||||
// uint8_t rollPitchRate;
|
||||
// uint8_t yawRate;
|
||||
// uint8_t dynThrPID;
|
||||
// uint8_t activate1[CHECKBOXITEMS];
|
||||
// uint8_t activate2[CHECKBOXITEMS];
|
||||
uint8_t rcOptions[CHECKBOXITEMS];
|
||||
uint8_t okToArm = 0;
|
||||
uint8_t accMode = 0; // if level mode is a activated
|
||||
|
|
@ -42,7 +35,6 @@ uint8_t magMode = 0; // if compass heading hold is a activated
|
|||
uint8_t baroMode = 0; // if altitude hold is activated
|
||||
|
||||
int16_t axisPID[3];
|
||||
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]
|
||||
|
||||
// **********************
|
||||
// GPS
|
||||
|
|
@ -60,7 +52,6 @@ uint16_t GPS_altitude, GPS_speed; // altitude in 0.1m and speed in 0.1m/s
|
|||
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
|
||||
|
||||
|
||||
//Automatic ACC Offset Calibration
|
||||
// **********************
|
||||
uint16_t InflightcalibratingA = 0;
|
||||
|
|
@ -76,7 +67,6 @@ uint16_t AccInflightCalibrationActive = 0;
|
|||
uint32_t pMeter[PMOTOR_SUM + 1]; // we use [0:7] for eight motors,one extra for sum
|
||||
uint8_t pMeterV; // dummy to satisfy the paramStruct logic in ConfigurationLoop()
|
||||
uint32_t pAlarm; // we scale the eeprom value from [0:255] to this value we can directly compare to the sum in pMeter[6]
|
||||
// uint8_t powerTrigger1 = 0;
|
||||
uint16_t powerValue = 0; // last known current
|
||||
uint16_t intPowerMeterSum, intPowerTrigger1;
|
||||
uint8_t batteryCellCount = 3; // cell count
|
||||
|
|
@ -119,7 +109,7 @@ void annexCode(void)
|
|||
static uint16_t vbatRawArray[8];
|
||||
uint8_t i;
|
||||
|
||||
//PITCH & ROLL only dynamic PID adjustemnt, depending on throttle value
|
||||
// PITCH & ROLL only dynamic PID adjustemnt, depending on throttle value
|
||||
if (rcData[THROTTLE] < 1500) {
|
||||
prop2 = 100;
|
||||
} else if (rcData[THROTTLE] < 2000) {
|
||||
|
|
@ -141,20 +131,20 @@ void annexCode(void)
|
|||
uint16_t tmp2 = tmp / 100;
|
||||
rcCommand[axis] = lookupRX[tmp2] + (tmp - tmp2 * 100) * (lookupRX[tmp2 + 1] - lookupRX[tmp2]) / 100;
|
||||
prop1 = 100 - (uint16_t) cfg.rollPitchRate * tmp / 500;
|
||||
prop1 = (uint16_t) prop1 *prop2 / 100;
|
||||
prop1 = (uint16_t)prop1 * prop2 / 100;
|
||||
} else { //YAW
|
||||
rcCommand[axis] = tmp;
|
||||
prop1 = 100 - (uint16_t) cfg.yawRate * tmp / 500;
|
||||
prop1 = 100 - (uint16_t)cfg.yawRate * tmp / 500;
|
||||
}
|
||||
dynP8[axis] = (uint16_t) cfg.P8[axis] * prop1 / 100;
|
||||
dynD8[axis] = (uint16_t) cfg.D8[axis] * prop1 / 100;
|
||||
dynP8[axis] = (uint16_t)cfg.P8[axis] * prop1 / 100;
|
||||
dynD8[axis] = (uint16_t)cfg.D8[axis] * prop1 / 100;
|
||||
if (rcData[axis] < cfg.midrc)
|
||||
rcCommand[axis] = -rcCommand[axis];
|
||||
}
|
||||
rcCommand[THROTTLE] = cfg.minthrottle + (int32_t) (cfg.maxthrottle - cfg.minthrottle) * (rcData[THROTTLE] - cfg.mincheck) / (2000 - cfg.mincheck);
|
||||
rcCommand[THROTTLE] = cfg.minthrottle + (int32_t)(cfg.maxthrottle - cfg.minthrottle) * (rcData[THROTTLE] - cfg.mincheck) / (2000 - cfg.mincheck);
|
||||
|
||||
if (headFreeMode) {
|
||||
float radDiff = (heading - headFreeModeHold) * 0.0174533f; // where PI/180 ~= 0.0174533
|
||||
float radDiff = (heading - headFreeModeHold) * M_PI / 180.0f;
|
||||
float cosDiff = cosf(radDiff);
|
||||
float sinDiff = sinf(radDiff);
|
||||
int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
|
||||
|
|
@ -274,7 +264,7 @@ void annexCode(void)
|
|||
}
|
||||
}
|
||||
|
||||
uint16_t readRawRC(uint8_t chan)
|
||||
uint16_t pwmReadRawRC(uint8_t chan)
|
||||
{
|
||||
uint16_t data;
|
||||
|
||||
|
|
@ -292,12 +282,9 @@ void computeRC(void)
|
|||
static uint8_t rc4ValuesIndex = 0;
|
||||
uint8_t chan, a;
|
||||
|
||||
#if defined(SBUS)
|
||||
readSBus();
|
||||
#endif
|
||||
rc4ValuesIndex++;
|
||||
for (chan = 0; chan < 8; chan++) {
|
||||
rcData4Values[chan][rc4ValuesIndex % 4] = readRawRC(chan);
|
||||
rcData4Values[chan][rc4ValuesIndex % 4] = rcReadRawFunc(chan);
|
||||
rcDataMean[chan] = 0;
|
||||
for (a = 0; a < 4; a++)
|
||||
rcDataMean[chan] += rcData4Values[chan][a];
|
||||
|
|
@ -324,16 +311,15 @@ void loop(void)
|
|||
static uint32_t rcTime = 0;
|
||||
static int16_t initialThrottleHold;
|
||||
|
||||
#if defined(SPEKTRUM)
|
||||
if (rcFrameComplete)
|
||||
// this will return false if spektrum is disabled. shrug.
|
||||
if (spektrumFrameComplete())
|
||||
computeRC();
|
||||
#endif
|
||||
|
||||
if (currentTime > rcTime) { // 50Hz
|
||||
rcTime = currentTime + 20000;
|
||||
#if !(defined(SPEKTRUM) ||defined(BTSERIAL))
|
||||
// TODO clean this up. computeRC should handle this check
|
||||
if (!feature(FEATURE_SPEKTRUM))
|
||||
computeRC();
|
||||
#endif
|
||||
// Failsafe routine - added by MIS
|
||||
#if defined(FAILSAFE)
|
||||
if (failsafeCnt > (5 * FAILSAVE_DELAY) && armed == 1) { // Stabilize, and set Throttle to specified level
|
||||
|
|
@ -376,7 +362,7 @@ void loop(void)
|
|||
}
|
||||
} else if (feature(FEATURE_INFLIGHT_ACC_CAL) && (armed == 0 && rcData[YAW] < cfg.mincheck && rcData[PITCH] > cfg.maxcheck && rcData[ROLL] > cfg.maxcheck)) {
|
||||
if (rcDelayCommand == 20) {
|
||||
if (AccInflightCalibrationMeasurementDone) { //trigger saving into eeprom after landing
|
||||
if (AccInflightCalibrationMeasurementDone) { // trigger saving into eeprom after landing
|
||||
AccInflightCalibrationMeasurementDone = 0;
|
||||
AccInflightCalibrationSavetoEEProm = 1;
|
||||
} else {
|
||||
|
|
@ -395,12 +381,10 @@ void loop(void)
|
|||
} else if (armed)
|
||||
armed = 0;
|
||||
rcDelayCommand = 0;
|
||||
} else if ((rcData[YAW] < cfg.mincheck || rcData[ROLL] < cfg.mincheck)
|
||||
&& armed == 1) {
|
||||
} else if ((rcData[YAW] < cfg.mincheck || rcData[ROLL] < cfg.mincheck) && armed == 1) {
|
||||
if (rcDelayCommand == 20)
|
||||
armed = 0; // rcDelayCommand = 20 => 20x20ms = 0.4s = time to wait for a specific RC command to be acknowledged
|
||||
} else if ((rcData[YAW] > cfg.maxcheck || rcData[ROLL] > cfg.maxcheck)
|
||||
&& rcData[PITCH] < cfg.maxcheck && armed == 0 && calibratingG == 0 && calibratedACC == 1) {
|
||||
} else if ((rcData[YAW] > cfg.maxcheck || rcData[ROLL] > cfg.maxcheck) && rcData[PITCH] < cfg.maxcheck && armed == 0 && calibratingG == 0 && calibratedACC == 1) {
|
||||
if (rcDelayCommand == 20) {
|
||||
armed = 1;
|
||||
headFreeModeHold = heading;
|
||||
|
|
@ -418,11 +402,11 @@ void loop(void)
|
|||
} else
|
||||
rcDelayCommand = 0;
|
||||
} else if (rcData[THROTTLE] > cfg.maxcheck && armed == 0) {
|
||||
if (rcData[YAW] < cfg.mincheck && rcData[PITCH] < cfg.mincheck) { //throttle=max, yaw=left, pitch=min
|
||||
if (rcData[YAW] < cfg.mincheck && rcData[PITCH] < cfg.mincheck) { // throttle=max, yaw=left, pitch=min
|
||||
if (rcDelayCommand == 20)
|
||||
calibratingA = 400;
|
||||
rcDelayCommand++;
|
||||
} else if (rcData[YAW] > cfg.maxcheck && rcData[PITCH] < cfg.mincheck) { //throttle=max, yaw=right, pitch=min
|
||||
} else if (rcData[YAW] > cfg.maxcheck && rcData[PITCH] < cfg.mincheck) { // throttle=max, yaw=right, pitch=min
|
||||
if (rcDelayCommand == 20)
|
||||
calibratingM = 1; // MAG calibration request
|
||||
rcDelayCommand++;
|
||||
|
|
@ -462,7 +446,7 @@ void loop(void)
|
|||
InflightcalibratingA = 50;
|
||||
AccInflightCalibrationArmed = 0;
|
||||
}
|
||||
if (rcOptions[BOXPASSTHRU]) { //Use the Passthru Option to activate : Passthru = TRUE Meausrement started, Land and passtrhu = 0 measurement stored
|
||||
if (rcOptions[BOXPASSTHRU]) { // Use the Passthru Option to activate : Passthru = TRUE Meausrement started, Land and passtrhu = 0 measurement stored
|
||||
if (!AccInflightCalibrationArmed) {
|
||||
AccInflightCalibrationArmed = 1;
|
||||
InflightcalibratingA = 50;
|
||||
|
|
@ -479,7 +463,7 @@ void loop(void)
|
|||
|| (((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]);
|
||||
}
|
||||
|
||||
//note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAVE_DELAY is always false
|
||||
// note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAVE_DELAY is always false
|
||||
if ((rcOptions[BOXACC] || (failsafeCnt > 5 * FAILSAVE_DELAY)) && (sensors(SENSOR_ACC))) {
|
||||
// bumpless transfer to Level mode
|
||||
if (!accMode) {
|
||||
|
|
@ -547,7 +531,7 @@ void loop(void)
|
|||
} else
|
||||
passThruMode = 0;
|
||||
} else { // not in rc loop
|
||||
static int8_t taskOrder = 0; //never call all function in the same loop, to avoid high delay spikes
|
||||
static int8_t taskOrder = 0; // never call all function in the same loop, to avoid high delay spikes
|
||||
switch (taskOrder) {
|
||||
case 0:
|
||||
taskOrder++;
|
||||
|
|
@ -570,7 +554,6 @@ void loop(void)
|
|||
GPS_NewData();
|
||||
#endif
|
||||
break;
|
||||
|
||||
default:
|
||||
taskOrder = 0;
|
||||
break;
|
||||
|
|
@ -578,7 +561,6 @@ void loop(void)
|
|||
}
|
||||
|
||||
computeIMU();
|
||||
|
||||
// Measure loop rate just afer reading the sensors
|
||||
currentTime = micros();
|
||||
cycleTime = currentTime - previousTime;
|
||||
|
|
@ -625,37 +607,38 @@ void loop(void)
|
|||
GPS_dist = GPS_distanceToHold;
|
||||
GPS_dir = GPS_directionToHold;
|
||||
}
|
||||
radDiff = (GPS_dir - heading) * 0.0174533f;
|
||||
GPS_angle[ROLL] = constrain(cfg.P8[PIDGPS] * sin(radDiff) * GPS_dist / 10, -cfg.D8[PIDGPS] * 10, +cfg.D8[PIDGPS] * 10); // with P=5.0, a distance of 1 meter = 0.5deg inclination
|
||||
GPS_angle[PITCH] = constrain(cfg.P8[PIDGPS] * cos(radDiff) * GPS_dist / 10, -cfg.D8[PIDGPS] * 10, +cfg.D8[PIDGPS] * 10); // max inclination = D deg
|
||||
radDiff = (GPS_dir - heading) * M_PI / 180.0f;
|
||||
GPS_angle[ROLL] = constrain(cfg.P8[PIDGPS] * sinf(radDiff) * GPS_dist / 10, -cfg.D8[PIDGPS] * 10, +cfg.D8[PIDGPS] * 10); // with P=5.0, a distance of 1 meter = 0.5deg inclination
|
||||
GPS_angle[PITCH] = constrain(cfg.P8[PIDGPS] * cosf(radDiff) * GPS_dist / 10, -cfg.D8[PIDGPS] * 10, +cfg.D8[PIDGPS] * 10); // max inclination = D deg
|
||||
}
|
||||
}
|
||||
//**** PITCH & ROLL & YAW PID ****
|
||||
|
||||
// **** PITCH & ROLL & YAW PID ****
|
||||
for (axis = 0; axis < 3; axis++) {
|
||||
if (accMode == 1 && axis < 2) { //LEVEL MODE
|
||||
if (accMode == 1 && axis < 2) { // LEVEL MODE
|
||||
// 50 degrees max inclination
|
||||
errorAngle = constrain(2 * rcCommand[axis] - GPS_angle[axis], -500, +500) - angle[axis] + cfg.accTrim[axis]; //16 bits is ok here
|
||||
#ifdef LEVEL_PDF
|
||||
PTerm = -(int32_t) angle[axis] * cfg.P8[PIDLEVEL] / 100;
|
||||
PTerm = -(int32_t)angle[axis] * cfg.P8[PIDLEVEL] / 100;
|
||||
#else
|
||||
PTerm = (int32_t) errorAngle * cfg.P8[PIDLEVEL] / 100; //32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
|
||||
PTerm = (int32_t)errorAngle * cfg.P8[PIDLEVEL] / 100; //32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
|
||||
#endif
|
||||
PTerm = constrain(PTerm, -cfg.D8[PIDLEVEL] * 5, +cfg.D8[PIDLEVEL] * 5);
|
||||
|
||||
errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); //WindUp //16 bits is ok here
|
||||
ITerm = ((int32_t) errorAngleI[axis] * cfg.I8[PIDLEVEL]) >> 12; //32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result
|
||||
} else { //ACRO MODE or YAW axis
|
||||
error = (int32_t) rcCommand[axis] * 10 * 8 / cfg.P8[axis]; //32 bits is needed for calculation: 500*5*10*8 = 200000 16 bits is ok for result if P8>2 (P>0.2)
|
||||
errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp // 16 bits is ok here
|
||||
ITerm = ((int32_t)errorAngleI[axis] * cfg.I8[PIDLEVEL]) >> 12; // 32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result
|
||||
} else { // ACRO MODE or YAW axis
|
||||
error = (int32_t)rcCommand[axis] * 10 * 8 / cfg.P8[axis]; //32 bits is needed for calculation: 500*5*10*8 = 200000 16 bits is ok for result if P8>2 (P>0.2)
|
||||
error -= gyroData[axis];
|
||||
|
||||
PTerm = rcCommand[axis];
|
||||
|
||||
errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); //WindUp //16 bits is ok here
|
||||
errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp // 16 bits is ok here
|
||||
if (abs(gyroData[axis]) > 640)
|
||||
errorGyroI[axis] = 0;
|
||||
ITerm = (errorGyroI[axis] / 125 * cfg.I8[axis]) >> 6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000
|
||||
}
|
||||
PTerm -= (int32_t) gyroData[axis] * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
|
||||
PTerm -= (int32_t)gyroData[axis] * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
|
||||
|
||||
delta = gyroData[axis] - lastGyro[axis]; //16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
|
||||
lastGyro[axis] = gyroData[axis];
|
||||
|
|
@ -663,7 +646,7 @@ void loop(void)
|
|||
delta2[axis] = delta1[axis];
|
||||
delta1[axis] = delta;
|
||||
|
||||
DTerm = ((int32_t) deltaSum * dynD8[axis]) >> 5; //32 bits is needed for calculation
|
||||
DTerm = ((int32_t)deltaSum * dynD8[axis]) >> 5; //32 bits is needed for calculation
|
||||
|
||||
axisPID[axis] = PTerm + ITerm - DTerm;
|
||||
}
|
||||
|
|
|
|||
11
src/mw.h
11
src/mw.h
|
|
@ -60,7 +60,7 @@
|
|||
//#define MMSERVOGIMBAL // Active Output Moving Average Function for Servos Gimbal
|
||||
//#define MMSERVOGIMBALVECTORLENGHT 32 // Lenght of Moving Average Vector
|
||||
|
||||
#define VERSION 203
|
||||
#define VERSION 20
|
||||
|
||||
// Syncronized with GUI. Only exception is mixer > 11, which is always returned as 11 during serialization.
|
||||
typedef enum MultiType
|
||||
|
|
@ -142,6 +142,10 @@ typedef struct config_t {
|
|||
int16_t accZero[3];
|
||||
int16_t magZero[3];
|
||||
int16_t accTrim[2];
|
||||
|
||||
// sensor-related stuff
|
||||
uint8_t acc_lpf_factor; // Set the Low Pass Filter factor for ACC. Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter
|
||||
uint16_t gyro_lpf; // mpuX050 LPF setting
|
||||
uint32_t gyro_smoothing_factor; // How much to smoothen with per axis (32bit value with Roll, Pitch, Yaw in bits 24, 16, 8 respectively
|
||||
|
||||
uint8_t activate1[CHECKBOXITEMS];
|
||||
|
|
@ -154,6 +158,7 @@ typedef struct config_t {
|
|||
// Radio/ESC-related configuration
|
||||
uint8_t rcmap[8]; // mapping of radio channels to internal RPYTA+ order
|
||||
uint8_t deadband; // introduce a deadband around the stick center. Must be greater than zero
|
||||
uint8_t spektrum_hires; // spektrum high-resolution y/n (1024/2048bit)
|
||||
uint16_t midrc; // Some radios have not a neutral point centered on 1500. can be changed here
|
||||
uint16_t mincheck; // minimum rc end
|
||||
uint16_t maxcheck; // maximum rc end
|
||||
|
|
@ -278,6 +283,10 @@ void featureClear(uint32_t mask);
|
|||
void featureClearAll(void);
|
||||
uint32_t featureMask(void);
|
||||
|
||||
// spektrum
|
||||
void spektrumInit(void);
|
||||
bool spektrumFrameComplete(void);
|
||||
|
||||
// cli
|
||||
void cliProcess(void);
|
||||
|
||||
|
|
|
|||
|
|
@ -45,6 +45,8 @@ void sensorsAutodetect(void)
|
|||
}
|
||||
// this is safe because either mpu6050 or mpu3050 sets it, and in case of fail, none do.
|
||||
gyro.init();
|
||||
// todo: this is driver specific :(
|
||||
mpu3050Config(cfg.gyro_lpf);
|
||||
}
|
||||
|
||||
uint16_t batteryAdcToVoltage(uint16_t src)
|
||||
|
|
@ -59,6 +61,7 @@ void batteryInit(void)
|
|||
uint8_t i;
|
||||
uint32_t voltage = 0;
|
||||
|
||||
// average up some voltage readings
|
||||
for (i = 0; i < 32; i++) {
|
||||
voltage += adcGetBattery();
|
||||
delay(10);
|
||||
|
|
@ -107,7 +110,7 @@ static void ACC_Common(void)
|
|||
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
|
||||
// Saving old zeropoints before measurement
|
||||
if (InflightcalibratingA == 50) {
|
||||
accZero_saved[ROLL] = cfg.accZero[ROLL];
|
||||
accZero_saved[PITCH] = cfg.accZero[PITCH];
|
||||
|
|
@ -127,7 +130,7 @@ static void ACC_Common(void)
|
|||
accADC[axis] = 0;
|
||||
cfg.accZero[axis] = 0;
|
||||
}
|
||||
//all values are measured
|
||||
// all values are measured
|
||||
if (InflightcalibratingA == 1) {
|
||||
AccInflightCalibrationActive = 0;
|
||||
AccInflightCalibrationMeasurementDone = 1;
|
||||
|
|
@ -142,7 +145,7 @@ static void ACC_Common(void)
|
|||
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
|
||||
if (AccInflightCalibrationSavetoEEProm == 1) { // the copter is landed, disarmed and the combo has been done again
|
||||
AccInflightCalibrationSavetoEEProm = 0;
|
||||
cfg.accZero[ROLL] = b[ROLL] / 50;
|
||||
cfg.accZero[PITCH] = b[PITCH] / 50;
|
||||
|
|
@ -158,7 +161,6 @@ static void ACC_Common(void)
|
|||
accADC[YAW] -= cfg.accZero[YAW];
|
||||
}
|
||||
|
||||
|
||||
void ACC_getADC(void)
|
||||
{
|
||||
acc.read(accADC);
|
||||
|
|
@ -171,7 +173,6 @@ static uint32_t baroDeadline = 0;
|
|||
static uint8_t baroState = 0;
|
||||
static uint16_t baroUT = 0;
|
||||
static uint32_t baroUP = 0;
|
||||
static int16_t baroTemp = 0;
|
||||
|
||||
void Baro_update(void)
|
||||
{
|
||||
|
|
@ -199,9 +200,8 @@ void Baro_update(void)
|
|||
break;
|
||||
case 3:
|
||||
baroUP = bmp085_get_up();
|
||||
baroTemp = bmp085_get_temperature(baroUT);
|
||||
bmp085_get_temperature(baroUT);
|
||||
pressure = bmp085_get_pressure(baroUP);
|
||||
|
||||
BaroAlt = (1.0f - pow(pressure / 101325.0f, 0.190295f)) * 4433000.0f; // centimeter
|
||||
baroState = 0;
|
||||
baroDeadline += 5000;
|
||||
|
|
@ -329,9 +329,7 @@ void Mag_getADC(void)
|
|||
}
|
||||
calibratingM = 0;
|
||||
}
|
||||
magADC[ROLL] = magADC[ROLL] * magCal[ROLL];
|
||||
magADC[PITCH] = magADC[PITCH] * magCal[PITCH];
|
||||
magADC[YAW] = magADC[YAW] * magCal[YAW];
|
||||
|
||||
if (magInit) { // we apply offset only once mag calibration is done
|
||||
magADC[ROLL] -= cfg.magZero[ROLL];
|
||||
magADC[PITCH] -= cfg.magZero[PITCH];
|
||||
|
|
|
|||
|
|
@ -0,0 +1,90 @@
|
|||
#include "board.h"
|
||||
#include "mw.h"
|
||||
|
||||
// driver for spektrum satellite receiver / sbus using UART2 (freeing up more motor outputs for stuff)
|
||||
|
||||
#define SPEK_MAX_CHANNEL 7
|
||||
#define SPEK_FRAME_SIZE 16
|
||||
static uint8_t spek_chan_shift;
|
||||
static uint8_t spek_chan_mask;
|
||||
static bool rcFrameComplete = false;
|
||||
static bool spekDataIncoming = false;
|
||||
volatile uint8_t spekFrame[SPEK_FRAME_SIZE];
|
||||
|
||||
static void spektrumDataReceive(uint16_t c);
|
||||
|
||||
void spektrumInit(void)
|
||||
{
|
||||
if (cfg.spektrum_hires) {
|
||||
// 11 bit frames
|
||||
spek_chan_shift = 3;
|
||||
spek_chan_mask = 0x07;
|
||||
} else {
|
||||
// 10 bit frames
|
||||
spek_chan_shift = 2;
|
||||
spek_chan_mask = 0x03;
|
||||
}
|
||||
|
||||
uart2Init(115200, spektrumDataReceive);
|
||||
}
|
||||
|
||||
// UART2 Receive ISR callback
|
||||
static void spektrumDataReceive(uint16_t c)
|
||||
{
|
||||
uint32_t spekTime;
|
||||
static uint32_t spekTimeLast, spekTimeInterval;
|
||||
static uint8_t spekFramePosition;
|
||||
|
||||
spekDataIncoming = true;
|
||||
spekTime = micros();
|
||||
spekTimeInterval = spekTime - spekTimeLast;
|
||||
spekTimeLast = spekTime;
|
||||
if (spekTimeInterval > 5000)
|
||||
spekFramePosition = 0;
|
||||
spekFrame[spekFramePosition] = (uint8_t)c;
|
||||
if (spekFramePosition == SPEK_FRAME_SIZE - 1) {
|
||||
rcFrameComplete = true;
|
||||
#if defined(FAILSAFE)
|
||||
if(failsafeCnt > 20)
|
||||
failsafeCnt -= 20;
|
||||
else
|
||||
failsafeCnt = 0; // clear FailSafe counter
|
||||
#endif
|
||||
} else {
|
||||
spekFramePosition++;
|
||||
}
|
||||
}
|
||||
|
||||
bool spektrumFrameComplete(void)
|
||||
{
|
||||
return rcFrameComplete;
|
||||
}
|
||||
|
||||
static const uint8_t spekRcChannelMap[SPEK_MAX_CHANNEL] = {1, 2, 3, 0, 4, 5, 6};
|
||||
|
||||
uint16_t spektrumReadRawRC(uint8_t chan)
|
||||
{
|
||||
uint16_t data;
|
||||
static uint32_t spekChannelData[SPEK_MAX_CHANNEL];
|
||||
uint8_t b;
|
||||
|
||||
if (rcFrameComplete) {
|
||||
for (b = 3; b < SPEK_FRAME_SIZE; b += 2) {
|
||||
uint8_t spekChannel = 0x0F & (spekFrame[b - 1] >> spek_chan_shift);
|
||||
if (spekChannel < SPEK_MAX_CHANNEL)
|
||||
spekChannelData[spekChannel] = ((uint32_t)(spekFrame[b - 1] & spek_chan_mask) << 8) + spekFrame[b];
|
||||
}
|
||||
rcFrameComplete = false;
|
||||
}
|
||||
|
||||
if (chan >= SPEK_MAX_CHANNEL || !spekDataIncoming) {
|
||||
data = 1500;
|
||||
} else {
|
||||
if (cfg.spektrum_hires)
|
||||
data = 988 + (spekChannelData[spekRcChannelMap[chan]] >> 1); // 2048 mode
|
||||
else
|
||||
data = 988 + spekChannelData[spekRcChannelMap[chan]]; // 1024 mode
|
||||
}
|
||||
|
||||
return data;
|
||||
}
|
||||
Loading…
Reference in New Issue