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

added inflight acc cal as a feature (NOT TESTED NOT GUARANTEED TO WORK ETC) 

changed gyro/acc to allow different drivers without recompile, this allowed adding mpu6050 support w/autodetect
moved sensor orientation code into driver - each driver provides its own
added support for mpu6050 - desolder mpu3050 and adxl345, replace with mpu6050 for instant ???
merged multiwii 2.0pre3 code changes (none that mattered except mag calibration and typo in baro stuff)
changes to sensor autodetect for new dynamic drivers
more of ledring stuff done (still doesn't work, so dont try)


git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@115 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop 2012-03-17 07:08:36 +00:00
parent 0f6a75af4a
commit 7592316c04
16 changed files with 2824 additions and 2698 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

130
baseflight.uvopt
View File

@ -161,9 +161,9 @@
<Bp>
<Number>0</Number>
<Type>0</Type>
<LineNumber>35</LineNumber>
<LineNumber>58</LineNumber>
<EnabledFlag>1</EnabledFlag>
<Address>134236980</Address>
<Address>134240618</Address>
<ByteObject>0</ByteObject>
<ManyObjects>0</ManyObjects>
<SizeOfObject>0</SizeOfObject>
@ -171,7 +171,7 @@
<BreakIfRCount>1</BreakIfRCount>
<Filename></Filename>
<ExecCommand></ExecCommand>
<Expression>\\baseflight\src/drv_mpu3050.c\35</Expression>
<Expression>\\baseflight\src/drv_mpu6050.c\58</Expression>
</Bp>
</Breakpoint>
<WatchWindow1>
@ -202,7 +202,7 @@
<Mm>
<WinNumber>1</WinNumber>
<SubType>0</SubType>
<ItemText>cmdline</ItemText>
<ItemText>buf</ItemText>
</Mm>
</MemoryWindow1>
<MemoryWindow2>
@ -244,7 +244,7 @@
</Target>
<Target>
<TargetName>STM32-FreeFlight</TargetName>
<TargetName>STM32-Release</TargetName>
<ToolsetNumber>0x4</ToolsetNumber>
<ToolsetName>ARM-ADS</ToolsetName>
<TargetOption>
@ -519,10 +519,10 @@
<FileType>1</FileType>
<tvExp>0</tvExp>
<Focus>0</Focus>
<ColumnNumber>26</ColumnNumber>
<ColumnNumber>15</ColumnNumber>
<tvExpOptDlg>0</tvExpOptDlg>
<TopLine>92</TopLine>
<CurrentLine>110</CurrentLine>
<TopLine>116</TopLine>
<CurrentLine>131</CurrentLine>
<bDave2>0</bDave2>
<PathWithFileName>.\src\config.c</PathWithFileName>
<FilenameWithoutPath>config.c</FilenameWithoutPath>
@ -535,8 +535,8 @@
<Focus>0</Focus>
<ColumnNumber>0</ColumnNumber>
<tvExpOptDlg>0</tvExpOptDlg>
<TopLine>229</TopLine>
<CurrentLine>238</CurrentLine>
<TopLine>320</TopLine>
<CurrentLine>340</CurrentLine>
<bDave2>0</bDave2>
<PathWithFileName>.\src\imu.c</PathWithFileName>
<FilenameWithoutPath>imu.c</FilenameWithoutPath>
@ -547,10 +547,10 @@
<FileType>1</FileType>
<tvExp>0</tvExp>
<Focus>0</Focus>
<ColumnNumber>0</ColumnNumber>
<ColumnNumber>13</ColumnNumber>
<tvExpOptDlg>0</tvExpOptDlg>
<TopLine>0</TopLine>
<CurrentLine>0</CurrentLine>
<TopLine>11</TopLine>
<CurrentLine>25</CurrentLine>
<bDave2>0</bDave2>
<PathWithFileName>.\src\main.c</PathWithFileName>
<FilenameWithoutPath>main.c</FilenameWithoutPath>
@ -563,8 +563,8 @@
<Focus>0</Focus>
<ColumnNumber>0</ColumnNumber>
<tvExpOptDlg>0</tvExpOptDlg>
<TopLine>268</TopLine>
<CurrentLine>268</CurrentLine>
<TopLine>37</TopLine>
<CurrentLine>68</CurrentLine>
<bDave2>0</bDave2>
<PathWithFileName>.\src\mixer.c</PathWithFileName>
<FilenameWithoutPath>mixer.c</FilenameWithoutPath>
@ -575,10 +575,10 @@
<FileType>1</FileType>
<tvExp>0</tvExp>
<Focus>0</Focus>
<ColumnNumber>0</ColumnNumber>
<ColumnNumber>20</ColumnNumber>
<tvExpOptDlg>0</tvExpOptDlg>
<TopLine>124</TopLine>
<CurrentLine>129</CurrentLine>
<TopLine>359</TopLine>
<CurrentLine>376</CurrentLine>
<bDave2>0</bDave2>
<PathWithFileName>.\src\mw.c</PathWithFileName>
<FilenameWithoutPath>mw.c</FilenameWithoutPath>
@ -589,10 +589,10 @@
<FileType>1</FileType>
<tvExp>0</tvExp>
<Focus>0</Focus>
<ColumnNumber>0</ColumnNumber>
<ColumnNumber>41</ColumnNumber>
<tvExpOptDlg>0</tvExpOptDlg>
<TopLine>216</TopLine>
<CurrentLine>231</CurrentLine>
<TopLine>276</TopLine>
<CurrentLine>298</CurrentLine>
<bDave2>0</bDave2>
<PathWithFileName>.\src\sensors.c</PathWithFileName>
<FilenameWithoutPath>sensors.c</FilenameWithoutPath>
@ -603,10 +603,10 @@
<FileType>1</FileType>
<tvExp>0</tvExp>
<Focus>0</Focus>
<ColumnNumber>0</ColumnNumber>
<ColumnNumber>18</ColumnNumber>
<tvExpOptDlg>0</tvExpOptDlg>
<TopLine>237</TopLine>
<CurrentLine>237</CurrentLine>
<TopLine>199</TopLine>
<CurrentLine>233</CurrentLine>
<bDave2>0</bDave2>
<PathWithFileName>.\src\serial.c</PathWithFileName>
<FilenameWithoutPath>serial.c</FilenameWithoutPath>
@ -617,10 +617,10 @@
<FileType>5</FileType>
<tvExp>0</tvExp>
<Focus>0</Focus>
<ColumnNumber>0</ColumnNumber>
<ColumnNumber>20</ColumnNumber>
<tvExpOptDlg>0</tvExpOptDlg>
<TopLine>54</TopLine>
<CurrentLine>72</CurrentLine>
<TopLine>1</TopLine>
<CurrentLine>29</CurrentLine>
<bDave2>0</bDave2>
<PathWithFileName>.\src\board.h</PathWithFileName>
<FilenameWithoutPath>board.h</FilenameWithoutPath>
@ -631,10 +631,10 @@
<FileType>5</FileType>
<tvExp>0</tvExp>
<Focus>0</Focus>
<ColumnNumber>14</ColumnNumber>
<ColumnNumber>32</ColumnNumber>
<tvExpOptDlg>0</tvExpOptDlg>
<TopLine>241</TopLine>
<CurrentLine>249</CurrentLine>
<TopLine>248</TopLine>
<CurrentLine>262</CurrentLine>
<bDave2>0</bDave2>
<PathWithFileName>.\src\mw.h</PathWithFileName>
<FilenameWithoutPath>mw.h</FilenameWithoutPath>
@ -652,10 +652,10 @@
<FileType>1</FileType>
<tvExp>0</tvExp>
<Focus>0</Focus>
<ColumnNumber>0</ColumnNumber>
<ColumnNumber>23</ColumnNumber>
<tvExpOptDlg>0</tvExpOptDlg>
<TopLine>0</TopLine>
<CurrentLine>0</CurrentLine>
<TopLine>1</TopLine>
<CurrentLine>1</CurrentLine>
<bDave2>0</bDave2>
<PathWithFileName>.\src\drv_adc.c</PathWithFileName>
<FilenameWithoutPath>drv_adc.c</FilenameWithoutPath>
@ -666,10 +666,10 @@
<FileType>1</FileType>
<tvExp>0</tvExp>
<Focus>0</Focus>
<ColumnNumber>0</ColumnNumber>
<ColumnNumber>31</ColumnNumber>
<tvExpOptDlg>0</tvExpOptDlg>
<TopLine>54</TopLine>
<CurrentLine>72</CurrentLine>
<TopLine>11</TopLine>
<CurrentLine>15</CurrentLine>
<bDave2>0</bDave2>
<PathWithFileName>.\src\drv_adxl345.c</PathWithFileName>
<FilenameWithoutPath>drv_adxl345.c</FilenameWithoutPath>
@ -696,8 +696,8 @@
<Focus>0</Focus>
<ColumnNumber>0</ColumnNumber>
<tvExpOptDlg>0</tvExpOptDlg>
<TopLine>33</TopLine>
<CurrentLine>51</CurrentLine>
<TopLine>0</TopLine>
<CurrentLine>0</CurrentLine>
<bDave2>0</bDave2>
<PathWithFileName>.\src\drv_hmc5883l.c</PathWithFileName>
<FilenameWithoutPath>drv_hmc5883l.c</FilenameWithoutPath>
@ -708,10 +708,10 @@
<FileType>1</FileType>
<tvExp>0</tvExp>
<Focus>0</Focus>
<ColumnNumber>29</ColumnNumber>
<ColumnNumber>0</ColumnNumber>
<tvExpOptDlg>0</tvExpOptDlg>
<TopLine>101</TopLine>
<CurrentLine>101</CurrentLine>
<TopLine>0</TopLine>
<CurrentLine>0</CurrentLine>
<bDave2>0</bDave2>
<PathWithFileName>.\src\drv_i2c.c</PathWithFileName>
<FilenameWithoutPath>drv_i2c.c</FilenameWithoutPath>
@ -722,10 +722,10 @@
<FileType>1</FileType>
<tvExp>0</tvExp>
<Focus>0</Focus>
<ColumnNumber>15</ColumnNumber>
<ColumnNumber>32</ColumnNumber>
<tvExpOptDlg>0</tvExpOptDlg>
<TopLine>13</TopLine>
<CurrentLine>35</CurrentLine>
<TopLine>1</TopLine>
<CurrentLine>1</CurrentLine>
<bDave2>0</bDave2>
<PathWithFileName>.\src\drv_mpu3050.c</PathWithFileName>
<FilenameWithoutPath>drv_mpu3050.c</FilenameWithoutPath>
@ -738,8 +738,8 @@
<Focus>0</Focus>
<ColumnNumber>5</ColumnNumber>
<tvExpOptDlg>0</tvExpOptDlg>
<TopLine>71</TopLine>
<CurrentLine>87</CurrentLine>
<TopLine>0</TopLine>
<CurrentLine>0</CurrentLine>
<bDave2>0</bDave2>
<PathWithFileName>.\src\drv_pwm.c</PathWithFileName>
<FilenameWithoutPath>drv_pwm.c</FilenameWithoutPath>
@ -752,8 +752,8 @@
<Focus>0</Focus>
<ColumnNumber>51</ColumnNumber>
<tvExpOptDlg>0</tvExpOptDlg>
<TopLine>1</TopLine>
<CurrentLine>4</CurrentLine>
<TopLine>0</TopLine>
<CurrentLine>0</CurrentLine>
<bDave2>0</bDave2>
<PathWithFileName>.\src\drv_system.c</PathWithFileName>
<FilenameWithoutPath>drv_system.c</FilenameWithoutPath>
@ -778,14 +778,42 @@
<FileType>1</FileType>
<tvExp>0</tvExp>
<Focus>0</Focus>
<ColumnNumber>1</ColumnNumber>
<ColumnNumber>17</ColumnNumber>
<tvExpOptDlg>0</tvExpOptDlg>
<TopLine>1</TopLine>
<CurrentLine>19</CurrentLine>
<TopLine>0</TopLine>
<CurrentLine>0</CurrentLine>
<bDave2>0</bDave2>
<PathWithFileName>.\src\drv_ledring.c</PathWithFileName>
<FilenameWithoutPath>drv_ledring.c</FilenameWithoutPath>
</File>
<File>
<GroupNumber>2</GroupNumber>
<FileNumber>0</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<Focus>0</Focus>
<ColumnNumber>21</ColumnNumber>
<tvExpOptDlg>0</tvExpOptDlg>
<TopLine>3</TopLine>
<CurrentLine>3</CurrentLine>
<bDave2>0</bDave2>
<PathWithFileName>.\src\drv_mpu6050.c</PathWithFileName>
<FilenameWithoutPath>drv_mpu6050.c</FilenameWithoutPath>
</File>
<File>
<GroupNumber>2</GroupNumber>
<FileNumber>0</FileNumber>
<FileType>5</FileType>
<tvExp>0</tvExp>
<Focus>0</Focus>
<ColumnNumber>0</ColumnNumber>
<tvExpOptDlg>0</tvExpOptDlg>
<TopLine>1</TopLine>
<CurrentLine>4</CurrentLine>
<bDave2>0</bDave2>
<PathWithFileName>.\src\drv_mpu6050.h</PathWithFileName>
<FilenameWithoutPath>drv_mpu6050.h</FilenameWithoutPath>
</File>
</Group>
<Group>

78
baseflight.uvproj
View File

@ -491,10 +491,74 @@
<FileType>1</FileType>
<FilePath>.\src\drv_ledring.c</FilePath>
</File>
<File>
<FileName>drv_mpu6050.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\drv_mpu6050.c</FilePath>
</File>
<File>
<FileName>drv_mpu6050.h</FileName>
<FileType>5</FileType>
<FilePath>.\src\drv_mpu6050.h</FilePath>
</File>
</Files>
</Group>
<Group>
<GroupName>System</GroupName>
<GroupOption>
<CommonProperty>
<UseCPPCompiler>0</UseCPPCompiler>
<RVCTCodeConst>0</RVCTCodeConst>
<RVCTZI>0</RVCTZI>
<RVCTOtherData>0</RVCTOtherData>
<ModuleSelection>0</ModuleSelection>
<IncludeInBuild>2</IncludeInBuild>
<AlwaysBuild>2</AlwaysBuild>
<GenerateAssemblyFile>2</GenerateAssemblyFile>
<AssembleAssemblyFile>2</AssembleAssemblyFile>
<PublicsOnly>2</PublicsOnly>
<StopOnExitCode>11</StopOnExitCode>
<CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules>
</CommonProperty>
<GroupArmAds>
<Cads>
<interw>2</interw>
<Optim>4</Optim>
<oTime>2</oTime>
<SplitLS>2</SplitLS>
<OneElfS>2</OneElfS>
<Strict>2</Strict>
<EnumInt>2</EnumInt>
<PlainCh>2</PlainCh>
<Ropi>2</Ropi>
<Rwpi>2</Rwpi>
<wLevel>0</wLevel>
<uThumb>2</uThumb>
<VariousControls>
<MiscControls></MiscControls>
<Define></Define>
<Undefine></Undefine>
<IncludePath></IncludePath>
</VariousControls>
</Cads>
<Aads>
<interw>2</interw>
<Ropi>2</Ropi>
<Rwpi>2</Rwpi>
<thumb>2</thumb>
<SplitLS>2</SplitLS>
<SwStkChk>2</SwStkChk>
<NoWarn>2</NoWarn>
<VariousControls>
<MiscControls></MiscControls>
<Define></Define>
<Undefine></Undefine>
<IncludePath></IncludePath>
</VariousControls>
</Aads>
</GroupArmAds>
</GroupOption>
<Files>
<File>
<FileName>core_cm3.c</FileName>
@ -566,7 +630,7 @@
</Groups>
</Target>
<Target>
<TargetName>STM32-FreeFlight</TargetName>
<TargetName>STM32-Release</TargetName>
<ToolsetNumber>0x4</ToolsetNumber>
<ToolsetName>ARM-ADS</ToolsetName>
<TargetOption>
@ -767,7 +831,7 @@
<hadIRAM2>0</hadIRAM2>
<hadIROM2>0</hadIROM2>
<StupSel>8</StupSel>
<useUlib>0</useUlib>
<useUlib>1</useUlib>
<EndSel>0</EndSel>
<uLtcg>0</uLtcg>
<RoSelD>3</RoSelD>
@ -1050,6 +1114,16 @@
<FileType>1</FileType>
<FilePath>.\src\drv_ledring.c</FilePath>
</File>
<File>
<FileName>drv_mpu6050.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\drv_mpu6050.c</FilePath>
</File>
<File>
<FileName>drv_mpu6050.h</FileName>
<FileType>5</FileType>
<FilePath>.\src\drv_mpu6050.h</FilePath>
</File>
</Files>
</Group>
<Group>

4812
obj/baseflight.hex
View File

File diff suppressed because it is too large Load Diff

34
src/board.h
View File

@ -9,17 +9,6 @@
#include "stm32f10x_conf.h"
#include "core_cm3.h"
#include "drv_system.h" // timers, delays, etc
#include "drv_adc.h"
#include "drv_adxl345.h"
#include "drv_bmp085.h"
#include "drv_hmc5883l.h"
#include "drv_i2c.h"
#include "drv_ledring.h"
#include "drv_mpu3050.h"
#include "drv_pwm.h"
#include "drv_uart.h"
typedef enum {
SENSOR_ACC = 1 << 0,
SENSOR_BARO = 1 << 1,
@ -38,8 +27,19 @@ typedef enum {
FEATURE_CAMTRIG = 1 << 6,
FEATURE_GYRO_SMOOTHING = 1 << 7,
FEATURE_LED_RING = 1 << 8,
FEATURE_INFLIGHT_ACC_CAL = 1 << 9,
} AvailableFeatures;
typedef void (* sensorInitFuncPtr)(void);
typedef void (* sensorReadFuncPtr)(int16_t *data);
typedef struct sensor_t
{
sensorInitFuncPtr init;
sensorReadFuncPtr read;
sensorReadFuncPtr orient;
} sensor_t;
#define digitalHi(p, i) { p->BSRR = i; }
#define digitalLo(p, i) { p->BRR = i; }
#define digitalToggle(p, i) { p->ODR ^= i; }
@ -69,3 +69,15 @@ typedef enum {
#define BARO_OFF digitalLo(BARO_GPIO, BARO_PIN);
#define BARO_ON digitalHi(BARO_GPIO, BARO_PIN);
#include "drv_system.h" // timers, delays, etc
#include "drv_adc.h"
#include "drv_adxl345.h"
#include "drv_bmp085.h"
#include "drv_hmc5883l.h"
#include "drv_i2c.h"
#include "drv_ledring.h"
#include "drv_mpu3050.h"
#include "drv_mpu6050.h"
#include "drv_pwm.h"
#include "drv_uart.h"

21
src/drv_adxl345.c
View File

@ -15,7 +15,11 @@
#define ADXL345_FULL_RANGE 0x08
#define ADXL345_RANGE_16G 0x03
bool adxl345Detect(void)
static void adxl345Init(void);
static void adxl345Read(int16_t *accelData);
static void adxl345Align(int16_t *accelData);
bool adxl345Detect(sensor_t *acc)
{
bool ack = false;
uint8_t sig = 0;
@ -23,6 +27,10 @@ bool adxl345Detect(void)
ack = i2cRead(ADXL345_ADDRESS, 0x00, 1, &sig);
if (!ack || sig != 0xE5)
return false;
acc->init = adxl345Init;
acc->read = adxl345Read;
acc->orient = adxl345Align;
return true;
}
@ -38,7 +46,7 @@ bool adxl345Detect(void)
#define ADXL_RANGE_8G 0x02
#define ADXL_RANGE_16G 0x03
void adxl345Init(void)
static void adxl345Init(void)
{
#ifdef FREEFLIGHT
i2cWrite(ADXL345_ADDRESS, ADXL345_BW_RATE, ADXL345_BW_RATE_200);
@ -60,12 +68,17 @@ void adxl345Init(void)
#endif /* FreeFlight */
}
void adxl345Read(int16_t *accelData)
static void adxl345Read(int16_t *accelData)
{
static uint8_t buf[6];
i2cRead(ADXL345_ADDRESS, ADXL345_DATA_OUT, 6, buf);
accelData[0] = buf[0] + (buf[1] << 8);
accelData[1] = buf[2] + (buf[3] << 8);
accelData[2] = buf[4] + (buf[5] << 8);
}
static void adxl345Align(int16_t *accData)
{
// official direction is RPY, nothing to change here.
}

4
src/drv_adxl345.h
View File

@ -1,5 +1,3 @@
#pragma once
bool adxl345Detect(void);
void adxl345Init(void);
void adxl345Read(int16_t *accelData);
bool adxl345Detect(sensor_t *acc);

41
src/drv_mpu3050.c
View File

@ -25,7 +25,28 @@
static uint8_t mpuLowPassFliter = MPU3050_DLPF_42HZ;
void mpu3050Init(void)
static void mpu3050Init(void);
static void mpu3050Read(int16_t *gyroData);
static void mpu3050Align(int16_t *gyroData);
bool mpu3050Detect(sensor_t *gyro)
{
bool ack;
delay(25); // datasheet page 13 says 20ms. other stuff could have been running meanwhile. but we'll be safe
ack = i2cWrite(MPU3050_ADDRESS, MPU3050_SMPLRT_DIV, 0);
if (!ack)
return false;
gyro->init = mpu3050Init;
gyro->read = mpu3050Read;
gyro->orient = mpu3050Align;
return true;
}
static void mpu3050Init(void)
{
bool ack;
@ -41,17 +62,25 @@ void mpu3050Init(void)
i2cWrite(MPU3050_ADDRESS, MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
}
static void mpu3050Align(int16_t *gyroData)
{
// official direction is RPY
gyroData[0] = gyroData[0] / 4;
gyroData[1] = gyroData[1] / 4;
gyroData[2] = -gyroData[2] / 4;
}
// Read 3 gyro values into user-provided buffer. No overrun checking is done.
void mpu3050Read(int16_t *gyroData)
static void mpu3050Read(int16_t *gyroData)
{
uint8_t buf[6];
i2cRead(MPU3050_ADDRESS, MPU3050_GYRO_OUT, 6, buf);
gyroData[0] = buf[0] << 8 | buf[1];
gyroData[1] = buf[2] << 8 | buf[3];
gyroData[2] = buf[4] << 8 | buf[5];
gyroData[0] = (buf[0] << 8) | buf[1];
gyroData[1] = (buf[2] << 8) | buf[3];
gyroData[2] = (buf[4] << 8) | buf[5];
}
int16_t mpu3050ReadTemp(void)
static int16_t mpu3050ReadTemp(void)
{
uint8_t buf[2];
i2cRead(MPU3050_ADDRESS, MPU3050_TEMP_OUT, 2, buf);

4
src/drv_mpu3050.h
View File

@ -1,5 +1,3 @@
#pragma once
void mpu3050Init(void);
void mpu3050Read(int16_t *gyroData);
int16_t mpu3050ReadTemp(void);
bool mpu3050Detect(sensor_t *gyro);

100
src/drv_mpu6050.c Normal file
View File

@ -0,0 +1,100 @@
#include "board.h"
// MPU6050, Standard address 0x68
#define MPU6050_ADDRESS 0x68
#define MPU6050_WHO_AM_I 0x75
#define MPU6050_SMPLRT_DIV 0 //8000Hz
#define MPU6050_DLPF_CFG 0
#define MPU6050_GYRO_OUT 0x43
#define MPU6050_ACC_OUT 0x3B
static void mpu6050AccInit(void);
static void mpu6050AccRead(int16_t *accData);
static void mpu6050AccAlign(int16_t *accData);
static void mpu6050GyroInit(void);
static void mpu6050GyroRead(int16_t *gyroData);
static void mpu6050GyroAlign(int16_t *gyroData);
extern uint16_t acc_1G;
bool mpu6050Detect(sensor_t *acc, sensor_t *gyro)
{
bool ack;
uint8_t sig;
delay(35); // datasheet page 13 says 30ms. other stuff could have been running meanwhile. but we'll be safe
ack = i2cRead(MPU6050_ADDRESS, MPU6050_WHO_AM_I, 1, &sig);
if (!ack)
return false;
if (sig != 0x68)
return false;
acc->init = mpu6050AccInit;
acc->read = mpu6050AccRead;
acc->orient = mpu6050AccAlign;
gyro->init = mpu6050GyroInit;
gyro->read = mpu6050GyroRead;
gyro->orient = mpu6050GyroAlign;
return true;
}
static void mpu6050AccInit(void)
{
i2cWrite(MPU6050_ADDRESS, 0x1C, 0x10); // ACCEL_CONFIG -- AFS_SEL=2 (Full Scale = +/-8G) ; ACCELL_HPF=0 //note something is wrong in the spec.
// note: something seems to be wrong in the spec here. With AFS=2 1G = 4096 but according to my measurement: 1G=2048 (and 2048/8 = 256)
// confirmed here: http://www.multiwii.com/forum/viewtopic.php?f=8&t=1080&start=10#p7480
// seems to be not a problem on MPU6000 lol
acc_1G = 1023;
}
static void mpu6050AccRead(int16_t *accData)
{
uint8_t buf[6];
i2cRead(MPU6050_ADDRESS, MPU6050_ACC_OUT, 6, buf);
accData[0] = (buf[0] << 8) | buf[1];
accData[1] = (buf[2] << 8) | buf[3];
accData[2] = (buf[4] << 8) | buf[5];
}
static void mpu6050AccAlign(int16_t *accData)
{
int16_t temp[2];
temp[0] = accData[0];
temp[1] = accData[1];
// official direction is RPY
accData[0] = temp[1] / 8;
accData[1] = -temp[0] / 8;
accData[2] = accData[2] / 8;
}
static void mpu6050GyroInit(void)
{
i2cWrite(MPU6050_ADDRESS, 0x6B, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
delay(5);
i2cWrite(MPU6050_ADDRESS, 0x19, 0x00); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
i2cWrite(MPU6050_ADDRESS, 0x1A, MPU6050_DLPF_CFG); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
i2cWrite(MPU6050_ADDRESS, 0x6B, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
i2cWrite(MPU6050_ADDRESS, 0x1B, 0x18); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
}
static void mpu6050GyroRead(int16_t *gyroData)
{
uint8_t buf[6];
i2cRead(MPU6050_ADDRESS, MPU6050_GYRO_OUT, 6, buf);
gyroData[0] = (buf[0] << 8) | buf[1];
gyroData[1] = (buf[2] << 8) | buf[3];
gyroData[2] = (buf[4] << 8) | buf[5];
}
static void mpu6050GyroAlign(int16_t *gyroData)
{
// official direction is RPY
gyroData[0] = gyroData[0] / 4;
gyroData[1] = gyroData[1] / 4;
gyroData[2] = -gyroData[2] / 4;
}

3
src/drv_mpu6050.h Normal file
View File

@ -0,0 +1,3 @@
#pragma once
bool mpu6050Detect(sensor_t *acc, sensor_t *gyro);

4
src/imu.c
View File

@ -336,8 +336,8 @@ void getEstimatedAltitude(void)
EstAlt = BaroHigh * 10 / (BARO_TAB_SIZE / 2);
temp32 = AltHold - EstAlt;
if (abs(temp32) < 10 && BaroPID < 10)
BaroPID = 0; // remove small D parametr to reduce noise near zoro position
if (abs(temp32) < 10 && abs(BaroPID) < 10)
BaroPID = 0; // remove small D parameter to reduce noise near zero position
// P
BaroPID += cfg.P8[PIDALT] * constrain(temp32, (-2) * cfg.P8[PIDALT], 2 * cfg.P8[PIDALT]) / 100;
BaroPID = constrain(BaroPID, -150, +150); // sum of P and D should be in range 150

15
src/main.c
View File

@ -31,24 +31,15 @@ int main(void)
LED1_TOGGLE;
LED0_TOGGLE;
delay(25);
BEEP_ON
BEEP_ON;
delay(25);
BEEP_OFF;
}
LED0_OFF;
LED1_OFF;
// drop out any sensors that don't seem to work
sensorsAutodetect();
// Init sensors
if (sensors(SENSOR_BARO))
bmp085Init();
if (sensors(SENSOR_ACC))
adxl345Init();
// if this fails, we get a beep + blink pattern. we're doomed.
mpu3050Init();
// drop out any sensors that don't seem to work, init all the others. halt if gyro is dead.
sensorsAutodetect();
imuInit(); // Mag is initialized inside imuInit
previousTime = micros();

95
src/mw.c
View File

@ -1,7 +1,7 @@
#include "board.h"
#include "mw.h"
// March 2012 V2.0_pre_version_1
// March 2012 V2.0_pre_version_3
#define CHECKBOXITEMS 11
#define PIDITEMS 8
@ -56,6 +56,7 @@ uint16_t GPS_distanceToHome; // in meters
int16_t GPS_directionToHome = 0; // in degrees
uint16_t GPS_distanceToHome, GPS_distanceToHold; // distance to home or hold point in meters
int16_t GPS_directionToHome, GPS_directionToHold; // direction to home or hol point in degrees
uint16_t GPS_altitude, GPS_speed; // altitude in 0.1m and speed in 0.1m/s - Added by Mis
uint8_t GPS_update = 0; // it's a binary toogle to distinct a GPS position update
int16_t GPS_angle[2]; // it's the angles that must be applied for GPS correction
@ -72,7 +73,7 @@ uint16_t AccInflightCalibrationActive = 0;
// power meter
// **********************
#define PMOTOR_SUM 8 // index into pMeter[] for sum
uint32_t pMeter[PMOTOR_SUM + 1]; //we use [0:7] for eight motors,one extra for sum
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;
@ -94,8 +95,9 @@ void blinkLED(uint8_t num, uint8_t wait, uint8_t repeat)
}
}
// this code is executed at each loop and won't interfere with control loop if it lasts less than 650 microseconds
void annexCode(void)
{ //this code is excetuted at each loop and won't interfere with control loop if it lasts less than 650 microseconds
{
static uint32_t buzzerTime, calibratedAccTime;
#if defined(LCD_TELEMETRY)
static uint16_t telemetryTimer = 0, telemetryAutoTimer = 0, psensorTimer = 0;
@ -173,19 +175,12 @@ void annexCode(void)
}
#endif
#define ADC_REF_VOLTAGE 3.3f
#define ADC_TO_VOLTAGE (ADC_REF_VOLTAGE / (1<<12)) // 12 bit ADC resolution
#define ADC_VOLTS_PRECISION 12
#define ADC_VOLTS_SLOPE ((10.0f + 1.0f) / 1.0f) // Rtop = 10K, Rbot = 1.0K
#define ADC_TO_VOLTS ((ADC_TO_VOLTAGE / ((1<<(ADC_VOLTS_PRECISION))+1)) * ADC_VOLTS_SLOPE)
if (feature(FEATURE_VBAT)) {
if (!(++vbatTimer % VBATFREQ)) {
// avgVolts = adcAvgVolts * ADC_TO_VOLTS;
vbatRawArray[(ind++) % 8] = adcGetBattery();
for (i = 0; i < 8; i++)
vbatRaw += vbatRawArray[i];
vbat = vbatRaw / (VBATSCALE / 2); // result is Vbatt in 0.1V steps
vbat = (((vbatRaw / 8) * 3.3f) / 4095) * 110; // result is Vbatt in 0.1V steps
}
if (rcOptions[BOXBEEPERON]) { // unconditional beeper on via AUXn switch
buzzerFreq = 7;
@ -232,13 +227,13 @@ void annexCode(void)
}
}
#if defined(LED_RING)
static uint32_t LEDTime;
if (currentTime > LEDTime) {
LEDTime = currentTime + 50000;
i2CLedRingState();
if (feature(FEATURE_LED_RING)) {
static uint32_t LEDTime;
if (currentTime > LEDTime) {
LEDTime = currentTime + 50000;
ledringState();
}
}
#endif
if (currentTime > calibratedAccTime) {
if (smallAngle25 == 0) {
@ -369,16 +364,20 @@ void loop(void)
calibratingG = 400;
} else if (rcData[YAW] > MAXCHECK && rcData[PITCH] > MAXCHECK && armed == 0) {
if (rcDelayCommand == 20) {
servo[0] = 1500; //we center the yaw gyro in conf mode
writeServos();
if (cfg.mixerConfiguration == MULTITYPE_TRI) {
servo[5] = 1500; // we center the yaw servo in conf mode
writeServos();
} else if (cfg.mixerConfiguration == MULTITYPE_FLYING_WING) {
servo[0] = cfg.wing_left_mid;
servo[1] = cfg.wing_right_mid;
writeServos();
}
#if defined(LCD_CONF)
configurationLoop(); //beginning LCD configuration
#endif
previousTime = micros();
}
}
#if defined(InflightAccCalibration)
else if (armed == 0 && rcData[YAW] < MINCHECK && rcData[PITCH] > MAXCHECK && rcData[ROLL] > MAXCHECK) {
} else if (feature(FEATURE_INFLIGHT_ACC_CAL) && (armed == 0 && rcData[YAW] < MINCHECK && rcData[PITCH] > MAXCHECK && rcData[ROLL] > MAXCHECK)) {
if (rcDelayCommand == 20) {
if (AccInflightCalibrationMeasurementDone) { //trigger saving into eeprom after landing
AccInflightCalibrationMeasurementDone = 0;
@ -392,9 +391,7 @@ void loop(void)
}
}
}
}
#endif
else if ((cfg.activate1[BOXARM] > 0) || (cfg.activate2[BOXARM] > 0)) {
} else if ((cfg.activate1[BOXARM] > 0) || (cfg.activate2[BOXARM] > 0)) {
if (rcOptions[BOXARM] && okToArm) {
armed = 1;
headFreeModeHold = heading;
@ -435,27 +432,23 @@ void loop(void)
} else if (rcData[PITCH] > MAXCHECK) {
cfg.accTrim[PITCH] += 2;
writeParams();
#if defined(LED_RING)
blinkLedRing();
#endif
if (feature(FEATURE_LED_RING))
ledringBlink();
} else if (rcData[PITCH] < MINCHECK) {
cfg.accTrim[PITCH] -= 2;
writeParams();
#if defined(LED_RING)
blinkLedRing();
#endif
if (feature(FEATURE_LED_RING))
ledringBlink();
} else if (rcData[ROLL] > MAXCHECK) {
cfg.accTrim[ROLL] += 2;
writeParams();
#if defined(LED_RING)
blinkLedRing();
#endif
if (feature(FEATURE_LED_RING))
ledringBlink();
} else if (rcData[ROLL] < MINCHECK) {
cfg.accTrim[ROLL] -= 2;
writeParams();
#if defined(LED_RING)
blinkLedRing();
#endif
if (feature(FEATURE_LED_RING))
ledringBlink();
} else {
rcDelayCommand = 0;
}
@ -467,22 +460,22 @@ void loop(void)
cycleTimeMin = cycleTime; // remember lowscore
#endif
#if defined(InflightAccCalibration)
if (AccInflightCalibrationArmed && armed == 1 && rcData[THROTTLE] > MINCHECK && !rcOptions[BOXARM]) { // Copter is airborne and you are turning it off via boxarm : start measurement
InflightcalibratingA = 50;
AccInflightCalibrationArmed = 0;
}
if (rcOptions[BOXPASSTHRU]) { //Use the Passthru Option to activate : Passthru = TRUE Meausrement started, Land and passtrhu = 0 measurement stored
if (!AccInflightCalibrationArmed) {
AccInflightCalibrationArmed = 1;
if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
if (AccInflightCalibrationArmed && armed == 1 && rcData[THROTTLE] > MINCHECK && !rcOptions[BOXARM]) { // Copter is airborne and you are turning it off via boxarm : start measurement
InflightcalibratingA = 50;
AccInflightCalibrationArmed = 0;
}
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;
}
} else if (AccInflightCalibrationMeasurementDone && armed == 0) {
AccInflightCalibrationArmed = 0;
AccInflightCalibrationMeasurementDone = 0;
AccInflightCalibrationSavetoEEProm = 1;
}
} else if (AccInflightCalibrationMeasurementDone && armed == 0) {
AccInflightCalibrationArmed = 0;
AccInflightCalibrationMeasurementDone = 0;
AccInflightCalibrationSavetoEEProm = 1;
}
#endif
for(i = 0; i < CHECKBOXITEMS; i++) {
rcOptions[i] = (((rcData[AUX1] < 1300) | (1300 < rcData[AUX1] && rcData[AUX1] < 1700) << 1 | (rcData[AUX1] > 1700) << 2 | (rcData[AUX2] < 1300) << 3 | (1300 < rcData[AUX2] && rcData[AUX2] < 1700) << 4 | (rcData[AUX2] > 1700) << 5) & cfg.activate1[i]) || (((rcData[AUX3] < 1300) | (1300 < rcData[AUX3] && rcData[AUX3] < 1700) << 1 | (rcData[AUX3] > 1700) << 2 | (rcData[AUX4] < 1300) << 3 | (1300 < rcData[AUX4] && rcData[AUX4] < 1700) << 4 | (rcData[AUX4] > 1700) << 5) & cfg.activate2[i]);
@ -599,7 +592,7 @@ void loop(void)
if (dif >= +180)
dif -= 360;
if (smallAngle25)
rcCommand[YAW] -= dif * cfg.P8[PIDMAG] / 30; //18 deg
rcCommand[YAW] -= dif * cfg.P8[PIDMAG] / 30; // 18 deg
} else
magHold = heading;
}

9
src/mw.h
View File

@ -62,7 +62,7 @@
with R1=33k and R2=51k
vbat = [0;1023]*16/VBATSCALE */
#define VBATFREQ 6 // to read battery voltage - keep equal to PSENSORFREQ (6) unless you know what you are doing
#define VBATSCALE 151 // change this value if readed Battery voltage is different than real voltage
#define VBATSCALE 160 // change this value if readed Battery voltage is different than real voltage
#define VBATLEVEL1_3S 107 // 10,7V
#define VBATLEVEL2_3S 103 // 10,3V
#define VBATLEVEL3_3S 99 // 9.9V
@ -75,7 +75,7 @@
//#define MMSERVOGIMBAL // Active Output Moving Average Function for Servos Gimbal
//#define MMSERVOGIMBALVECTORLENGHT 32 // Lenght of Moving Average Vector
#define VERSION 201
#define VERSION 203
// Syncronized with GUI. Only exception is mixer > 11, which is always returned as 11 during serialization.
typedef enum MultiType
@ -230,10 +230,14 @@ extern int16_t GPS_directionToHome;
extern uint8_t GPS_update;
extern uint8_t GPSModeHome;
extern uint8_t GPSModeHold;
extern uint16_t GPS_altitude;
extern uint16_t GPS_speed; // altitude in 0.1m and speed in 0.1m/s - Added by Mis
extern uint8_t vbat;
extern int16_t lookupRX[7]; // lookup table for expo & RC rate
extern config_t cfg;
extern sensor_t acc;
extern sensor_t gyro;
// main
void loop(void);
@ -257,6 +261,7 @@ void Mag_getADC(void);
void mixerInit(void);
void writeServos(void);
void writeMotors(void);
void writeAllMotors(int16_t mc);
void mixTable(void);
// Serial

153
src/sensors.c
View File

@ -8,14 +8,41 @@ uint8_t calibratingM = 0;
uint16_t acc_1G = 256; // this is the 1G measured acceleration
int16_t heading, magHold;
extern uint16_t InflightcalibratingA;
extern int16_t AccInflightCalibrationArmed;
extern uint16_t AccInflightCalibrationMeasurementDone;
extern uint16_t AccInflightCalibrationSavetoEEProm;
extern uint16_t AccInflightCalibrationActive;
sensor_t acc; // acc access functions
sensor_t gyro; // gyro access functions
void sensorsAutodetect(void)
{
if (!adxl345Detect())
// Detect what's available
if (!adxl345Detect(&acc))
sensorsClear(SENSOR_ACC);
if (!bmp085Init())
sensorsClear(SENSOR_BARO);
if (!hmc5883lDetect())
sensorsClear(SENSOR_MAG);
// Init sensors
if (sensors(SENSOR_ACC))
acc.init();
if (sensors(SENSOR_BARO))
bmp085Init();
// special case for supported gyros - MPU3050 and MPU6050
if (mpu6050Detect(&acc, &gyro)) { // first, try MPU6050, and re-enable acc (if ADXL345 is missing) since this chip has it built in
sensorsSet(SENSOR_ACC);
acc.init();
} else if (!mpu3050Detect(&gyro)) {
// if this fails, we get a beep + blink pattern. we're doomed, no gyro or i2c error.
failureMode(3);
}
// this is safe because either mpu6050 or mpu3050 sets it, and in case of fail, none do.
gyro.init();
}
static void ACC_Common(void)
@ -45,54 +72,57 @@ static void ACC_Common(void)
}
calibratingA--;
}
#if defined(InflightAccCalibration)
static int32_t b[3];
static int16_t accZero_saved[3] = { 0, 0, 0 };
static int16_t accTrim_saved[2] = { 0, 0 };
//Saving old zeropoints before measurement
if (InflightcalibratingA == 50) {
accZero_saved[ROLL] = accZero[ROLL];
accZero_saved[PITCH] = accZero[PITCH];
accZero_saved[YAW] = accZero[YAW];
accTrim_saved[ROLL] = accTrim[ROLL];
accTrim_saved[PITCH] = accTrim[PITCH];
}
if (InflightcalibratingA > 0) {
for (uint8_t axis = 0; axis < 3; axis++) {
// Reset a[axis] at start of calibration
if (InflightcalibratingA == 50)
b[axis] = 0;
// Sum up 50 readings
b[axis] += accADC[axis];
// Clear global variables for next reading
accADC[axis] = 0;
accZero[axis] = 0;
if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
static int32_t b[3];
static int16_t accZero_saved[3] = { 0, 0, 0 };
static int16_t accTrim_saved[2] = { 0, 0 };
//Saving old zeropoints before measurement
if (InflightcalibratingA == 50) {
accZero_saved[ROLL] = cfg.accZero[ROLL];
accZero_saved[PITCH] = cfg.accZero[PITCH];
accZero_saved[YAW] = cfg.accZero[YAW];
accTrim_saved[ROLL] = cfg.accTrim[ROLL];
accTrim_saved[PITCH] = cfg.accTrim[PITCH];
}
//all values are measured
if (InflightcalibratingA == 1) {
AccInflightCalibrationActive = 0;
AccInflightCalibrationMeasurementDone = 1;
blinkLED(10, 10, 2); //buzzer for indicatiing the start inflight
// recover saved values to maintain current flight behavior until new values are transferred
accZero[ROLL] = accZero_saved[ROLL];
accZero[PITCH] = accZero_saved[PITCH];
accZero[YAW] = accZero_saved[YAW];
accTrim[ROLL] = accTrim_saved[ROLL];
accTrim[PITCH] = accTrim_saved[PITCH];
if (InflightcalibratingA > 0) {
uint8_t axis;
for (axis = 0; axis < 3; axis++) {
// Reset a[axis] at start of calibration
if (InflightcalibratingA == 50)
b[axis] = 0;
// Sum up 50 readings
b[axis] += accADC[axis];
// Clear global variables for next reading
accADC[axis] = 0;
cfg.accZero[axis] = 0;
}
//all values are measured
if (InflightcalibratingA == 1) {
AccInflightCalibrationActive = 0;
AccInflightCalibrationMeasurementDone = 1;
blinkLED(10, 10, 2); //buzzer for indicatiing the start inflight
// recover saved values to maintain current flight behavior until new values are transferred
cfg.accZero[ROLL] = accZero_saved[ROLL];
cfg.accZero[PITCH] = accZero_saved[PITCH];
cfg.accZero[YAW] = accZero_saved[YAW];
cfg.accTrim[ROLL] = accTrim_saved[ROLL];
cfg.accTrim[PITCH] = accTrim_saved[PITCH];
}
InflightcalibratingA--;
}
// Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
if (AccInflightCalibrationSavetoEEProm == 1) { //the copter is landed, disarmed and the combo has been done again
AccInflightCalibrationSavetoEEProm = 0;
cfg.accZero[ROLL] = b[ROLL] / 50;
cfg.accZero[PITCH] = b[PITCH] / 50;
cfg.accZero[YAW] = b[YAW] / 50 - acc_1G; // for nunchuk 200=1G
cfg.accTrim[ROLL] = 0;
cfg.accTrim[PITCH] = 0;
writeParams(); // write accZero in EEPROM
}
InflightcalibratingA--;
}
// Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
if (AccInflightCalibrationSavetoEEProm == 1) { //the copter is landed, disarmed and the combo has been done again
AccInflightCalibrationSavetoEEProm = 0;
accZero[ROLL] = b[ROLL] / 50;
accZero[PITCH] = b[PITCH] / 50;
accZero[YAW] = b[YAW] / 50 - acc_1G; // for nunchuk 200=1G
accTrim[ROLL] = 0;
accTrim[PITCH] = 0;
writeParams(); // write accZero in EEPROM
}
#endif
accADC[ROLL] -= cfg.accZero[ROLL];
accADC[PITCH] -= cfg.accZero[PITCH];
accADC[YAW] -= cfg.accZero[YAW];
@ -101,11 +131,9 @@ static void ACC_Common(void)
void ACC_getADC(void)
{
int16_t rawADC[3];
adxl345Read(rawADC);
acc.read(accADC);
acc.orient(accADC);
ACC_ORIENTATION(-(rawADC[1]), (rawADC[0]), (rawADC[2]));
ACC_Common();
}
@ -207,15 +235,9 @@ static void GYRO_Common(void)
void Gyro_getADC(void)
{
int16_t rawADC[3];
mpu3050Read(rawADC);
// range: +/- 8192; +/- 2000 deg/sec
GYRO_ORIENTATION(+((rawADC[1]) / 4), -((rawADC[0]) / 4), -((rawADC[2]) / 4));
gyroADC[ROLL] = rawADC[0] / 4;
gyroADC[PITCH] = rawADC[1] / 4;
gyroADC[YAW] = -rawADC[2] / 4;
gyro.read(gyroADC);
gyro.orient(gyroADC);
GYRO_Common();
}
@ -228,17 +250,10 @@ static void Mag_getRawADC(void)
static int16_t rawADC[3];
hmc5883lRead(rawADC);
#if 0
// no way? is this finally the proper orientation?? (seems -180 swapped)
magADC[ROLL] = -rawADC[2]; // X
magADC[PITCH] = rawADC[0]; // Y
magADC[YAW] = rawADC[1]; // Z
#else
// no way? is THIS finally the proper orientation?? (by GrootWitBaas)
magADC[ROLL] = rawADC[2]; // X
magADC[PITCH] = -rawADC[0]; // Y
magADC[YAW] = -rawADC[1]; // Z
#endif
}
void Mag_init(void)
@ -271,12 +286,16 @@ void Mag_getADC(void)
// Read mag sensor
Mag_getRawADC();
magADC[ROLL] = magADC[ROLL] * magCal[ROLL];
magADC[PITCH] = magADC[PITCH] * magCal[PITCH];
magADC[YAW] = magADC[YAW] * magCal[YAW];
if (calibratingM == 1) {
tCal = t;
for (axis = 0; axis < 3; axis++) {
cfg.magZero[axis] = 0;
magZeroTempMin[axis] = 0;
magZeroTempMax[axis] = 0;
magZeroTempMin[axis] = magADC[axis];
magZeroTempMax[axis] = magADC[axis];
}
calibratingM = 0;
}

19
src/serial.c
View File

@ -185,7 +185,7 @@ void serialCom(void)
}
serialize16(GPS_distanceToHome);
serialize16(GPS_directionToHome);
serialize16(GPS_directionToHome + 180);
serialize8(GPS_numSat);
serialize8(GPS_fix);
serialize8(GPS_update);
@ -214,17 +214,24 @@ void serialCom(void)
for (i = 0; i < 6; i++) {
serialize16(rcData[i]);
} //44
serialize8(sensors(SENSOR_ACC) << 1 | sensors(SENSOR_BARO) << 2 | sensors(SENSOR_MAG) << 3);
serialize8(accMode | baroMode << 1 | magMode << 2);
serialize8(sensors(SENSOR_ACC) << 1 | sensors(SENSOR_BARO) << 2 | sensors(SENSOR_MAG) << 3 | sensors(SENSOR_GPS) << 4);
serialize8(accMode | baroMode << 1 | magMode << 2 | GPSModeHome << 3 | GPSModeHold << 4 | armed << 5);
serialize8(vbat); // Vbatt 47
serialize8(VERSION); // MultiWii Firmware version
serialize8('O'); //49
// UartSendData();
serialize8(GPS_fix); // Fix indicator for OSD
serialize8(GPS_numSat);
serialize16(GPS_latitude);
serialize16(GPS_latitude >> 16);
serialize16(GPS_longitude);
serialize16(GPS_longitude >> 16);
serialize16(GPS_altitude);
serialize16(GPS_speed); // Speed for OSD
serialize8('O'); // NOT 49 anymore
break;
case 'R': // reboot to bootloader (oops, apparently this w as used for other trash, fix later)
systemReset(true);
break;
case 'X': // dynamic mixer
i = uartReadPoll();
if (i > 64 && i < 64 + MULTITYPE_LAST) {