aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_HAL_AVR: Implemented SPIDriver based on Arduino, but it doesn't work yet

Browse Source 
* I don't have an APM2.5 board I can easily connect a logic analyzer to see
  the traffic on the bus...
This commit is contained in:
Pat Hickey 2012-08-30 16:25:35 -07:00 committed by Andrew Tridgell
parent b2a34800e2
commit b0d8f43111
9 changed files with 304 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_HAL/SPIDriver.h
View File

@ -4,10 +4,19 @@
#include "AP_HAL_Namespace.h"
/**
* A simple SPIDriver interface directly copied from Arduino SPI driver.
* This will not be the final AP_HAL interface.
* We will need an abstraction for selecting slave devices and performing bulk
* transfers to be portable to other platforms.
*/
class AP_HAL::SPIDriver {
public:
SPIDriver() {}
virtual void init() = 0;
virtual void init(void *) = 0;
virtual void set_freq(uint32_t freq_hz) = 0;
virtual uint8_t transfer (uint8_t data) = 0;
};
#endif // __AP_HAL_SPI_DRIVER_H__

2
libraries/AP_HAL_AVR/HAL_AVR.cpp
View File

@ -9,7 +9,7 @@ void HAL_AVR::init(void* opts) const {
/* The AVR RCInput drivers take an AP_HAL_AVR::ISRRegistry*
* as the init argument */
rcin->init((void*)&isr_registry);
rcout->init(NULL);
spi->init(NULL);
};

68
libraries/AP_HAL_AVR/SPIDriver.cpp Normal file
View File

@ -0,0 +1,68 @@
#include <avr/io.h>
#include "utility/pins_arduino_mega.h"
#include <AP_HAL.h>
#include "SPIDriver.h"
using namespace AP_HAL_AVR;
extern const AP_HAL::HAL& hal;
void ArduinoSPIDriver::init(void* machtnichts) {
hal.gpio->pinMode(SCK, GPIO_OUTPUT);
hal.gpio->pinMode(MOSI, GPIO_OUTPUT);
hal.gpio->pinMode(SS, GPIO_OUTPUT);
hal.gpio->write(SCK, 0);
hal.gpio->write(MOSI,0);
hal.gpio->write(SS, 0);
SPCR |= _BV(MSTR);
SPCR |= _BV(SPE);
// set_freq(10000000);
// dbg: set_freq is broken
// lets just set SPR0 (div/16)
SPCR |= _BV(SPR0);
}
void ArduinoSPIDriver::set_freq(uint32_t freq_hz) {
int div = F_CPU / freq_hz;
/* can't divide clock by more than 128. */
uint8_t b = _divider_bits( div > 128 ? 128 : ((uint8_t) div));
_set_clock_divider_bits(b);
}
/* from Arduino SPI.h:
#define SPI_CLOCK_DIV2 0x04
#define SPI_CLOCK_DIV4 0x00
#define SPI_CLOCK_DIV8 0x05
#define SPI_CLOCK_DIV16 0x01
#define SPI_CLOCK_DIV32 0x06
#define SPI_CLOCK_DIV64 0x02
#define SPI_CLOCK_DIV128 0x03
*/
uint8_t ArduinoSPIDriver::_divider_bits(uint8_t div) {
/* Closest bits for divider without going over (ending up faster) */
if (div > 64) return 0x03;
if (div > 32) return 0x02;
if (div > 16) return 0x06;
if (div > 8) return 0x01;
if (div > 4) return 0x05;
if (div > 2) return 0x00;
return 0x04;
}
#define SPI_CLOCK_MASK 0x03 // SPR1 = bit 1, SPR0 = bit 0 on SPCR
#define SPI_2XCLOCK_MASK 0x01 // SPI2X = bit 0 on SPSR
void ArduinoSPIDriver::_set_clock_divider_bits(uint8_t b) {
SPCR = (SPCR & ~SPI_CLOCK_MASK) | ( b & SPI_CLOCK_MASK);
SPSR = (SPSR & ~SPI_2XCLOCK_MASK) | ((b >> 2) & SPI_2XCLOCK_MASK);
}
uint8_t ArduinoSPIDriver::transfer(uint8_t data) {
SPDR = data;
while(!(SPSR & _BV(SPIF)));
return SPDR;
}

9
libraries/AP_HAL_AVR/SPIDriver.h
View File

@ -7,10 +7,13 @@
class AP_HAL_AVR::ArduinoSPIDriver : public AP_HAL::SPIDriver {
public:
ArduinoSPIDriver() : _init(0) {}
void init() { _init = 1; }
ArduinoSPIDriver() {}
void init(void* machtnichts);
void set_freq(uint32_t freq_hz);
uint8_t transfer(uint8_t data);
private:
int _init;
uint8_t _divider_bits(uint8_t divider);
void _set_clock_divider_bits(uint8_t b);
};
#endif // __AP_HAL_ARDUINO_SPI_DRIVER_H__

0
libraries/AP_HAL_AVR/examples/SPIDriver_MPU6000/Arduino.h Normal file
View File

81
libraries/AP_HAL_AVR/examples/SPIDriver_MPU6000/MPU6000.h Normal file
View File

@ -0,0 +1,81 @@
#ifndef __MPU6000_H__
#define __MPU6000_H__
// MPU 6000 registers
#define MPUREG_WHOAMI 0x75 //
#define MPUREG_SMPLRT_DIV 0x19 //
#define MPUREG_CONFIG 0x1A //
#define MPUREG_GYRO_CONFIG 0x1B
#define MPUREG_ACCEL_CONFIG 0x1C
#define MPUREG_FIFO_EN 0x23
#define MPUREG_INT_PIN_CFG 0x37
#define MPUREG_INT_ENABLE 0x38
#define MPUREG_INT_STATUS 0x3A
#define MPUREG_ACCEL_XOUT_H 0x3B //
#define MPUREG_ACCEL_XOUT_L 0x3C //
#define MPUREG_ACCEL_YOUT_H 0x3D //
#define MPUREG_ACCEL_YOUT_L 0x3E //
#define MPUREG_ACCEL_ZOUT_H 0x3F //
#define MPUREG_ACCEL_ZOUT_L 0x40 //
#define MPUREG_TEMP_OUT_H 0x41//
#define MPUREG_TEMP_OUT_L 0x42//
#define MPUREG_GYRO_XOUT_H 0x43 //
#define MPUREG_GYRO_XOUT_L 0x44 //
#define MPUREG_GYRO_YOUT_H 0x45 //
#define MPUREG_GYRO_YOUT_L 0x46 //
#define MPUREG_GYRO_ZOUT_H 0x47 //
#define MPUREG_GYRO_ZOUT_L 0x48 //
#define MPUREG_USER_CTRL 0x6A //
#define MPUREG_PWR_MGMT_1 0x6B //
#define MPUREG_PWR_MGMT_2 0x6C //
#define MPUREG_FIFO_COUNTH 0x72
#define MPUREG_FIFO_COUNTL 0x73
#define MPUREG_FIFO_R_W 0x74
#define MPUREG_PRODUCT_ID 0x0C // Product ID Register
// Configuration bits MPU 3000 and MPU 6000 (not revised)?
#define BIT_SLEEP 0x40
#define BIT_H_RESET 0x80
#define BITS_CLKSEL 0x07
#define MPU_CLK_SEL_PLLGYROX 0x01
#define MPU_CLK_SEL_PLLGYROZ 0x03
#define MPU_EXT_SYNC_GYROX 0x02
#define BITS_FS_250DPS 0x00
#define BITS_FS_500DPS 0x08
#define BITS_FS_1000DPS 0x10
#define BITS_FS_2000DPS 0x18
#define BITS_FS_MASK 0x18
#define BITS_DLPF_CFG_256HZ_NOLPF2 0x00
#define BITS_DLPF_CFG_188HZ 0x01
#define BITS_DLPF_CFG_98HZ 0x02
#define BITS_DLPF_CFG_42HZ 0x03
#define BITS_DLPF_CFG_20HZ 0x04
#define BITS_DLPF_CFG_10HZ 0x05
#define BITS_DLPF_CFG_5HZ 0x06
#define BITS_DLPF_CFG_2100HZ_NOLPF 0x07
#define BITS_DLPF_CFG_MASK 0x07
#define BIT_INT_ANYRD_2CLEAR 0x10
#define BIT_RAW_RDY_EN 0x01
#define BIT_I2C_IF_DIS 0x10
#define BIT_INT_STATUS_DATA 0x01
// Product ID Description for MPU6000
// high 4 bits low 4 bits
// Product Name Product Revision
#define MPU6000ES_REV_C4 0x14 // 0001 0100
#define MPU6000ES_REV_C5 0x15 // 0001 0101
#define MPU6000ES_REV_D6 0x16 // 0001 0110
#define MPU6000ES_REV_D7 0x17 // 0001 0111
#define MPU6000ES_REV_D8 0x18 // 0001 1000
#define MPU6000_REV_C4 0x54 // 0101 0100
#define MPU6000_REV_C5 0x55 // 0101 0101
#define MPU6000_REV_D6 0x56 // 0101 0110
#define MPU6000_REV_D7 0x57 // 0101 0111
#define MPU6000_REV_D8 0x58 // 0101 1000
#define MPU6000_REV_D9 0x59 // 0101 1001
#endif // __MPU6000_H__

1
libraries/AP_HAL_AVR/examples/SPIDriver_MPU6000/Makefile Normal file
View File

@ -0,0 +1 @@
include ../../../AP_Common/Arduino.mk

137
libraries/AP_HAL_AVR/examples/SPIDriver_MPU6000/SPIDriver_MPU6000.pde Normal file
View File

@ -0,0 +1,137 @@
/*******************************************
* 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;
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);
// 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.uart0->begin(115200);
hal.uart0->printf_P(PSTR("Initializing MPU6000\r\n"));
/* 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.uart0->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.uart0->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;
}
}

0
libraries/AP_HAL_AVR/examples/SPIDriver_MPU6000/nocore.inoflag Normal file
View File