mirror of https://github.com/ArduPilot/ardupilot
desktop: import updated DataFlash code
this fixes a logging bug
This commit is contained in:
parent
673b24b4bc
commit
dba786198e
|
@ -1,3 +1,4 @@
|
||||||
|
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
/*
|
/*
|
||||||
hacked up DataFlash library for Desktop support
|
hacked up DataFlash library for Desktop support
|
||||||
*/
|
*/
|
||||||
|
@ -114,30 +115,31 @@ void DataFlash_APM1::ChipErase ()
|
||||||
// *** DATAFLASH PUBLIC FUNCTIONS ***
|
// *** DATAFLASH PUBLIC FUNCTIONS ***
|
||||||
void DataFlash_APM1::StartWrite(int16_t PageAdr)
|
void DataFlash_APM1::StartWrite(int16_t PageAdr)
|
||||||
{
|
{
|
||||||
df_BufferNum = 1;
|
df_BufferNum=1;
|
||||||
df_BufferIdx = 4;
|
df_BufferIdx=4;
|
||||||
df_PageAdr = PageAdr;
|
df_PageAdr=PageAdr;
|
||||||
df_Stop_Write = 0;
|
df_Stop_Write=0;
|
||||||
|
WaitReady();
|
||||||
BufferWrite(df_BufferNum,0,df_FileNumber>>8); // High byte
|
// We are starting a new page - write FileNumber and FilePage
|
||||||
BufferWrite(df_BufferNum,1,df_FileNumber&0xFF); // Low byte
|
BufferWrite(df_BufferNum,0,df_FileNumber>>8); // High byte
|
||||||
BufferWrite(df_BufferNum,2,df_FilePage>>8); // High byte
|
BufferWrite(df_BufferNum,1,df_FileNumber&0xFF); // Low byte
|
||||||
BufferWrite(df_BufferNum,3,df_FilePage&0xFF); // Low byte
|
BufferWrite(df_BufferNum,2,df_FilePage>>8); // High byte
|
||||||
|
BufferWrite(df_BufferNum,3,df_FilePage&0xFF); // Low byte
|
||||||
}
|
}
|
||||||
|
|
||||||
void DataFlash_APM1::FinishWrite(void)
|
void DataFlash_APM1::FinishWrite(void)
|
||||||
{
|
{
|
||||||
df_BufferIdx = 0;
|
df_BufferIdx=0;
|
||||||
BufferToPage(df_BufferNum,df_PageAdr,0); // Write Buffer to memory, NO WAIT
|
BufferToPage(df_BufferNum,df_PageAdr,0); // Write Buffer to memory, NO WAIT
|
||||||
df_PageAdr++;
|
df_PageAdr++;
|
||||||
if (OVERWRITE_DATA==1)
|
if (OVERWRITE_DATA==1)
|
||||||
{
|
{
|
||||||
if (df_PageAdr>=DF_LAST_PAGE) // If we reach the end of the memory, start from the begining
|
if (df_PageAdr>DF_LAST_PAGE) // If we reach the end of the memory, start from the begining
|
||||||
df_PageAdr = 1;
|
df_PageAdr = 1;
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
if (df_PageAdr>=DF_LAST_PAGE) // If we reach the end of the memory, stop here
|
if (df_PageAdr>DF_LAST_PAGE) // If we reach the end of the memory, stop here
|
||||||
df_Stop_Write=1;
|
df_Stop_Write=1;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -145,12 +147,6 @@ void DataFlash_APM1::FinishWrite(void)
|
||||||
df_BufferNum=2;
|
df_BufferNum=2;
|
||||||
else
|
else
|
||||||
df_BufferNum=1;
|
df_BufferNum=1;
|
||||||
|
|
||||||
BufferWrite(df_BufferNum,0,df_FileNumber>>8); // High byte
|
|
||||||
BufferWrite(df_BufferNum,1,df_FileNumber&0xFF); // Low byte
|
|
||||||
df_FilePage++;
|
|
||||||
BufferWrite(df_BufferNum,2,df_FilePage>>8); // High byte
|
|
||||||
BufferWrite(df_BufferNum,3,df_FilePage&0xFF); // Low byte
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@ -162,17 +158,17 @@ void DataFlash_APM1::WriteByte(byte data)
|
||||||
df_BufferIdx++;
|
df_BufferIdx++;
|
||||||
if (df_BufferIdx >= df_PageSize) // End of buffer?
|
if (df_BufferIdx >= df_PageSize) // End of buffer?
|
||||||
{
|
{
|
||||||
df_BufferIdx=4;
|
df_BufferIdx=4; //(4 bytes for FileNumber, FilePage)
|
||||||
BufferToPage(df_BufferNum,df_PageAdr,0); // Write Buffer to memory, NO WAIT
|
BufferToPage(df_BufferNum,df_PageAdr,0); // Write Buffer to memory, NO WAIT
|
||||||
df_PageAdr++;
|
df_PageAdr++;
|
||||||
if (OVERWRITE_DATA==1)
|
if (OVERWRITE_DATA==1)
|
||||||
{
|
{
|
||||||
if (df_PageAdr>=DF_LAST_PAGE) // If we reach the end of the memory, start from the begining
|
if (df_PageAdr>DF_LAST_PAGE) // If we reach the end of the memory, start from the begining
|
||||||
df_PageAdr = 1;
|
df_PageAdr = 1;
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
if (df_PageAdr>=DF_LAST_PAGE) // If we reach the end of the memory, stop here
|
if (df_PageAdr>DF_LAST_PAGE) // If we reach the end of the memory, stop here
|
||||||
df_Stop_Write=1;
|
df_Stop_Write=1;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -180,6 +176,12 @@ void DataFlash_APM1::WriteByte(byte data)
|
||||||
df_BufferNum=2;
|
df_BufferNum=2;
|
||||||
else
|
else
|
||||||
df_BufferNum=1;
|
df_BufferNum=1;
|
||||||
|
// We are starting a new page - write FileNumber and FilePage
|
||||||
|
BufferWrite(df_BufferNum,0,df_FileNumber>>8); // High byte
|
||||||
|
BufferWrite(df_BufferNum,1,df_FileNumber&0xFF); // Low byte
|
||||||
|
df_FilePage++;
|
||||||
|
BufferWrite(df_BufferNum,2,df_FilePage>>8); // High byte
|
||||||
|
BufferWrite(df_BufferNum,3,df_FilePage&0xFF); // Low byte
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -217,7 +219,16 @@ void DataFlash_APM1::StartRead(int16_t PageAdr)
|
||||||
df_Read_PageAdr=PageAdr;
|
df_Read_PageAdr=PageAdr;
|
||||||
WaitReady();
|
WaitReady();
|
||||||
PageToBuffer(df_Read_BufferNum,df_Read_PageAdr); // Write Memory page to buffer
|
PageToBuffer(df_Read_BufferNum,df_Read_PageAdr); // Write Memory page to buffer
|
||||||
|
//Serial.print(df_Read_PageAdr, DEC); Serial.print("\t");
|
||||||
df_Read_PageAdr++;
|
df_Read_PageAdr++;
|
||||||
|
|
||||||
|
// We are starting a new page - read FileNumber and FilePage
|
||||||
|
df_FileNumber = BufferRead(df_Read_BufferNum,0); // High byte
|
||||||
|
//Serial.print(df_FileNumber, DEC); Serial.print("\t");
|
||||||
|
df_FileNumber = (df_FileNumber<<8) | BufferRead(df_Read_BufferNum,1); // Low byte
|
||||||
|
//Serial.println(df_FileNumber, DEC); Serial.print("\t");
|
||||||
|
df_FilePage = BufferRead(df_Read_BufferNum,2); // High byte
|
||||||
|
df_FilePage = (df_FilePage<<8) | BufferRead(df_Read_BufferNum,3); // Low byte
|
||||||
}
|
}
|
||||||
|
|
||||||
byte DataFlash_APM1::ReadByte()
|
byte DataFlash_APM1::ReadByte()
|
||||||
|
@ -229,14 +240,20 @@ byte DataFlash_APM1::ReadByte()
|
||||||
df_Read_BufferIdx++;
|
df_Read_BufferIdx++;
|
||||||
if (df_Read_BufferIdx >= df_PageSize) // End of buffer?
|
if (df_Read_BufferIdx >= df_PageSize) // End of buffer?
|
||||||
{
|
{
|
||||||
df_Read_BufferIdx=4;
|
df_Read_BufferIdx=4; //(4 bytes for FileNumber, FilePage)
|
||||||
PageToBuffer(df_Read_BufferNum,df_Read_PageAdr); // Write memory page to Buffer
|
PageToBuffer(df_Read_BufferNum,df_Read_PageAdr); // Write memory page to Buffer
|
||||||
df_Read_PageAdr++;
|
df_Read_PageAdr++;
|
||||||
if (df_Read_PageAdr>=DF_LAST_PAGE) // If we reach the end of the memory, start from the begining
|
if (df_Read_PageAdr>DF_LAST_PAGE) // If we reach the end of the memory, start from the begining
|
||||||
{
|
{
|
||||||
df_Read_PageAdr = 0;
|
df_Read_PageAdr = 0;
|
||||||
df_Read_END = true;
|
df_Read_END = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// We are starting a new page - read FileNumber and FilePage
|
||||||
|
df_FileNumber = BufferRead(df_Read_BufferNum,0); // High byte
|
||||||
|
df_FileNumber = (df_FileNumber<<8) | BufferRead(df_Read_BufferNum,1); // Low byte
|
||||||
|
df_FilePage = BufferRead(df_Read_BufferNum,2); // High byte
|
||||||
|
df_FilePage = (df_FilePage<<8) | BufferRead(df_Read_BufferNum,3); // Low byte
|
||||||
}
|
}
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue