From 8681d23cbd8051381ceb0bbdf25d4cabdc3ad8e9 Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Fri, 22 Jul 2016 01:03:48 -0300 Subject: [PATCH] AP_HAL_Linux: remove unused Dataflash This is selectable by a define and is never changed. Just remove everything referencing it: we can come up with a better solution if it is actually used later. --- .../AP_HAL_Linux/AP_HAL_Linux_Namespace.h | 1 - libraries/AP_HAL_Linux/HAL_Linux_Class.cpp | 7 - libraries/AP_HAL_Linux/SPIDriver.cpp | 2 - libraries/AP_HAL_Linux/Storage.h | 4 - libraries/AP_HAL_Linux/Storage_FRAM.cpp | 260 ------------------ libraries/AP_HAL_Linux/Storage_FRAM.h | 38 --- 6 files changed, 312 deletions(-) delete mode 100644 libraries/AP_HAL_Linux/Storage_FRAM.cpp delete mode 100644 libraries/AP_HAL_Linux/Storage_FRAM.h diff --git a/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h b/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h index d878d5478a..afe6b9797c 100644 --- a/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h +++ b/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h @@ -13,7 +13,6 @@ namespace Linux { class GPIO_RPI; class GPIO_Sysfs; class Storage; - class Storage_FRAM; class DigitalSource; class DigitalSource_Sysfs; class PeriodicThread; diff --git a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp index cda750987d..9433b05b36 100644 --- a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp +++ b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp @@ -64,14 +64,7 @@ static AnalogIn_Navio2 analogIn; static Empty::AnalogIn analogIn; #endif -/* - select between FRAM and FS - */ -#if LINUX_STORAGE_USE_FRAM == 1 -static Storage_FRAM storageDriver; -#else static Storage storageDriver; -#endif /* use the BBB gpio driver on ERLE, PXF and BBBMINI diff --git a/libraries/AP_HAL_Linux/SPIDriver.cpp b/libraries/AP_HAL_Linux/SPIDriver.cpp index 3de8a32757..bdb7a23b6e 100644 --- a/libraries/AP_HAL_Linux/SPIDriver.cpp +++ b/libraries/AP_HAL_Linux/SPIDriver.cpp @@ -31,7 +31,6 @@ SPIDeviceDriver SPIDeviceManager::_device[] = { SPIDeviceDriver("mpu6000", 2, 0, AP_HAL::SPIDevice_Type, SPI_MODE_3, 8, BBB_P9_28, 500*1000, 20*MHZ), /* MPU9250 is restricted to 1MHz for non-data and interrupt registers */ SPIDeviceDriver("mpu9250", 2, 0, AP_HAL::SPIDevice_Type, SPI_MODE_3, 8, BBB_P9_23, 1*MHZ, 20*MHZ), - SPIDeviceDriver("dataflash", 2, 0, AP_HAL::SPIDevice_Dataflash, SPI_MODE_3, 8, BBB_P8_12, 6*MHZ, 6*MHZ), }; #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE SPIDeviceDriver SPIDeviceManager::_device[] = { @@ -67,7 +66,6 @@ SPIDeviceDriver SPIDeviceManager::_device[] = { SPIDeviceDriver("ms5611", 0, 0, AP_HAL::SPIDevice_Type, SPI_MODE_3, 8, RPI_GPIO_23, 10*MHZ, 10*MHZ), SPIDeviceDriver("lsm9ds0_am", 0, 0, AP_HAL::SPIDevice_Type, SPI_MODE_3, 8, RPI_GPIO_22, 10*MHZ, 10*MHZ), SPIDeviceDriver("lsm9ds0_g", 0, 0, AP_HAL::SPIDevice_Type, SPI_MODE_3, 8, RPI_GPIO_12, 10*MHZ, 10*MHZ), - SPIDeviceDriver("dataflash", 0, 0, AP_HAL::SPIDevice_Dataflash, SPI_MODE_3, 8, RPI_GPIO_5, 6*MHZ, 6*MHZ), SPIDeviceDriver("raspio", 0, 0, AP_HAL::SPIDevice_RASPIO, SPI_MODE_3, 8, RPI_GPIO_7, 10*MHZ, 10*MHZ), }; #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH diff --git a/libraries/AP_HAL_Linux/Storage.h b/libraries/AP_HAL_Linux/Storage.h index 7a98e31f57..b56ac5a28e 100644 --- a/libraries/AP_HAL_Linux/Storage.h +++ b/libraries/AP_HAL_Linux/Storage.h @@ -1,7 +1,5 @@ #pragma once -#define LINUX_STORAGE_USE_FRAM 0 - #include #include "AP_HAL_Linux_Namespace.h" @@ -41,5 +39,3 @@ protected: uint8_t _buffer[LINUX_STORAGE_SIZE]; volatile uint32_t _dirty_mask; }; - -#include "Storage_FRAM.h" diff --git a/libraries/AP_HAL_Linux/Storage_FRAM.cpp b/libraries/AP_HAL_Linux/Storage_FRAM.cpp deleted file mode 100644 index 65880ef87c..0000000000 --- a/libraries/AP_HAL_Linux/Storage_FRAM.cpp +++ /dev/null @@ -1,260 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "Storage.h" - -using namespace Linux; - -/* - This stores 'eeprom' data on the FRAM, with a 4k size, and a - in-memory buffer. This keeps the latency down. - */ - -// name the storage file after the sketch so you can use the same board -// card for ArduCopter and ArduPlane - -extern const AP_HAL::HAL& hal; -Storage_FRAM::Storage_FRAM(): -_spi(NULL), -_spi_sem(NULL) -{} - -void Storage_FRAM::_storage_create(void) -{ - - int fd = open(); - hal.console->println("Storage: FRAM is getting reset to default values"); - - if (fd == -1) { - AP_HAL::panic("Failed to load FRAM"); - } - for (uint16_t loc=0; loc>LINUX_STORAGE_LINE_SHIFT); n++) { - if (!(_dirty_mask & (1<<(n+i)))) { - break; - } - // mark that line clean - write_mask |= (1<<(n+i)); - } - - /* - write the lines. This also updates _dirty_mask. Note that - because this is a SCHED_FIFO thread it will not be preempted - by the main task except during blocking calls. This means we - don't need a semaphore around the _dirty_mask updates. - */ - if (lseek(_fd, i<device(AP_HAL::SPIDevice_Dataflash); - uint8_t signature[4] = {0x00,0xaf,0xf0,0x0f}; - uint8_t j = 0; - for(int i=0;true;i++){ - manufacturerID = _register_read(0,OPCODE_RDID); - - if(manufacturerID == 0x7F){ - break; - } - else{ - hal.scheduler->delay(1000); - } - if(i==4){ - AP_HAL::panic("FRAM: Failed to receive Manufacturer ID 5 times"); - } - } - - while(j<4){ - if(_register_read(j+4100,OPCODE_READ) == -1){ - continue; - } - else if((uint8_t)_register_read(j+4100,OPCODE_READ) != signature[j]){ - while(_register_write(signature,4100,4) == -1); - return -1; - } - else{ - j++; - } - } - _initialised = true; - hal.console->println("FRAM: Online"); - return 0; -} - -int32_t Storage_FRAM::write(uint16_t fd,uint8_t *Buff, uint16_t NumBytes){ - if( _register_write(Buff,fptr,NumBytes) == -1){ - return -1; - } - return NumBytes; -} -int32_t Storage_FRAM::read(uint16_t fd, uint8_t *Buff, uint16_t NumBytes){ - for(uint16_t i=fptr;i<(fptr+NumBytes);i++){ - Buff[i-fptr]= _register_read(i,OPCODE_READ); - - if(Buff[i-fptr]==UINT8_MAX){ - return -1; - } - } - fptr+=NumBytes; - return NumBytes; -} -uint32_t Storage_FRAM::lseek(uint16_t fd,uint32_t offset,uint16_t whence){ - fptr = offset; - return offset; -} - -//FRAM I/O functions - -int8_t Storage_FRAM::_register_write( uint8_t* src, uint16_t addr, uint16_t len ){ - - uint8_t *tx; - uint8_t *rx; - uint16_t i; - tx = new uint8_t[len+3]; - rx = new uint8_t[len+3]; - _write_enable(true); - - tx[0] = OPCODE_WRITE; - tx[1] = addr>>8; - tx[2] = addr; - - for(i=0;i> 8U)), (uint8_t)(addr & 0xFF), 0}; - uint8_t rx[4]; - - if(transaction(tx, rx, 4) == -1){ - return -1; - } - return rx[3]; -} - -int8_t Storage_FRAM::transaction(uint8_t* tx, uint8_t* rx, uint16_t len){ - _spi_sem = _spi->get_semaphore(); - if (!_spi_sem->take(100)) { - // FRAM: Unable to get semaphore - return -1; - } - _spi->transaction(tx, rx, len); - _spi_sem->give(); - return 0; -} diff --git a/libraries/AP_HAL_Linux/Storage_FRAM.h b/libraries/AP_HAL_Linux/Storage_FRAM.h deleted file mode 100644 index 133b7211b1..0000000000 --- a/libraries/AP_HAL_Linux/Storage_FRAM.h +++ /dev/null @@ -1,38 +0,0 @@ -#pragma once - -#include -#include "AP_HAL_Linux_Namespace.h" - -#define OPCODE_WREN 0b0110 /* Write Enable Latch */ -#define OPCODE_WRDI 0b0100 /* Reset Write Enable Latch */ -#define OPCODE_RDSR 0b0101 /* Read Status Register */ -#define OPCODE_WRSR 0b0001 /* Write Status Register */ -#define OPCODE_READ 0b0011 /* Read Memory */ -#define OPCODE_WRITE 0b0010 /* Write Memory */ -#define OPCODE_RDID 0b10011111 /* Read Device ID */ - -class Linux::Storage_FRAM : public Linux::Storage -{ -public: - Storage_FRAM(); - void _timer_tick(void); - -private: - uint32_t fptr; - - int32_t write(uint16_t fd, uint8_t *Buff, uint16_t NumBytes); - int32_t read(uint16_t fd, uint8_t *Buff, uint16_t NumBytes); - uint32_t lseek(uint16_t fd,uint32_t offset,uint16_t whence); - int8_t open(); - - int8_t _register_write( uint8_t* src, uint16_t addr, uint16_t len ); - int8_t _register_read( uint16_t addr, uint8_t opcode ); - int8_t _write_enable(bool enable); - int8_t transaction(uint8_t* tx, uint8_t* rx, uint16_t len); - - void _storage_create(void); - void _storage_open(void); - - AP_HAL::SPIDeviceDriver *_spi; - AP_HAL::Semaphore *_spi_sem; -};