The unified diff between revisions [dea51752..] and [65df00aa..] 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 [65df00aa2705ce33fd74f4dd706d2879fe99b2b0]
#
# patch "wmp.c"
#  from [5f733d08f12cc7ab58dbe720d7ac20c713bf5687]
#    to [9acbe9ae30b2b7c95b8a700f4f7bb97c38097e21]
#
============================================================
--- wmp.c	5f733d08f12cc7ab58dbe720d7ac20c713bf5687
+++ wmp.c	9acbe9ae30b2b7c95b8a700f4f7bb97c38097e21
@@ -1,47 +1,80 @@
 
 #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 */
+	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,36 +99,58 @@ 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_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 = !(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;
 }
 
+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");
+
+	}
+}