|
|
|
@ -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"); |
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|