coolfactor/quadrotor
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Offload I2C work onto interrupts. For now, we just spin while waiting

Browse Source 
for the I2C activity to complete.
This commit is contained in:
Gavan Fantom
2011-05-26 01:57:16 +00:00
parent 6a3c096bc5
commit 0f508fb424
5 changed files with 197 additions and 135 deletions
Download Patch File Download Diff File Expand all files Collapse all files

124
wmp.c
View File

@@ -2,46 +2,77 @@
#include "wmp.h"
#include "i2c.h"
unsigned char wmp_init_command[2] = {0xfe, 0x04};
i2c_result wmp_result;
struct i2c_transaction wmp_init_transaction = {
(0x53 << 1) + 0, /* write */
2,
wmp_init_command,
&wmp_result,
NULL
};
unsigned char wmp_read_cal_command[1] = {0x20};
struct i2c_transaction wmp_read_cal_transaction2;
struct i2c_transaction wmp_read_cal_transaction = {
(0x53 << 1) + 0, /* write */
1,
wmp_read_cal_command,
&wmp_result,
&wmp_read_cal_transaction2
};
struct i2c_transaction wmp_read_cal_transaction2 = {
(0x53 << 1) + 1, /* read */
0x20,
wmp_calibration_data,
&wmp_result,
NULL
};
unsigned char wmp_sample_command[1] = {0x00};
unsigned char wmp_sample_data[6];
struct i2c_transaction wmp_sample_transaction2;
struct i2c_transaction wmp_sample_transaction = {
(0x52 << 1) + 0, /* write */
1,
wmp_sample_command,
&wmp_result,
&wmp_sample_transaction2
};
struct i2c_transaction wmp_sample_transaction2 = {
(0x52 << 1) + 1, /* read */
6,
wmp_sample_data,
&wmp_result,
NULL
};
bool wmp_init(void)
{
if (!i2c_send_start())
if (!i2c_start_transaction(&wmp_init_transaction))
return FALSE;
if (!i2c_send_address(0x53, TRUE))
return FALSE;
if (!i2c_send_data(0xfe))
return FALSE;
if (!i2c_send_data(0x04))
return FALSE;
i2c_send_stop();
return TRUE;
while (i2c_busy()) ;
return (wmp_result == I2C_SUCCESS);
}
unsigned char wmp_calibration_data[0x20];
bool wmp_read_calibration_data(void)
{
int i;
if (!i2c_send_start())
if (!i2c_start_transaction(&wmp_read_cal_transaction))
return FALSE;
if (!i2c_send_address(0x53, TRUE))
return FALSE;
if (!i2c_send_data(0x20))
return FALSE;
i2c_send_stop();
if (!i2c_send_start())
return FALSE;
if (!i2c_send_address(0x53, FALSE))
return FALSE;
for (i = 0; i < 0x20; i++) {
unsigned int data;
if (!i2c_receive_data(&data, (i == 0x1f)))
return FALSE;
wmp_calibration_data[i] = data;
}
i2c_send_stop();
return TRUE;
while (i2c_busy());
return (wmp_result == I2C_SUCCESS);
}
unsigned int wmp_yaw;
@@ -66,35 +97,22 @@ bool wmp_roll_fast;
bool wmp_sample(void)
{
int i;
unsigned int b[6];
if (!i2c_start_transaction(&wmp_sample_transaction))
return FALSE;
if (!i2c_send_start())
return FALSE;
if (!i2c_send_address(0x52, TRUE))
return FALSE;
if (!i2c_send_data(0x00))
return FALSE;
i2c_send_stop();
while (i2c_busy());
if (!i2c_send_start())
if (wmp_result != I2C_SUCCESS)
return FALSE;
if (!i2c_send_address(0x52, FALSE))
return FALSE;
for (i = 0; i < 6; i++) {
if (!i2c_receive_data(&(b[i]), (i == 5)))
return FALSE;
}
i2c_send_stop();
wmp_yaw = ((b[3]>>2)<<8) + b[0];
wmp_pitch = ((b[4]>>2)<<8) + b[1];
wmp_roll = ((b[5]>>2)<<8) + b[2];
wmp_yaw = ((wmp_sample_data[3]>>2)<<8) + wmp_sample_data[0];
wmp_pitch = ((wmp_sample_data[4]>>2)<<8) + wmp_sample_data[1];
wmp_roll = ((wmp_sample_data[5]>>2)<<8) + wmp_sample_data[2];
/* XXX We don't take into account the fast/slow mode flag here */
wmp_yaw_fast = !(b[3] & 0x2);
wmp_pitch_fast = !(b[3] & 0x1);
wmp_roll_fast = !(b[4] & 0x2);
wmp_yaw_fast = !(wmp_sample_data[3] & 0x2);
wmp_pitch_fast = !(wmp_sample_data[3] & 0x1);
wmp_roll_fast = !(wmp_sample_data[4] & 0x2);
return TRUE;
}