The unified diff between revisions [a39fe798..] and [8f4e93ab..] is displayed below. It can also be downloaded as a raw diff.
This diff has been restricted to the following files: 'wmp.c'
# # old_revision [a39fe7980c8f14b70401f4c97f3e10232dce016a] # new_revision [8f4e93ab4d89edfdbd524b06b83511c6867a9150] # # patch "wmp.c" # from [bba0c9cfc92f2ce2c56d9a49d01b47f374a40a29] # to [9acbe9ae30b2b7c95b8a700f4f7bb97c38097e21] # ============================================================ --- wmp.c bba0c9cfc92f2ce2c56d9a49d01b47f374a40a29 +++ wmp.c 9acbe9ae30b2b7c95b8a700f4f7bb97c38097e21 @@ -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"); + + } +}