void CVQualityMapGuidedUn::BackUnwrap(double *phasemap, int *labelmap, int width, int height,
CPoint startpoint, Queue *queue, double *absolutephasemap)
{
int PopIndex = 0;
CPoint NeighborPoint, CurrentPoint;
PopIndex = PopQueue(queue);
while(PopIndex > -1)
{
CurrentPoint.x = PopIndex%width;
CurrentPoint.y = PopIndex/width;
for(int i = 0; i < 8; i++)
{
NeighborPoint.x = CurrentPoint.x + NeighborDirection[i][0];
NeighborPoint.y = CurrentPoint.y + NeighborDirection[i][1];
if(((abs(NeighborPoint.x - startpoint.x) > abs(CurrentPoint.x - startpoint.x))||(abs(NeighborPoint.y - startpoint.y) > abs(CurrentPoint.y - startpoint.y)))
&&(labelmap[NeighborPoint.x + NeighborPoint.y * width] == UNWRAP))
{
labelmap[PopIndex] = UNWRAP;
absolutephasemap[CurrentPoint.x + CurrentPoint.y * width] = this->LineScan(phasemap[CurrentPoint.x + CurrentPoint.y * width],
phasemap[NeighborPoint.x + NeighborPoint.y * width], absolutephasemap[NeighborPoint.x + NeighborPoint.y * width], CurrentPoint, NeighborPoint) ;
break;
}
}
PopIndex = PopQueue(queue);
}
}
/**
* @function BuildManifold
*/
void HP2D::BuildManifold( Vertice _v0 ) {
Vertice* va;
Vertice vt;
Vertice* vb;
std::vector<Vertice> S;
std::vector<Vertice*> B;
InsertQueue( _v0 );
do{
va = PopQueue();
S = Successors( va );
for( int i = 0; i < S.size(); i++ ) {
vt = S[i];
InsertVertice( vt );
InsertEdge( &vt, va );
if( vt.GetDist() < mDIST_MAX ) {
InsertQueue( vt );
}
B = GetAdjacent( va->Adjacent() );
for( unsigned j = 0; j < B.size(); j++ ) {
vb = B[j];
if( InSet( vb->GetPos(), vt.Neighbors() ) ) {
InsertEdge( &vt, vb );
}
}
}
} while( GetSizeQueue() > 0 );
}
void McuMessageHandler::ProcessGetCanData(int len, int seq)
{
unsigned long data;
unsigned long data2;
vector<unsigned char> vrecord;
if(len < 16)
{
LogUtility::Log(LOG_LEVEL_ERROR, "McuMessageHandler::ProcessGetCanData len invalid.");
return;
}
if(PopQueue(MCU_CAN_GET_DATA, seq, &data, &data2) < 0)
{
LogUtility::Log(LOG_LEVEL_ERROR, "McuMessageHandler::ProcessGetCanData out of order.");
return;
}
for(int i = 0; i < 16; i ++)
{
vrecord.push_back(MCU_GET_BYTE(m_rxPacket, 7 + i));
}
int dir = GpsLocationMessageHandler::Instance()->GetDirection();
if(dir == 0)
{
vrecord.push_back(0x00);
}
else
{
vrecord.push_back(0x01);
}
CanDataWriteToFile(vrecord);
return;
}
DMPAPI(unsigned int) usb_Read(void *vusb)
{
BYTE val;
DWORD pretime;
USB_Device *usb = (USB_Device *)vusb;
if (usb == NULL) { err_print((char*)"%s: USB device is null.\n", __FUNCTION__); return 0xffff; }
if (usb->TimeOut != USB_NO_TIMEOUT) {
pretime = timer_nowtime();
while (usb->rcvd->count <= 0 && (timer_nowtime() - pretime) < usb->TimeOut);
if (usb->rcvd->count <= 0) {
if (USB_TIMEOUT_DEBUG)
err_print((char*)"%s: USB device receive timeout.\n", __FUNCTION__);
return (unsigned int)0xffff;
}
}
else while (usb->rcvd->count <= 0);
io_DisableINT();
{
val = (BYTE)PopQueue(usb->rcvd);
}
io_RestoreINT();
if (!(io_inpdw(usb->EP[2].OutDLR) & 0x80000000L) && usb->rcvd->count < (usb->rcvd->size - NEAR_FULL_SIZE))
SetEPnDLR(usb, EP2, OUT, ENABLE | EP2_MAX_PACKET_SIZE_OUT);
return (unsigned int)val;
}
void McuMessageHandler::ProcessGetDeal(int len, int seq)
{
unsigned long data;
unsigned long data2;
vector<unsigned char> vrecord;
unsigned char dlen = MCU_GET_BYTE(m_rxPacket, 8);
if(len < 0x46)
{
LogUtility::Log(LOG_LEVEL_ERROR, "McuMessageHandler::ProcessGetDeal len invalid.");
return;
}
if(PopQueue(MCU_POS_GET_DEAL, seq, &data, &data2) < 0)
{
LogUtility::Log(LOG_LEVEL_ERROR, "McuMessageHandler::ProcessGetDeal out of order.");
return;
}
for(int i = 0; i < 96; i ++)
{
vrecord.push_back(MCU_GET_BYTE(m_rxPacket, 13 + i));
}
WriteToFile(vrecord);
m_passengerRecord[(int)data2].state = STATE_TIMER;
m_passengerRecord[(int)data2].posPassengerRecordTimer->timeout = TIMER_SEC_TO_MSEC(MCU_PASSENGER_INTERVAL);
StartTimer(m_passengerRecord[(int)data2].posPassengerRecordTimer);
return;
}
static int _recv_int(SerialPort *port, unsigned char* buf, int bsize)
{
int i;
unsigned long pretime;
for (i = 0; i < bsize; i++)
{
if (port->RxTimeOut < 0) {
while (QueueEmpty(port->rcvd));
} else {
pretime = timer_NowTime();
while (QueueEmpty(port->rcvd) && (timer_NowTime() - pretime) < port->RxTimeOut);
if (QueueEmpty(port->rcvd)) {
if (UART_TIMEOUT_DEBUG)
err_print((char*)"%s: COM%d receive timeout.\n", __FUNCTION__, port->com + 1);
break;
}
}
io_DisableINT();
{
buf[i] = PopQueue(port->rcvd);
switch (port->control)
{
case NO_CONTROL: break;
case RTS_CTS:
{
if (QueueSize(port->rcvd) < (RX_QUEUE_SIZE - NEAR_FULL_SIZE) && port->rts == 0) {
io_outpb(port->MCR, io_inpb(port->MCR) | 0x02);
port->rts = 1;
}
}
break;
case XON_XOFF:
{
if (QueueSize(port->rcvd) < (RX_QUEUE_SIZE - NEAR_FULL_SIZE) && port->xonxoff_xmit != XON_XMIT) {
port->xon = 1;
io_outpb(port->IER, port->ier |= 0x02);
}
}
break;
default: break;
};
}
io_RestoreINT();
}
return i;
}
void McuMessageHandler::ProcessCommonReply(int len, int id, int seq)
{
if(len != 1)
{
LogUtility::Log(LOG_LEVEL_ERROR, "McuMessageHandler::ProcessCommonReply len invalid.");
return;
}
int result = MCU_GET_BYTE(m_rxPacket, 7);
LogUtility::Log(LOG_LEVEL_DEBUG, "McuMessageHandler::ProcessCommonReply %d, %04x, %d.", seq, id, result);
PopQueue(id, seq, NULL, NULL);
}
// 二叉树的镜像非递归
void MirrorBinTreeNor(pBTNode pRoot)
{
LQueue q;
InitQueue(&q);
PushQueue(&q, pRoot);
while (!QueueEmpyt(&q)) {
pBTNode pCur = QueueFront(&q);
PopQueue(&q);
if (pCur->_pLeft)
PushQueue(&q, pCur->_pLeft);
if (pCur->_pRight)
PushQueue(&q, pCur->_pRight);
swap(&pCur->_pLeft, &pCur->_pRight);
}
}
// 层序遍历
void LevelOrder(pBTNode pRoot)
{
LQueue q;
InitQueue(&q);
PushQueue(&q, pRoot);
while (!QueueEmpyt(&q)) {
pBTNode pCur = QueueFront(&q);
PopQueue(&q);
printf("%c ", pCur->_data);
if(pCur->_pLeft)
PushQueue(&q, pCur->_pLeft);
if (pCur->_pRight)
PushQueue(&q, pCur->_pRight);
}
}
void McuMessageHandler::ProcessPosTime(int len, int seq)
{
if(len != 0x0A)
{
LogUtility::Log(LOG_LEVEL_ERROR, "McuMessageHandler::ProcessPosTime len invalid.");
return;
}
if(PopQueue(MCU_POS_TIME, seq, NULL, NULL) < 0)
{
LogUtility::Log(LOG_LEVEL_ERROR, "McuMessageHandler::ProcessPosTime out of order.");
return;
}
return;
}
void McuMessageHandler::ProcessPosStation(int len, int seq)
{
if(len < 10)
{
LogUtility::Log(LOG_LEVEL_ERROR, "McuMessageHandler::ProcessPosStation len invalid.");
return;
}
if(PopQueue(MCU_POS_STATION, seq, NULL, NULL) < 0)
{
LogUtility::Log(LOG_LEVEL_ERROR, "McuMessageHandler::ProcessPosStation out of order.");
return;
}
return;
}
/* Read bytes array with designated length from RX Queue. */
ssize_t USART_Read(USART_TypeDef *USARTx, void *buf, ssize_t l, uint8_t flags) {
ssize_t i = -1;
uint8_t *b;
b = buf;
if(USARTx == USART6) {
for(i=0; i<l; i++) {
if(!PopQueue(rxQueue, &(b[i]))){
break;
}
}
}
return i;
}
void KeyboardController::handleEvent(void) {
int scode = 0, pressFlag = 0;
// handle keyboard event:
// 1. press key
// 2. release key
if(kbQueue != NULL && QueueEmpty(kbQueue) == false)
{
scode = PopQueue(kbQueue);
pressFlag = scode & 0x80;
scode &= 0x7f;
if (pressFlag)
KeyboardEvent(false, scode);
else
KeyboardEvent(true, scode);
}
}
DMP_INLINE(void) EP2_InHandler(USB_Device *usb)
{
int len = 0;
if (usb->bulk_in_transmitting == true) return;
if (usb->xmit->count <= 0) return;
#ifdef DMP_86DUINO_MODE
TX_LED_ON();
#endif
do {
usb->EP[2].InBuf[len++] =(BYTE)PopQueue(usb->xmit);
} while(usb->xmit->count > 0 && len < EP2_MAX_PACKET_SIZE_IN);
#if defined DMP_DOS_DJGPP
dosmemput(usb->EP[2].InBuf, EP2_MAX_PACKET_SIZE_IN, usb->EP[2].InPhysical);
#endif
SetEPnDLR(usb, EP2, IN, ENABLE | len);
usb->bulk_in_transmitting = true;
}
int main(void)
{
PQ q = CreateQueue(10);
PushQueue(q, GetRand());
PushQueue(q, GetRand());
PushQueue(q, GetRand());
PushQueue(q, GetRand());
PrintQueue(q);
int i = 0;
for( ; i < 10; ++i)
{
PushQueue(q, GetRand());
PopQueue(q);
PrintQueue(q);
}
ReleaseQueue(q);
return 0;
}
void McuMessageHandler::ProcessGetPassenger(int len, int seq)
{
unsigned long data;
unsigned long data2;
if(len < 0x0A)
{
LogUtility::Log(LOG_LEVEL_ERROR, "McuMessageHandler::ProcessGetPassenger len invalid.");
return;
}
if(PopQueue(MCU_POS_GET_PASSENGER, seq, &data, &data2) < 0)
{
LogUtility::Log(LOG_LEVEL_ERROR, "McuMessageHandler::ProcessGetPassenger out of order.");
return;
}
if(data == (unsigned long)CLEAR_RECORD)
{
LogUtility::Log(LOG_LEVEL_INFO, "McuMessageHandler::ProcessGetPassenger get clear response %d.", (int)data2);
m_passengerRecord[(int)data2].passengerUpCount = 0;
m_passengerRecord[(int)data2].passengerDownCount = 0;
m_passengerRecord[(int)data2].posPassengerRecordTimer->timeout = TIMER_SEC_TO_MSEC(MCU_PASSENGER_INTERVAL),
StartTimer(m_passengerRecord[(int)data2].posPassengerRecordTimer);
return;
}
unsigned short dealnumber = MCU_GET_SHORT(m_rxPacket, 11);
unsigned short upnumber = MCU_GET_SHORT(m_rxPacket, 13);
unsigned short downnumber = MCU_GET_SHORT(m_rxPacket, 15);
LogUtility::Log(LOG_LEVEL_DEBUG, "McuMessageHandler::ProcessGetPassenger delnum:%d up:%d down: %d",dealnumber,upnumber,downnumber);
if((upnumber <= m_passengerRecord[(int)data2].passengerUpCount) &&
(downnumber <= m_passengerRecord[(int)data2].passengerDownCount) &&
((upnumber != 0) || (downnumber != 0)))
{
SendGetPosPassengerRecord((int)data2, CLEAR_RECORD);
}
else
{
m_passengerRecord[(int)data2].accuPassengerUpCount += upnumber;
m_passengerRecord[(int)data2].accuPassengerDownCount += downnumber;
m_passengerup_count += upnumber;
m_passengerdown_count += downnumber;
if((upnumber!=0)||(downnumber!=0))
{
SendPosPassengerNotify((int)data2, m_pos_addr[(int)data2],
m_passengerup_count, m_passengerdown_count);
}
}
m_passengerRecord[(int)data2].dealCount = dealnumber;
m_passengerRecord[(int)data2].passengerUpCount = upnumber;
m_passengerRecord[(int)data2].passengerDownCount = downnumber;
if(m_passengerRecord[(int)data2].lastDealCount == 0)
m_passengerRecord[(int)data2].lastDealCount = dealnumber;
LogUtility::Log(LOG_LEVEL_DEBUG, "McuMessageHandler::ProcessGetPassenger (%d,%d)(%d,%d)(%d,%d)",
m_passengerup_count,m_passengerdown_count,m_passengerRecord[(int)data2].accuPassengerUpCount,m_passengerRecord[(int)data2].accuPassengerDownCount,
dealnumber,m_passengerRecord[(int)data2].lastDealCount);
if(dealnumber && m_passengerRecord[(int)data2].lastDealCount < dealnumber)
{
m_passengerRecord[(int)data2].state = STATE_DEAL_SENT;
//SendGetDealRecord((int)data2, CLEAR_RECORD, dealnumber);
m_passengerRecord[(int)data2].lastDealCount ++;
SendGetDealRecord((int)data2, KEEP_RECORD, m_passengerRecord[(int)data2].lastDealCount);
}
else
{
m_passengerRecord[(int)data2].state = STATE_TIMER;
m_passengerRecord[(int)data2].posPassengerRecordTimer->timeout = TIMER_SEC_TO_MSEC(MCU_PASSENGER_INTERVAL);
StartTimer(m_passengerRecord[(int)data2].posPassengerRecordTimer);
}
return;
}
void McuMessageHandler::ProcessHandshake(int len, int seq)
{
if(len < 2)
{
LogUtility::Log(LOG_LEVEL_ERROR, "McuMessageHandler::ProcessHandshake len invalid.");
return;
}
if(PopQueue(MCU_PACKET_HANDSHAKE, seq, NULL, NULL) < 0)
{
LogUtility::Log(LOG_LEVEL_ERROR, "McuMessageHandler::ProcessHandshake out of order.");
return;
}
unsigned char flag = MCU_GET_BYTE(m_rxPacket, 7);
if(flag != 0x01)
{
LogUtility::Log(LOG_LEVEL_ERROR, "McuMessageHandler::ProcessHandshake invalid flag %d.", flag);
return;
}
unsigned char number = MCU_GET_BYTE(m_rxPacket, 8);
if((len < number + 1) || (number > MCU_MAX_POS))
{
LogUtility::Log(LOG_LEVEL_ERROR, "McuMessageHandler::ProcessHandshake number invalid %d.", number);
return;
}
m_pos_num = number;
LogUtility::Log(LOG_LEVEL_INFO, "McuMessageHandler::ProcessHandshake pos number %d.", m_pos_num);
for(int i = 0; i < number; i ++)
{
m_pos_addr[i] = MCU_GET_BYTE(m_rxPacket, 9 + i);
LogUtility::Log(LOG_LEVEL_INFO, "McuMessageHandler::ProcessHandshake pos %d addr: %d.", i, m_pos_addr[i]);
}
m_handshake = true;
m_passengerRecord = new StruPassengerRecord[m_pos_num];
for(int i = 0; i < m_pos_num; i ++)
{
m_passengerRecord[i].passengerUpCount = 0;
m_passengerRecord[i].accuPassengerUpCount = 0;
m_passengerRecord[i].passengerDownCount = 0;
m_passengerRecord[i].accuPassengerDownCount = 0;
m_passengerRecord[i].dealCount = 0;
m_passengerRecord[i].lastDealCount = 0;
m_passengerRecord[i].posPassengerRecordTimer = CreateTimer(MCU_PASSENGER_RECORD_TIMER,
TIMER_SEC_TO_MSEC(MCU_PASSENGER_INTERVAL + i),
this,
McuTimerHandler,
false);
m_passengerRecord[i].state = STATE_TIMER;
m_passengerRecord[i].posPassengerRecordTimer->data2 = (void *)i;
StartTimer(m_passengerRecord[i].posPassengerRecordTimer);
}
m_passengerup_count = 0;
m_passengerdown_count = 0;
StartTimer(m_dealReportTimer);
StartTimer(m_CanDataTimer);
StartTimer(m_CanUploadTimer);
return;
}
