void Wiimote::setReportingMode(bool insideReader)
{
/* Assemble the data request packet: */
unsigned char requestData[]= {0xa2,0x12,0x00,0x00};
if(readContinuously)
requestData[2]|=0x04;
if(extensionDevice!=NONE)
{
if(readAccelerometers&&readIRTracking)
requestData[3]=0x37;
else if(readAccelerometers)
requestData[3]=0x35;
else if(readIRTracking)
requestData[3]=0x36;
else
requestData[3]=0x32;
}
else
{
if(readAccelerometers&&readIRTracking)
requestData[3]=0x33;
else if(readAccelerometers)
requestData[3]=0x31;
else if(readIRTracking)
requestData[3]=0x36;
else
requestData[3]=0x30;
}
/* Set the appropriate IR camera reporting mode for the data reporting mode: */
if(readIRTracking)
{
if(insideReader)
{
/* Write to the camera's register area: */
unsigned char IRMode[]= {0xa2,0x16,0x04,0xb0,0x00,0x33,0x01,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
if(extensionDevice==NONE&&readAccelerometers&&readIRTracking)
IRMode[7]=0x03;
writePacket(IRMode,sizeof(IRMode));
/* Wait for acknowledgment: */
unsigned char packet[21];
waitForPacket(0x22,packet,6);
}
else
{
unsigned char IRMode=0x01;
if(extensionDevice==NONE&&readAccelerometers&&readIRTracking)
IRMode=0x03;
uploadData(true,0xb00033,&IRMode,sizeof(unsigned char));
}
}
/* Send the packet: */
writePacket(requestData,sizeof(requestData));
}
bool SeaNet::isFullDuplex(int timeout)
{
std::vector<uint8_t> packet = SeaNetPacket::createPaket(device_type,mtSendBBUser);
writePacket(&packet[0],packet.size());
waitForPacket(mtBBUserData,timeout);
BBUserData settings;
sea_net_packet.decodeBBUserData(settings);
return settings.full_duplex;
}
void SeaNet::getVersion(VersionData &version,int timeout)
{
std::vector<uint8_t> send_packet = SeaNetPacket::createPaket(device_type,mtSendVersion);
writePacket(&send_packet[0],send_packet.size());
//wait until we received a mtVersionData package
waitForPacket(mtVersionData,timeout);
sea_net_packet.decodeVersionData(version);
}
void SeaNet::reboot(int timeout)
{
LOG_DEBUG_S << "rebooting device" ;
std::vector<uint8_t> packet = SeaNetPacket::createPaket(device_type,mtReBoot);
writePacket(&packet[0],packet.size());
clear();
//now wait for an alive message
waitForPacket(mtAlive,timeout);
}
void Server::run()
{
while(m_running)
{
// check if there are any outgoing packets waiting to be send
Packet *packet = nextOutgoingPacket();
while(packet != 0)
{
sendPacket(packet);
delete packet;
packet = nextOutgoingPacket();
}
// get next incoming packet
packet = waitForPacket(100);
if(packet != 0)
{
addIncomingPacket(packet);
emit newPacketArrived();
}
}
}
void SeaNet::writeHeadCommand(HeadCommand &head_config, int timeout)
{
if (has_pending_data)
throw std::runtime_error("requestData() called and the corresponding receiveData() has not been called");
iodrivers_base::Timeout time_out(timeout);
std::vector<uint8_t> packet =
SeaNetPacket::createPaket(device_type,
mtHeadCommand,
(uint8_t*)&head_config,
sizeof(head_config));
LOG_DEBUG_S <<"Sent mtHeadCommand packet" ;
writePacket(&packet[0],packet.size(),time_out.timeLeft());
//wait for an alive packet to check if the sonar is configured
AliveData alive_data;
bool received_alive = false;
while(!alive_data.ready && alive_data.no_config && !alive_data.config_send)
{
try
{
waitForPacket(mtAlive,time_out.timeLeft());
}
catch(std::runtime_error e)
{
if(received_alive)
throw std::runtime_error("Configure failed: Configuration was not accepted by the device.");
else
throw std::runtime_error("Configure failed: Device is not responding. Timeout too small?");
}
received_alive = true;
sea_net_packet.decodeAliveData(alive_data);
}
}
Packet *Server::nextPacket()
{
if(m_socket == 0)
{
m_error = QObject::tr("Socket is not open");
return 0;
}
int bytesRead;
if(m_packetSize == 0)
{
// read packet length
if(m_socket->bytesAvailable() > 0)
bytesRead = m_socket->read(m_buffer, 4);
else
return 0;
if(bytesRead < 0)
{
// error
m_error = QObject::tr("Error while reading from socket: ") + m_socket->errorString();
return new DisconnectPacket();
}
else if(bytesRead == 0)
return 0;
m_packetSize = Packet::readInt(m_buffer);
m_offset = bytesRead;
}
if(m_socket->bytesAvailable() > 0)
bytesRead = m_socket->read(&m_buffer[m_offset], m_packetSize - m_offset);
else
return 0;
if(bytesRead < 0)
{
// error
m_error = QObject::tr("Error while reading from socket: ") + m_socket->errorString();
return new DisconnectPacket();
}
else if(bytesRead == 0)
{
// no data to read
return 0;
}
else
{
// read some data
m_offset += bytesRead;
if(m_offset == m_packetSize)
{
Packet *result = PacketFactory::getPacket(m_buffer[4]);
if(result != 0)
{
m_offset = m_packetSize = 0;
if(result->type() == PlayerListPacket::Type)
{
PlayerListPacket *playerList = dynamic_cast<PlayerListPacket*>(result);
int size = Packet::readInt(m_buffer + 5);
for(int i = 0; i < size; i++)
{
Packet *p = waitForPacket(20000);
if(p == 0 || p->type() != PlayerPacket::Type)
{
m_error = QObject::tr("Corrupted player list");
delete result;
if(p != 0)
delete p;
return new DisconnectPacket();
}
playerList->addPlayer(dynamic_cast<PlayerPacket*>(p));
}
}
else
{
result->read(m_buffer + 5);
}
}
else
// unknown packet
m_error = QObject::tr("Unknown packet ID: ") + QString("%1").arg((int)m_buffer[4]);
return result;
}
}
return 0;
}
void SeaNet::receiveData(int timeout)
{
LOG_DEBUG_S << "waiting for a mtHeadData packet" ;
waitForPacket(mtHeadData, timeout);
}
