Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_HAL & AP_HAL_AVR: new SPI driver model

Browse Source
This commit is contained in:
Pat Hickey 2012-11-28 17:57:20 -08:00 committed by Andrew Tridgell
parent 2a12392b9e
commit f543cede01
19 changed files with 659 additions and 330 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_HAL/AP_HAL_Namespace.h
View File

@ -12,7 +12,10 @@ namespace AP_HAL {
/* Toplevel class names for drivers: */
class UARTDriver;
class I2CDriver;
class SPIDriver;
class SPIDeviceDriver;
class SPIDeviceManager;
class AnalogSource;
class AnalogIn;
class Storage;
@ -23,6 +26,7 @@ namespace AP_HAL {
class RCInput;
class RCOutput;
class Scheduler;
class Semaphore;
class EmptyUARTDriver;
@ -34,6 +38,20 @@ namespace AP_HAL {
/* Typdefs for function pointers (Procedure, Timed Procedure) */
typedef void(*Proc)(void);
typedef void(*TimedProc)(uint32_t);
/**
* Global names for all of the existing SPI devices on all platforms.
*/
enum SPIDevice {
SPIDevice_Dataflash,
SPIDevice_ADS7844,
SPIDevice_MS5611,
SPIDevice_MPU6000,
SPIDevice_ADNS3080_SPI0,
SPIDevice_ADNS3080_SPI3
};
}

4
libraries/AP_HAL/HAL.h
View File

@ -21,7 +21,7 @@ public:
AP_HAL::UARTDriver* _uart2,
AP_HAL::UARTDriver* _uart3,
AP_HAL::I2CDriver* _i2c,
AP_HAL::SPIDriver* _spi,
AP_HAL::SPIDeviceManager* _spi,
AP_HAL::AnalogIn* _analogin,
AP_HAL::Storage* _storage,
AP_HAL::Dataflash* _dataflash,
@ -54,7 +54,7 @@ public:
AP_HAL::UARTDriver* uart2;
AP_HAL::UARTDriver* uart3;
AP_HAL::I2CDriver* i2c;
AP_HAL::SPIDriver* spi;
AP_HAL::SPIDeviceManager* spi;
AP_HAL::AnalogIn* analogin;
AP_HAL::Storage* storage;
AP_HAL::Dataflash* dataflash;

20
libraries/AP_HAL/SPIDriver.h
View File

@ -4,18 +4,24 @@
#include "AP_HAL_Namespace.h"
class AP_HAL::SPIDeviceManager {
public:
virtual void init(void *) = 0;
virtual SPIDeviceDriver* device(enum AP_HAL::SPIDevice) = 0;
};
/**
* 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
* We still need an abstraction for performing bulk
* transfers to be portable to other platforms.
*/
class AP_HAL::SPIDriver {
class AP_HAL::SPIDeviceDriver {
public:
SPIDriver() {}
virtual void init(void *) = 0;
virtual void set_freq(uint32_t freq_hz) = 0;
virtual void init() = 0;
virtual AP_HAL::Semaphore* get_semaphore() = 0;
virtual void cs_assert() = 0;
virtual void cs_release() = 0;
virtual uint8_t transfer (uint8_t data) = 0;
};

10
libraries/AP_HAL_AVR/AP_HAL_AVR.cpp
View File

@ -31,8 +31,8 @@ static AVRUARTDriverInstance(avrUart3Driver, 3);
static EmptyUARTDriver emptyUartDriver;
static AVRI2CDriver avrI2CDriver;
static ArduinoSPIDriver arduinoSPIDriver;
static APM1SPIDriver apm1SPIDriver;
static APM1SPIDeviceManager apm1SPIDriver;
static APM2SPIDeviceManager apm2SPIDriver;
static AVRAnalogIn avrAnalogIn;
static AVREEPROMStorage avrEEPROMStorage;
static APM1Dataflash apm1Dataflash;
@ -51,7 +51,8 @@ const HAL_AVR AP_HAL_AVR_APM1(
(UARTDriver*) &emptyUartDriver,
(UARTDriver*) &avrUart3Driver,
&avrI2CDriver,
&arduinoSPIDriver,
&apm1SPIDriver,
&avrAnalogIn,
&avrEEPROMStorage,
&apm1Dataflash,
&consoleDriver,
@ -66,7 +67,8 @@ const HAL_AVR AP_HAL_AVR_APM2(
(UARTDriver*) &avrUart2Driver,
(UARTDriver*) &emptyUartDriver,
&avrI2CDriver,
&arduinoSPIDriver,
&apm2SPIDriver,
&avrAnalogIn,
&avrEEPROMStorage,
&apm2Dataflash,
&consoleDriver,

13
libraries/AP_HAL_AVR/AP_HAL_AVR_Namespace.h
View File

@ -7,7 +7,11 @@ namespace AP_HAL_AVR {
class AVRUARTDriver;
class AVRI2CDriver;
class ArduinoSPIDriver;
class APM1SPIDeviceManager;
class APM2SPIDeviceManager;
class AVRSPI0DeviceDriver;
class AVRSPI2DeviceDriver;
class AVRSPI3DeviceDriver;
class ADCSource;
class AVRAnalogIn;
class AVREEPROMStorage;
@ -15,13 +19,14 @@ namespace AP_HAL_AVR {
class APM1Dataflash;
class APM2Dataflash;
class AVRConsoleDriver;
class ArduinoGPIO;
class ArduinoDigitalSource;
class AVRGPIO;
class AVRDigitalSource;
class APM1RCInput;
class APM2RCInput;
class APM1RCOutput;
class APM2RCOutput;
class ArduinoScheduler;
class AVRScheduler;
class AVRSemaphore;
class ISRRegistry;
}

11
libraries/AP_HAL_AVR/Dataflash.h
View File

@ -89,7 +89,7 @@ protected:
*/
class AP_HAL_AVR::APM1Dataflash : public AP_HAL_AVR::CommonDataflash {
public:
void init(void* machtnichts);
void init(void* spiDevice);
void read_mfg_id();
bool media_present();
private:
@ -108,13 +108,12 @@ private:
uint8_t _read_status_reg();
uint8_t _read_status();
void _cs_active();
void _cs_inactive();
AP_HAL::SPIDeviceDriver *_spi;
};
class AP_HAL_AVR::APM2Dataflash : public AP_HAL_AVR::CommonDataflash {
public:
void init(void* machtnichts);
void init(void* spiDevice);
void read_mfg_id();
bool media_present();
private:
@ -133,9 +132,7 @@ private:
uint8_t _read_status_reg();
uint8_t _read_status();
void _cs_active();
void _cs_inactive();
uint8_t _transfer(uint8_t data);
AP_HAL::SPIDeviceDriver *_spi;
};
#endif // __AP_HAL_AVR_DATAFLASH_H__

164
libraries/AP_HAL_AVR/Dataflash_APM1.cpp
View File

@ -8,12 +8,7 @@ extern const AP_HAL::HAL& hal;
/* flash size */
#define DF_LAST_PAGE 4096
/* Arduino Mega SPI pins */
#define DF_DATAOUT 51 /* MOSI */
#define DF_DATAIN 50 /* MISO */
#define DF_SPICLOCK 52 /* SCK */
#define DF_SLAVESELECT 53 /* SS (PB0) */
#define DF_RESET 31 /* RESET (PC6) */
#define DF_RESET_PIN 31 /* (PC6) */
/* AT45DB161D commands (from datasheet) */
#define DF_TRANSFER_PAGE_TO_BUFFER_1 0x53
@ -37,20 +32,14 @@ extern const AP_HAL::HAL& hal;
void APM1Dataflash::init(void* machtnichts) {
hal.gpio->pinMode(DF_DATAOUT, GPIO_OUTPUT);
hal.gpio->pinMode(DF_DATAIN , GPIO_INPUT);
hal.gpio->pinMode(DF_SPICLOCK, GPIO_OUTPUT);
hal.gpio->pinMode(DF_SLAVESELECT, GPIO_OUTPUT);
hal.gpio->pinMode(DF_RESET, GPIO_OUTPUT);
hal.gpio->pinMode(DF_RESET_PIN, GPIO_OUTPUT);
/* Reset the dataflash chip */
hal.gpio->write(DF_RESET, 0);
hal.gpio->write(DF_RESET_PIN, 0);
hal.scheduler->delay(1);
hal.gpio->write(DF_RESET, 1);
hal.gpio->write(DF_RESET_PIN, 1);
_cs_inactive();
hal.spi->set_freq(8000000L);
_spi = hal.spi->device(AP_HAL::SPIDevice_Dataflash);
_num_pages = DF_LAST_PAGE - 1;
uint8_t status = _read_status_reg();
@ -58,14 +47,14 @@ void APM1Dataflash::init(void* machtnichts) {
}
void APM1Dataflash::read_mfg_id() {
_cs_active();
hal.spi->transfer(DF_READ_MANUFACTURER_AND_DEVICE_ID);
_mfg = hal.spi->transfer(0xFF);
_device = hal.spi->transfer(0xFF);
_device = (_device << 8) | hal.spi->transfer(0xFF);
_spi->cs_assert();
_spi->transfer(DF_READ_MANUFACTURER_AND_DEVICE_ID);
_mfg = _spi->transfer(0xFF);
_device = _spi->transfer(0xFF);
_device = (_device << 8) | _spi->transfer(0xFF);
/* fourth byte is dont care */
hal.spi->transfer(0xFF);
_cs_inactive();
_spi->transfer(0xFF);
_spi->cs_release();
}
bool APM1Dataflash::media_present() {
@ -77,93 +66,93 @@ void APM1Dataflash::_wait_ready() {
}
void APM1Dataflash::_page_to_buffer(uint8_t buffer_num, uint16_t page_addr) {
_cs_active();
_spi->cs_assert();
if (_buffer_num == 1) {
hal.spi->transfer(DF_TRANSFER_PAGE_TO_BUFFER_1);
_spi->transfer(DF_TRANSFER_PAGE_TO_BUFFER_1);
} else {
hal.spi->transfer(DF_TRANSFER_PAGE_TO_BUFFER_2);
_spi->transfer(DF_TRANSFER_PAGE_TO_BUFFER_2);
}
if (_page_size == 512) {
hal.spi->transfer( page_addr >> 7 );
hal.spi->transfer( page_addr << 1 );
_spi->transfer( page_addr >> 7 );
_spi->transfer( page_addr << 1 );
} else {
hal.spi->transfer( page_addr >> 6 );
hal.spi->transfer( page_addr << 2 );
_spi->transfer( page_addr >> 6 );
_spi->transfer( page_addr << 2 );
}
/* finally send one dont care byte */
hal.spi->transfer(0x00);
_spi->transfer(0x00);
_cs_inactive();
_spi->cs_release();
_wait_ready();
}
void APM1Dataflash::_buffer_to_page(uint8_t buffer_num, uint16_t page_addr, bool wait) {
_cs_active();
_spi->cs_assert();
if (_buffer_num == 1) {
hal.spi->transfer(DF_BUFFER_1_TO_PAGE_WITH_ERASE);
_spi->transfer(DF_BUFFER_1_TO_PAGE_WITH_ERASE);
} else {
hal.spi->transfer(DF_BUFFER_2_TO_PAGE_WITH_ERASE);
_spi->transfer(DF_BUFFER_2_TO_PAGE_WITH_ERASE);
}
if (_page_size == 512) {
hal.spi->transfer( page_addr >> 7 );
hal.spi->transfer( page_addr << 1 );
_spi->transfer( page_addr >> 7 );
_spi->transfer( page_addr << 1 );
} else {
hal.spi->transfer( page_addr >> 6 );
hal.spi->transfer( page_addr << 2 );
_spi->transfer( page_addr >> 6 );
_spi->transfer( page_addr << 2 );
}
/* finally send one dont care byte */
hal.spi->transfer(0x00);
_spi->transfer(0x00);
_cs_inactive();
_spi->cs_release();
if (wait) {
_wait_ready();
}
}
void APM1Dataflash::_page_erase(uint16_t page_addr) {
_cs_active();
hal.spi->transfer(DF_PAGE_ERASE);
_spi->cs_assert();
_spi->transfer(DF_PAGE_ERASE);
if (_page_size == 512) {
hal.spi->transfer( page_addr >> 7 );
hal.spi->transfer( page_addr << 1 );
_spi->transfer( page_addr >> 7 );
_spi->transfer( page_addr << 1 );
} else {
hal.spi->transfer( page_addr >> 6 );
hal.spi->transfer( page_addr << 2 );
_spi->transfer( page_addr >> 6 );
_spi->transfer( page_addr << 2 );
}
/* finally send one dont care byte */
hal.spi->transfer(0x00);
_cs_inactive();
_spi->transfer(0x00);
_spi->cs_release();
_wait_ready();
}
void APM1Dataflash::_block_erase(uint16_t block_addr) {
_cs_active();
hal.spi->transfer(DF_BLOCK_ERASE);
_spi->cs_assert();
_spi->transfer(DF_BLOCK_ERASE);
if (_page_size == 512) {
hal.spi->transfer( block_addr >> 7 );
hal.spi->transfer( block_addr << 1 );
_spi->transfer( block_addr >> 7 );
_spi->transfer( block_addr << 1 );
} else {
hal.spi->transfer( block_addr >> 6 );
hal.spi->transfer( block_addr << 2 );
_spi->transfer( block_addr >> 6 );
_spi->transfer( block_addr << 2 );
}
/* finally send one dont care byte */
hal.spi->transfer(0x00);
_cs_inactive();
_spi->transfer(0x00);
_spi->cs_release();
_wait_ready();
}
void APM1Dataflash::_chip_erase() {
_cs_active();
hal.spi->transfer(DF_CHIP_ERASE_0);
hal.spi->transfer(DF_CHIP_ERASE_1);
hal.spi->transfer(DF_CHIP_ERASE_2);
hal.spi->transfer(DF_CHIP_ERASE_3);
_cs_inactive();
_spi->cs_assert();
_spi->transfer(DF_CHIP_ERASE_0);
_spi->transfer(DF_CHIP_ERASE_1);
_spi->transfer(DF_CHIP_ERASE_2);
_spi->transfer(DF_CHIP_ERASE_3);
_spi->cs_release();
while(!_read_status()) {
hal.scheduler->delay(1);
@ -171,48 +160,48 @@ void APM1Dataflash::_chip_erase() {
}
void APM1Dataflash::_buffer_write(uint8_t buffer_num, uint16_t page_addr, uint8_t data) {
_cs_active();
_spi->cs_assert();
if (buffer_num == 1) {
hal.spi->transfer(DF_BUFFER_1_WRITE);
_spi->transfer(DF_BUFFER_1_WRITE);
} else {
hal.spi->transfer(DF_BUFFER_2_WRITE);
_spi->transfer(DF_BUFFER_2_WRITE);
}
/* Don't care */
hal.spi->transfer(0);
_spi->transfer(0);
/* Internal buffer address */
hal.spi->transfer((uint8_t)(page_addr >> 8));
hal.spi->transfer((uint8_t)(page_addr & 0xFF));
_spi->transfer((uint8_t)(page_addr >> 8));
_spi->transfer((uint8_t)(page_addr & 0xFF));
/* Byte to write */
hal.spi->transfer(data);
_cs_inactive();
_spi->transfer(data);
_spi->cs_release();
}
uint8_t APM1Dataflash::_buffer_read(uint8_t buffer_num, uint16_t page_addr) {
_cs_active();
_spi->cs_assert();
if (buffer_num == 1) {
hal.spi->transfer(DF_BUFFER_1_READ);
_spi->transfer(DF_BUFFER_1_READ);
} else {
hal.spi->transfer(DF_BUFFER_2_READ);
_spi->transfer(DF_BUFFER_2_READ);
}
/* Don't care */
hal.spi->transfer(0);
_spi->transfer(0);
/* Internal buffer address */
hal.spi->transfer((uint8_t)(page_addr >> 8));
hal.spi->transfer((uint8_t)(page_addr & 0xFF));
_spi->transfer((uint8_t)(page_addr >> 8));
_spi->transfer((uint8_t)(page_addr & 0xFF));
/* Don't care */
hal.spi->transfer(0);
_spi->transfer(0);
/* Read data byte */
uint8_t res = hal.spi->transfer(0);
_cs_inactive();
uint8_t res = _spi->transfer(0);
_spi->cs_release();
return res;
}
inline uint8_t APM1Dataflash::_read_status_reg() {
_cs_active();
hal.spi->transfer(DF_STATUS_REGISTER_READ);
_spi->cs_assert();
_spi->transfer(DF_STATUS_REGISTER_READ);
/* Read the first byte of the result */
uint8_t res = hal.spi->transfer(0);
_cs_inactive();
uint8_t res = _spi->transfer(0);
_spi->cs_release();
return res;
}
@ -221,10 +210,3 @@ inline uint8_t APM1Dataflash::_read_status() {
return _read_status_reg() & 0x80;
}
inline void APM1Dataflash::_cs_inactive() {
hal.gpio->write(DF_SLAVESELECT, 1);
}
inline void APM1Dataflash::_cs_active() {
hal.gpio->write(DF_SLAVESELECT, 0);
}

190
libraries/AP_HAL_AVR/Dataflash_APM2.cpp
View File

@ -7,12 +7,8 @@ using namespace AP_HAL_AVR;
extern const AP_HAL::HAL& hal;
/* Connected to USART3 in SPI mode */
#define DF_DATAOUT 14 /* MOSI */
#define DF_DATAIN 15 /* MISO */
#define DF_SPICLOCK PJ2 /* SCK (not used via gpio) */
#define DF_SLAVESELECT 28 /* SS (PA6) */
#define DF_RESET 41 /* RESET (PG0) */
#define DF_CARDDETECT 33 /* PC4 */
#define DF_RESET_PIN 41 /* RESET (PG0) */
#define DF_CARDDETECT_PIN 33 /* PC4 */
// AT45DB321D Commands (from Datasheet)
#define DF_TRANSFER_PAGE_TO_BUFFER_1 0x53
@ -34,6 +30,8 @@ extern const AP_HAL::HAL& hal;
#define DF_CHIP_ERASE_2 0x80
#define DF_CHIP_ERASE_3 0x9A
#define DEBUG
#ifdef DEBUG
#define LOGD(format, ...) do { hal.console->printf_P(PSTR("DBG/Dataflash: "format), __VA_ARGS__); } while (0)
#else
@ -43,29 +41,15 @@ extern const AP_HAL::HAL& hal;
void APM2Dataflash::init(void* machtnichts) {
/* setup gpio pins */
hal.gpio->pinMode(DF_DATAOUT, GPIO_OUTPUT);
hal.gpio->pinMode(DF_DATAIN, GPIO_OUTPUT);
hal.gpio->pinMode(DF_SLAVESELECT, GPIO_OUTPUT);
hal.gpio->pinMode(DF_RESET, GPIO_OUTPUT);
hal.gpio->pinMode(DF_CARDDETECT, GPIO_INPUT);
hal.gpio->pinMode(DF_RESET_PIN, GPIO_OUTPUT);
hal.gpio->pinMode(DF_CARDDETECT_PIN, GPIO_INPUT);
/* Reset device */
hal.gpio->write(DF_RESET, 0);
hal.gpio->write(DF_RESET_PIN, 0);
hal.scheduler->delay(1);
hal.gpio->write(DF_RESET, 1);
hal.gpio->write(DF_RESET_PIN, 1);
_cs_inactive();
/* Setup USART3 in SPI mode (MSPI), Mode 0, clock 8Mhz */
UBRR3 = 0;
/* DF_SPICLOCK: use XCK3 (PJ2) as output. Enables SPI master mode. */
DDRJ |= _BV(PJ2);
/* Set MSPI mode of operation and SPI data mode 0 */
UCSR3C = _BV(UMSEL31) | _BV(UMSEL30);
/* Enable transmitter and receiver */
UCSR3B = _BV(RXEN3) | _BV(TXEN3);
/* Set baud rate to 8Mhz */
UBRR3 = 0;
_spi = hal.spi->device(AP_HAL::SPIDevice_Dataflash);
uint8_t status = _read_status_reg();
_page_size = (status & 0x01) ? 512 : 528;
@ -87,13 +71,13 @@ void APM2Dataflash::init(void* machtnichts) {
}
void APM2Dataflash::read_mfg_id() {
_cs_active();
_transfer(DF_READ_MANUFACTURER_AND_DEVICE_ID);
_mfg = _transfer(0xFF);
_device = _transfer(0xFF);
_device = (_device << 8) | _transfer(0xFF);
_transfer(0xFF);
_cs_inactive();
_spi->cs_assert();
_spi->transfer(DF_READ_MANUFACTURER_AND_DEVICE_ID);
_mfg = _spi->transfer(0xFF);
_device = _spi->transfer(0xFF);
_device = (_device << 8) | _spi->transfer(0xFF);
_spi->transfer(0xFF);
_spi->cs_release();
}
bool APM2Dataflash::media_present() {
@ -107,96 +91,96 @@ void APM2Dataflash::_wait_ready() {
void APM2Dataflash::_page_to_buffer(uint8_t buffer_num, uint16_t page_addr) {
LOGD("_page_to_buffer: buf: %d page: %d\r\n", (int) buffer_num, page_addr);
_cs_active();
_spi->cs_assert();
if (_buffer_num == 1) {
_transfer(DF_TRANSFER_PAGE_TO_BUFFER_1);
_spi->transfer(DF_TRANSFER_PAGE_TO_BUFFER_1);
} else {
_transfer(DF_TRANSFER_PAGE_TO_BUFFER_2);
_spi->transfer(DF_TRANSFER_PAGE_TO_BUFFER_2);
}
if (_page_size == 512) {
_transfer((uint8_t)(page_addr >> 7));
_transfer((uint8_t)(page_addr << 1));
_spi->transfer((uint8_t)(page_addr >> 7));
_spi->transfer((uint8_t)(page_addr << 1));
} else {
_transfer((uint8_t)(page_addr >> 6));
_transfer((uint8_t)(page_addr << 2));
_spi->transfer((uint8_t)(page_addr >> 6));
_spi->transfer((uint8_t)(page_addr << 2));
}
/* finally send one dont care byte */
_transfer(0x00);
_spi->transfer(0x00);
_cs_inactive();
_spi->cs_release();
_wait_ready();
}
void APM2Dataflash::_buffer_to_page(uint8_t buffer_num, uint16_t page_addr, bool wait) {
LOGD("_buffer_to_page buf: %d, page: %d\r\n",
(int) buffer_num, page_addr);
_cs_active();
_spi->cs_assert();
if (_buffer_num == 1) {
_transfer(DF_BUFFER_1_TO_PAGE_WITH_ERASE);
_spi->transfer(DF_BUFFER_1_TO_PAGE_WITH_ERASE);
} else {
_transfer(DF_BUFFER_2_TO_PAGE_WITH_ERASE);
_spi->transfer(DF_BUFFER_2_TO_PAGE_WITH_ERASE);
}
if (_page_size == 512) {
_transfer((uint8_t)(page_addr >> 7));
_transfer((uint8_t)(page_addr << 1));
_spi->transfer((uint8_t)(page_addr >> 7));
_spi->transfer((uint8_t)(page_addr << 1));
} else {
_transfer((uint8_t)(page_addr >> 6));
_transfer((uint8_t)(page_addr << 2));
_spi->transfer((uint8_t)(page_addr >> 6));
_spi->transfer((uint8_t)(page_addr << 2));
}
/* finally send one dont care byte */
_transfer(0x00);
_spi->transfer(0x00);
_cs_inactive();
_spi->cs_release();
if (wait) {
_wait_ready();
}
}
void APM2Dataflash::_page_erase(uint16_t page_addr) {
_cs_active();
_transfer(DF_PAGE_ERASE);
_spi->cs_assert();
_spi->transfer(DF_PAGE_ERASE);
if (_page_size == 512) {
_transfer( page_addr >> 7 );
_transfer( page_addr << 1 );
_spi->transfer( page_addr >> 7 );
_spi->transfer( page_addr << 1 );
} else {
_transfer( page_addr >> 6 );
_transfer( page_addr << 2 );
_spi->transfer( page_addr >> 6 );
_spi->transfer( page_addr << 2 );
}
/* finally send one dont care byte */
_transfer(0x00);
_cs_inactive();
_spi->transfer(0x00);
_spi->cs_release();
_wait_ready();
}
void APM2Dataflash::_block_erase(uint16_t block_addr) {
LOGD("_block_erase %d\r\n", block_addr);
_cs_active();
_transfer(DF_BLOCK_ERASE);
_spi->cs_assert();
_spi->transfer(DF_BLOCK_ERASE);
if (_page_size == 512) {
_transfer( block_addr >> 7 );
_transfer( block_addr << 1 );
_spi->transfer( block_addr >> 7 );
_spi->transfer( block_addr << 1 );
} else {
_transfer( block_addr >> 6 );
_transfer( block_addr << 2 );
_spi->transfer( block_addr >> 6 );
_spi->transfer( block_addr << 2 );
}
/* finally send one dont care byte */
_transfer(0x00);
_cs_inactive();
_spi->transfer(0x00);
_spi->cs_release();
_wait_ready();
}
void APM2Dataflash::_chip_erase() {
_cs_active();
_transfer(DF_CHIP_ERASE_0);
_transfer(DF_CHIP_ERASE_1);
_transfer(DF_CHIP_ERASE_2);
_transfer(DF_CHIP_ERASE_3);
_cs_inactive();
_spi->cs_assert();
_spi->transfer(DF_CHIP_ERASE_0);
_spi->transfer(DF_CHIP_ERASE_1);
_spi->transfer(DF_CHIP_ERASE_2);
_spi->transfer(DF_CHIP_ERASE_3);
_spi->cs_release();
while(!_read_status()) {
hal.scheduler->delay(1);
@ -206,50 +190,50 @@ void APM2Dataflash::_chip_erase() {
void APM2Dataflash::_buffer_write(uint8_t buffer_num, uint16_t page_addr, uint8_t data) {
LOGD("_buffer_write buf: %d page: %d data: %d\r\n",
(int) buffer_num, page_addr, (int) data);
_cs_active();
_spi->cs_assert();
if (buffer_num == 1) {
_transfer(DF_BUFFER_1_WRITE);
_spi->transfer(DF_BUFFER_1_WRITE);
} else {
_transfer(DF_BUFFER_2_WRITE);
_spi->transfer(DF_BUFFER_2_WRITE);
}
/* Don't care */
_transfer(0);
_spi->transfer(0);
/* Internal buffer address */
_transfer((uint8_t)(page_addr >> 8));
_transfer((uint8_t)(page_addr & 0xFF));
_spi->transfer((uint8_t)(page_addr >> 8));
_spi->transfer((uint8_t)(page_addr & 0xFF));
/* Byte to write */
_transfer(data);
_cs_inactive();
_spi->transfer(data);
_spi->cs_release();
}
uint8_t APM2Dataflash::_buffer_read(uint8_t buffer_num, uint16_t page_addr) {
_cs_active();
_spi->cs_assert();
if (buffer_num == 1) {
_transfer(DF_BUFFER_1_READ);
_spi->transfer(DF_BUFFER_1_READ);
} else {
_transfer(DF_BUFFER_2_READ);
_spi->transfer(DF_BUFFER_2_READ);
}
/* Don't care */
_transfer(0);
_spi->transfer(0);
/* Internal buffer address */
_transfer((uint8_t)(page_addr >> 8));
_transfer((uint8_t)(page_addr & 0xFF));
_spi->transfer((uint8_t)(page_addr >> 8));
_spi->transfer((uint8_t)(page_addr & 0xFF));
/* Don't care */
_transfer(0);
_spi->transfer(0);
/* Read data byte */
uint8_t res = _transfer(0);
_cs_inactive();
uint8_t res = _spi->transfer(0);
_spi->cs_release();
LOGD("_buffer_read num: %d pageaddr: %d result: %d\r\n",
(int) buffer_num, (int) page_addr, (int) res);
return res;
}
inline uint8_t APM2Dataflash::_read_status_reg() {
_cs_active();
_transfer(DF_STATUS_REGISTER_READ);
_spi->cs_assert();
_spi->transfer(DF_STATUS_REGISTER_READ);
/* Read the first byte of the result */
uint8_t res = _transfer(0);
_cs_inactive();
uint8_t res = _spi->transfer(0);
_spi->cs_release();
return res;
}
@ -258,23 +242,3 @@ inline uint8_t APM2Dataflash::_read_status() {
return _read_status_reg() & 0x80;
}
inline void APM2Dataflash::_cs_inactive() {
hal.gpio->write(DF_SLAVESELECT, 1);
}
inline void APM2Dataflash::_cs_active() {
hal.gpio->write(DF_SLAVESELECT, 0);
}
/* APM2 uses USART3 in SPI mode to talk to the dataflash */
inline uint8_t APM2Dataflash::_transfer(uint8_t data) {
/* Wait for empty transmit buffer */
while (!(UCSR3A & _BV(UDRE3)));
/* Put data into buffer to start sending */
UDR3 = data;
/* Wait for receive buffer to be full */
while (!(UCSR3A & _BV(RXC3)));
/* Return received data */
return UDR3;
}

2
libraries/AP_HAL_AVR/HAL_AVR.h
View File

@ -19,7 +19,7 @@ public:
AP_HAL::UARTDriver* _uart2,
AP_HAL::UARTDriver* _uart3,
AP_HAL::I2CDriver* _i2c,
AP_HAL::SPIDriver* _spi,
AP_HAL::SPIDeviceManager* _spi,
AP_HAL::AnalogIn* _analogin,
AP_HAL::Storage* _storage,
AP_HAL::Dataflash* _dataflash,

49
libraries/AP_HAL_AVR/SPIDeviceManager_APM1.cpp Normal file
View File

@ -0,0 +1,49 @@
#include <avr/io.h>
#include <AP_HAL.h>
#include "SPIDriver.h"
#include "SPIDevices.h"
#include "GPIO.h"
#include "pins_arduino_mega.h"
using namespace AP_HAL_AVR;
extern const AP_HAL::HAL& hal;
void APM1SPIDeviceManager::init(void* machtnichts) {
/* dataflow cs is on arduino pin 53, PORTB0 */
AVRDigitalSource* df_cs = new AVRDigitalSource(_BV(0), PB);
/* dataflash: divide clock by 2 to 8Mhz, set SPI_MODE_3
* spcr gets 0x0C to set SPI_MODE_3
* spsr gets bit SPI2X for clock divider */
_dataflash = new AVRSPI0DeviceDriver(df_cs, 0x0C, _BV(SPI2X));
_dataflash->init();
/* optflow cs is on Arduino pin 34, PORTC3 */
AVRDigitalSource* opt_cs = new AVRDigitalSource(_BV(3), PC);
/* optflow: divide clock by 8 to 2Mhz
* spcr gets bit SPR0, spsr gets bit SPI2X */
_optflow = new AVRSPI0DeviceDriver(opt_cs, _BV(SPR0), 0);
_optflow->init();
/* adc cs is on Arduino pin 33, PORTC4 */
AVRDigitalSource* adc_cs = new AVRDigitalSource(_BV(4), PC);
/* adc: ubbr2 gets value of 2 to run at 2.6Mhz
* (config value cribbed from AP_ADC_ADS7844 driver pre-port) */
_adc = new AVRSPI2DeviceDriver(adc_cs, 0, 2);
_adc->init();
}
AP_HAL::SPIDeviceDriver* APM1SPIDeviceManager::device(enum AP_HAL::SPIDevice d)
{
switch (d) {
case AP_HAL::SPIDevice_Dataflash:
return _dataflash;
case AP_HAL::SPIDevice_ADS7844:
return _adc;
case AP_HAL::SPIDevice_ADNS3080_SPI0:
return _optflow;
default:
return NULL;
};
}

64
libraries/AP_HAL_AVR/SPIDeviceManager_APM2.cpp Normal file
View File

@ -0,0 +1,64 @@
#include <avr/io.h>
#include <AP_HAL.h>
#include "SPIDriver.h"
#include "SPIDevices.h"
#include "pins_arduino_mega.h"
using namespace AP_HAL_AVR;
extern const AP_HAL::HAL& hal;
void APM2SPIDeviceManager::init(void* machtnichts) {
/* ms5611 cs is on Arduino pin 40, PORTG1 */
AVRDigitalSource* ms5611_cs = new AVRDigitalSource(_BV(1), PG);
/* ms5611: divide clock by 32 to 500khz
* spcr gets bit SPR1, spsr gets bit SPI2X */
_ms5611 = new AVRSPI0DeviceDriver(ms5611_cs, _BV(SPR1), _BV(SPI2X));
_ms5611->init();
/* ms5611 cs is on Arduino pin 53, PORTB0 */
AVRDigitalSource* mpu6k_cs = new AVRDigitalSource(_BV(0), PB);
/* mpu6k: divide clock by 32 to 500khz
* spcr gets bit SPR1, spsr gets bit SPI2X */
_mpu6k = new AVRSPI0DeviceDriver(mpu6k_cs, _BV(SPR1), _BV(SPI2X));
_mpu6k->init();
/* optflow cs is on Arduino pin A3, PORTF3 */
AVRDigitalSource* optflow_cs = new AVRDigitalSource(_BV(3), PF);
/* optflow: divide clock by 8 to 2Mhz
* spcr gets bit SPR0, spsr gets bit SPI2X */
_optflow_spi0 = new AVRSPI0DeviceDriver(optflow_cs, _BV(SPR0), _BV(SPI2X));
_optflow_spi0->init();
/* Dataflash CS is on Arduino pin 28, PORTA6 */
AVRDigitalSource* df_cs = new AVRDigitalSource(_BV(6), PA);
/* dataflash uses mode 0 and a clock of 8mhz
* ucsr3c = 0
* ubrr3 = 0 */
_dataflash = new AVRSPI3DeviceDriver(df_cs, 0, 0);
_dataflash->init();
/* XXX need correct mode and baud */
_optflow_spi3 = new AVRSPI3DeviceDriver(optflow_cs, 0, 0);
_optflow_spi3->init();
}
AP_HAL::SPIDeviceDriver* APM2SPIDeviceManager::device(enum AP_HAL::SPIDevice d)
{
switch (d) {
case AP_HAL::SPIDevice_Dataflash:
return _dataflash;
case AP_HAL::SPIDevice_MS5611:
return _ms5611;
case AP_HAL::SPIDevice_MPU6000:
return _mpu6k;
case AP_HAL::SPIDevice_ADNS3080_SPI0:
return _optflow_spi0;
case AP_HAL::SPIDevice_ADNS3080_SPI3:
return _optflow_spi3;
default:
return NULL;
};
}

61
libraries/AP_HAL_AVR/SPIDevice_SPI0.cpp Normal file
View File

@ -0,0 +1,61 @@
#include <avr/io.h>
#include <AP_HAL.h>
#include "SPIDevices.h"
#include "GPIO.h"
#include "Semaphore.h"
#include "pins_arduino_mega.h"
using namespace AP_HAL_AVR;
extern const AP_HAL::HAL& hal;
#define SPI0_MISO_PIN 50
#define SPI0_MOSI_PIN 51
#define SPI0_SCK_PIN 52
AVRSemaphore AVRSPI0DeviceDriver::_semaphore;
void AVRSPI0DeviceDriver::init() {
hal.gpio->pinMode(SPI0_MISO_PIN, GPIO_INPUT);
hal.gpio->pinMode(SPI0_MOSI_PIN, GPIO_OUTPUT);
hal.gpio->pinMode(SPI0_SCK_PIN, GPIO_OUTPUT);
_cs_pin->mode(GPIO_OUTPUT);
_cs_pin->write(1);
/* Enable the SPI0 peripheral as a master */
SPCR = _BV(SPE) | _BV(MSTR);
}
AP_HAL::Semaphore* AVRSPI0DeviceDriver::get_semaphore() {
return &_semaphore;
}
void AVRSPI0DeviceDriver::cs_assert() {
const uint8_t valid_spcr_mask =
(_BV(CPOL) | _BV(CPHA) | _BV(SPR1) | _BV(SPR0));
uint8_t new_spcr = SPCR | (_spcr & valid_spcr_mask);
SPCR = new_spcr;
const uint8_t valid_spsr_mask = _BV(SPI2X);
uint8_t new_spsr = SPSR | (_spsr & valid_spsr_mask);
SPSR = new_spsr;
_cs_pin->write(0);
}
void AVRSPI0DeviceDriver::cs_release() {
_cs_pin->write(1);
}
uint8_t AVRSPI0DeviceDriver::transfer(uint8_t data) {
SPDR = data;
if (SPSR & _BV(WCOL)) {
hal.console->println_P(PSTR("PANIC: SPI0 write collision"));
return 0;
}
while(!(SPSR & _BV(SPIF)));
return SPDR;
}

69
libraries/AP_HAL_AVR/SPIDevice_SPI2.cpp Normal file
View File

@ -0,0 +1,69 @@
#include <avr/io.h>
#include <AP_HAL.h>
#include "SPIDevices.h"
#include "GPIO.h"
#include "Semaphore.h"
#include "pins_arduino_mega.h"
using namespace AP_HAL_AVR;
extern const AP_HAL::HAL& hal;
AVRSemaphore AVRSPI2DeviceDriver::_semaphore;
void AVRSPI2DeviceDriver::init() {
AVRDigitalSource spi2_miso(_BV(0), PH);
spi2_miso.mode(GPIO_INPUT);
AVRDigitalSource spi2_mosi(_BV(1), PH);
spi2_mosi.mode(GPIO_OUTPUT);
AVRDigitalSource spi2_sck(_BV(2), PH);
spi2_sck.mode(GPIO_OUTPUT);
/* UMSELn1 and UMSELn2: USART in SPI Master mode */
UCSR2C = _BV(UMSEL21) | _BV(UMSEL20);
/* Enable RX and TX. */
UCSR2B = _BV(RXEN2) | _BV(TXEN2);
/* Setup chip select pin */
_cs_pin->mode(GPIO_OUTPUT);
_cs_pin->write(1);
}
AP_HAL::Semaphore* AVRSPI2DeviceDriver::get_semaphore() {
return &_semaphore;
}
void AVRSPI2DeviceDriver::cs_assert() {
/* set the device UCSRnC configuration bits.
* only sets data order, clock phase, and clock polarity bits (lowest
* three bits) */
const uint8_t new_ucsr2c = UCSR2C | (_ucsr2c & (0x07));
UCSR2C = new_ucsr2c;
/* set the device baud rate */
UBRR2 = _ubrr2;
_cs_pin->write(0);
}
void AVRSPI2DeviceDriver::cs_release() {
_cs_pin->write(1);
}
uint8_t AVRSPI2DeviceDriver::transfer(uint8_t data) {
/* Wait for empty transmit buffer */
while ( !( UCSR2A & _BV(UDRE2)) ) ;
/* Put data into buffer, sends the data */
UDR2 = data;
/* Wait for data to be received */
while ( !(UCSR2A & _BV(RXC2)) ) ;
/* Get and return received data from buffer */
return UDR2;
}

73
libraries/AP_HAL_AVR/SPIDevice_SPI3.cpp Normal file
View File

@ -0,0 +1,73 @@
#include <avr/io.h>
#include <AP_HAL.h>
#include "SPIDevices.h"
#include "GPIO.h"
#include "Semaphore.h"
#include "pins_arduino_mega.h"
using namespace AP_HAL_AVR;
#define SPI3_MOSI 14
#define SPI3_MISO 15
extern const AP_HAL::HAL& hal;
AVRSemaphore AVRSPI3DeviceDriver::_semaphore;
void AVRSPI3DeviceDriver::init() {
/* the spi3 (USART3) sck pin PORTJ2 is not enumerated
* by the arduino pin numbers, so we access it directly
* with AVRDigitalSource. */
AVRDigitalSource spi3_sck(_BV(2), PJ);
spi3_sck.mode(GPIO_OUTPUT);
hal.gpio->pinMode(SPI3_MOSI, GPIO_OUTPUT);
hal.gpio->pinMode(SPI3_MISO, GPIO_INPUT);
/* UMSELn1 and UMSELn2: USART in SPI Master mode */
UCSR3C = _BV(UMSEL31) | _BV(UMSEL30);
/* Enable RX and TX. */
UCSR3B = _BV(RXEN3) | _BV(TXEN3);
/* Setup chip select pin */
_cs_pin->mode(GPIO_OUTPUT);
_cs_pin->write(1);
}
AP_HAL::Semaphore* AVRSPI3DeviceDriver::get_semaphore() {
return &_semaphore;
}
void AVRSPI3DeviceDriver::cs_assert() {
/* set the device UCSRnC configuration bits.
* only sets data order, clock phase, and clock polarity bits (lowest
* three bits) */
const uint8_t new_ucsr3c = UCSR3C | (_ucsr3c & (0x07));
UCSR3C = new_ucsr3c;
/* set the device baud rate */
UBRR3 = _ubrr3;
_cs_pin->write(0);
}
void AVRSPI3DeviceDriver::cs_release() {
_cs_pin->write(1);
}
uint8_t AVRSPI3DeviceDriver::transfer(uint8_t data) {
/* Wait for empty transmit buffer */
while ( !( UCSR3A & _BV(UDRE3)) ) ;
/* Put data into buffer, sends the data */
UDR3 = data;
/* Wait for data to be received */
while ( !(UCSR3A & _BV(RXC3)) ) ;
/* Get and return received data from buffer */
return UDR3;
}

89
libraries/AP_HAL_AVR/SPIDevices.h Normal file
View File

@ -0,0 +1,89 @@
#ifndef __AP_HAL_AVR_SPI_DEVICES_H__
#define __AP_HAL_AVR_SPI_DEVICES_H__
#include <AP_HAL.h>
#include "AP_HAL_AVR_Namespace.h"
class AP_HAL_AVR::AVRSPI0DeviceDriver : public AP_HAL::SPIDeviceDriver {
public:
AVRSPI0DeviceDriver(
AP_HAL_AVR::AVRDigitalSource *cs_pin,
uint8_t spcr,
uint8_t spsr
) :
_cs_pin(cs_pin),
_spcr(spcr),
_spsr(spsr)
{}
void init();
AP_HAL::Semaphore* get_semaphore();
void cs_assert();
void cs_release();
uint8_t transfer(uint8_t data);
private:
static AP_HAL_AVR::AVRSemaphore _semaphore;
AP_HAL_AVR::AVRDigitalSource *_cs_pin;
const uint8_t _spcr;
const uint8_t _spsr;
};
class AP_HAL_AVR::AVRSPI2DeviceDriver : public AP_HAL::SPIDeviceDriver {
public:
AVRSPI2DeviceDriver(
AP_HAL_AVR::AVRDigitalSource *cs_pin,
uint8_t ucsr2c,
uint16_t ubrr2
) :
_cs_pin(cs_pin),
_ucsr2c(ucsr2c),
_ubrr2(ubrr2)
{}
void init();
AP_HAL::Semaphore* get_semaphore();
void cs_assert();
void cs_release();
uint8_t transfer(uint8_t data);
private:
static AP_HAL_AVR::AVRSemaphore _semaphore;
AP_HAL_AVR::AVRDigitalSource *_cs_pin;
uint8_t _ucsr2c;
uint16_t _ubrr2;
};
class AP_HAL_AVR::AVRSPI3DeviceDriver : public AP_HAL::SPIDeviceDriver {
public:
AVRSPI3DeviceDriver(
AP_HAL_AVR::AVRDigitalSource *cs_pin,
uint8_t ucsr3c,
uint16_t ubrr3
) :
_cs_pin(cs_pin),
_ucsr3c(ucsr3c),
_ubrr3(ubrr3)
{}
void init();
AP_HAL::Semaphore* get_semaphore();
void cs_assert();
void cs_release();
uint8_t transfer(uint8_t data);
private:
static AP_HAL_AVR::AVRSemaphore _semaphore;
AP_HAL_AVR::AVRDigitalSource *_cs_pin;
uint8_t _ucsr3c;
uint16_t _ubrr3;
};
#endif // __AP_HAL_AVR_SPI_DEVICES_H__

67
libraries/AP_HAL_AVR/SPIDriver.cpp
View File

@ -1,67 +0,0 @@
#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( 1000000);
}
void ArduinoSPIDriver::set_freq(uint32_t freq_hz) {
const uint16_t freq_khz = (uint16_t)(freq_hz / 1000UL);
const uint16_t fcpu_khz = (uint16_t)(F_CPU / 1000UL);
const int16_t div = fcpu_khz / freq_khz;
/* 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;
}

36
libraries/AP_HAL_AVR/SPIDriver.h
View File

@ -1,20 +1,38 @@
#ifndef __AP_HAL_ARDUINO_SPI_DRIVER_H__
#define __AP_HAL_ARDUINO_SPI_DRIVER_H__
#ifndef __AP_HAL_AVR_SPI_DRIVER_H__
#define __AP_HAL_AVR_SPI_DRIVER_H__
#include <AP_HAL.h>
#include "AP_HAL_AVR_Namespace.h"
#include "GPIO.h"
#include "SPIDevices.h"
#include "Semaphore.h"
class AP_HAL_AVR::ArduinoSPIDriver : public AP_HAL::SPIDriver {
class AP_HAL_AVR::APM1SPIDeviceManager : public AP_HAL::SPIDeviceManager {
public:
ArduinoSPIDriver() {}
void init(void* machtnichts);
void set_freq(uint32_t freq_hz);
uint8_t transfer(uint8_t data);
AP_HAL::SPIDeviceDriver* device(enum AP_HAL::SPIDevice d);
private:
uint8_t _divider_bits(uint8_t divider);
void _set_clock_divider_bits(uint8_t b);
AVRSPI0DeviceDriver* _dataflash;
AVRSPI0DeviceDriver* _optflow;
AVRSPI2DeviceDriver* _adc;
};
#endif // __AP_HAL_ARDUINO_SPI_DRIVER_H__
class AP_HAL_AVR::APM2SPIDeviceManager : public AP_HAL::SPIDeviceManager {
public:
void init(void* machtnichts);
AP_HAL::SPIDeviceDriver* device(enum AP_HAL::SPIDevice d);
private:
AVRSPI0DeviceDriver* _mpu6k;
AVRSPI0DeviceDriver* _ms5611;
AVRSPI0DeviceDriver* _optflow_spi0;
AVRSPI3DeviceDriver* _dataflash;
AVRSPI3DeviceDriver* _optflow_spi3;
};
#endif // __AP_HAL_AVR_SPI_DRIVER_H__

1
libraries/AP_HAL_AVR/Semaphore.cpp
View File

@ -2,6 +2,7 @@
#include <AP_HAL.h>
#include <AP_HAL_AVR.h>
#include "Semaphore.h"
using namespace AP_HAL_AVR;
extern const AP_HAL::HAL& hal;

46
libraries/AP_HAL_AVR/examples/SPIDriver_MPU6000/SPIDriver_MPU6000.pde
View File

@ -17,30 +17,33 @@
const AP_HAL_AVR::HAL_AVR& hal = AP_HAL_AVR_APM2;
const uint8_t _cs_pin = 53;
const uint8_t _baro_cs_pin = 40;
AP_HAL::SPIDeviceDriver* spidev;
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);
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;
hal.gpio->write(_cs_pin, 0);
hal.spi->transfer(regaddr);
uint8_t val = hal.spi->transfer(0);
hal.gpio->write(_cs_pin, 1);
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 = hal.spi->transfer(0);
byte_l = hal.spi->transfer(0);
byte_h = spidev->transfer(0);
byte_l = spidev->transfer(0);
return (((int16_t) byte_h)<< 8) | byte_l;
}
@ -95,30 +98,28 @@ static void mpu6k_init(void) {
static void mpu6k_read(int16_t* data) {
hal.gpio->write(_cs_pin, 0);
hal.spi->transfer( MPUREG_ACCEL_XOUT_H | 0x80 );
spidev->cs_assert();
spidev->transfer( MPUREG_ACCEL_XOUT_H | 0x80 );
for (int i = 0; i < 7; i++) {
data[i] = spi_read_16();
}
hal.gpio->write(_cs_pin, 1);
spidev->cs_release();
}
static void setup() {
hal.console->printf_P(PSTR("Initializing MPU6000\r\n"));
spidev = hal.spi->device(AP_HAL::SPIDevice_MPU6000);
#if 0
/* 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);
#endif
/* Setup CS pin hardware */
hal.gpio->pinMode(_cs_pin, GPIO_OUTPUT);
hal.gpio->write(_cs_pin, 1);
hal.scheduler->delay(1);
spidev->cs_assert();
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);
spidev->cs_release();
mpu6k_init();
}
@ -132,9 +133,6 @@ static void loop() {
hal.scheduler->delay(10);
}
extern "C" {
int main (void) {
hal.init(NULL);