list<string> Linker::link(register list<AssemblerRet> & asm_list)
{
list<string> error_list; // Error list.
for (register list<AssemblerRet>::iterator cur_obj = asm_list.begin(); cur_obj != asm_list.end(); cur_obj++)
{
for (register list<Label>::iterator cur_label = cur_obj->label_list.begin(); cur_label != cur_obj->label_list.end(); cur_label++) // Go through all the labels.
{
if (cur_label->isExtern) // Check if the label is extern.
{
register list<Label>::iterator * label_iter = findLabelInFiles(cur_label->label_name, asm_list, error_list); // Find a pointer to the label in another file.
if (!label_iter) // Is the label not found.
{
error_list.push_back(Assembler::getErrorMessage("Unresolved symbol " + cur_label->label_name, "Linking", 0, 0));
continue;
}
else if ((list<Label>::iterator *) label_iter < 0) continue; // There are multiple references for this label found.
cur_label->label_address = (*label_iter)->label_address; // Store the reference address.
/*
Go through all the generated codes and replace all the resolving ids
with the actual address.
*/
for (register list<char>::iterator cur_code = cur_obj->gen_code.begin(); cur_code != cur_obj->gen_code.end(); cur_code++)
{
register int cur_instruction_opcode = bytesToInt(new char[sizeof(int)]{ *cur_code, *(++cur_code), *(++cur_code), *(++cur_code) }); // Current opcode.
register Instruction * cur_instruction = findInstructionWithOpcode(cur_instruction_opcode); // Handle to current instruction.
register list<char>::iterator memory_begin; // Memory address pointer begin.
//if (!cur_instruction){ for (register int c = 0; c < 3; c++) ++cur_code; continue; } // Skip this instruction.
if (!cur_instruction) continue; // Skip this instruction.
if ((cur_instruction->instruction_type == INSTRUCTION_MEMORY) || (cur_instruction->instruction_type == INSTRUCTION_REGISTER_MEMORY) || (cur_instruction->instruction_type == (INSTRUCTION_MEMORY_REGISTER | INSTRUCTION_REGISTER_MEMORY)))
{
memory_begin = ++cur_code; // Setup the begin address.
register int memory_id = bytesToInt(new char[sizeof(int)]{ *cur_code, *(++cur_code), *(++cur_code), *(++cur_code) }); // Get the resolve id.
if (cur_label->resolve_id == memory_id) // Check if the resolve id matches.
{
/*
Replace the id with the address in the address pointer.
*/
*memory_begin = ((char *)&cur_label->label_address)[0];
*(++memory_begin) = ((char *)&cur_label->label_address)[1];
*(++memory_begin) = ((char *)&cur_label->label_address)[2];
*(++memory_begin) = ((char *)&cur_label->label_address)[3];
}
}
}
}
}
}
return error_list; // Return the list.
}
bool BTree::load() {
jobject page = env->CallObjectMethod(pagePoolProxy, midPagePoolProxyAcquire);
jboolean isCopy;
uint8_t *bytes;
jbyteArray buffer;
pageIds->clear();
env->SetIntField(page, fidPageId, klassIndexPageId);
pageIds->add(klassIndexPageId);
env->CallVoidMethod(pagedFile, midPagedFileReadPage, page);
buffer = (jbyteArray)env->GetObjectField(page, fidPageData);
bytes = (uint8_t*)env->GetByteArrayElements(buffer, &isCopy);
int type;
bytesToInt(type, bytes);
root = NULL;
if (type == BTreeElement::ELEMENT_NODE)
root = BTreeNode::fromBytes(bytes, nodeCapacity, this);
else if (type == BTreeElement::ELEMENT_LEAF)
root = BTreeLeaf::fromBytes(bytes, leafCapacity, this);
env->ReleaseByteArrayElements(buffer, (jbyte*)bytes, JNI_ABORT);
env->CallVoidMethod(pagePoolProxy, midPagePoolProxyRelease, page);
return true;
}
void IpcReader::read()
{
QMutexLocker locker(&m_Mutex);
std::cout << "ready read" << std::endl;
while (m_Socket->bytesAvailable()) {
std::cout << "bytes available" << std::endl;
char codeBuf[5];
readStream(codeBuf, 4);
codeBuf[4] = 0;
std::cout << "ipc read: " << codeBuf << std::endl;
if (memcmp(codeBuf, kIpcMsgLogLine, 4) == 0) {
std::cout << "reading log line" << std::endl;
char lenBuf[4];
readStream(lenBuf, 4);
int len = bytesToInt(lenBuf, 4);
char* data = new char[len];
readStream(data, len);
QString line = QString::fromUtf8(data, len);
delete data;
readLogLine(line);
}
else {
std::cerr << "aborting, message invalid" << std::endl;
return;
}
}
std::cout << "read done" << std::endl;
}
/**
* Auxiliary function for sensor reading with integer value
* @param packetID is the sensor reading indicated with packet ID
* @return integer value
*/
int Create::sensorIntReturn(char packetID) {
std::vector<char> output;
sensors(packetID, output);
return bytesToInt(output[0], output[1]);
}
int ReadKeySize(Key *key)
{
key->size = mtdData_new(key->offesetSize, SizeLength);
if (ReadMtd(key->size, false) < 0)
return 0;
return bytesToInt(key->size.data);
}
/*
主机发送查询监测数据指令
0x42 0x4d 0x01 0x00 0x00 0x00 0x90
模块返回数据：
0x42 0x4d 0x08 0x14 0x05 0x03 0x00 0x0a 0x00 0xbd
即 监测气体位甲醛 浓度为 0.10mg/m3
详细说明
0x42 0x4d 固定表示符号
0x08 后续发送 8 个字节数（包括自身）
0x14 监测类型表示 甲醛
0x05 数据单位标识 mg/m3
0x03 数据当量表示 100
0x00 0x0a 返回数据 0x000a = 10
实际数据=返回数据/数据当量=10/100=0.10
0x00 0xbd 数据校验高低字节
*/
float HchoSensor::read(void) {
byte cmdBytes[7] = {0x42, 0x4d, 0x01, 0x00, 0x00, 0x00, 0x90};
serialHCHO->write(cmdBytes,7);
serialHCHO->listen();
serialHCHO->overflow();
for ( int attempts = 0; attempts < MAX_ATTEMPTS; attempts++ ) {
//批量读取数据源10个字节
int i = 0;
while (serialHCHO->available()) {
//Serial.print(i);
bufferBytes[i] = serialHCHO->read();
//Serial.print(bufferBytes[i],HEX);
//Serial.print(" ");
//起始确认，有可能不是，得根据后续的校验
if(i==4){
if(bufferBytes[0]==0x42&&bufferBytes[1]==0x4d&&bufferBytes[2]==0x08&&bufferBytes[3]==0x14&&bufferBytes[4]==0x05){
//起始符号正确
}else{
i--; //读取5个字节后，验证，如果不正确，则回退一位重新开始
bufferBytes[0] = bufferBytes[1];
bufferBytes[1] = bufferBytes[2];
bufferBytes[2] = bufferBytes[3];
bufferBytes[3] = bufferBytes[4];
bufferBytes[4] = NULL;
}
}
if(i>=9){
lastHCHO = bytesToInt(bufferBytes[6], bufferBytes[7]);
if(bufferBytes[5]==0x01){
lastHCHO = lastHCHO / 1.00;
}else if(bufferBytes[5]==0x02){
lastHCHO = lastHCHO / 10.00;
}else if(bufferBytes[5]==0x03){
lastHCHO = lastHCHO / 100.00;
}else if(bufferBytes[5]==0x04){
lastHCHO = lastHCHO / 100.00;
}
//Serial.print("lastHCHO=");
//Serial.println(lastHCHO);
return lastHCHO;
}
i++;
delay(10);
}
//*
Serial.print("attempts=");
Serial.print(attempts);
Serial.println("___");
Serial.println(i);
//*/
}
return -1.0;
}
void CampoCadena::inicializarConRegistro(Byte * registro) {
// los primeros 4 bytes corresponden a la longitud del string
Byte * regTamDescripcion;
int inicio = 0;
int tamRegTamDescripcion = sizeof(int);
obtenerPorcion(registro, ®TamDescripcion, inicio, tamRegTamDescripcion);
int tamDescripcion = bytesToInt(regTamDescripcion);
// conocida la longitud del string, procedo a leer esa cantidad de bytes
Byte * regDescripcion;
inicio = tamRegTamDescripcion; //incremento la posicion de lectura
obtenerPorcion(registro, ®Descripcion, inicio, tamDescripcion);
// dado que string y char * o en su defecto Byte * son el mismo tipo,
// la asignacion es directa
descripcion = regDescripcion; // inicializo descripcion
}
bool XMLRPC::bytesToStringList(t_strList* _destination)
{
t_int list_size = 0;
_destination->clear();
// First, get the size of the list:
BTG_ICHECK( bytesToInt(&list_size) );
for (t_int i=0; i<list_size; i++)
{
std::string element("");
BTG_ICHECK(bytesToString(&element));
_destination->push_back(element);
}
return true;
}
//解讀封包
void AHRS::handle_packet(Packet *packet){
uint8_t channel = packet->Ch_Status;
uint8_t command = packet->PacketType;
uint8_t datalength = packet->DataLength;
uint8_t CRC = packet->CRC8;
if (CRC = Check_CRC8(*packet) && command == 0x0f){
/*
*********Controller_Status*********
Count 1
Position Target 4
Motor position 4
Velocity External 2
Velocity internal 2
Motor velocity 2
torque external 2
torque internal 2
Mortor torque 2
dutycycle 1
*/
unsigned char a[4];
unsigned char b[2];
unsigned char c[1];
//Count
c[0] = packet->Data[0];
data->Controller_Status[channel][0] = (int*)bytesTounsignedchar(c, 1);
//Position Target
a[3] = packet->Data[1];
a[2] = packet->Data[2];
a[1] = packet->Data[3];
a[0] = packet->Data[4];
data->Controller_Status[channel][1] = (int*)bytesToInt(a, 4);
//Motor position
a[3] = packet->Data[5];
a[2] = packet->Data[6];
a[1] = packet->Data[7];
a[0] = packet->Data[8];
data->Controller_Status[channel][2] = (int*)bytesToInt(a, 4);
//Velocity External
b[1] = packet->Data[9];
b[0] = packet->Data[10];
data->Controller_Status[channel][3] = (int*)bytesToShort(b, 4);
//Velocity internal
b[1] = packet->Data[11];
b[0] = packet->Data[12];
data->Controller_Status[channel][4] = (int*)bytesToShort(b, 4);
//Motor velocity
b[1] = packet->Data[13];
b[0] = packet->Data[14];
data->Controller_Status[channel][5] = (int*)bytesToShort(b, 4);
//torque external
b[1] = packet->Data[15];
b[0] = packet->Data[16];
data->Controller_Status[channel][6] = (int*)bytesToShort(b, 4);
//torque internal
b[1] = packet->Data[17];
b[0] = packet->Data[18];
data->Controller_Status[channel][7] = (int*)bytesToShort(b, 4);
//Mortor torque
b[1] = packet->Data[19];
b[0] = packet->Data[20];
data->Controller_Status[channel][8] = (int*)bytesToShort(b, 4);
//dutycycle
c[0] = packet->Data[21];
data->Controller_Status[channel][9] = (int*)bytesTounsignedchar(c, 1);
//view->refresh(data);
}
else{
}
/*
int *x = 0;
unsigned char a[4];
a[3] = packet->Data[5];
a[2] = packet->Data[6];
a[1] = packet->Data[7];
a[0] = packet->Data[8];
*x = *(int *)a;
data->Controller_Status[channel][0] = x;
*/
//view->refresh(data);
};
/*
Envoi au serveur une demande de connexion via une requète 'R_demandeCo'
*/
int demandeDeConnexion(client *cl){
int nbrtentativeReco = 0;
int nbrMaxTentative = 3;
int erreur;
char buffer[R_tailleMaxReq];
memset(buffer,0,R_tailleMaxReq);
requete reqSend;
requete reqRecv;
// Initialise une requète de type R_demandeCo et la converti en octets
initRequete(&reqSend,R_demandeCo, R_idNull, 0,NULL);
requeteToBytes(buffer,reqSend);
/*
tentative de connexion au serveur.
Si le serveur ne répond pas, réessaye un certain nombre de fois
*/
do{
erreur = sendto(cl->fdSocket, buffer, sizeofReq(reqSend),
0, (struct sockaddr*) &(cl->addrSend), sizeof(struct sockaddr_in));
if (erreur < 0){
perror("Erreur lors de l'appel à demandeDeConnexion, sendto renvois une erreur");
return -1;
}
erreur = definirTimeOut(cl->fdSocket, C_timeoutDemandeCo);
if (erreur < 0){
perror("Erreur lors de l'appel à demandeDeConnexion, problème lors du select");
return -2;
}else if(erreur == 0){
nbrtentativeReco++;
printf("le serveur ne répond pas, tentative n°%d sur %d\n", nbrtentativeReco, nbrMaxTentative);
}else{/* si le serveur à reçu la demande de connexion */
socklen_t fromlen = sizeof(struct sockaddr_in);
erreur = recvfrom(cl->fdSocket,buffer,R_tailleMaxReq,
0,(struct sockaddr*) &(cl->addrRecv),&fromlen);
if (erreur < 0){
perror("Erreur lors de l'appel à demandeDeConnexion, recvfrom renvois une erreur");
return -3;
}
initRequeteFromBytes(&reqRecv, buffer);
int id = 0;
/* si le serveur répond ok à la demande de connexion, récupérer l'id
se trouvant dans la partie data de la requète reçu*/
if (reqRecv.typeReq == R_okDemandeCo){
id = bytesToInt(reqRecv.data);
cl->id = id;
printf("Connexion effectuée\n");
return 0;
}else if(reqRecv.typeReq == R_serverPlein){
close(cl->fdSocket);
printf("Impossible de ce connecter au serveur, le serveur est plein\n");
return -4;
}else{
printf("Requète reçu non prévue lors de la demande de connexion : %d",reqRecv.typeReq);
return -5;
}
}
} while (nbrtentativeReco < nbrMaxTentative);
printf("Echec de demande de connexion au serveur, aucune réponse\n");
return -6;
}
/*
Envoi une requète de type R_demanderFicherAudio au serveur, nomFichier correspond au nom du fichier
que le client souhaite avoir et sera stocké dans la partie data de la requète
Les varaibles rate, size et channels seront modifier en fonction de se qu'envois le serveur
*/
int demanderFichierAudio(client *cl, char *nomFichier, int *rate, int *size, int *channels){
int nbrtentativeDeco = 0;
int nbrMaxTentative = 5;
int erreur;
char buffer[R_tailleMaxReq];
memset(buffer,0,R_tailleMaxReq);
requete reqSend;
requete reqRecv;
// Initialise une requète de type R_demanderFicherAudio et la converti en octets
initRequete(&reqSend, R_demanderFicherAudio, cl->id, strlen(nomFichier)+1,nomFichier);
requeteToBytes(buffer,reqSend);
do{
erreur = sendto(cl->fdSocket, buffer, sizeofReq(reqSend),
0, (struct sockaddr*) &(cl->addrSend), sizeof(struct sockaddr_in));
if (erreur < 0){
perror("Erreur lors de l'appel à demanderFichierAudio, sendto renvois une erreur");
return -1;
}
erreur = definirTimeOut(cl->fdSocket, C_timeoutDemandeFichier);
if (erreur < 0){
perror("Erreur lors de l'appel à demanderFichierAudio, problème lors du select");
return -2;
}else if(erreur == 0){
nbrtentativeDeco++;
printf("Le serveur ne répond pas, tentative n°%d\n", nbrtentativeDeco);
}else{/* si le serveur à reçu la demande de connexion */
socklen_t fromlen = sizeof(struct sockaddr_in);
erreur = recvfrom(cl->fdSocket,buffer,R_tailleMaxReq,
0,(struct sockaddr*) &(cl->addrRecv),&fromlen);
if (erreur < 0){
perror("Erreur lors de l'appel à demanderFichierAudio, recvfrom renvois une erreur");
return -3;
}
// Initialise une requète avec les octets envoyer par le serveur
initRequeteFromBytes(&reqRecv, buffer);
if (reqRecv.typeReq == R_okDemanderFichierAudio){
/*
Le serveur possède bien ce fichier
La partie data de la requète envoyer par le serveur est composé de 3 information :
rate se trouve dans l'interval [0,sizeof(int)[
size se trouve dans l'interval [sizeof(int),sizeof(int)*2[
channels se trouve dans l'interval [sizeof(int)*2,sizeof(int)*3[
*/
*rate = bytesToInt(reqRecv.data);
*size = bytesToInt(reqRecv.data+sizeof(int));
*channels = bytesToInt(reqRecv.data+sizeof(int)*2);
return 0;
}else if(reqRecv.typeReq == R_fichierAudioNonTrouver){
printf("Le serveur ne possède pas ce fichier\n");
return -6;
}else if(reqRecv.typeReq == R_idInexistant){
printf("Le serveur ne reconnais pas l'id qui à été envoyer lors de la demande d'info du fichier audio\n");
return -5;
}else{
printf("La requète reçu en réponse à la demande de fichier n'est pas prévue: %d\n",reqRecv.typeReq);
return -4;
}
}
} while (nbrtentativeDeco < nbrMaxTentative);
printf("Echec de demande de fichier, aucune réponse\n");
return -5;
}
int main()
{
QFile testfile("test.mobi");
testfile.open(QIODevice::ReadOnly);
QDataStream inStream(&testfile);
/* file starts with pdb header,
* 4bytes at 60 should be 'BOOK'
* 4bytes at 64 should be 'MOBI'
* if file is a valid .mobi
*/
//goto pos 60
if (!testfile.seek(60)) {
printf("failed to seek to offset 60\n");
return 0;
}
//read 8 bytes
char * tmpchar = new char[8];
if (inStream.readRawData(tmpchar,8) != 8) {
printf("failed to read 8 bytes\n");
return 0;
}
//check'em
if (strncmp(tmpchar,"BOOKMOBI",8) == 0)
printf ( "got MOBI header\n");
/* position 76 has number of PDB records
* records follow straight after
*
*/
//goto pos 76
testfile.seek(76);
delete tmpchar;
tmpchar = new char[2];
unsigned int records = 0 ;
if (inStream.readRawData(tmpchar,2) == 2) {
records = bytesToInt(tmpchar,0,2) ; //needs a batter way to figure it out
printf ( "mobi records : %d \n", records);
}
printf("parsing record0\n");
//records start at 78 and take up 8*records bytes of data
//http://wiki.mobileread.com/wiki/PDB#Palm_Database_Format
testfile.seek(78);
delete tmpchar;
tmpchar = new char[8];
//4b - record offset from start of file
//1b - record flags
//3b - record id
testfile.seek(testfile.pos() + 8*records);
/* for (int i=0;i<records;i++) {
printf("record %d\n", i);
if (inStream.readRawData(tmpchar,8) != 8) {
printf("file ended prematurely. exiting\n");
return 0;
}
//printbytes(tmpchar,8);
unsigned int offset = bytesToInt(tmpchar,0,4) ; //needs a batter way to figure it out
int flags = tmpchar2[4];
unsigned int record_id = bytesToInt(tmpchar,5,2);
printf ( " -- offset : %0x [%d] \n", offset, offset);
printf ( " -- flags : %0x [%d] \n", flags, flags);
printf ( " -- record_id : %0x [%d] \n", record_id, record_id);
}
*/
/* there are 2 bytes of padding after records, skip them */
testfile.seek(testfile.pos() + 2);
/* 16 byte palmdoc header starts here, read it */
delete tmpchar;
//save this location as reference for later
qint64 header0pos = testfile.pos();
/* tmpchar = new char[16];
testresult = inStream.readRawData(tmpchar,16);
if (testresult == 16) {
printbytes(tmpchar,16);
//we don't really need this, just for verification
printf("compression : %0x \n",bytesToInt(tmpchar,0,2));
printf("dummy : %0x (expected 0)\n",bytesToInt(tmpchar,2,2));
printf("textLength : %0x \n",bytesToInt(tmpchar,4,4));
printf("recCount : %0x \n",bytesToInt(tmpchar,8,2));
printf("recSize : %0x (expected 0x1000)\n",bytesToInt(tmpchar,10,2));
printf("encType : %0x \n",bytesToInt(tmpchar,12,2));
printf("unk : %0x \n",bytesToInt(tmpchar,14,2));
}
*/
/* instead of parsing, we just go over it and skip to record #0 that follows */
testfile.seek(testfile.pos() + 16);
//go through MOBI header
//first let's see if we have a header, 4 bytes should be 'MOBI', 16 bytes from beginning of PalmDoc header
tmpchar = new char[4];
if ((inStream.readRawData(tmpchar,4) == 4) && (strncmp(tmpchar,"MOBI",4) == 0))
printf("got MOBI header in record 0\n");
//next up is header length that includes that 'MOBI' signature just read.
unsigned int mobiHeaderSize = 0;
if (inStream.readRawData(tmpchar,4) == 4) {
mobiHeaderSize = bytesToInt(tmpchar,0,4);
}
printf("mobi header size : %d [ %0x ]\n", mobiHeaderSize, mobiHeaderSize);
printf("EXTH should start at %llx\n", mobiHeaderSize + header0pos + 0x10); //add 16 bytes for palmdoc header parsed previously
//check if EXTH record exists
testfile.seek(header0pos + 0x80) ; //location of EXTH flags in header;
bool got_exth_header = false;
if (inStream.readRawData(tmpchar,4) == 4)
{
// printbytes(tmpchar,4);
unsigned int exth_flags = bytesToInt(tmpchar,0,4);
if ( exth_flags & 0x40)
got_exth_header = true;
}
if (!got_exth_header) {
printf("EXTH not found, exiting\n");
return 0;
}
printf("EXTH header exists\n");
//if EXTH header size is not divisible by 4, it has to be padded to a multiple of 4, and then actual data starts.
int exth_padding = 0;
if (mobiHeaderSize % 4 )
exth_padding = 4 - mobiHeaderSize % 4 ;
printf(" exth padding : %d\n", exth_padding);
//go through EXTH header, if found (indicated in MOBI header)
//navigating to start of EXTH
printf("seeking to exth header\n");
qint64 exth_pos = header0pos + mobiHeaderSize+0x10; //first palmdoc entry offset + 16bytes + entire mobi header is where exth starts
testfile.seek(exth_pos);
printf("at position %llu [ %llx ]\n", testfile.pos(), testfile.pos());
/*
'EXTH'
4 bytes - length of entire header
4 bytes - number of records
< records follow >
record {
4 bytes type
4 bytes length of entire record (that is, including 8 bytes for type and length)
length-8 actual record data
}
0-3 bytes padding (in exth_padding)
*/
delete tmpchar;
tmpchar = new char [12];
printf("reading 12 bytes\n");
if (inStream.readRawData(tmpchar,12) == 12) {
// printbytes(tmpchar,12);
if (strncpy(tmpchar,"EXTH",4) == 0)
printf("got EXTH header\n");
unsigned int headerlength = bytesToInt(tmpchar,4,4);
unsigned int exthRecords = bytesToInt(tmpchar,8,4);
printf("header is %x long \n", headerlength);
printf("there are %x records \n",exthRecords);
//go through the EXTH records
for (unsigned int j=0;j<exthRecords;j++) {
char * tmprecord = new char [8];
if (inStream.readRawData(tmprecord,8) ==8 ) {
// printf("record %d/%d\n",j,exthRecords);
unsigned int recType = bytesToInt(tmprecord,0,4);
// printf(" type : %d\n",recType);
unsigned int recLength = bytesToInt(tmprecord,4,4);
// printf(" length : %0x\n", recLength);
char * recordData = new char[(int)recLength - 8];
if (inStream.readRawData(recordData,(int)recLength - 8) == (int)recLength -8) {
// printf(" data : %s\n", recordData);
// printbytes(recordData,(int)recLength -8);
}
switch (recType) {
case 100:
printf("author : %s \n", recordData);
break;
case 106:
printf("date : %s \n", recordData);
break;
case 105:
printf("genre : %s \n", recordData);
break;
case 503:
printf("title : %s \n", recordData);
break;
case 524:
printf("lang : %s \n", recordData);
break;
default:
break;
}
}
delete tmprecord;
}
}
return 0;
}
void CRemoteInterface::handleCommand(EMessage cmd)
{
if (cmd == MSG_CMD_MOTORSPEED)
{
motorControl.update = true;
motorControl.leftSpeed = tempSerialMsgBuffer.pop();
motorControl.rightSpeed = tempSerialMsgBuffer.pop();
motorControl.mdirLeft = static_cast<EMotorDirection>(tempSerialMsgBuffer.pop());
motorControl.mdirRight = static_cast<EMotorDirection>(tempSerialMsgBuffer.pop());
motorControl.duration = bytesToInt(tempSerialMsgBuffer.pop(), tempSerialMsgBuffer.pop());
motorControl.distance = 0;
motorControl.moveType = SMotorControl::MOVE_STRAIGHT;
}
else if (cmd == MSG_CMD_DRIVEDIST)
{
motorControl.update = true;
motorControl.driveSpeed = tempSerialMsgBuffer.pop();
motorControl.distance = bytesToInt(tempSerialMsgBuffer.pop(), tempSerialMsgBuffer.pop());
motorControl.driveDir = static_cast<EMotorDirection>(tempSerialMsgBuffer.pop());
motorControl.duration = 0;
motorControl.moveType = SMotorControl::MOVE_STRAIGHT;
}
else if (cmd == MSG_CMD_TURN)
{
motorControl.update = true;
motorControl.turnSpeed = tempSerialMsgBuffer.pop();
motorControl.turnDir = static_cast<ETurnDirection>(tempSerialMsgBuffer.pop());
motorControl.duration = bytesToInt(tempSerialMsgBuffer.pop(), tempSerialMsgBuffer.pop());
motorControl.moveType = SMotorControl::MOVE_TURN;
motorControl.angle = 0;
}
else if (cmd == MSG_CMD_TURNANGLE)
{
motorControl.update = true;
motorControl.turnSpeed = tempSerialMsgBuffer.pop();
motorControl.angle = bytesToInt(tempSerialMsgBuffer.pop(), tempSerialMsgBuffer.pop());
motorControl.turnDir = static_cast<ETurnDirection>(tempSerialMsgBuffer.pop());
motorControl.duration = 0;
motorControl.moveType = SMotorControl::MOVE_TURN;
}
#ifdef MECANUM_MOVEMENT
else if (cmd == MSG_CMD_TRANSLATE)
{
motorControl.update = true;
motorControl.translateSpeed = tempSerialMsgBuffer.pop();
motorControl.translateDir = static_cast<ETranslateDirection>(tempSerialMsgBuffer.pop());
motorControl.duration = bytesToInt(tempSerialMsgBuffer.pop(), tempSerialMsgBuffer.pop());
motorControl.moveType = SMotorControl::MOVE_TRANSLATE;
motorControl.distance = 0;
}
else if (cmd == MSG_CMD_TRANSLATEDIST)
{
motorControl.update = true;
motorControl.translateSpeed = tempSerialMsgBuffer.pop();
motorControl.distance = bytesToInt(tempSerialMsgBuffer.pop(), tempSerialMsgBuffer.pop());
motorControl.translateDir = static_cast<ETranslateDirection>(tempSerialMsgBuffer.pop());
motorControl.duration = 0;
motorControl.moveType = SMotorControl::MOVE_TRANSLATE;
}
#endif
else if (cmd == MSG_CMD_STOP)
{
motorControl.update = true;
motorControl.moveType = SMotorControl::MOVE_NONE;
}
else if (cmd == MSG_CMD_FRONTLEDS)
{
digitalWrite(PIN_FRONTLEDS, tempSerialMsgBuffer.pop());
}
}
bool Pm25HardSerial::read(void) {
for ( int attempts = 0; attempts < PM25_MAX_ATTEMPTS; attempts++ ) {
//批量读取数据源 PM25_BUFFER_SIZE 个字节
int i = 0;
while (serialPM25->available()) {
bufferBytes[i] = serialPM25->read();
//Serial.print(bufferBytes[i],HEX);
//起始确认，有可能不是，得根据后续的校验
if(i==3){
if(bufferBytes[0]==0x42&&bufferBytes[1]==0x4d&&bufferBytes[2]==0x00&&bufferBytes[3]==PM25_BUFFER_SIZE-4){
//起始符号正确
}else{
i--; //读取4个字节后，验证，如果不正确，则回退一位重新开始
bufferBytes[0] = bufferBytes[1];
bufferBytes[1] = bufferBytes[2];
bufferBytes[2] = bufferBytes[3];
bufferBytes[3] = NULL;
}
}
if(i>=PM25_BUFFER_SIZE-1){
int checksum = 0;
for(int j=0;j<=PM25_BUFFER_SIZE-3;j++){
checksum += (int)bufferBytes[j];
//Serial.println(j);
}
//Serial.println(checksum);
//Serial.println(bytesToInt(bufferBytes[PM25_BUFFER_SIZE-2], bufferBytes[PM25_BUFFER_SIZE-1]));
if(checksum == bytesToInt(bufferBytes[PM25_BUFFER_SIZE-2], bufferBytes[PM25_BUFFER_SIZE-1])){
lastPM10 = bytesToInt(bufferBytes[10], bufferBytes[11]);
lastPM25 = bytesToInt(bufferBytes[12], bufferBytes[13]);
lastPM100 = bytesToInt(bufferBytes[14], bufferBytes[15]);
lastHCHO = ((float)(bytesToInt(bufferBytes[28], bufferBytes[29]))) / 1000.0;
if(PM25_BUFFER_SIZE>=40){
lastT = ((float)(bytesToInt(bufferBytes[30], bufferBytes[31]))) / 10.0;
lastH = ((float)(bytesToInt(bufferBytes[32], bufferBytes[33]))) / 10.0;
}else{
lastT = 0.0;
lastH = 0.0;
}
//Serial.println("true");
return true;
}else{
return false;
}
}
i++;
delay(10);
}
/*
Serial.print("attempts=");
Serial.print(attempts);
Serial.println("___");
Serial.println(i);
*/
}
return false;
}
void initiateSound(Xint32 soundIndex, Xint32 loop)
{
Xint32 compressed;
Xint32 numChannels;
Xint32 sampleRate;
Xint32 blockAlign;
Xint32 sigBitsPerSample;
Xint32 dataSize;
Xint32 fmtSize;
Xint32 dataStartAddress;
Xint8* fileLocation = soundAddresses[soundIndex];
//check to make sure size of block is correct - 28 is size without extra fmt chunks info
fmtSize = bytesToInt(&fileLocation[16], 4, LITTLE_ENDIAN);
compressed = (bytesToInt(&fileLocation[20], 2, LITTLE_ENDIAN) == 1) ? 0 : 1;//not compressed if code is 1 else is compressed
numChannels = bytesToInt(&fileLocation[22], 2, LITTLE_ENDIAN);
sampleRate = bytesToInt(&fileLocation[24], 4, LITTLE_ENDIAN);
blockAlign = bytesToInt(&fileLocation[32], 2, LITTLE_ENDIAN);
sigBitsPerSample = bytesToInt(&fileLocation[34], 2, LITTLE_ENDIAN);
//TODO: Add checks to make sure we are getting right data - i.e. if location == "data"
//get data size
// if(fmtSize != 16)
// {
// xil_printf("fmt block not 16\r\n");
// return;
// }
if (compressed == 1)
{
xil_printf("sound file is compressed\r\n");
return;
}
if (numChannels != 1)
{
xil_printf("sound file not single channel\r\n");
return;
}
dataSize = bytesToInt(&fileLocation[24 + fmtSize], 4, LITTLE_ENDIAN);
// else
// dataSize = bytesToInt(&fileLocation[40], 4, LITTLE_ENDIAN);
#ifdef DEBUG
xil_printf("--fileInfo: %x\r\n", fileLocation);
xil_printf("compressed: %d\r\n", compressed);
xil_printf("numChannels: %d\r\n", numChannels);
xil_printf("sampleRate: %d\r\n", sampleRate);
xil_printf("blockAlign: %d\r\n", blockAlign);
xil_printf("sigBitsPerSample: %d\r\n", sigBitsPerSample);
xil_printf("dataSize: 0x%x\r\n", dataSize);
#endif
// Program the AC97 for the correct sample rate
// TODO: How do we want to handle Sample rate?
// TODO: This will take the last initalized sound and program the AC97 with that - assumes they are all the same rate
//XAC97_WriteReg(XPAR_AUDIO_CODEC_BASEADDR, AC97_PCM_DAC_Rate, sampleRate);
dataStartAddress = (Xint32)(fileLocation + 44); // 44 is the offset between the fileLocation and the data section
//Program DMA Registers
switch(soundIndex)
{
case 0:
{
Xil_Out32(CHANNEL_0_SOURCE_ADDRESS, dataStartAddress);
Xil_Out32(CHANNEL_0_LENGTH_ADDRESS, dataSize);
Xil_Out32(CHANNEL_0_LOOP_ADDRESS, loop);
break;
}
case 1:
{
Xil_Out32(CHANNEL_1_SOURCE_ADDRESS, dataStartAddress);
Xil_Out32(CHANNEL_1_LENGTH_ADDRESS, dataSize);
Xil_Out32(CHANNEL_1_LOOP_ADDRESS, loop);
break;
}
case 2:
{
Xil_Out32(CHANNEL_2_SOURCE_ADDRESS, dataStartAddress);
Xil_Out32(CHANNEL_2_LENGTH_ADDRESS, dataSize);
Xil_Out32(CHANNEL_2_LOOP_ADDRESS, loop);
break;
}
case 3:
{
Xil_Out32(CHANNEL_3_SOURCE_ADDRESS, dataStartAddress);
Xil_Out32(CHANNEL_3_LENGTH_ADDRESS, dataSize);
Xil_Out32(CHANNEL_3_LOOP_ADDRESS, loop);
break;
}
case 4:
{
Xil_Out32(CHANNEL_4_SOURCE_ADDRESS, dataStartAddress);
Xil_Out32(CHANNEL_4_LENGTH_ADDRESS, dataSize);
Xil_Out32(CHANNEL_4_LOOP_ADDRESS, loop);
break;
}
case 5:
{
Xil_Out32(CHANNEL_5_SOURCE_ADDRESS, dataStartAddress);
Xil_Out32(CHANNEL_5_LENGTH_ADDRESS, dataSize);
Xil_Out32(CHANNEL_5_LOOP_ADDRESS, loop);
break;
}
case 6:
{
Xil_Out32(CHANNEL_6_SOURCE_ADDRESS, dataStartAddress);
Xil_Out32(CHANNEL_6_LENGTH_ADDRESS, dataSize);
Xil_Out32(CHANNEL_6_LOOP_ADDRESS, loop);
break;
}
case 7:
{
Xil_Out32(CHANNEL_7_SOURCE_ADDRESS, dataStartAddress);
Xil_Out32(CHANNEL_7_LENGTH_ADDRESS, dataSize);
Xil_Out32(CHANNEL_7_LOOP_ADDRESS, loop);
break;
}
case 8:
{
Xil_Out32(CHANNEL_8_SOURCE_ADDRESS, dataStartAddress);
Xil_Out32(CHANNEL_8_LENGTH_ADDRESS, dataSize);
Xil_Out32(CHANNEL_8_LOOP_ADDRESS, loop);
break;
}
case 9:
{
Xil_Out32(CHANNEL_9_SOURCE_ADDRESS, dataStartAddress);
Xil_Out32(CHANNEL_9_LENGTH_ADDRESS, dataSize);
Xil_Out32(CHANNEL_9_LOOP_ADDRESS, loop);
break;
}
default:
{
xil_printf("Error in initiateSound\r\n");
}
}
}
//Little endian: first byte is least significant.
uint32_t BitManipulation::separateBytesToInt(uint8_t byte0, uint8_t byte1, uint8_t byte2, uint8_t byte3 ){
uint8_t bytes[] = {byte0,byte1,byte2,byte3};
return bytesToInt(bytes);
}
void LiveViewConsumer::buildLiveView(){
//LOG_LIVEVIETHREAD_DEBUG << "\n++++++++++++++++++++++++++++++++++++++";
//LOG_LIVEVIETHREAD_DEBUG << "buildLiveViewPic";
//LOG_LIVEVIETHREAD_DEBUG<< "offset" << offset << "starttime: " << starttime << "endtime: " << endtime;
QByteArray array;
#ifdef __USE_STANDARD_HEADER
int commonHeaderLength = 1 + 1 + 2 + 4;
int payloadHeaderLength = 4 + 3 + 1 + 4 + 1 + 115;
// Header = 136 Bytes;
commonHeaderLength =commonHeaderLength;
payloadHeaderLength = payloadHeaderLength;
jpegSize = 0;
offset = 0;
mutex.lock();
bufferIsNotEmpty.wait(&mutex);
//#define _USECHAR //! for Testing only
#ifdef _USECHAR
char consumerP[gRealLoadSize];
qint64 loadedBytes = ringBuffer->getData(consumerP,gRealLoadSize);
LOG_MIN_LIVEVIETHREAD_DEBUG << "";
LOG_MIN_LIVEVIETHREAD_DEBUG << "Consumer loadedBytes: " << loadedBytes;
QByteArray tmpArray;
tmpArray.resize(loadedBytes);
tmpArray.setRawData(consumerP,loadedBytes);
if(loadedBytes >0){
inputStream.append(tmpArray,loadedBytes);
}
if(loadedBytes < gRealLoadSize){
char consumerP[gRealLoadSize-loadedBytes];
loadedBytes = ringBuffer->getData(consumerP,gRealLoadSize-loadedBytes);
LOG_LIVEVIETHREAD_DEBUG << "loadedBytes 2: " << loadedBytes;
if(loadedBytes >0){
tmpArray.resize(loadedBytes);
tmpArray.setRawData(consumerP,loadedBytes);
inputStream.append(tmpArray);
}
}
#else
char tmpBuffer[gRealLoadSize];
qint64 loadedBytes = ringBuffer->getData(tmpBuffer,gRealLoadSize);
LOG_MIN_LIVEVIETHREAD_DEBUG << "";
LOG_MIN_LIVEVIETHREAD_DEBUG << "Consumer loadedBytes: " << loadedBytes;
if(loadedBytes >0){
inputStream.append(tmpBuffer,loadedBytes);
}
if(loadedBytes < gRealLoadSize){
char tmpBuffer[gRealLoadSize-loadedBytes];
loadedBytes = ringBuffer->getData(tmpBuffer,gRealLoadSize-loadedBytes);
LOG_LIVEVIETHREAD_DEBUG << "loadedBytes 2: " << loadedBytes;
if(loadedBytes >0){
inputStream.append(tmpBuffer,loadedBytes);
}
}
#endif
LOG_LIVEVIETHREAD_DEBUG << "inputStream.size() 1: " << inputStream.size() << "\n";
if(!inputStream.isNull()){
while (offset < inputStream.length() -15) {
if (inputStream.at(offset) == (char) 0xFF) {
//LOG_LIVEVIETHREAD_DEBUG << "Start byte Found at: " << offset;
switch (inputStream.at(offset+1)) {
case (char) 0x12:
LOG_MIN_LIVEVIETHREAD_DEBUG << "Informati Header at: " << offset+1;
break;
case (char) 0x01:
LOG_MIN_LIVEVIETHREAD_DEBUG << "Start byte Found at: " << offset;
//offset+=8;
break;
case (char) 0x11:
LOG_MIN_LIVEVIETHREAD_DEBUG << "Payload at: " << offset+1;
//offset+=8;
break;
default:
break;
}
if (inputStream.at(offset+8) == (char) 0x24
&& inputStream.at(offset+9) == (char) 0x35
&& inputStream.at(offset+10) == (char) 0x68
&& inputStream.at(offset+11) == (char) 0x79) {
hjpegSize = bytesToInt(inputStream, offset+12, 3);
paddingSize = bytesToInt(inputStream, offset+15, 1);
LOG_MIN_LIVEVIETHREAD_DEBUG << "Payloadheader Found at: " << offset+8;
offset+=8;
offset+=128;
}
}
offset++;
//LOG_LIVEVIETHREAD_DEBUG << "offset: " << offset;
if(hjpegSize > 0 && hjpegSize < (inputStream.size() - offset)//){
&& inputStream.at(offset-1) == (char) 0xff
&& inputStream.at(offset) == (char)0xd8){
LOG_MIN_LIVEVIETHREAD_DEBUG << "hjpegSize: " << hjpegSize;
LOG_MIN_LIVEVIETHREAD_DEBUG << "paddingSize: " << paddingSize;
int arraysize = 0;
array.resize(hjpegSize);
for (; arraysize < hjpegSize; arraysize++,offset++){
array[arraysize] = inputStream.at(offset-1);
/*
if(array[arraysize] == (char)0xd9
&& array[arraysize-1] == (char)0xff){
LOG_MIN_LIVEVIETHREAD_DEBUG << "publishLiveViewBytes: " << arraysize;
offset+= hjpegSize-arraysize-1;
arraysize = hjpegSize;
}*/
}
emit publishLiveViewBytes(array);
inputStream.remove(0,offset-1);
LOG_MIN_LIVEVIETHREAD_DEBUG << "publishLiveViewBytes: " << arraysize;
LOG_MIN_LIVEVIETHREAD_DEBUG << "inputStream.size() : " << inputStream.size();
hjpegSize = 0;
paddingSize = 0;
offset = 0;
}
}//while
}// if not empty
mutex.unlock();
#else
//bool foundstart = false;
//bool foundend = false;
//starttime=0;
//endtime=0;
//foundstartOffset =0;
//LOG_LIVEVIETHREAD_DEBUG << "consumerBufferLen: " << consumerBufferLen;
//LOG_LIVEVIETHREAD_DEBUG << "ringBuffer->Free_Space(): " << ringBuffer->Free_Space();
//LOG_LIVEVIETHREAD_DEBUG << "ringBuffer->Buffered_Bytes(): " <<ringBuffer->Buffered_Bytes();
mutex.lock();
bufferIsNotEmpty.wait(&mutex);
char consumerP[gRealLoadSize];
qint64 loadedBytes = ringBuffer->getData(consumerP,gRealLoadSize);
LOG_LIVEVIETHREAD_DEBUG << "Consumer loadedBytes: " << loadedBytes;
QByteArray tmpArray;
tmpArray.resize(loadedBytes);
tmpArray.setRawData(consumerP,loadedBytes);
if(loadedBytes >0){
inputStream.append(tmpArray,loadedBytes);
}
if(loadedBytes < gRealLoadSize){
char consumerP[gRealLoadSize-loadedBytes];
loadedBytes = ringBuffer->getData(consumerP,gRealLoadSize-loadedBytes);
LOG_LIVEVIETHREAD_DEBUG << "loadedBytes 2: " << loadedBytes;
if(loadedBytes >0){
tmpArray.resize(loadedBytes);
tmpArray.setRawData(consumerP,loadedBytes);
inputStream.append(tmpArray);
}
}
LOG_LIVEVIETHREAD_DEBUG << "inputStream.size() 1: " << inputStream.size() << "\n";
if(!inputStream.isEmpty()){
//LOG_LIVEVIETHREAD_DEBUG << "!inputStream.isEmpty()" << inputStream.size();
//! search in Buffer, perhaps we have more than one image
while (offset < (inputStream.length() -1)) {
//! suche nach Bildanfang ab Stelle offset
while(!foundstart && offset < (inputStream.length() -1) ){
if (inputStream.at(offset) == (char) 0xFF) {
LOG_LIVEVIETHREAD_DEBUG << "Start byte Found at: " << offset;
switch (inputStream.at(offset+1)) {
case (char) 0x12:
LOG_LIVEVIETHREAD_DEBUG << "Information Header at: " << offset+1;
break;
case (char) 0x01:
LOG_LIVEVIETHREAD_DEBUG << "Nothing at: " << offset+1;
break;
case (char) 0x11:
LOG_LIVEVIETHREAD_DEBUG << "Payload at: " << offset+1;
break;
default:
break;
}
if (inputStream.at(offset+8) == (char) 0x24
&& inputStream.at(offset+9) == (char) 0x35
&& inputStream.at(offset+10) == (char) 0x68
&& inputStream.at(offset+11) == (char) 0x79) {
//char calculatejpeg[3];
hjpegSize = bytesToInt(inputStream, offset+12, 3);
paddingSize = bytesToInt(inputStream, offset+15, 1);
LOG_LIVEVIETHREAD_DEBUG << "Payloadheader Found at: " << offset+8;
}
}
// osx: -1,40 Android: 255,216
if(inputStream.at(offset) == (char) 0xff && inputStream.at(offset+1) == (char)0xd8){
starttime = offset;
foundstart = true;
LOG_LIVEVIETHREAD_DEBUG << "start Found at: " << starttime << "\n";
foundstartOffset = offset;
}
offset++;
}//while
LOG_LIVEVIETHREAD_DEBUG << "actual arrayiter: " << arrayiter;
//! suche nach Bildende ab Stelle offset
while(foundstart && !foundend && offset<(inputStream.length()-1)){
// osx: -1,39 Android: 255,217
if(inputStream.at(offset) == (char)0xff && inputStream.at(offset+1) == (char)0xd9){
endtime = offset;
foundend = true;
LOG_LIVEVIETHREAD_DEBUG << "end Found at: " << endtime;
}
array[arrayiter] = inputStream.at(offset-1);
arrayiter++;
offset++;
}//while
/*
for (int i = 0; i< hjpegSize;i++){
array[i] = inputStream.at(foundstartOffset);
foundstartOffset++;
}*/
//! Bild gefunden
if(foundstart && foundend){
if(carry){
jpegSize = endtime-starttime;
}
else
jpegSize = endtime-starttime;
LOG_LIVEVIETHREAD_DEBUG << "Picture Found " <<"\n"
<< "arrayiter: " << arrayiter << "\n"
<< "Starttime: " << starttime << "\n"
<< "endtime: " << endtime << "\n"
<< "offset: " << offset << "\n"
<< "inputStream.size(): " << inputStream.size() << "\n"
<< "jpegSize calculated: " << jpegSize << "\n"
<< "jpegSize from Header: " << hjpegSize << "\n"
<< "paddingSize: " << paddingSize << "\n"
<< "jpegSize full size: " << endtime << "\n"
<< "inputStream.size(): " << inputStream.size() << "\n";
emit publishLiveViewBytes(array);
inputStream.remove(0,endtime);
offset = 0;
arrayiter = 0;
foundend = false;
foundstart = false;
starttime = 0;
endtime = 0;
carry = false;
LOG_LIVEVIETHREAD_DEBUG << "inputStream new size: " << inputStream.size() << "\n";
//LOG_LIVEVIETHREAD_DEBUG << "publishLiveViewBytes";
}
else if (foundstart && !foundend){
carry = true;
//arrayiter = foundstartOffset;
//offset = 0;
LOG_LIVEVIETHREAD_DEBUG << "arrayiter " << arrayiter <<"\n";
}
} // while search in tmp buffer;
}// !inputStream.isEmpty()
mutex.unlock();
#endif
}
