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

Use the timer to kick off WMP readings regularly.

Browse Source
This commit is contained in:
Gavan Fantom
2011-06-04 20:56:20 +00:00
parent fda1b2a672
commit 65b11ff92e
5 changed files with 56 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

37
wmp.c
View File

@@ -1,10 +1,12 @@
#include "wmp.h"
#include "i2c.h"
#include "uart.h"
unsigned char wmp_init_command[2] = {0xfe, 0x04};
i2c_result wmp_result;
unsigned int wmp_generation;
struct i2c_transaction wmp_init_transaction = {
(0x53 << 1) + 0, /* write */
@@ -105,6 +107,8 @@ bool wmp_sample(void)
if (wmp_result != I2C_SUCCESS)
return FALSE;
wmp_result = I2C_IN_PROGRESS;
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];
@@ -117,3 +121,36 @@ bool wmp_sample(void)
return TRUE;
}
bool wmp_start_sample(void)
{
return i2c_start_transaction(&wmp_sample_transaction);
}
void wmp_event_handler(void)
{
if (wmp_result != I2C_SUCCESS)
return;
wmp_result = I2C_IN_PROGRESS;
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 = !(wmp_sample_data[3] & 0x2);
wmp_pitch_fast = !(wmp_sample_data[3] & 0x1);
wmp_roll_fast = !(wmp_sample_data[4] & 0x2);
wmp_generation++;
if ((wmp_generation % 100) == 0) {
putstr("(");
puthex(wmp_roll);
putstr(", ");
puthex(wmp_pitch);
putstr(", ");
puthex(wmp_yaw);
putstr(")\r\n");
}
}