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

fixed up mpu6050 DMP stuff. 

increased i2c send buffer to 16bytes (was needed for DMP)
decreased interrupt priority of both uarts - with GPS running at 115200, this would cause I2C errors (i2c interrupt must be highest priority).
GPS was tested with $25 FMP04 GPS module ghettoed up outside my window, locking 5 satellites in a few minutes.

git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@140 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop 2012-04-02 16:08:24 +00:00
parent 14f87034c7
commit 861b274039
6 changed files with 146 additions and 107 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

144
obj/baseflight.hex
View File

@ -12,7 +12,7 @@
:1000A0004F7400084F7400084F7400084F74000824
:1000B00085460008974600084F740008D1420008A2
:1000C000AF400008D5420008D34200084F74000832
:1000D0004F7400084F740008354F00084F74000833
:1000D0004F7400084F740008374F00084F74000831
:1000E000ED3B00084F7400084F740008DFF80CD097
:1000F00007F00EFF004800474D130008C01700200E
:1001000070B51546B0FBF5F60446A84203D3304659
@ -166,8 +166,8 @@
:100A4000260100202C20493243204572726F7273B8
:100A50003A2000004166726F333220434C492076C1
:100A6000657273696F6E20322E302D70726533207F
:100A70004D61722033312032303132202F2031321B
:100A80003A34353A35310000280200200D0A456E0F
:100A70004170722020332032303132202F2030302C
:100A80003A31353A33380000280200200D0A456E0D
:100A9000746572696E6720434C49204D6F64652C04
:100AA00020747970652027657869742720746F2019
:100AB00072657475726E2C206F72202768656C7079
@ -949,18 +949,18 @@
:103B3000FAD10121204601F0CAFC10B070BD034843
:103B400000887047080002404C24014040020020D9
:103B5000704710B50446214B06223221532000F055
:103B600000FC1E4908784A7800EB02202080887803
:103B600001FC1E4908784A7800EB02202080887802
:103B7000CA7800EB022060800879497900EB0120C7
:103B8000A08010BD10B508222D21532000F0E3FBCA
:103B90000B223121532000F0DEFB09222C21BDE84D
:103BA0001040532000F0D7BB38B5044600208DF8F4
:103BB000000001466B460122532000F0D2FB18B1F1
:103B8000A08010BD10B508222D21532000F0E4FBC9
:103B90000B223121532000F0DFFB09222C21BDE84C
:103BA0001040532000F0D8BB38B5044600208DF8F3
:103BB000000001466B460122532000F0D3FB18B1F0
:103BC0009DF80000E52801D0002038BD04482060A1
:103BD000044860600448A060012038BD4202002013
:103BE000853B0008533B0008513B000810B54FF4DB
:103BF0008044204601F0C6FE012805D1204601F090
:103C0000CFFEBC490120087010BD10B5B94C86B07C
:103C10006B4660681622AA21807E00F0A2FBBDF8E8
:103C10006B4660681622AA21807E00F0A3FBBDF8E7
:103C2000000041BA60680180BDF8021049BA4180C5
:103C3000BDF8041049BA8180BDF8061049BAC180A8
:103C4000BDF8081049BA0181BDF80A1049BA41818E
@ -976,22 +976,22 @@
:103CE00001FE28208DF810000F208DF811008DF8AE
:103CF00012008DF8135004A801F01DFB0C2000F0F9
:103D0000F8FE616877206B46CE7688760122D02156
:103D100000F027FB60689DF800100222C175028444
:103D100000F028FB60689DF800100222C175028443
:103D2000552901D00020AFE75521C176807E6B4632
:103D30000122D12100F015FB9DF80000616800F020
:103D30000122D12100F016FB9DF80000616800F01F
:103D40000F0200090A764876FFF75FFF65709AE771
:103D5000684808B542F21071027800E000BF22B94D
:103D60000B00A1F1010189B2F8D140686B46022233
:103D7000807EF62100F0F5FABDF8000040BA08BDDB
:103D7000807EF62100F0F6FABDF8000040BA08BDDA
:103D80005C494968CA7E552A01D0C8690EE04A8953
:103D9000B1F91430801A0A895043C013B1F91220C6
:103DA0000344D20292FBF3F21044C86108300011C0
:103DB00000B270474F4800212E2201704068F42164
:103DC000807E00F0C8BA10B54A4C00202070FFF782
:103DC000807E00F0C9BA10B54A4C00202070FFF781
:103DD000F1FF20780028FDD0FFF7BAFFBDE81040C2
:103DE000CEE738B5434C42F21070217800E000BFB6
:103DF00021B90200A0F1010080B2F8D160686B46E1
:103E00000322807EF62100F0ACFA9DF800009DF8B8
:103E00000322807EF62100F0ADFA9DF800009DF8B7
:103E10000110000440EA01209DF802100843616887
:103E2000B1F92010C1F10801C84038BD70B5314961
:103E30004A68D169B2F90E40A1F57A6101FB01F33C
@ -1004,18 +1004,18 @@
:103EA0000112494340F6DE325143144A0914424399
:103EB00001EB224101F6CF6100EB211070BD0D48EE
:103EC000342341680A8C03EB821200230370887E3E
:103ED000D2B2F42100F03FBA10B5064C0020207099
:103ED000D2B2F42100F040BA10B5064C0020207098
:103EE000FFF7EDFF20780028FDD0FFF77AFFBDE84F
:103EF00010409BE7480200200010014043E3FFFF11
:103F000008B500208DF800006B4601220A211E2012
:103F100000F027FA18B19DF80000482801D00020D1
:103F100000F028FA18B19DF80000482801D00020D0
:103F200008BD012008BD10B5642000F0E2FD71223B
:103F300000211E2000F00FFA322000F0DAFD60228E
:103F400001211E2000F007FA01220221BDE81040E5
:103F50001E2000F000BA10B5702200211E2000F0D3
:103F6000FAF9202201211E2000F0F5F90022022199
:103F7000BDE810401E2000F0EEB91CB504466B46AB
:103F8000062203211E2000F0ECF9BDF8000040BA23
:103F300000211E2000F010FA322000F0DAFD60228D
:103F400001211E2000F008FA01220221BDE81040E4
:103F50001E2000F001BA10B5702200211E2000F0D2
:103F6000FBF9202201211E2000F0F6F90022022197
:103F7000BDE810401E2000F0EFB91CB504466B46AA
:103F8000062203211E2000F0EDF9BDF8000040BA22
:103F90002080BDF8020040BA6080BDF8040040BA3D
:103FA000A0801CBD30B587B005464FF44060ADF829
:103FB000140003208DF816001C208DF8170005A9A9
@ -1068,21 +1068,21 @@
:1042A000207A0028F6D1A0790028F3D094F90210E2
:1042B000A079401C814209D12670607828B1002283
:1042C0004FF44071E06801F044FCE670BDE8F08115
:1042D000EEE6B8E6ECE67CB5374C400047F23055E8
:1042D000EEE6B8E6ECE67FB5374C400047F23055E5
:1042E000207161710120E07100212172C4F810D0A9
:1042F000C4F814D06946A271E070072A01D90020E1
:104300007CBD002003E01E5C0E54401CC0B29042F5
:10431000F9D3E068818889050ED40188C90505D4E0
:1043200001888905FCD4012101F0D9FB01224FF459
:104330004071E06801F00DFCE07810B16D1EFBD11A
:1043400002E00DB101207CBD6089401C6081E06805
:10435000FFF728FED3E707B502AB0122FFF7BBFF4B
:104360000EBD70B5144C4FEA400047F23055207135
:1043700061710020E0710120207263612361A271EC
:10438000E070E068818889050ED40188C90505D4EC
:1043900001888905FCD4012101F0A1FB01224FF421
:1043A0004071E06801F0D5FBE07848B16D1EFBD1AB
:1043B00009E00000000C014050020020801A0600B5
:1042F000C4F814D06946A271E070102A02D90020D7
:1043000004B070BD002003E01E5C0E54401CC0B21F
:104310009042F9D3E068818889050ED40188C905E7
:1043200005D401888905FCD4012101F0D8FB0122C4
:104330004FF44071E06801F00CFCE07810B16D1EA4
:10434000FBD102E00DB10120DAE76089401C6081F9
:10435000E068FFF727FED2E707B502AB0122FFF7BF
:10436000BAFF0EBD70B5144C4FEA400047F230550D
:10437000207161710020E07101202072636123616E
:10438000A271E070E068818889050ED40188C905B2
:1043900005D401888905FCD4012101F0A0FB01228C
:1043A0004FF44071E06801F0D4FBE07840B16D1E3D
:1043B000FBD108E0000C014050020020801A0600EA
:1043C0000DB1012070BD6089401C6081E068FFF77D
:1043D000E9FD002070BD2F48408970472DE9F84362
:1043E0004FF44067ADF800704FF003088DF802807D
@ -1100,19 +1100,19 @@
:1044A000CA1701EB927189100180B0F90210CA1786
:1044B00001EB927189104180B0F904104942CA178A
:1044C00001EB92718910818070471CB504466B46E0
:1044D00006221D216820FFF744FFBDF8000040BA06
:1044D00006221D216820FFF745FFBDF8000040BA05
:1044E0002080BDF8020040BA6080BDF8040040BAE8
:1044F000A0801CBD10B5192000F0FBFA0022152188
:104500006820FFF728FF10B9032000F099FB284826
:104510001621007840F018026820FFF71CFF0022E7
:1045200017216820FFF717FF01223D216820FFF7C0
:1045300012FF01223E21BDE810406820FFF70BBFAB
:104500006820FFF729FF10B9032000F099FB284825
:104510001621007840F018026820FFF71DFF0022E6
:1045200017216820FFF718FF01223D216820FFF7BF
:1045300013FF01223E21BDE810406820FFF70CBFA9
:1045400010B50446192000F0D4FA00221521682085
:10455000FFF701FF002806D016482060164860606B
:10455000FFF702FF002806D016482060164860606A
:104560001648A060012010BD1149622817D006DC52
:104570000A281AD0142816D02A2809D111E0BC28FC
:1045800004D0B0F5807F03D1002000E00120087046
:104590000878162140F018026820FFF7DCBE0220E0
:104590000878162140F018026820FFF7DDBE0220DF
:1045A000F5E70320F3E70420F1E70520EFE700003B
:1045B00068020020F5440008CB4400089D44000830
:1045C0002DE9F84F4FF0000BDFF81CA4DFF81C9426
@ -1233,15 +1233,15 @@
:104CF0009D480121443000F09ABD10B54FF400509A
:104D000000F0D3FD98480021443000F090FD954814
:104D1000001D03C8814202D0BDE81040D6E710BD97
:104D20002DE9F04192B005464FF40070ADF83C001B
:104D300003208DF83E0018208C4C8DF83F000FA901
:104D4000204602F03EF82015ADF83C0048208DF8D2
:104D50003F000FA9204602F034F80E208DF84000E5
:104D600001208DF8410000248DF842408DF8430069
:104D700010A800F0E0FAADF834400C20DFF8F08124
:104D8000ADF83040ADF836000B95ADF83240ADF8D7
:104D20002DE9F04192B004464FF40070ADF83C001C
:104D300003208DF83E0018208C4D8DF83F000FA900
:104D4000284602F03EF82815ADF83C0048208DF8C2
:104D50003F000FA9284602F034F80E208DF84000DD
:104D600001218DF841108DF842108DF8431010A8E4
:104D700000F0E1FA0B940024ADF834400C20DFF889
:104D8000F081ADF83040ADF83600ADF83240ADF806
:104D900038400BA9404601F055FE734F583738464E
:104DA00000F0AEFC08F10405B8140095CDE9090443
:104DA00000F0AEFCA81408F104050095CDE9090453
:104DB0007048CDE9010480260494CDE90564A81566
:104DC00003902020CDE907406946384600F005FDF4
:104DD0000121384600F02BFD01224021404601F020
@ -1258,20 +1258,20 @@
:104E80000028FBD0BDE81040EAE7364A344B916871
:104E90005854491CC8B290603348406CC00700D1D8
:104EA00014E7704710B5044602E0641CFFF7EDFFFD
:104EB00020780028F9D110BDF0B50F460646012133
:104EC00087B0480401F010F826208DF81000002467
:104ED0008DF8114001208DF812008DF8130004A800
:104EE00000F029FA0820ADF8140004258DF81750B9
:104EF00005A91E4801F065FFADF80440ADF8064075
:104F0000ADF80840ADF80C401B4C0096ADF80A50C7
:104F10006946204601F096FD012240F225512046C7
:104F200001F012FE0121204601F004FE0D48C76089
:104F300007B0F0BD70B5104D40F22551284601F084
:104F4000BAFE01280AD1074CE068002806D028469E
:104F500001F04AFEE168BDE87040084770BD0000FE
:104EB00020780028F9D110BD70B50D4606460121B5
:104EC00086B0480401F010F826208DF8100001206B
:104ED0008DF8110002218DF812108DF8130004A82E
:104EE00000F029FA0820ADF8140004248DF81740CA
:104EF00005A91E4801F065FF0020ADF80800ADF8D7
:104F00000A40ADF804001C4CADF80600ADF80C00EA
:104F100000966946204601F095FD012240F2255198
:104F2000204601F011FE0121204601F003FE0D484C
:104F3000C56006B070BD70B50F4D40F225512846D2
:104F400001F0B9FE01280AD1064CE068002806D01D
:104F5000284601F049FEE168BDE87040084770BD91
:104F6000BE060020E80200200000024000080140C8
:104F700000380140BE0500200044004010B5652205
:104F8000FF216D20FFF7E7F9002800D0012010BDB8
:104F8000FF216D20FFF7E8F9002800D0012010BDB7
:104F9000FEB5384C0125207878B1012822D00228AE
:104FA0001FD164208DF8000033488DF80150002691
:104FB0000078A0B38DF8025043E07A208DF800000D
@ -1291,23 +1291,23 @@
:105090000180B0F90210CA1701EB927189104180AA
:1050A000B0F904104942CA1701EB9271891081804E
:1050B00070471CB504466B46062243216820FFF763
:1050C00050F9BDF8000040BA2080BDF8020040BA97
:1050C00051F9BDF8000040BA2080BDF8020040BA96
:1050D0006080BDF8040040BAA0801CBD10B58022DD
:1050E0006B216820FFF737F90520FFF702FD00224A
:1050F00019216820FFF72FF900221A216820FFF7F5
:105100002AF903226B216820FFF725F918221B21B9
:10511000BDE810406820FFF71EB9B0F90210B0F9E1
:1050E0006B216820FFF738F90520FFF702FD002249
:1050F00019216820FFF730F900221A216820FFF7F4
:105100002BF903226B216820FFF726F918221B21B7
:10511000BDE810406820FFF71FB9B0F90210B0F9E0
:105120000020CB1701EB5371C91001805142CA17FF
:1051300001EB5271C9104180B0F90410CA1701EB9C
:105140005271C910818070471CB504466B46062217
:105150003B216820FFF705F9BDF8000040BA208028
:105150003B216820FFF706F9BDF8000040BA208027
:10516000BDF8020040BA6080BDF8040040BAA080DB
:105170001CBD10B510221C216820FFF7ECF8144963
:105170001CBD10B510221C216820FFF7EDF8144962
:1051800040F2FF30088010BD38B504460D4623209C
:10519000FFF7AFFC6B46012275216820FFF7E1F8AD
:10519000FFF7AFFC6B46012275216820FFF7E2F8AC
:1051A000002804D09DF80000682801D0002038BDF8
:1051B00008482060084860600848A06008482860E7
:1051C000084868600848A860012038BD70470000A2
:1051C000084868600848A860012038BD70477047EB
:1051D000E001002073510008495100081B510008EC
:1051E000DD500008B350000885500008474810B54E
:1051F000016841F0010101604168454A1140416088

11
src/drv_i2c.c
View File

@ -76,7 +76,7 @@ static void i2c_er_handler(void)
bool i2cWriteBuffer(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data)
{
uint8_t i;
uint8_t my_data[8];
uint8_t my_data[16];
uint32_t timeout = I2C_DEFAULT_TIMEOUT;
addr = addr_ << 1;
@ -89,7 +89,7 @@ bool i2cWriteBuffer(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data)
busy = 1;
// too long
if (len_ > 7)
if (len_ > 16)
return false;
for (i = 0; i < len_; i++)
@ -268,7 +268,7 @@ void i2cInit(I2C_TypeDef *I2C)
GPIO_Init(GPIOB, &GPIO_InitStructure);
I2Cx = I2C;
// clock out stuff to make sure slaves arent stuck
i2cUnstick();
@ -281,7 +281,7 @@ void i2cInit(I2C_TypeDef *I2C)
I2C_InitStructure.I2C_DutyCycle = I2C_DutyCycle_2;
I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
I2C_InitStructure.I2C_ClockSpeed = 400000;
I2C_Cmd(I2Cx, ENABLE);
I2C_Cmd(I2Cx, ENABLE);
I2C_Init(I2Cx, &I2C_InitStructure);
NVIC_PriorityGroupConfig(0x500);
@ -292,6 +292,7 @@ void i2cInit(I2C_TypeDef *I2C)
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
// I2C EV Interrupt
NVIC_InitStructure.NVIC_IRQChannel = I2C2_EV_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
@ -327,7 +328,7 @@ static void i2cUnstick(void)
GPIO_SetBits(GPIOB, GPIO_Pin_10); //Set bus high
delayMicroseconds(10);
}
// Generate a start then stop condition
// SCL PB10
// SDA PB11

87
src/drv_mpu6050.c
View File

@ -122,6 +122,8 @@ static void mpu6050GyroAlign(int16_t * gyroData);
#ifdef MPU6050_DMP
static void mpu6050DmpInit(void);
float dmpdata[2];
int16_t dmpGyroData[3];
#endif
extern uint16_t acc_1G;
@ -171,10 +173,12 @@ static void mpu6050AccRead(int16_t * accData)
#ifndef MPU6050_DMP
i2cRead(MPU6050_ADDRESS, MPU_RA_ACCEL_XOUT_H, 6, buf);
#endif
accData[0] = (buf[0] << 8) | buf[1];
accData[1] = (buf[2] << 8) | buf[3];
accData[2] = (buf[4] << 8) | buf[5];
#else
accData[0] = accData[1] = accData[2] = 0;
#endif
}
static void mpu6050AccAlign(int16_t * accData)
@ -206,10 +210,14 @@ static void mpu6050GyroRead(int16_t * gyroData)
uint8_t buf[6];
#ifndef MPU6050_DMP
i2cRead(MPU6050_ADDRESS, MPU_RA_GYRO_XOUT_H, 6, buf);
#endif
gyroData[0] = (buf[0] << 8) | buf[1];
gyroData[1] = (buf[2] << 8) | buf[3];
gyroData[2] = (buf[4] << 8) | buf[5];
#else
gyroData[0] = dmpGyroData[0];
gyroData[1] = dmpGyroData[1];
gyroData[2] = dmpGyroData[2];
#endif
}
static void mpu6050GyroAlign(int16_t * gyroData)
@ -404,10 +412,9 @@ const uint8_t dmp_updates[29][9] = {
{0x07, 0x46, 0x01, 0x9A}, //CFG_GYRO_SOURCE inv_send_gyro
{0x07, 0x47, 0x04, 0xF1, 0x28, 0x30, 0x38}, //CFG_9 inv_send_gyro -> inv_construct3_fifo
{0x07, 0x6C, 0x04, 0xF1, 0x28, 0x30, 0x38}, //CFG_12 inv_send_accel -> inv_construct3_fifo
{0x02, 0x16, 0x02, 0x00, 0x0A}, //D_0_22 inv_set_fifo_rate
{0x02, 0x16, 0x02, 0x00, 0x00}, //D_0_22 inv_set_fifo_rate
};
static long dmp_lastRead = 0;
static uint8_t dmp_processed_packet[8];
static uint8_t dmp_received_packet[50];
@ -423,7 +430,7 @@ static void mpu6050DmpBankSelect(uint8_t bank);
static bool mpu6050DmpFifoReady(void);
static void mpu6050DmpGetPacket(void);
static void mpu6050DmpProcessQuat(void);
static void mpu6050DmpResetFifo(void);
void mpu6050DmpResetFifo(void);
static void mpu6050DmpInit(void)
{
@ -451,7 +458,6 @@ static void mpu6050DmpInit(void)
delay(5);
mpu6050DmpMemInit();
delay(20);
}
void mpu6050DmpLoop(void)
@ -460,8 +466,9 @@ void mpu6050DmpLoop(void)
uint8_t buf[2];
if (mpu6050DmpFifoReady()) {
LED1_ON;
mpu6050DmpGetPacket();
i2cRead(MPU6050_ADDRESS, MPU_RA_INT_STATUS, 1, &temp);
if (dmp_firstPacket) {
delay(1);
@ -482,37 +489,48 @@ void mpu6050DmpLoop(void)
if (dmp_fifoCountL == 42) {
mpu6050DmpProcessQuat();
}
LED1_OFF;
}
}
double dmpdata[3];
#define dmp_quatTake32(a, b) (((a)[4*(b)+0]<<8) | ((a)[4*(b)+1]<<0))
extern int16_t angle[2];
static void mpu6050DmpProcessQuat(void)
{
float quat0, quat1, quat2, quat3;
int32_t quatl0, quatl1, quatl2, quatl3;
double quat0, quat1, quat2, quat3;
double rot6, rot7, rot8;
#define dmp_quatTake32(a, b) (((a)[4*(b)+3]<<24) | ((a)[4*(b)+2]<<16) | ((a)[4*(b)+1]<<8) | ((a)[4*(b)+0]))
quatl0 = dmp_quatTake32(dmp_received_packet, 0);
quatl1 = dmp_quatTake32(dmp_received_packet, 1);
quatl2 = dmp_quatTake32(dmp_received_packet, 2);
quatl3 = dmp_quatTake32(dmp_received_packet, 3);
quat0 = (double) quatl0 / 32768.0;
quat1 = (double) quatl1 / 32768.0;
quat2 = (double) quatl2 / 32768.0;
quat3 = (double) quatl3 / 32768.0;
rot6 = (quat1 * quat3) - (quat2 * quat0);
rot7 = (quat2 * quat3) + (quat1 * quat0);
rot8 = (quat3 * quat3) + (quat0 * quat0) - 1.0;
if (quatl0 > 32767)
quatl0 -= 65536;
if (quatl1 > 32767)
quatl1 -= 65536;
if (quatl2 > 32767)
quatl2 -= 65536;
if (quatl3 > 32767)
quatl3 -= 65536;
dmpdata[0] = atan2(rot7, rot8);
dmpdata[1] = atan2(rot8, rot6);
dmpdata[2] = ((dmp_received_packet[4 * 6 + 0] << 8) | (dmp_received_packet[4 * 6 + 1] << 0));
quat0 = ((float) quatl0) / 16384.0f;
quat1 = ((float) quatl1) / 16384.0f;
quat2 = ((float) quatl2) / 16384.0f;
quat3 = ((float) quatl3) / 16384.0f;
dmpdata[0] = atan2f(2 * ((quat0 * quat1) + (quat2 * quat3)), 1.0 - (2 * ((quat1 * quat1) + (quat2 * quat2)))) * (180.0f / M_PI);
dmpdata[1] = asinf(2 * ((quat0 * quat2) - (quat3 * quat1))) * (180.0f / M_PI);
angle[0] = dmpdata[0] * 10;
angle[1] = dmpdata[1] * 10;
dmpGyroData[0] = ((dmp_received_packet[4 * 4 + 0] << 8) | (dmp_received_packet[4 * 4 + 1] << 0));
dmpGyroData[1] = ((dmp_received_packet[4 * 5 + 0] << 8) | (dmp_received_packet[4 * 5 + 1] << 0));
dmpGyroData[2] = ((dmp_received_packet[4 * 6 + 0] << 8) | (dmp_received_packet[4 * 6 + 1] << 0));
}
static void mpu6050DmpResetFifo(void)
void mpu6050DmpResetFifo(void)
{
uint8_t ctrl;
@ -521,7 +539,6 @@ static void mpu6050DmpResetFifo(void)
i2cWrite(MPU6050_ADDRESS, MPU_RA_USER_CTRL, ctrl);
}
static void mpu6050DmpGetPacket(void)
{
if (dmp_fifoCountL > 32) {
@ -538,6 +555,8 @@ static void mpu6050DmpGetPacket(void)
}
}
uint16_t dmpFifoLevel = 0;
static bool mpu6050DmpFifoReady(void)
{
uint8_t buf[2];
@ -545,11 +564,24 @@ static bool mpu6050DmpFifoReady(void)
i2cRead(MPU6050_ADDRESS, MPU_RA_FIFO_COUNTH, 2, buf);
dmp_fifoCountL = buf[1];
dmpFifoLevel = buf[0] << 8 | buf[1];
if (dmp_fifoCountL == 42 || dmp_fifoCountL == 44)
return true;
else
return false;
else {
// lame hack to empty out fifo, as dmpResetFifo doesn't actually seem to do it...
if (dmpFifoLevel > 100) {
// clear out fifo
uint8_t crap[16];
do {
i2cRead(MPU6050_ADDRESS, MPU_RA_FIFO_R_W, dmpFifoLevel > 16 ? 16 : dmpFifoLevel, crap);
i2cRead(MPU6050_ADDRESS, MPU_RA_FIFO_COUNTH, 2, buf);
dmpFifoLevel = buf[0] << 8 | buf[1];
} while (dmpFifoLevel);
}
}
return false;
}
static void mpu6050DmpBankSelect(uint8_t bank)
@ -682,5 +714,10 @@ static void mpu6050DmpMemInit(void)
void mpu6050DmpLoop(void)
{
}
void mpu6050DmpResetFifo(void)
{
}
#endif /* MPU6050_DMP */

1
src/drv_mpu6050.h
View File

@ -2,3 +2,4 @@
bool mpu6050Detect(sensor_t *acc, sensor_t *gyro);
void mpu6050DmpLoop(void);
void mpu6050DmpResetFifo(void);

2
src/drv_system.c
View File

@ -72,7 +72,7 @@ void systemInit(void)
GPIO_Init(GPIOA, &GPIO_InitStructure);
GPIO_Init(GPIOB, &GPIO_InitStructure);
GPIO_Init(GPIOC, &GPIO_InitStructure);
// Turn off JTAG port 'cause we're using the GPIO for leds
GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE);

8
src/drv_uart.c
View File

@ -59,7 +59,7 @@ void uartInit(uint32_t speed)
// DMA TX Interrupt
NVIC_InitStructure.NVIC_IRQChannel = DMA1_Channel4_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
@ -70,7 +70,7 @@ void uartInit(uint32_t speed)
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
USART_Init(USART1, &USART_InitStructure);
// Receive DMA into a circular buffer
DMA_DeInit(DMA1_Channel5);
DMA_InitStructure.DMA_Priority = DMA_Priority_Medium;
@ -163,8 +163,8 @@ void uart2Init(uint32_t speed, uartReceiveCallbackPtr func)
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_IRQChannelPreemptionPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);