// This function determines the number of whole or partial log files in the DataFlash
// Wholly overwritten files are (of course) lost.
uint16_t DataFlash_Block::get_num_logs(void)
{
uint16_t lastpage;
uint16_t last;
uint16_t first;
if (find_last_page() == 1) {
return 0;
}
StartRead(1);
if (GetFileNumber() == 0xFFFF) {
return 0;
}
lastpage = find_last_page();
StartRead(lastpage);
last = GetFileNumber();
StartRead(lastpage + 2);
first = GetFileNumber();
if(first > last) {
StartRead(1);
first = GetFileNumber();
}
if (last == first) {
return 1;
}
return (last - first + 1);
}
/*
* we need to erase if the logging format has changed
*/
bool DataFlash_Block::NeedErase(void)
{
uint32_t version = 0;
StartRead(df_NumPages+1);
ReadBlock(&version, sizeof(version));
StartRead(1);
return version != DF_LOGGING_FORMAT;
}
/*
* we need to erase if the logging format has changed
*/
bool DataFlash_Block::NeedErase(void)
{
uint32_t version = 0;
StartRead(DF_LAST_PAGE); // Last Page
ReadBlock(&version, sizeof(version));
StartRead(1); //1
return version != DF_LOGGING_FORMAT;
}
// This function finds the last page of a particular log file
uint16_t DataFlash_Block::find_last_page_of_log(uint16_t log_number)
{
uint16_t look;
uint16_t bottom;
uint16_t top;
uint32_t look_hash;
uint32_t check_hash;
if(check_wrapped())
{
StartRead(1);
bottom = GetFileNumber();
if (bottom > log_number)
{
bottom = find_last_page();
top = df_NumPages;
} else {
bottom = 1;
top = find_last_page();
}
} else {
bottom = 1;
top = find_last_page();
}
check_hash = (int32_t)log_number<<16 | 0xFFFF;
while(top-bottom > 1)
{
look = (top+bottom)/2;
StartRead(look);
look_hash = (int32_t)GetFileNumber()<<16 | GetFilePage();
if (look_hash >= 0xFFFF0000) look_hash = 0;
if(look_hash > check_hash) {
// move down
top = look;
} else {
// move up
bottom = look;
}
}
StartRead(top);
if (GetFileNumber() == log_number) return top;
StartRead(bottom);
if (GetFileNumber() == log_number) return bottom;
return -1;
}
// This function finds the first and last pages of a log file
// The first page may be greater than the last page if the DataFlash has been filled and partially overwritten.
void DataFlash_Block::get_log_boundaries(uint16_t log_num, uint16_t & start_page, uint16_t & end_page)
{
uint16_t num = get_num_logs();
uint16_t look;
if (df_BufferIdx != 0) {
FinishWrite();
hal.scheduler->delay(100);
}
if(num == 1)
{
StartRead(df_NumPages);
if (GetFileNumber() == 0xFFFF)
{
start_page = 1;
end_page = find_last_page_of_log((uint16_t)log_num);
} else {
end_page = find_last_page_of_log((uint16_t)log_num);
start_page = end_page + 1;
}
} else {
if(log_num==1) {
StartRead(df_NumPages);
if(GetFileNumber() == 0xFFFF) {
start_page = 1;
} else {
start_page = find_last_page() + 1;
}
} else {
if(log_num == find_last_log() - num + 1) {
start_page = find_last_page() + 1;
} else {
look = log_num-1;
do {
start_page = find_last_page_of_log(look) + 1;
look--;
} while (start_page <= 0 && look >=1);
}
}
}
if (start_page == df_NumPages+1 || start_page == 0) {
start_page = 1;
}
end_page = find_last_page_of_log(log_num);
if (end_page == 0) {
end_page = start_page;
}
}
XnStatus NodeStateReadyRecord::Decode()
{
XnStatus nRetVal = XN_STATUS_OK;
nRetVal = StartRead();
XN_IS_STATUS_OK(nRetVal);
return XN_STATUS_OK;
}
XnStatus GeneralPropRecord::Decode()
{
XnStatus nRetVal = XN_STATUS_OK;
nRetVal = StartRead();
XN_IS_STATUS_OK(nRetVal);
nRetVal = ReadString(m_strPropName);
XN_IS_STATUS_OK(nRetVal);
nRetVal = Read(&m_nPropDataSize, sizeof(m_nPropDataSize));
XN_IS_STATUS_OK(nRetVal);
//The property data is not copied but just pointed to
XnUInt8* pData = const_cast<XnUInt8*>(GetReadPos());
#if (XN_PLATFORM == XN_PLATFORM_LINUX_ARM || XN_PLATFORM == XN_PLATFORM_ARC || XN_PLATFORM == XN_PLATFORM_ANDROID_ARM)
// under ARM we have some alignment issues. Move this buffer so it will be aligned.
XnUInt32 nAlignFix = XN_DEFAULT_MEM_ALIGN - ((XnUInt32)pData % XN_DEFAULT_MEM_ALIGN);
if (nAlignFix != 0)
{
xnOSMemMove(pData + nAlignFix, pData, m_nPropDataSize);
pData += nAlignFix;
}
#endif
m_pPropData = pData;
XN_IS_STATUS_OK(nRetVal);
return XN_STATUS_OK;
}
XnStatus DataIndexRecordHeader::Decode()
{
XnStatus nRetVal = StartRead();
XN_IS_STATUS_OK(nRetVal);
//No call to FinishRead() - this record is not done yet
return XN_STATUS_OK;
}
void ClientSession::OnRead(const boost::system::error_code& ec)
{
if (ec)
return;
// do we have bytes on connection
boost::system::error_code err;
size_t avail_bytes = socket_.available(err);
if (err || avail_bytes == 0)
return;
std::cout << "Client id = " << this->client_id_ << " msg: ";
for (size_t read_bytes = 0; read_bytes < avail_bytes;)
{
char data[1024];
size_t len = socket_.read_some(boost::asio::buffer(data), err);
read_bytes += len;
for (char* dataPtr = data; len; --len, ++dataPtr)
{
std::cout << *dataPtr;
// TODO: call commands here
}
}
std::cout << std::endl;
StartRead();
}
// This function starts a new log file in the DataFlash
void DataFlash_Block::start_new_log(void)
{
uint16_t last_page = find_last_page();
StartRead(last_page);
//Serial.print("last page: "); Serial.println(last_page);
//Serial.print("file #: "); Serial.println(GetFileNumber());
//Serial.print("file page: "); Serial.println(GetFilePage());
if(find_last_log() == 0 || GetFileNumber() == 0xFFFF) {
SetFileNumber(1);
StartWrite(1);
//Serial.println("start log from 0");
return;
}
// Check for log of length 1 page and suppress
if(GetFilePage() <= 1) {
SetFileNumber(GetFileNumber()); // Last log too short, reuse its number
StartWrite(last_page); // and overwrite it
//Serial.println("start log from short");
} else {
if(last_page == 0xFFFF) last_page=0;
SetFileNumber(GetFileNumber()+1);
StartWrite(last_page + 1);
//Serial.println("start log normal");
}
}
TInt DExampleChannel::DoRequest(TInt aFunction, TRequestStatus* aStatus, TAny* a1, TAny* a2)
{
TInt r = KErrNotSupported;
switch (aFunction)
{
case RPagingExample::ERequestNotify:
r = StartNotify(aStatus);
break;
case RPagingExample::ERequestAsyncGetValue:
r = StartAsyncGetValue((TInt*)a1, aStatus);
break;
case RPagingExample::ERequestAsyncGetValue2:
r = StartAsyncGetValue2((TInt*)a1, (TInt*)a2, aStatus);
break;
case RPagingExample::ERequestRead:
r = StartRead(a1, (TInt)a2, aStatus);
break;
case RPagingExample::ERequestReadDes:
r = StartReadDes((TDes8*)a1, aStatus);
break;
case RPagingExample::ERequestWrite:
r = StartWrite(a1, (TInt)a2, aStatus);
break;
case RPagingExample::ERequestWriteDes:
r = StartWriteDes((TDes8*)a1, aStatus);
break;
}
return r;
}
XnStatus EndRecord::Decode()
{
XnStatus nRetVal = StartRead();
XN_IS_STATUS_OK(nRetVal);
nRetVal = FinishRead();
XN_IS_STATUS_OK(nRetVal);
return XN_STATUS_OK;
}
bool DataFlash_Block::check_wrapped(void)
{
StartRead(df_NumPages);
if(GetFileNumber() == 0xFFFF)
return 0;
else
return 1;
}
XnStatus NodeDataBeginRecord::Decode()
{
XnStatus nRetVal = XN_STATUS_OK;
nRetVal = StartRead();
XN_IS_STATUS_OK(nRetVal);
nRetVal = Read(&m_seekInfo, sizeof(m_seekInfo));
XN_IS_STATUS_OK(nRetVal);
return XN_STATUS_OK;
}
/*
dump header information from all log pages
*/
void DataFlash_Block::DumpPageInfo(AP_HAL::BetterStream *port)
{
for (uint16_t count=1; count<=df_NumPages; count++) {
StartRead(count);
port->printf_P(PSTR("DF page, log file #, log page: %u,\t"), (unsigned)count);
port->printf_P(PSTR("%u,\t"), (unsigned)GetFileNumber());
port->printf_P(PSTR("%u\n"), (unsigned)GetFilePage());
}
}
XnStatus NodeAdded_1_0_0_4_Record::Decode()
{
XnStatus nRetVal = XN_STATUS_OK;
nRetVal = StartRead();
XN_IS_STATUS_OK(nRetVal);
nRetVal = DecodeImpl();
XN_IS_STATUS_OK(nRetVal);
return XN_STATUS_OK;
}
/*
Read the log and print it on port
*/
void DataFlash_Block::LogReadProcess(uint16_t log_num,
uint16_t start_page, uint16_t end_page,
print_mode_fn print_mode,
AP_HAL::BetterStream *port)
{
uint8_t log_step = 0;
uint16_t page = start_page;
bool first_entry = true;
if (df_BufferIdx != 0) {
FinishWrite();
hal.scheduler->delay(100);
}
StartRead(start_page);
while (true) {
uint8_t data;
if (!ReadBlock(&data, 1)) {
break;
}
// This is a state machine to read the packets
switch(log_step) {
case 0:
if (data == HEAD_BYTE1) {
log_step++;
}
break;
case 1:
if (data == HEAD_BYTE2) {
log_step++;
} else {
log_step = 0;
}
break;
case 2:
log_step = 0;
if (first_entry && data != LOG_FORMAT_MSG) {
_print_log_formats(port);
}
first_entry = false;
_print_log_entry(data, print_mode, port);
break;
}
uint16_t new_page = GetPage();
if (new_page != page) {
if (new_page == end_page+1 || new_page == start_page) {
return;
}
page = new_page;
}
}
}
XnStatus NodeAddedRecord::Decode()
{
XnStatus nRetVal = XN_STATUS_OK;
nRetVal = StartRead();
XN_IS_STATUS_OK(nRetVal);
nRetVal = NodeAdded_1_0_0_5_Record::DecodeImpl();
XN_IS_STATUS_OK(nRetVal);
nRetVal = Read(&m_nSeekTablePosition, sizeof(m_nSeekTablePosition));
XN_IS_STATUS_OK(nRetVal);
return XN_STATUS_OK;
}
XnStatus NewDataRecordHeader::Decode()
{
XnStatus nRetVal = XN_STATUS_OK;
nRetVal = StartRead();
XN_IS_STATUS_OK(nRetVal);
nRetVal = Read(&m_nTimeStamp, sizeof(m_nTimeStamp));
XN_IS_STATUS_OK(nRetVal);
nRetVal = Read(&m_nFrameNumber, sizeof(m_nFrameNumber));
XN_IS_STATUS_OK(nRetVal);
//No call to FinishRead() - this record is not done yet
return XN_STATUS_OK;
}
void Client::OnConnect(const boost::system::error_code& ec)
{
if (!ec)
{
connected_ = true;
std::cout << "connection to server success" << std::endl;
StartRead();
//socket_.async_write_some(boost::asio::buffer(msg, strlen(msg)), boost::bind(&Client::OnWrite, this, _1, _2));
}
}
// This function finds the last page of the last file
uint16_t DataFlash_Block::find_last_page(void)
{
uint16_t look;
uint16_t bottom = 1;
uint16_t top = df_NumPages;
uint32_t look_hash;
uint32_t bottom_hash;
uint32_t top_hash;
StartRead(bottom);
bottom_hash = ((int32_t)GetFileNumber()<<16) | GetFilePage();
while(top-bottom > 1) {
look = (top+bottom)/2;
StartRead(look);
look_hash = (int32_t)GetFileNumber()<<16 | GetFilePage();
if (look_hash >= 0xFFFF0000) look_hash = 0;
if(look_hash < bottom_hash) {
// move down
top = look;
} else {
// move up
bottom = look;
bottom_hash = look_hash;
}
}
StartRead(top);
top_hash = ((int32_t)GetFileNumber()<<16) | GetFilePage();
if (top_hash >= 0xFFFF0000) {
top_hash = 0;
}
if (top_hash > bottom_hash) {
return top;
}
return bottom;
}
int audioStreamer_KS::Read(char *buf, int len) // returns 0 if blocked, < 0 if error, > 0 if data
{
if (read_pos < 0)
{
int i;
for (i = 0; i < m_nbufs; i ++)
{
StartRead(i,len);
}
read_pos=0;
}
WaitForSingleObject(hEventPool[read_pos],INFINITE);
//myPrintf("read: %x\n",Packets[dwWait].Header.DataUsed);
int rlen=Packets[read_pos].Header.DataUsed;
memcpy(buf,Packets[read_pos].Header.Data,rlen);
StartRead(read_pos,len);
read_pos++;
if (read_pos >= m_nbufs) read_pos=0;
return rlen;
}
/*
Read the log and print it on port
*/
void DataFlash_Block::LogReadProcess(uint16_t log_num,
uint16_t start_page, uint16_t end_page,
uint8_t num_types,
const struct LogStructure *structure,
void (*print_mode)(AP_HAL::BetterStream *port, uint8_t mode),
AP_HAL::BetterStream *port)
{
uint8_t log_step = 0;
uint16_t page = start_page;
if (df_BufferIdx != 0) {
FinishWrite();
hal.scheduler->delay(100);
}
StartRead(start_page);
while (true) {
uint8_t data;
ReadBlock(&data, 1);
// This is a state machine to read the packets
switch(log_step) {
case 0:
if (data == HEAD_BYTE1) {
log_step++;
}
break;
case 1:
if (data == HEAD_BYTE2) {
log_step++;
} else {
log_step = 0;
}
break;
case 2:
log_step = 0;
_print_log_entry(data, num_types, structure, print_mode, port);
break;
}
uint16_t new_page = GetPage();
if (new_page != page) {
if (new_page == end_page+1 || new_page == start_page) {
return;
}
page = new_page;
}
}
}
XnStatus NodeAddedRecord::Decode()
{
XnStatus nRetVal = XN_STATUS_OK;
nRetVal = StartRead();
XN_IS_STATUS_OK(nRetVal);
nRetVal = NodeAdded_1_0_0_4_Record::DecodeImpl();
XN_IS_STATUS_OK(nRetVal);
nRetVal = Read(&m_nNumberOfFrames, sizeof(m_nNumberOfFrames));
XN_IS_STATUS_OK(nRetVal);
nRetVal = Read(&m_nMinTimestamp, sizeof(m_nMinTimestamp));
XN_IS_STATUS_OK(nRetVal);
nRetVal = Read(&m_nMaxTimestamp, sizeof(m_nMaxTimestamp));
XN_IS_STATUS_OK(nRetVal);
return XN_STATUS_OK;
}
/*
Read the DataFlash log memory
Call the callback() function on each log message found in the page
range. Return the number of log messages found
Note that for the block oriented backend the log_num is ignored
*/
uint16_t DataFlash_Block::log_read_process(uint8_t log_num,
uint16_t start_page, uint16_t end_page,
void (*callback)(uint8_t msgid))
{
uint8_t log_step = 0;
uint16_t page = start_page;
uint16_t packet_count = 0;
StartRead(start_page);
while (true) {
uint8_t data;
ReadBlock(&data, 1);
// This is a state machine to read the packets
switch(log_step) {
case 0:
if (data == HEAD_BYTE1) {
log_step++;
}
break;
case 1:
if (data == HEAD_BYTE2) {
log_step++;
} else {
log_step = 0;
}
break;
case 2:
log_step = 0;
callback(data);
packet_count++;
break;
}
uint16_t new_page = GetPage();
if (new_page != page) {
if (new_page == end_page || new_page == start_page) {
return packet_count;
}
page = new_page;
}
}
return packet_count;
}
void Server::Accept()
{
auto new_client_session = std::make_shared< ClientSession >(*this);
acceptor_.async_accept(new_client_session->GetSocket(),
[this, new_client_session](const boost::system::error_code& ec)
{
if (!ec)
{
new_client_session->StartRead();
sessions_.insert(new_client_session.get());
std::cout << "client connected, id = " << new_client_session->GetClientId() << std::endl;
}
Accept();
}
);
}
/**
get raw data from a log
*/
int16_t DataFlash_Block::get_log_data_raw(uint16_t log_num, uint16_t page, uint32_t offset, uint16_t len, uint8_t *data)
{
uint16_t data_page_size = df_PageSize - sizeof(struct PageHeader);
if (offset >= data_page_size) {
page += offset / data_page_size;
offset = offset % data_page_size;
if (page > df_NumPages) {
// pages are one based, not zero
page = 1 + page - df_NumPages;
}
}
if (log_write_started || df_Read_PageAdr != page) {
StartRead(page);
}
df_Read_BufferIdx = offset + sizeof(struct PageHeader);
ReadBlock(data, len);
return (int16_t)len;
}
// This function starts a new log file in the DataFlash
uint16_t DataFlash_Block::start_new_log(void)
{
uint16_t last_page = find_last_page();
StartRead(last_page);
//Serial.print("last page: "); Serial.println(last_page);
//Serial.print("file #: "); Serial.println(GetFileNumber());
//Serial.print("file page: "); Serial.println(GetFilePage());
if(find_last_log() == 0 || GetFileNumber() == 0xFFFF) {
SetFileNumber(1);
StartWrite(1);
//Serial.println("start log from 0");
log_write_started = true;
return 1;
}
uint16_t new_log_num;
// Check for log of length 1 page and suppress
if(GetFilePage() <= 1) {
new_log_num = GetFileNumber();
// Last log too short, reuse its number
// and overwrite it
SetFileNumber(new_log_num);
StartWrite(last_page);
} else {
new_log_num = GetFileNumber()+1;
if (last_page == 0xFFFF) {
last_page=0;
}
SetFileNumber(new_log_num);
StartWrite(last_page + 1);
}
log_write_started = true;
return new_log_num;
}
XnStatus NodeRemovedRecord::Decode()
{
return StartRead();
}
// This funciton finds the last log number
uint16_t DataFlash_Block::find_last_log(void)
{
uint16_t last_page = find_last_page();
StartRead(last_page);
return GetFileNumber();
}
