132 lines
3.8 KiB
Plaintext
132 lines
3.8 KiB
Plaintext
/*******************************************
|
|
* Sample sketch that configures an MPU6000
|
|
* and reads back the three axis of accel,
|
|
* temperature, three axis of gyro data
|
|
*******************************************/
|
|
|
|
#include <AP_Common.h>
|
|
#include <AP_Math.h>
|
|
#include <AP_Param.h>
|
|
#include <StorageManager.h>
|
|
#include <AP_Progmem.h>
|
|
|
|
#include <AP_HAL.h>
|
|
#include <AP_HAL_AVR.h>
|
|
|
|
/* register #defines */
|
|
#include "MPU6000.h"
|
|
|
|
// debug only:
|
|
#include <avr/io.h>
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
|
|
const AP_HAL::HAL& hal = AP_HAL_AVR_APM2;
|
|
#elif CONFIG_HAL_BOARD == HAL_BOARD_APM1
|
|
const AP_HAL::HAL& hal = AP_HAL_AVR_APM1;
|
|
#endif
|
|
|
|
AP_HAL::SPIDeviceDriver* spidev;
|
|
|
|
static void register_write(uint8_t reg, uint8_t val) {
|
|
hal.console->printf_P(PSTR("write reg %d val %d\r\n"),
|
|
(int) reg, (int) val);
|
|
spidev->cs_assert();
|
|
spidev->transfer(reg);
|
|
spidev->transfer(val);
|
|
spidev->cs_release();
|
|
}
|
|
|
|
static uint8_t register_read(uint8_t reg) {
|
|
/* set most significant bit to read register */
|
|
uint8_t regaddr = reg | 0x80;
|
|
spidev->cs_assert();
|
|
spidev->transfer(regaddr);
|
|
uint8_t val = spidev->transfer(0);
|
|
spidev->cs_release();
|
|
return val;
|
|
}
|
|
|
|
static uint16_t spi_read_16(void) {
|
|
uint8_t byte_h, byte_l;
|
|
byte_h = spidev->transfer(0);
|
|
byte_l = spidev->transfer(0);
|
|
return (((int16_t) byte_h)<< 8) | byte_l;
|
|
}
|
|
|
|
|
|
static void mpu6k_init(void) {
|
|
// chip reset
|
|
register_write(MPUREG_PWR_MGMT_1, BIT_H_RESET);
|
|
hal.scheduler->delay(100);
|
|
// Wake up device and select GyroZ clock (better performance)
|
|
register_write(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
|
|
hal.scheduler->delay(1);
|
|
register_write(MPUREG_PWR_MGMT_2, 0);
|
|
hal.scheduler->delay(1);
|
|
// Disable I2C bus (recommended on datasheet)
|
|
register_write(MPUREG_USER_CTRL, BIT_I2C_IF_DIS);
|
|
hal.scheduler->delay(1);
|
|
// SAMPLE RATE
|
|
//// Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz
|
|
register_write(MPUREG_SMPLRT_DIV,0x04);
|
|
hal.scheduler->delay(1);
|
|
// FS & DLPF FS=2000º/s, DLPF = 98Hz (low pass filter)
|
|
register_write(MPUREG_CONFIG, BITS_DLPF_CFG_98HZ);
|
|
hal.scheduler->delay(1);
|
|
register_write(MPUREG_GYRO_CONFIG,BITS_FS_2000DPS); // Gyro scale 2000º/s
|
|
hal.scheduler->delay(1);
|
|
|
|
// read the product ID rev c has 1/2 the sensitivity of rev d
|
|
uint8_t _product_id = register_read(MPUREG_PRODUCT_ID);
|
|
|
|
//Serial.printf("Product_ID= 0x%x\n", (unsigned) _product_id);
|
|
|
|
if ((_product_id == MPU6000ES_REV_C4) || (_product_id == MPU6000ES_REV_C5)
|
|
||(_product_id == MPU6000_REV_C4) || (_product_id == MPU6000_REV_C5)){
|
|
// Accel scale 8g (4096 LSB/g)
|
|
// Rev C has different scaling than rev D
|
|
register_write(MPUREG_ACCEL_CONFIG,1<<3);
|
|
} else {
|
|
// Accel scale 8g (4096 LSB/g)
|
|
register_write(MPUREG_ACCEL_CONFIG,2<<3);
|
|
}
|
|
hal.scheduler->delay(1);
|
|
|
|
// INT CFG => Interrupt on Data Ready
|
|
//// INT: Raw data ready
|
|
register_write(MPUREG_INT_ENABLE,BIT_RAW_RDY_EN);
|
|
hal.scheduler->delay(1);
|
|
// INT: Clear on any read
|
|
register_write(MPUREG_INT_PIN_CFG,BIT_INT_ANYRD_2CLEAR);
|
|
hal.scheduler->delay(1);
|
|
|
|
}
|
|
|
|
static void mpu6k_read(int16_t* data) {
|
|
|
|
spidev->cs_assert();
|
|
spidev->transfer( MPUREG_ACCEL_XOUT_H | 0x80 );
|
|
for (int i = 0; i < 7; i++) {
|
|
data[i] = spi_read_16();
|
|
}
|
|
spidev->cs_release();
|
|
}
|
|
|
|
static void setup() {
|
|
hal.console->printf_P(PSTR("Initializing MPU6000\r\n"));
|
|
spidev = hal.spi->device(AP_HAL::SPIDevice_MPU6000);
|
|
|
|
mpu6k_init();
|
|
}
|
|
|
|
static void loop() {
|
|
int16_t sensors[7];
|
|
mpu6k_read(sensors);
|
|
hal.console->printf_P(PSTR("mpu6k: %d %d %d %d %d %d %d\r\n"),
|
|
sensors[0], sensors[1], sensors[2],
|
|
sensors[3], sensors[4], sensors[5], sensors[6]);
|
|
hal.scheduler->delay(10);
|
|
}
|
|
|
|
AP_HAL_MAIN();
|