AP_InertialSensor: methods added to access DMP functionality

libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h

@@ -10,6 +10,11 @@
 #include "../AP_Math/AP_Math.h"
 #include "AP_InertialSensor.h"
 
+#define DMP_FIFO_BUFFER_SIZE 72        // DMP FIFO buffer size
+
+//dmpMem from dmpDefaultMantis.c
+extern unsigned char dmpMem[8][16][16] PROGMEM;
+
 class AP_InertialSensor_MPU6000 : public AP_InertialSensor
 {
   public:
@@ -17,6 +22,8 @@ class AP_InertialSensor_MPU6000 : public AP_InertialSensor
   AP_InertialSensor_MPU6000( uint8_t cs_pin );
 
   uint16_t init( AP_PeriodicProcess * scheduler );
+  static void dmp_init();       // Initialise MPU6000's DMP
+  static void dmp_reset();		// Reset the DMP (required for changes in gains or offsets to take effect)
 
   /* Concrete implementation of AP_InertialSensor functions: */
   bool update();
@@ -35,6 +42,14 @@ class AP_InertialSensor_MPU6000 : public AP_InertialSensor
   void reset_sample_time();
   float get_gyro_drift_rate();
 
+  // set_gyro_offsets - updates gyro offsets in mpu6000 registers
+  static void set_gyro_offsets_scaled(float offX, float offY, float offZ);
+  static void set_gyro_offsets(int16_t offsetX, int16_t offsetY, int16_t offsetZ);
+
+  // set_accel_offsets - updates accel offsets in DMP registers
+  static void set_accel_offsets_scaled(float offX, float offY, float offZ);
+  static void set_accel_offsets(int16_t offsetX, int16_t offsetY, int16_t offsetZ);
+
   private:
 
   static void read(uint32_t);
@@ -66,7 +81,37 @@ class AP_InertialSensor_MPU6000 : public AP_InertialSensor
   static uint8_t _cs_pin;
 
   // ensure we can't initialise twice
-  unsigned _initialised:1;
+  bool _initialised;
+
+  // dmp related methods and parameters
+  static void dmp_register_write(uint8_t bank, uint8_t address, uint8_t num_bytes, uint8_t data[]);  // Method to write multiple bytes into dmp registers.  Requires a "bank" 
+  static void dmp_set_rate(uint8_t rate);  // set DMP output rate (see constants)
+
+public:
+  static Quaternion quaternion;				// holds the 4 quaternions representing attitude taken directly from the DMP
+
+  static bool FIFO_ready();					// returns true if new attitude data is available in FIFO buffer
+  static void FIFO_reset();					// clear attitude data from FIFO buffer
+  static void FIFO_getPacket();				// get latest attitude data from FIFO buffer
+  static void dmp_set_gyro_calibration();
+  static void dmp_set_accel_calibration();
+  static void dmp_apply_endian_accel();
+  static void dmp_set_mpu_sensors();			// To configure for SIX_AXIS output
+  static void dmp_set_bias_from_no_motion();	// Turn on bias from no motion
+  static void dmp_set_bias_none();				// Turn off internal bias correction (we will use this and we handle the gyro bias correction externally)
+  static void dmp_set_fifo_interrupt();
+  static void dmp_send_quaternion();			// Send quaternion data to FIFO
+  static void dmp_send_gyro();					// Send gyro data to FIFO
+  static void dmp_send_accel();				// Send accel data to FIFO
+  static void dmp_set_fifo_rate(uint8_t rate);	// This functions defines the rate at wich attitude data is send to FIFO.  Rate: 0 => SAMPLE_RATE(ex:200Hz), 1=> SAMPLE_RATE/2 (ex:100Hz), 2=> SAMPLE_RATE/3 (ex:66Hz)
+  static void dmp_set_sensor_fusion_accel_gain(uint8_t gain);	// This function defines the weight of the accel on the sensor fusion. Default value is 0x80. The official invensense name is inv_key_0_96 (?)
+  static void dmp_load_mem();					// Load initial memory values into DMP memory banks
+
+  static uint8_t _received_packet[DMP_FIFO_BUFFER_SIZE];    // FIFO packet buffer
+  static uint8_t _fifoCountH;					// high byte of number of elements in fifo buffer
+  static uint8_t _fifoCountL;					// low byte of number of elements in fifo buffer
+
+  static bool _dmp_initialised;
 };
 
 #endif // __AP_INERTIAL_SENSOR_MPU6000_H__