Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: methods added to access DMP functionality

This commit is contained in:
rmackay9 2012-07-28 14:14:43 +09:00
parent cb0eacca37
commit 6cd0918134
2 changed files with 1035 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1057
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

File diff suppressed because it is too large Load Diff

47
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
View File

@ -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__