void MAVConnSerial::async_write_end(error_code error, size_t bytes_transferred)
{
if (error) {
ROS_ERROR_STREAM_NAMED("mavconn", "serial" << channel << ":write: " << error.message());
close();
return;
}
lock_guard lock(mutex);
if (tx_q.empty()) {
tx_in_progress = false;
return;
}
MsgBuffer *buf = tx_q.front();
buf->pos += bytes_transferred;
if (buf->nbytes() == 0) {
tx_q.pop_front();
delete buf;
}
if (!tx_q.empty())
do_write(false);
else
tx_in_progress = false;
}
void MAVConnSerial::async_write_end(error_code error, size_t bytes_transferred)
{
if (error) {
logError("serial%d:write: %s", channel, error.message().c_str());
close();
return;
}
lock_guard lock(mutex);
if (tx_q.empty()) {
tx_in_progress = false;
return;
}
MsgBuffer *buf = tx_q.front();
buf->pos += bytes_transferred;
if (buf->nbytes() == 0) {
tx_q.pop_front();
delete buf;
}
if (!tx_q.empty())
do_write(false);
else
tx_in_progress = false;
}
void CtrlHub::debugProdMe(uint32 dst, char *msg)
{
DBG("CtrlHub::debugProdMe()\n");
MsgBuffer *buf = new MsgBuffer(dst, 0, MsgBufferEnums::SERIAL_SEND_DATA, 8);
buf->writeNextCharString(msg);
m_queue->insert(buf);
}
void MAVConnUDP::async_sendto_end(error_code error, size_t bytes_transferred)
{
if (error) {
logError(PFXd "sendto: %s", channel, error.message().c_str());
close();
return;
}
iostat_tx_add(bytes_transferred);
lock_guard lock(mutex);
if (tx_q.empty()) {
tx_in_progress = false;
return;
}
MsgBuffer *buf = tx_q.front();
buf->pos += bytes_transferred;
if (buf->nbytes() == 0) {
tx_q.pop_front();
delete buf;
}
if (!tx_q.empty())
do_sendto(false);
else
tx_in_progress = false;
}
uint32
DestinationsProviderPublic::removeFavorite( uint32 favoriteId,
uint32 dst) const
{
if (dst == MsgBufferEnums::ADDR_DEFAULT) {
dst = m_defaultDst;
}
uint32 src = m_owner->getRequestId();
MsgBuffer *buf = new MsgBuffer(dst, src, MsgBufferEnums::REMOVE_FAVORITE_REQ,
64);
buf->writeNextUnaligned32bit(favoriteId);
m_queue->insert(buf);
return src;
}
uint32
DestinationsProviderPublic::sortFavorites(GuiProtEnums::SortingType sortingOrder,
uint32 dst) const
{
if (dst == MsgBufferEnums::ADDR_DEFAULT) {
dst = m_defaultDst;
}
uint32 src = m_owner->getRequestId();
MsgBuffer *buf = new MsgBuffer(dst, src, MsgBufferEnums::SORT_FAVORITES_REQ,
64);
buf->writeNextUnaligned16bit(sortingOrder);
m_queue->insert(buf);
return src;
}
uint32
DestinationsProviderPublic::getFavoritesAllData(uint16 startIdx, uint16 endIdx,
uint32 dst) const
{
if (dst == MsgBufferEnums::ADDR_DEFAULT) {
dst = m_defaultDst;
}
uint32 src = m_owner->getRequestId();
MsgBuffer *buf = new MsgBuffer(dst, src, MsgBufferEnums::GET_FAVORITES_ALL_DATA_REQ,
64);
buf->writeNextUnaligned16bit(startIdx);
buf->writeNextUnaligned16bit(endIdx);
m_queue->insert(buf);
return src;
} // DestinationsProviderPublic::getFavoritesAllData
void MAVConnSerial::do_write(bool check_tx_state)
{
if (check_tx_state && tx_in_progress)
return;
lock_guard lock(mutex);
if (tx_q.empty())
return;
tx_in_progress = true;
MsgBuffer *buf = tx_q.front();
serial_dev.async_write_some(
buffer(buf->dpos(), buf->nbytes()),
boost::bind(&MAVConnSerial::async_write_end,
this,
boost::asio::placeholders::error,
boost::asio::placeholders::bytes_transferred));
}
uint32
DestinationsConsumerPublic::getFavoritesReply( vector<GuiFavorite*>& guiFavorites, uint32 dst ) const
{
if (dst == MsgBufferEnums::ADDR_DEFAULT) {
dst = m_defaultDst;
}
uint32 src = m_owner->getRequestId();
uint32 bufferSize = guiFavorites.size() * GuiFavorite::MAX_GUI_FAV_SIZE + 2;
MsgBuffer *buf = new MsgBuffer(dst, src,
MsgBufferEnums::GET_FAVORITES_REPLY,
bufferSize);
buf->writeNextUnaligned16bit(guiFavorites.size());
for (uint16 i = 0; i < guiFavorites.size(); i++){
guiFavorites[i]->serialize(buf);
}
m_queue->insert(buf);
return src;
} // DestinationsConsumerPublic::getFavoritesReply
void MAVConnUDP::do_sendto(bool check_tx_state)
{
if (check_tx_state && tx_in_progress)
return;
lock_guard lock(mutex);
if (tx_q.empty())
return;
tx_in_progress = true;
MsgBuffer *buf = tx_q.front();
socket.async_send_to(
buffer(buf->dpos(), buf->nbytes()),
remote_ep,
boost::bind(&MAVConnUDP::async_sendto_end,
this,
boost::asio::placeholders::error,
boost::asio::placeholders::bytes_transferred));
}
uint32
DestinationsConsumerPublic::getFavoritesAllDataReply( vector<Favorite*>& favorites, uint32 dst ) const
{
if (dst == MsgBufferEnums::ADDR_DEFAULT) {
dst = m_defaultDst;
}
uint32 src = m_owner->getRequestId();
uint32 bufferSize = 2;
uint16 i = 0;
for ( i = 0 ; i < favorites.size() ; ++i ) {
bufferSize += favorites[ i ]->getSize();
}
MsgBuffer *buf = new MsgBuffer(dst, src,
MsgBufferEnums::GET_FAVORITES_ALL_DATA_REPLY,
bufferSize);
buf->writeNextUnaligned16bit(favorites.size());
for ( i = 0 ; i < favorites.size() ; ++i ) {
favorites[ i ]->serialize( buf );
}
m_queue->insert(buf);
return src;
} // DestinationsConsumerPublic::getFavoritesAllDataReply
MsgBuffer* ModuleQueue::getMessage(){
lock();
MsgBuffer* ret = NULL;
ListTimer now;
do{
// Time is now...
now.now();
while(m_timers != NULL && ((*m_timers) <= now)){
// all timers that should have expired by now are treated
// here.
DBG("(%s) preexpired id %i, %"PRIu32", now %"PRIx32, THREADNAME,
m_timers->id, m_timers->millis(), now.millis());
MsgBuffer* timerBuf =
new MsgBuffer(uint32(MsgBufferEnums::ADDR_LINK_LOCAL), 0,
MsgBufferEnums::TIMER);
timerBuf->setExtraData(m_timers->id);
insert(timerBuf);
ListTimer* timer = m_timers;
timer->unlink(&m_timers);
timer->insert(&m_expiredTimers);
}
if(m_queueHead.next == &m_queueHead){
// the list is empty
// find the timout
long timeout = DEFAULT_TIMEOUT;
uint16 timerID = MAX_UINT16;
if(m_timers != NULL){
DBG("%s:%d - m_timers = %p", __FILE__, __LINE__,m_timers);
isabTime timer(*m_timers);
timer.sub(now);
timeout = timer.millis();
timerID = m_timers->id;
logTimers(THREADNAME, __LINE__, m_log);
}
logTimers(THREADNAME, __LINE__, m_log);
// Set timeout in TimerThread
MsgBuffer* timerBuf =
new MsgBuffer( uint32(MsgBufferEnums::ADDR_LINK_LOCAL), 0,
MsgBufferEnums::TIMER );
timerBuf->setExtraData( timerID );
DBG("(%s) set tt %i, %"PRIi32", this=%p, buf=%p",
THREADNAME,
timerID, timeout, this, timerBuf);
((TimerThread *)m_timerThread)->setTimer(this, timerID,
timeout, timerBuf );
DBG("(%s) timer %u set", THREADNAME, timerID);
logTimers(THREADNAME, __LINE__, m_log);
// wait for timeout/notify
wait( timeout );
logTimers(THREADNAME, __LINE__, m_log);
// Check if timer then move to expired
QueueItem* cur = m_queueHead.next;
while( cur != &m_queueHead ) {
DBG("%d: cur: %p != &m_queueHead(%p)",
__LINE__, cur, &m_queueHead);
DBG("%d: cur->buf->msgtype: %#x, TIMER: %#x", __LINE__,
cur->buffer->getMsgType(), MsgBufferEnums::TIMER);
DBG("%d: cur->buf->xtradata: %#hx, timerID: %#hx", __LINE__,
cur->buffer->getExtraData(), timerID);
if ( cur->buffer->getMsgType() == MsgBufferEnums::TIMER &&
cur->buffer->getExtraData() == timerID ) {
if ( m_timers != NULL ) {
// Find timer and move to expired
logTimers(THREADNAME, __LINE__, m_log);
ListTimer* q = ListTimer::findID( m_timers, timerID );
logTimers(THREADNAME, __LINE__, m_log);
if ( q != NULL ) {
#ifndef NO_LOG_OUTPUT
isabTime now2;
DBG("(%s) expired id %i (%p), %"PRIu32", now %"PRIu32,
THREADNAME,
m_timers->id, cur->buffer,
m_timers->millis(), now2.millis());
logTimers(THREADNAME, __LINE__, m_log);
#endif
q->unlink( &m_timers );
logTimers(THREADNAME, __LINE__, m_log);
q->insert( &m_expiredTimers );
logTimers(THREADNAME, __LINE__, m_log);
}
}
if ( cur->buffer->getExtraData() == MAX_UINT16 ) {
// Dummy timeout timer, remove
// deleting a QueueItem unlinks it from its list.
delete cur->buffer;
delete cur;
}
break; // Only one outstanding timer
}
DBG("%d: cur: %p, cur->next: %p", __LINE__, cur, cur->next);
cur = cur->next;
}
// Unset timer
((TimerThread *)m_timerThread)->unSetTimer( this, timerID );
} // End if m_queueHead.next == &m_queueHead (empty list)
}while(m_queueHead.next == &m_queueHead);
DBG("%d: empty queue", __LINE__);
// The list is not empty
if(m_queueHead.next != &m_queueHead){
//this must be a bug ?
DBG("%d: queue not empty after all?", __LINE__);
DBG("%s:%d: m_queueHead.next = %p", THREADNAME, __LINE__,
m_queueHead.next);
ret = m_queueHead.next->buffer;
DBG("%s:%d: m_queueHead.next->buffer = %p", THREADNAME,__LINE__,
m_queueHead.next->buffer);
// deleting a QueueItem unlinks it from its list.
delete m_queueHead.next;
DBG("%s:%d: Deleting worked", THREADNAME, __LINE__);
}
unlock();
DBG("%s:%d: returning buf %p", THREADNAME, __LINE__, ret);
return ret;
}
void respond(){
udp::endpoint dest;
ClientPacket clpack;
MsgBuffer msg;
char* pmsg = (char*)msg.data();
handle_disconnected();
if(rcv_queue.empty()){
recv_timer.expires_from_now(boost::posix_time::milliseconds(50));
recv_timer.async_wait(
data_strand.wrap(std::bind(&Server::respond,this)));
return;
}
std::tie(dest, clpack) = rcv_queue.front();
rcv_queue.pop_front();
int id, oppid;
glm::vec3 curr_pos, curr_dir;
switch(clpack.cmd){
case ClientCommand::Connect:
id = get_free();
std::cerr << "connected: " << id << "\n";
players[id].dest = dest;
players[id].last = std::chrono::system_clock::now();
players[id].next_respawn = players[id].last
+ std::chrono::minutes(60);
packSelfInfo(send_buffer[0], id, players[id].pos, players[id].dir);
sock.async_send_to(boost::asio::buffer(send_buffer), dest,
std::bind(&Server::handle_send, this, std::placeholders::_1, std::placeholders::_2));
active.fetch_add(1);
std::snprintf(pmsg, msg.size(), "player %d connected", id);
msg_queue.push(msg);
break;
case ClientCommand::Disconnect:
id = clpack.data[0];
if(!validate(id, dest)) break;
players[id].present = false;
break;
case ClientCommand::MoveTo:
id = clpack.data[0];
if(!validate(id, dest)) break;
players[id].last = std::chrono::system_clock::now();
memcpy(&curr_pos, &clpack.data[1], sizeof(glm::vec3));
memcpy(&curr_dir, &clpack.data[4], sizeof(glm::vec3));
players[id].pos = curr_pos;
players[id].dir = curr_dir;
break;
case ClientCommand::Action:
id = clpack.data[0];
oppid = clpack.data[1];
if(!validate(id, dest)) break;
if(oppid >= players.size()) break;
players[oppid].health = std::max(players[oppid].health - 10, 0);
break;
case ClientCommand::Msg:
id = clpack.data[0];
if(!validate(id, dest)) break;
memcpy(msg.data(), &clpack.data[1], msg.size());
msg.back() = '\0';
msg_queue.push(msg);
break;
}
handle_respawn();
io_service.post(data_strand.wrap(std::bind(&Server::respond,this)));
}
