Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

DataFlash: change to new style Semaphore library

This commit is contained in:
Pat Hickey 2013-01-03 10:34:25 -08:00
parent f178d1bd02
commit 390e96311e
2 changed files with 75 additions and 123 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

110
libraries/DataFlash/DataFlash_APM1.cpp
View File

@ -86,11 +86,17 @@ void DataFlash_APM1::Init(void)
_spi = hal.spi->device(AP_HAL::SPIDevice_Dataflash); _spi = hal.spi->device(AP_HAL::SPIDevice_Dataflash);
if (_spi == NULL) { if (_spi == NULL) {
hal.console->println_P( hal.scheduler->panic(
PSTR("PANIC: DataFlash SPIDeviceDriver not found")); PSTR("PANIC: DataFlash SPIDeviceDriver not found"));
return; return; /* never reached */
} }
_spi_sem = _spi->get_semaphore(); _spi_sem = _spi->get_semaphore();
if (_spi_sem == NULL) {
hal.scheduler->panic(
PSTR("PANIC: DataFlash SPIDeviceDriver semaphore is null"));
return; /* never reached */
}
// get page size: 512 or 528 (by default: 528) // get page size: 512 or 528 (by default: 528)
df_PageSize = PageSize(); df_PageSize = PageSize();
@ -102,10 +108,8 @@ void DataFlash_APM1::Init(void)
// This function is mainly to test the device // This function is mainly to test the device
void DataFlash_APM1::ReadManufacturerID() void DataFlash_APM1::ReadManufacturerID()
{ {
if (_spi_sem) { if (!_spi_sem->take(5))
bool got = _spi_sem->get(this); return;
if (!got) return;
}
// activate dataflash command decoder // activate dataflash command decoder
_spi->cs_assert(); _spi->cs_assert();
@ -120,9 +124,7 @@ void DataFlash_APM1::ReadManufacturerID()
// release SPI bus for use by other sensors // release SPI bus for use by other sensors
_spi->cs_release(); _spi->cs_release();
if (_spi_sem) { _spi_sem->give();
_spi_sem->release(this);
}
} }
// This function return 1 if Card is inserted on SD slot // This function return 1 if Card is inserted on SD slot
@ -161,15 +163,12 @@ uint8_t DataFlash_APM1::ReadStatus()
inline inline
uint16_t DataFlash_APM1::PageSize() uint16_t DataFlash_APM1::PageSize()
{ {
if (_spi_sem) { if (!_spi_sem->take(5))
bool got = _spi_sem->get(this); return 0;
if (!got) return 0;
}
return(528-((ReadStatusReg()&0x01) << 4)); // if first bit 1 trhen 512 else 528 bytes
if (_spi_sem) {
_spi_sem->release(this);
}
return(528-((ReadStatusReg()&0x01) << 4)); // if first bit 1 trhen 512 else 528 bytes
_spi_sem->give();
} }
// Wait until DataFlash is in ready state... // Wait until DataFlash is in ready state...
@ -181,10 +180,9 @@ void DataFlash_APM1::WaitReady()
void DataFlash_APM1::PageToBuffer(uint8_t BufferNum, uint16_t PageAdr) void DataFlash_APM1::PageToBuffer(uint8_t BufferNum, uint16_t PageAdr)
{ {
if (_spi_sem) { if (!_spi_sem->take(1))
bool got = _spi_sem->get(this); return;
if (!got) return;
}
// activate dataflash command decoder // activate dataflash command decoder
_spi->cs_assert(); _spi->cs_assert();
@ -206,17 +204,14 @@ void DataFlash_APM1::PageToBuffer(uint8_t BufferNum, uint16_t PageAdr)
_spi->cs_release(); _spi->cs_release();
while(!ReadStatus()) ; //monitor the status register, wait until busy-flag is high while(!ReadStatus()) ; //monitor the status register, wait until busy-flag is high
if (_spi_sem) { _spi_sem->give();
_spi_sem->release(this);
}
} }
void DataFlash_APM1::BufferToPage (uint8_t BufferNum, uint16_t PageAdr, uint8_t wait) void DataFlash_APM1::BufferToPage (uint8_t BufferNum, uint16_t PageAdr, uint8_t wait)
{ {
if (_spi_sem) { if (!_spi_sem->take(1))
bool got = _spi_sem->get(this); return;
if (!got) return;
}
// activate dataflash command decoder // activate dataflash command decoder
_spi->cs_assert(); _spi->cs_assert();
@ -240,18 +235,15 @@ void DataFlash_APM1::BufferToPage (uint8_t BufferNum, uint16_t PageAdr, uint8_t
// Check if we need to wait to write the buffer to memory or we can continue... // Check if we need to wait to write the buffer to memory or we can continue...
if (wait) if (wait)
while(!ReadStatus()) ; //monitor the status register, wait until busy-flag is high while(!ReadStatus()) ; //monitor the status register, wait until busy-flag is high
if (_spi_sem) { _spi_sem->give();
_spi_sem->release(this);
}
} }
void DataFlash_APM1::BufferWrite (uint8_t BufferNum, uint16_t IntPageAdr, uint8_t Data) void DataFlash_APM1::BufferWrite (uint8_t BufferNum, uint16_t IntPageAdr, uint8_t Data)
{ {
if (_spi_sem) { if (!_spi_sem->take(1))
bool got = _spi_sem->get(this); return;
if (!got) return;
}
// activate dataflash command decoder // activate dataflash command decoder
_spi->cs_assert(); _spi->cs_assert();
@ -267,19 +259,16 @@ void DataFlash_APM1::BufferWrite (uint8_t BufferNum, uint16_t IntPageAdr, uint8_
// release SPI bus for use by other sensors // release SPI bus for use by other sensors
_spi->cs_release(); _spi->cs_release();
if (_spi_sem) { _spi_sem->give();
_spi_sem->release(this);
}
} }
uint8_t DataFlash_APM1::BufferRead (uint8_t BufferNum, uint16_t IntPageAdr) uint8_t DataFlash_APM1::BufferRead (uint8_t BufferNum, uint16_t IntPageAdr)
{ {
uint8_t tmp; uint8_t tmp;
if (_spi_sem) { if (!_spi_sem->take(1))
bool got = _spi_sem->get(this); return 0;
if (!got) return 0;
}
// activate dataflash command decoder // activate dataflash command decoder
_spi->cs_assert(); _spi->cs_assert();
@ -297,19 +286,16 @@ uint8_t DataFlash_APM1::BufferRead (uint8_t BufferNum, uint16_t IntPageAdr)
// release SPI bus for use by other sensors // release SPI bus for use by other sensors
_spi->cs_release(); _spi->cs_release();
if (_spi_sem) { _spi_sem->give();
_spi_sem->release(this);
}
return (tmp); return (tmp);
} }
// *** END OF INTERNAL FUNCTIONS *** // *** END OF INTERNAL FUNCTIONS ***
void DataFlash_APM1::PageErase (uint16_t PageAdr) void DataFlash_APM1::PageErase (uint16_t PageAdr)
{ {
if (_spi_sem) { if (!_spi_sem->take(1))
bool got = _spi_sem->get(this); return;
if (!got) return;
}
// activate dataflash command decoder // activate dataflash command decoder
_spi->cs_assert(); _spi->cs_assert();
@ -329,18 +315,15 @@ void DataFlash_APM1::PageErase (uint16_t PageAdr)
//initiate flash page erase //initiate flash page erase
_spi->cs_release(); _spi->cs_release();
_spi_sem->give();
while(!ReadStatus()) ; while(!ReadStatus()) ;
if (_spi_sem) {
_spi_sem->release(this);
}
} }
void DataFlash_APM1::BlockErase (uint16_t BlockAdr) void DataFlash_APM1::BlockErase (uint16_t BlockAdr)
{ {
if (_spi_sem) { if (!_spi_sem->take(1))
bool got = _spi_sem->get(this); return;
if (!got) return;
}
// activate dataflash command decoder // activate dataflash command decoder
_spi->cs_assert(); _spi->cs_assert();
@ -370,19 +353,14 @@ void DataFlash_APM1::BlockErase (uint16_t BlockAdr)
//initiate flash page erase //initiate flash page erase
_spi->cs_release(); _spi->cs_release();
while(!ReadStatus()) ; while(!ReadStatus()) ;
if (_spi_sem) { _spi_sem->give();
_spi_sem->release(this);
}
} }
void DataFlash_APM1::ChipErase() void DataFlash_APM1::ChipErase()
{ {
//serialDebug("Chip Erase\n"); if (!_spi_sem->take(5))
if (_spi_sem) { return;
bool got = _spi_sem->get(this);
if (!got) return;
}
// activate dataflash command decoder // activate dataflash command decoder
_spi->cs_assert(); _spi->cs_assert();
@ -400,8 +378,6 @@ void DataFlash_APM1::ChipErase()
hal.scheduler->delay(6); hal.scheduler->delay(6);
} }
if (_spi_sem) { _spi_sem->give();
_spi_sem->release(this);
}
} }

88
libraries/DataFlash/DataFlash_APM2.cpp
View File

@ -87,24 +87,26 @@ void DataFlash_APM2::Init(void)
_spi = hal.spi->device(AP_HAL::SPIDevice_Dataflash); _spi = hal.spi->device(AP_HAL::SPIDevice_Dataflash);
if (_spi == NULL) { if (_spi == NULL) {
hal.console->println_P( hal.scheduler->panic(
PSTR("PANIC: DataFlash SPIDeviceDriver not found")); PSTR("PANIC: DataFlash SPIDeviceDriver not found"));
return; return;
} }
_spi_sem = _spi->get_semaphore(); _spi_sem = _spi->get_semaphore();
if (_spi_sem) { if (_spi_sem == NULL) {
bool got = _spi_sem->get(this); hal.scheduler->panic(
if (!got) return; PSTR("PANIC: DataFlash SPIDeviceDriver semaphore is null"));
return; /* never reached */
} }
if (!_spi_sem->take(5))
return;
// get page size: 512 or 528 (by default: 528) // get page size: 512 or 528 (by default: 528)
df_PageSize=PageSize(); df_PageSize=PageSize();
ReadManufacturerID(); ReadManufacturerID();
if (_spi_sem) { _spi_sem->give();
_spi_sem->release(this);
}
// see page 22 of the spec for the density code // see page 22 of the spec for the density code
uint8_t density_code = (df_device >> 8) & 0x1F; uint8_t density_code = (df_device >> 8) & 0x1F;
@ -186,10 +188,9 @@ void DataFlash_APM2::WaitReady()
void DataFlash_APM2::PageToBuffer(uint8_t BufferNum, uint16_t PageAdr) void DataFlash_APM2::PageToBuffer(uint8_t BufferNum, uint16_t PageAdr)
{ {
if (_spi_sem) { if (!_spi_sem->take(1))
bool got = _spi_sem->get(this); return;
if (!got) return;
}
// activate dataflash command decoder // activate dataflash command decoder
_spi->cs_assert(); _spi->cs_assert();
@ -215,17 +216,13 @@ void DataFlash_APM2::PageToBuffer(uint8_t BufferNum, uint16_t PageAdr)
// release SPI bus for use by other sensors // release SPI bus for use by other sensors
_spi->cs_release(); _spi->cs_release();
if (_spi_sem) { _spi_sem->give();
_spi_sem->release(this);
}
} }
void DataFlash_APM2::BufferToPage (uint8_t BufferNum, uint16_t PageAdr, uint8_t wait) void DataFlash_APM2::BufferToPage (uint8_t BufferNum, uint16_t PageAdr, uint8_t wait)
{ {
if (_spi_sem) { if (!_spi_sem->take(1))
bool got = _spi_sem->get(this); return;
if (!got) return;
}
// activate dataflash command decoder // activate dataflash command decoder
_spi->cs_assert(); _spi->cs_assert();
@ -251,17 +248,13 @@ void DataFlash_APM2::BufferToPage (uint8_t BufferNum, uint16_t PageAdr, uint8_t
while(!ReadStatus()) ; //monitor the status register, wait until busy-flag is high while(!ReadStatus()) ; //monitor the status register, wait until busy-flag is high
// release SPI bus for use by other sensors // release SPI bus for use by other sensors
if (_spi_sem) { _spi_sem->give();
_spi_sem->release(this);
}
} }
void DataFlash_APM2::BufferWrite (uint8_t BufferNum, uint16_t IntPageAdr, uint8_t Data) void DataFlash_APM2::BufferWrite (uint8_t BufferNum, uint16_t IntPageAdr, uint8_t Data)
{ {
if (_spi_sem) { if (!_spi_sem->take(1))
bool got = _spi_sem->get(this); return;
if (!got) return;
}
// activate dataflash command decoder // activate dataflash command decoder
_spi->cs_assert(); _spi->cs_assert();
@ -277,17 +270,14 @@ void DataFlash_APM2::BufferWrite (uint8_t BufferNum, uint16_t IntPageAdr, uint8_
// release SPI bus for use by other sensors // release SPI bus for use by other sensors
_spi->cs_release(); _spi->cs_release();
if (_spi_sem) { _spi_sem->give();
_spi_sem->release(this);
}
} }
uint8_t DataFlash_APM2::BufferRead (uint8_t BufferNum, uint16_t IntPageAdr) uint8_t DataFlash_APM2::BufferRead (uint8_t BufferNum, uint16_t IntPageAdr)
{ {
if (_spi_sem) { if (!_spi_sem->take(1))
bool got = _spi_sem->get(this); return 0;
if (!got) return 0;
}
uint8_t tmp; uint8_t tmp;
// activate dataflash command decoder // activate dataflash command decoder
@ -307,19 +297,15 @@ uint8_t DataFlash_APM2::BufferRead (uint8_t BufferNum, uint16_t IntPageAdr)
// release SPI bus for use by other sensors // release SPI bus for use by other sensors
_spi->cs_release(); _spi->cs_release();
if (_spi_sem) { _spi_sem->give();
_spi_sem->release(this);
}
return (tmp); return (tmp);
} }
// *** END OF INTERNAL FUNCTIONS *** // *** END OF INTERNAL FUNCTIONS ***
void DataFlash_APM2::PageErase (uint16_t PageAdr) void DataFlash_APM2::PageErase (uint16_t PageAdr)
{ {
if (_spi_sem) { if (!_spi_sem->take(1))
bool got = _spi_sem->get(this); return;
if (!got) return;
}
// activate dataflash command decoder // activate dataflash command decoder
_spi->cs_assert(); _spi->cs_assert();
@ -341,18 +327,14 @@ void DataFlash_APM2::PageErase (uint16_t PageAdr)
while(!ReadStatus()) ; while(!ReadStatus()) ;
// release SPI bus for use by other sensors // release SPI bus for use by other sensors
if (_spi_sem) { _spi_sem->give();
_spi_sem->release(this);
}
} }
// erase a block of 8 pages. // erase a block of 8 pages.
void DataFlash_APM2::BlockErase(uint16_t BlockAdr) void DataFlash_APM2::BlockErase(uint16_t BlockAdr)
{ {
if (_spi_sem) { if (!_spi_sem->take(1))
bool got = _spi_sem->get(this); return;
if (!got) return;
}
// activate dataflash command decoder // activate dataflash command decoder
_spi->cs_assert(); _spi->cs_assert();
@ -374,18 +356,14 @@ void DataFlash_APM2::BlockErase(uint16_t BlockAdr)
while(!ReadStatus()) ; while(!ReadStatus()) ;
// release SPI bus for use by other sensors // release SPI bus for use by other sensors
if (_spi_sem) { _spi_sem->give();
_spi_sem->release(this);
}
} }
void DataFlash_APM2::ChipErase() void DataFlash_APM2::ChipErase()
{ {
if (_spi_sem) { if (!_spi_sem->take(1))
bool got = _spi_sem->get(this); return;
if (!got) return;
}
//serialDebug("Chip Erase\n"); //serialDebug("Chip Erase\n");
// activate dataflash command decoder // activate dataflash command decoder
@ -405,7 +383,5 @@ void DataFlash_APM2::ChipErase()
} }
// release SPI bus for use by other sensors // release SPI bus for use by other sensors
if (_spi_sem) { _spi_sem->give();
_spi_sem->release(this);
}
} }