Exemplo n.º 1
0
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;
}
Exemplo n.º 2
0
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;
}
Exemplo n.º 3
0
 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);
 }
Exemplo n.º 4
0
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;
}
Exemplo n.º 5
0
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;
}
Exemplo n.º 6
0
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;
}
Exemplo n.º 7
0
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
Exemplo n.º 8
0
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));
}
Exemplo n.º 9
0
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
Exemplo n.º 10
0
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));
}
Exemplo n.º 11
0
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
Exemplo n.º 12
0
   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;
   }
Exemplo n.º 13
0
 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)));
 }