c56c4ae240
just assume uart0 is initialized by the HAL, because it is. DRY. also, don't ever use uart0 explicitly in example sketches, use console and let the hal figure it out.
145 lines
4.0 KiB
Plaintext
145 lines
4.0 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_HAL.h>
|
|
#include <AP_HAL_AVR.h>
|
|
|
|
/* register #defines */
|
|
#include "MPU6000.h"
|
|
|
|
// debug only:
|
|
#include <avr/io.h>
|
|
|
|
const AP_HAL_AVR::HAL_AVR& hal = AP_HAL_AVR_APM2;
|
|
|
|
const uint8_t _cs_pin = 53;
|
|
const uint8_t _baro_cs_pin = 40;
|
|
|
|
static void register_write(uint8_t reg, uint8_t val) {
|
|
hal.gpio->write(_cs_pin, 0);
|
|
hal.spi->transfer(reg);
|
|
hal.spi->transfer(val);
|
|
hal.gpio->write(_cs_pin, 1);
|
|
}
|
|
|
|
static uint8_t register_read(uint8_t reg) {
|
|
/* set most significant bit to read register */
|
|
uint8_t regaddr = reg | 0x80;
|
|
hal.gpio->write(_cs_pin, 0);
|
|
hal.spi->transfer(regaddr);
|
|
uint8_t val = hal.spi->transfer(0);
|
|
hal.gpio->write(_cs_pin, 1);
|
|
return val;
|
|
}
|
|
|
|
static uint16_t spi_read_16(void) {
|
|
uint8_t byte_h, byte_l;
|
|
byte_h = hal.spi->transfer(0);
|
|
byte_l = hal.spi->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) {
|
|
|
|
hal.gpio->write(_cs_pin, 0);
|
|
hal.spi->transfer( MPUREG_ACCEL_XOUT_H | 0x80 );
|
|
for (int i = 0; i < 7; i++) {
|
|
data[i] = spi_read_16();
|
|
}
|
|
hal.gpio->write(_cs_pin, 1);
|
|
}
|
|
|
|
static void setup() {
|
|
hal.console->printf_P(PSTR("Initializing MPU6000\r\n"));
|
|
|
|
/* Setup the barometer cs to stop it from holding the bus */
|
|
hal.gpio->pinMode(_baro_cs_pin, GPIO_OUTPUT);
|
|
hal.gpio->write(_baro_cs_pin, 1);
|
|
|
|
/* Setup CS pin hardware */
|
|
hal.gpio->pinMode(_cs_pin, GPIO_OUTPUT);
|
|
hal.gpio->write(_cs_pin, 1);
|
|
hal.scheduler->delay(1);
|
|
|
|
uint8_t spcr = SPCR;
|
|
uint8_t spsr = SPSR;
|
|
hal.console->printf_P(PSTR("SPCR 0x%x, SPSR 0x%x\r\n"),
|
|
(int)spcr, (int)spsr);
|
|
|
|
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);
|
|
}
|
|
|
|
|
|
|
|
|
|
extern "C" {
|
|
int main (void) {
|
|
hal.init(NULL);
|
|
setup();
|
|
for(;;) loop();
|
|
return 0;
|
|
}
|
|
}
|