uint8_t NazaDecoderLib::decode(int input) { if((seq == 0) && (input == 0x55)) { seq++; } // header (part 1 - 0x55) else if((seq == 1) && (input == 0xAA)) { cs1 = 0; cs2 = 0; seq++; } // header (part 2 - 0xAA) else if((seq == 2) && ((input == 0x10) || (input == 0x20))) { msgId = input; updateCS(input); seq++; } // message id else if(seq == 3) { msgLen = input; cnt = 0; updateCS(input); seq++; } // message payload lenght else if(seq == 4) { gpsPayload[cnt++] = input; updateCS(input); if(cnt >= msgLen) { seq++; } } // store payload in buffer else if((seq == 5) && (input == cs1)) { seq++; } // verify checksum #1 else if((seq == 6) && (input == cs2)) { seq++; } // verify checksum #2 else seq = 0; if(seq == 7) // all data in buffer { seq = 0; // Decode GPS data if(msgId == NAZA_MESSAGE_GPS) { uint8_t mask = gpsPayload[55]; uint32_t time = decodeLong(0, mask); second = time & 0b00111111; time >>= 6; minute = time & 0b00111111; time >>= 6; hour = time & 0b00001111; time >>= 4; day = time & 0b00011111; time >>= 5; if(hour > 7) day++; month = time & 0b00001111; time >>= 4; year = time & 0b01111111; lon = (uint32_t)decodeLong(4, mask) / 1; lat = (uint32_t)decodeLong(8, mask) / 1; gpsAlt = (double)decodeLong(12, mask) / 1000; double nVel = (double)decodeLong(28, mask) / 100; double eVel = (double)decodeLong(32, mask) / 100; spd = sqrt(nVel * nVel + eVel * eVel); cog = atan2(eVel, nVel) * 180.0 / M_PI; if(cog < 0) cog += 360.0; gpsVsi = -(double)decodeLong(36, mask) / 100; vdop = (double)decodeShort(42, mask) / 100; double ndop = (double)decodeShort(44, mask) / 100; double edop = (double)decodeShort(46, mask) / 100; hdop = sqrt(ndop * ndop + edop * edop); sat = gpsPayload[48]; uint8_t fixType = gpsPayload[50] ^ mask; uint8_t fixFlags = gpsPayload[52] ^ mask; switch(fixType) { case 2 : fix = FIX_2D; break; case 3 : fix = FIX_3D; break; default: fix = NO_FIX; break; } if((fix != NO_FIX) && (fixFlags & 0x02)) fix = FIX_DGPS; }
void LobbyLogic::processNewMessage(SOCKET s,short id,vector<unsigned char> data) { std::vector<unsigned char> erfg; switch (id) { // 00: Server -> Client (sendet Lobby-Daten) // 01: Client -> Server (fordert Lobby-Daten an) case 0x0201: { for (map<short, GameLobbyLogic*>::iterator it = gamesCreated.begin(); it != gamesCreated.end();) { if (it->second->getPlayers().size() == 0) { it = gamesCreated.erase(it); continue; } else { std::vector<unsigned char> tmp; //erfg.push_back(it->first); tmp = code(it->first); erfg.push_back(tmp[0]); erfg.push_back(tmp[1]); tmp = code(it->second->getPlayerlimit()); erfg.push_back(tmp[0]); erfg.push_back(tmp[1]); string gamelobbyname = it->second->getName(); short len = gamelobbyname.length(); tmp = code(len); erfg.push_back(tmp[0]); erfg.push_back(tmp[1]); tmp = code(gamelobbyname); for (unsigned int i = 0; i < tmp.size(); i++) erfg.push_back(tmp[i]); string master = it->second->getGamemaster().Name; len = master.length(); tmp = code(len); erfg.push_back(tmp[0]); erfg.push_back(tmp[1]); tmp = code(master); for (unsigned int i = 0; i < tmp.size(); i++) erfg.push_back(tmp[i]); short countplayers = it->second->getPlayers().size(); tmp = code(countplayers); erfg.push_back(tmp[0]); erfg.push_back(tmp[1]); for (unsigned int i = 0; i < it->second->getPlayers().size(); i++) { string name = it->second->getPlayers().at(i).Name; len = name.length(); tmp = code(len); erfg.push_back(tmp[0]); erfg.push_back(tmp[1]); tmp = code(name); for (unsigned int i = 0; i < tmp.size(); i++) erfg.push_back(tmp[i]); } } it++; } server->write(s, 0x0200,erfg); break; } // 02: Client -> Server (will zu Spiellobby connecten) // 03: Server -> Client (connect Bestätigung) case 0x0202: { // gameid aus daten lesen, den user da reinschreiben short mapid = data[0]; for (unsigned int i = 0; i < server->connectedPlayers.size(); i++) if (server->connectedPlayers[i].s == s && gamesCreated[mapid]->isAlreadyConnected(s) == false) { gamesCreated[mapid]->addPlayer(server->connectedPlayers[i]); break; } erfg.push_back(1); server->write(s, 0x0203, erfg); break; } // 04: Client -> Server (will Spiellobby erstellen) // 05: Server -> Client (erstellen Bestätigung) case 0x0204: { //data: string gamename auslesen TODO! id aus gamelobbylogic entfernen static short id = 0; id++; string gamename; short len = decodeShort(data, 0); gamename = decodeString(data, 2, len); PlayerData requester; for (unsigned int i = 0; i < server->connectedPlayers.size(); i++) if (server->connectedPlayers[i].s == s) { requester = server->connectedPlayers[i]; } if(!requester.isGamemaster) { GameLobbyLogic* GameLobby = new GameLobbyLogic(id, requester, gamename); std::map<short, GameLobbyLogic*>::iterator it = this->gamesCreated.begin(); this->gamesCreated.insert (it, std::pair<short, GameLobbyLogic*>(id,GameLobby)); for (unsigned int i = 0; i < server->connectedPlayers.size(); i++) if (server->connectedPlayers[i].s == s) { server->connectedPlayers[i].isGamemaster = true; } erfg.push_back(1); server->write(s, 0x0205, erfg); } else { erfg.push_back(0); server->write(s, 0x0205, erfg); } break; } } }