The unified diff between revisions [dea51752..] and [cc8258a6..] 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 [dea51752ff3061ddca80de6685b04dac53ac77e1] # new_revision [cc8258a6c3643514892e84cf24fed008bc6f9660] # # patch "wmp.c" # from [5f733d08f12cc7ab58dbe720d7ac20c713bf5687] # to [bba0c9cfc92f2ce2c56d9a49d01b47f374a40a29] # ============================================================ --- wmp.c 5f733d08f12cc7ab58dbe720d7ac20c713bf5687 +++ wmp.c bba0c9cfc92f2ce2c56d9a49d01b47f374a40a29 @@ -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_sample(void) bool wmp_sample(void) { - int i; - unsigned int b[6]; - - if (!i2c_send_start()) + if (!i2c_start_transaction(&wmp_sample_transaction)) return FALSE; - if (!i2c_send_address(0x52, TRUE)) - return FALSE; - if (!i2c_send_data(0x00)) - return FALSE; - i2c_send_stop(); - if (!i2c_send_start()) + while (i2c_busy()); + + 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; }