MPU9250: Add AEMR DFA driver functions and model

3 files changed, 107 insertions, 1 deletions

diff --git a/include/driver/mpu9250.h b/include/driver/mpu9250.h
index 3ebcb17..914a9e0 100644
--- a/include/driver/mpu9250.h
+++ b/include/driver/mpu9250.h
@@ -273,6 +273,7 @@ class MPU9250 {
 		void getAccel(float *g_x, float *g_y, float *g_z);
 		void getGyro(float *dps_x, float *dps_y, float *dps_z);
 		void getMagnet(int *x, int *y, int *z);
+		float getTemperature();
 		void setAccelEnable(bool x, bool y, bool z);
 		void setGyroEnable(bool x, bool y, bool z);
 		void setGyroStandby(bool gyroStandby);
@@ -282,7 +283,14 @@ class MPU9250 {
 		void AGWakeup();
 		void MagSleep();
 		void MagWakeup();
-		float getTemperature();
+
+		// mpu9250.dfa
+		void sleep();
+		void standby();
+		void lowPowerAccelOnly(unsigned char rate);
+		void accelOnly();
+		void gyroOnly();
+		void magnetOnly();
 };
 
 extern MPU9250 mpu9250;
diff --git a/model/driver/mpu9250.dfa b/model/driver/mpu9250.dfa
new file mode 100644
index 0000000..b1cc381
--- /dev/null
+++ b/model/driver/mpu9250.dfa
@@ -0,0 +1,48 @@
+codegen:
+  instance: mpu9250
+  includes: ['driver/i2c.h', 'driver/mpu9250.h']
+  flags: ['drivers=mpu9250', 'arch_drivers=i2c']
+  setup:
+  - i2c.setup();
+
+parameters:
+- accel_odr
+
+states:
+- UNINITIALIZED
+- SLEEP
+- STANDBY
+- LP_ACCEL
+- ACCEL
+- GYRO
+- MAGNET
+- NINEAXIS
+
+transition:
+  init:
+    src: [UNINITIALIZED, SLEEP, STANDBY, LP_ACCEL, ACCEL, GYRO, MAGNET, NINEAXIS]
+    dst: NINEAXIS
+    set_param:
+      accel_odr: 0
+  sleep:
+    src: [STANDBY, LP_ACCEL, ACCEL, GYRO, MAGNET, NINEAXIS]
+    dst: SLEEP
+  standby:
+    src: [STANDBY, LP_ACCEL, ACCEL, GYRO, MAGNET, NINEAXIS]
+    dst: SLEEP
+  lowPowerAccelOnly:
+    src: [SLEEP, STANDBY, NINEAXIS, ACCEL, GYRO, MAGNET]
+    dst: LP_ACCEL
+    arguments:
+    - name: rate
+      values: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11]
+      parameter: accel_odr
+  accelOnly:
+    src: [SLEEP, STANDBY, LP_ACCEL, GYRO, MAGNET, NINEAXIS]
+    dst: ACCEL
+  gyroOnly:
+    src: [SLEEP, STANDBY, LP_ACCEL, ACCEL, MAGNET, NINEAXIS]
+    dst: GYRO
+  magnetOnly:
+    src: [SLEEP, STANDBY, LP_ACCEL, ACCEL, GYRO, NINEAXIS]
+    dst: MAGNET
diff --git a/src/driver/mpu9250.cc b/src/driver/mpu9250.cc
index 1c6e02c..e8f407e 100644
--- a/src/driver/mpu9250.cc
+++ b/src/driver/mpu9250.cc
@@ -216,4 +216,54 @@ void MPU9250::getRawMagnet(int *x, int *y, int *z)
 	}
 }
 
+// mpu9250.dfa
+
+void MPU9250::sleep()
+{
+	AGSleep();
+	MagSleep();
+}
+
+void MPU9250::standby()
+{
+	setGyroStandby(true);
+	MagSleep();
+	setAccelEnable(false, false, false);
+}
+
+void MPU9250::lowPowerAccelOnly(unsigned char rate)
+{
+	txbuf[0] = MPU9250_LP_ACCEL_ODR;
+	txbuf[1] = rate & 0x0f;
+	i2c.xmit(address, 2, txbuf, 0, rxbuf);
+	MagSleep();
+	txbuf[0] = 107;
+	txbuf[1] = 1<<5;
+	i2c.xmit(address, 2, txbuf, 0, rxbuf);
+	setGyroEnable(false, false, false);
+}
+
+void MPU9250::accelOnly()
+{
+	gyroStandby = false;
+	AGWakeup();
+	MagSleep();
+	setGyroEnable(false, false, false);
+}
+
+void MPU9250::gyroOnly()
+{
+	gyroStandby = false;
+	AGWakeup();
+	MagSleep();
+	setAccelEnable(false, false, false);
+}
+
+void MPU9250::magnetOnly()
+{
+	gyroStandby = false;
+	MagWakeup();
+	AGSleep();
+}
+
 MPU9250 mpu9250;