From eb0756f3628eb311afeb18a19f0e5160cd038c48 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 7 Nov 2011 22:22:54 +1100 Subject: [PATCH] Desktop: added support for DataFlash in Desktop build this will allow Jason to look at the logs of the quad sim --- libraries/Desktop/Desktop.mk | 2 +- libraries/Desktop/Makefile.desktop | 17 +- libraries/Desktop/support/DataFlash.cpp | 251 ++++++++++++++++++++++++ 3 files changed, 256 insertions(+), 14 deletions(-) create mode 100644 libraries/Desktop/support/DataFlash.cpp diff --git a/libraries/Desktop/Desktop.mk b/libraries/Desktop/Desktop.mk index b5bc8732ce..16a9e52e22 100644 --- a/libraries/Desktop/Desktop.mk +++ b/libraries/Desktop/Desktop.mk @@ -171,7 +171,7 @@ else endif # these are library objects we don't want in the desktop build (maybe we'll add them later) -NODESKTOP := FastSerial/FastSerial.cpp AP_Compass/AP_Compass_HMC5843.cpp APM_BMP085/APM_BMP085.cpp AP_IMU/AP_IMU_Oilpan.cpp AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp +NODESKTOP := DataFlash/DataFlash.cpp FastSerial/FastSerial.cpp AP_Compass/AP_Compass_HMC5843.cpp APM_BMP085/APM_BMP085.cpp AP_IMU/AP_IMU_Oilpan.cpp AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp # # Find sketchbook libraries referenced by the sketch. diff --git a/libraries/Desktop/Makefile.desktop b/libraries/Desktop/Makefile.desktop index 8f30f70f13..e72457c21b 100644 --- a/libraries/Desktop/Makefile.desktop +++ b/libraries/Desktop/Makefile.desktop @@ -5,26 +5,17 @@ include ../libraries/Desktop/Desktop.mk default: make -f $(DESKTOP)/Makefile.desktop -nologging: - make -f $(DESKTOP)/Makefile.desktop EXTRAFLAGS="-DLOGGING_ENABLED=DISABLED" - -nogps: - make -f $(DESKTOP)/Makefile.desktop EXTRAFLAGS="-DGPS_PROTOCOL=GPS_PROTOCOL_NONE -DLOGGING_ENABLED=DISABLED" - hil: - make -f $(DESKTOP)/Makefile.desktop EXTRAFLAGS="-DHIL_MODE=HIL_MODE_ATTITUDE -DLOGGING_ENABLED=DISABLED" + make -f $(DESKTOP)/Makefile.desktop EXTRAFLAGS="-DHIL_MODE=HIL_MODE_ATTITUDE" hilsen: - make -f $(DESKTOP)/Makefile.desktop EXTRAFLAGS="-DHIL_MODE=HIL_MODE_SENSORS -DCLI_ENABLED=DISABLED -DLOGGING_ENABLED=DISABLED" - -hilnocli: - make -f $(DESKTOP)/Makefile.desktop EXTRAFLAGS="-DHIL_MODE=HIL_MODE_ATTITUDE -DCLI_ENABLED=DISABLED -DLOGGING_ENABLED=DISABLED" + make -f $(DESKTOP)/Makefile.desktop EXTRAFLAGS="-DHIL_MODE=HIL_MODE_SENSORS" heli: make -f $(DESKTOP)/Makefile.desktop EXTRAFLAGS="-DFRAME_CONFIG=HELI_FRAME" helihil: - make -f $(DESKTOP)/Makefile.desktop EXTRAFLAGS="-DFRAME_CONFIG=HELI_FRAME -DHIL_MODE=HIL_MODE_ATTITUDE -DCLI_ENABLED=DISABLED -DLOGGING_ENABLED=DISABLED" + make -f $(DESKTOP)/Makefile.desktop EXTRAFLAGS="-DFRAME_CONFIG=HELI_FRAME -DHIL_MODE=HIL_MODE_ATTITUDE" mavlink10: - make -f $(DESKTOP)/Makefile.desktop EXTRAFLAGS="-DHIL_MODE=HIL_MODE_ATTITUDE -DMAVLINK10 -DCLI_ENABLED=DISABLED -DLOGGING_ENABLED=DISABLED" + make -f $(DESKTOP)/Makefile.desktop EXTRAFLAGS="-DHIL_MODE=HIL_MODE_ATTITUDE -DMAVLINK10" diff --git a/libraries/Desktop/support/DataFlash.cpp b/libraries/Desktop/support/DataFlash.cpp new file mode 100644 index 0000000000..6b16e6eff5 --- /dev/null +++ b/libraries/Desktop/support/DataFlash.cpp @@ -0,0 +1,251 @@ +/* + hacked up DataFlash library for Desktop support +*/ + +#include +#include +#include +#include +#include +#include "DataFlash.h" +#include + +#define DF_PAGE_SIZE 512 +#define DF_NUM_PAGES 4096 + +static int flash_fd; +static uint8_t buffer[2][DF_PAGE_SIZE]; + +#define OVERWRITE_DATA 0 // 0: When reach the end page stop, 1: Start overwritten from page 1 + +// Constructors //////////////////////////////////////////////////////////////// +DataFlash_Class::DataFlash_Class() +{ +} + +// Public Methods ////////////////////////////////////////////////////////////// +void DataFlash_Class::Init(void) +{ + if (flash_fd == 0) { + flash_fd = open("dataflash.bin", O_RDWR|O_CREAT, 0777); + ftruncate(flash_fd, DF_NUM_PAGES*DF_PAGE_SIZE); + } + df_PageSize = DF_PAGE_SIZE; + df_BufferNum = 1; + df_BufferIdx = 0; + df_PageAdr = 0; + df_Stop_Write = 0; +} + +// This function is mainly to test the device +void DataFlash_Class::ReadManufacturerID() +{ + df_manufacturer = 1; + df_device_0 = 2; + df_device_1 = 3; +} + +// Read the status register +byte DataFlash_Class::ReadStatusReg() +{ + return 0; +} + +// Read the status of the DataFlash +inline +byte DataFlash_Class::ReadStatus() +{ + return 1; +} + + +inline +uint16_t DataFlash_Class::PageSize() +{ + return df_PageSize; +} + + +// Wait until DataFlash is in ready state... +void DataFlash_Class::WaitReady() +{ + while(!ReadStatus()); +} + +void DataFlash_Class::PageToBuffer(unsigned char BufferNum, uint16_t PageAdr) +{ + pread(flash_fd, buffer[BufferNum-1], DF_PAGE_SIZE, PageAdr*DF_PAGE_SIZE); +} + +void DataFlash_Class::BufferToPage (unsigned char BufferNum, uint16_t PageAdr, unsigned char wait) +{ + pwrite(flash_fd, buffer[BufferNum-1], DF_PAGE_SIZE, PageAdr*DF_PAGE_SIZE); +} + +void DataFlash_Class::BufferWrite (unsigned char BufferNum, uint16_t IntPageAdr, unsigned char Data) +{ + buffer[BufferNum-1][IntPageAdr] = (uint8_t)Data; +} + +unsigned char DataFlash_Class::BufferRead (unsigned char BufferNum, uint16_t IntPageAdr) +{ + return (unsigned char)buffer[BufferNum-1][IntPageAdr]; +} + +// *** END OF INTERNAL FUNCTIONS *** + +void DataFlash_Class::PageErase (uint16_t PageAdr) +{ + uint8_t zero[DF_PAGE_SIZE] = {0,}; + pwrite(flash_fd, zero, DF_PAGE_SIZE, PageAdr*DF_PAGE_SIZE); +} + + +void DataFlash_Class::ChipErase () +{ + for (int i=0; DF_NUM_PAGES; i++) { + PageErase(i); + } +} + +// *** DATAFLASH PUBLIC FUNCTIONS *** +void DataFlash_Class::StartWrite(int16_t PageAdr) +{ + df_BufferNum = 1; + df_BufferIdx = 0; + df_PageAdr = PageAdr; + df_Stop_Write = 0; +} + +void DataFlash_Class::FinishWrite(void) +{ + df_BufferIdx = 0; + BufferToPage(df_BufferNum,df_PageAdr,0); // Write Buffer to memory, NO WAIT + df_PageAdr++; + if (OVERWRITE_DATA==1) + { + if (df_PageAdr>=4096) // If we reach the end of the memory, start from the begining + df_PageAdr = 1; + } + else + { + if (df_PageAdr>=4096) // If we reach the end of the memory, stop here + df_Stop_Write=1; + } + + if (df_BufferNum==1) // Change buffer to continue writing... + df_BufferNum=2; + else + df_BufferNum=1; +} + + +void DataFlash_Class::WriteByte(byte data) +{ + if (!df_Stop_Write) + { + BufferWrite(df_BufferNum,df_BufferIdx,data); + df_BufferIdx++; + if (df_BufferIdx >= df_PageSize) // End of buffer? + { + df_BufferIdx=0; + BufferToPage(df_BufferNum,df_PageAdr,0); // Write Buffer to memory, NO WAIT + df_PageAdr++; + if (OVERWRITE_DATA==1) + { + if (df_PageAdr>=4096) // If we reach the end of the memory, start from the begining + df_PageAdr = 1; + } + else + { + if (df_PageAdr>=4096) // If we reach the end of the memory, stop here + df_Stop_Write=1; + } + + if (df_BufferNum==1) // Change buffer to continue writing... + df_BufferNum=2; + else + df_BufferNum=1; + } + } +} + +void DataFlash_Class::WriteInt(int16_t data) +{ + WriteByte(data>>8); // High byte + WriteByte(data&0xFF); // Low byte +} + +void DataFlash_Class::WriteLong(int32_t data) +{ + WriteByte(data>>24); // First byte + WriteByte(data>>16); + WriteByte(data>>8); + WriteByte(data&0xFF); // Last byte +} + +// Get the last page written to +int16_t DataFlash_Class::GetWritePage() +{ + return(df_PageAdr); +} + +// Get the last page read +int16_t DataFlash_Class::GetPage() +{ + return(df_Read_PageAdr-1); +} + +void DataFlash_Class::StartRead(int16_t PageAdr) +{ + df_Read_BufferNum=1; + df_Read_BufferIdx=0; + df_Read_PageAdr=PageAdr; + WaitReady(); + PageToBuffer(df_Read_BufferNum,df_Read_PageAdr); // Write Memory page to buffer + df_Read_PageAdr++; +} + +byte DataFlash_Class::ReadByte() +{ + byte result; + + WaitReady(); + result = BufferRead(df_Read_BufferNum,df_Read_BufferIdx); + df_Read_BufferIdx++; + if (df_Read_BufferIdx >= df_PageSize) // End of buffer? + { + df_Read_BufferIdx=0; + PageToBuffer(df_Read_BufferNum,df_Read_PageAdr); // Write memory page to Buffer + df_Read_PageAdr++; + if (df_Read_PageAdr>=4096) // If we reach the end of the memory, start from the begining + { + df_Read_PageAdr = 0; + df_Read_END = true; + } + } + return result; +} + +int16_t DataFlash_Class::ReadInt() +{ + int result; + + result = ReadByte(); // High byte + result = (result<<8) | ReadByte(); // Low byte + return result; +} + +int32_t DataFlash_Class::ReadLong() +{ + int32_t result; + + result = ReadByte(); // First byte + result = (result<<8) | ReadByte(); + result = (result<<8) | ReadByte(); + result = (result<<8) | ReadByte(); // Last byte + return result; +} + +// make one instance for the user to use +DataFlash_Class DataFlash;