Example #1
0
void Dialog::processPendingDatagrams()
{
    mavlink_message_t msg;
    mavlink_status_t status;

    while (udpSocketFromQGroundControl->hasPendingDatagrams()) {
        QByteArray datagram;
        datagram.resize(udpSocketFromQGroundControl->pendingDatagramSize());
        udpSocketFromQGroundControl->readDatagram(datagram.data(), datagram.size());
        ui->textEdit->setText(tr("Received (%1) bytes datagram: \"%2\"")
                .arg(datagram.size())
                .arg(datagram.data()));

        for (int i=0; i<datagram.size();i++)
        {
            if (mavlink_parse_char(MAVLINK_COMM_0,datagram.at(i),&msg,&status))
                qDebug()<<"Received packet: SYS:"<< msg.sysid <<"COMP:"<<msg.compid<<"LEN:"<<msg.len<<"MSG ID:"<<msg.msgid<<endl;
                //qDebug()<<"msg: "<< msg.payload<<endl;
//            switch (msg.msgid)
//            {
//                case ""
//            }
        }


    }
}
Example #2
0
void cDataLink::receive_loop(void)
{
    const uint16_t BUFFER_LENGTH = 2048;

    uint8_t buf[BUFFER_LENGTH];
    ssize_t recsize;
    socklen_t fromlen;

    memset(buf, 0, BUFFER_LENGTH);

    while(execute_receive_thread)
    {
        recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen);
        if (recsize > 0)
        {
            // Something received - print out all bytes and parse packet
            mavlink_message_t msg;
            mavlink_status_t status;

            for (ssize_t i = 0; i < recsize; ++i)
            {
                if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status))
                {
                    // Packet received
                    handleMessage(&msg);
                }// parse is ok
            }// for all bytes received
        }
        memset(buf, 0, BUFFER_LENGTH);
        boost::this_thread::sleep(boost::posix_time::time_duration(boost::posix_time::milliseconds(10)));
    }//while
}
Example #3
0
/**
 * Receive data from UART.
 */
static void *
receive_thread(void *arg)
{
	int uart_fd = *((int *)arg);

	const int timeout = 1000;
	uint8_t buf[32];

	mavlink_message_t msg;

	prctl(PR_SET_NAME, "mavlink uart rcv", getpid());

	while (!thread_should_exit) {

		struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } };

		if (poll(fds, 1, timeout) > 0) {
			/* non-blocking read. read may return negative values */
			ssize_t nread = read(uart_fd, buf, sizeof(buf));

			/* if read failed, this loop won't execute */
			for (ssize_t i = 0; i < nread; i++) {
				if (mavlink_parse_char(chan, buf[i], &msg, &status)) {
					/* handle generic messages and commands */
					handle_message(&msg);

					/* Handle packet with waypoint component */
					mavlink_wpm_message_handler(&msg, &global_pos, &local_pos);

					/* Handle packet with parameter component */
					mavlink_pm_message_handler(MAVLINK_COMM_0, &msg);
				}
			}
		}
	}
Example #4
0
void
GCS_MAVLINK::update(void (*run_cli)(AP_HAL::UARTDriver *))
{
    // receive new packets
    mavlink_message_t msg;
    mavlink_status_t status;
    status.packet_rx_drop_count = 0;

    // process received bytes
    uint16_t nbytes = comm_get_available(chan);
    for (uint16_t i=0; i<nbytes; i++)
    {
        uint8_t c = comm_receive_ch(chan);

        if (run_cli != NULL) {
            /* allow CLI to be started by hitting enter 3 times, if no
             *  heartbeat packets have been received */
            if (!mavlink_active && (hal.scheduler->millis() - _cli_timeout) < 20000 && 
                comm_is_idle(chan)) {
                if (c == '\n' || c == '\r') {
                    crlf_count++;
                } else {
                    crlf_count = 0;
                }
                if (crlf_count == 3) {
                    run_cli(_port);
                }
            }
        }

        // Try to get a new message
        if (mavlink_parse_char(chan, c, &msg, &status)) {
            // we exclude radio packets to make it possible to use the
            // CLI over the radio
            if (msg.msgid != MAVLINK_MSG_ID_RADIO && msg.msgid != MAVLINK_MSG_ID_RADIO_STATUS) {
                mavlink_active = true;
            }
            handleMessage(&msg);
        }
    }

    if (!waypoint_receiving) {
        return;
    }

    uint32_t tnow = hal.scheduler->millis();
    uint32_t wp_recv_time = 1000U + (stream_slowdown*20);

    if (waypoint_receiving &&
        waypoint_request_i <= waypoint_request_last &&
        tnow - waypoint_timelast_request > wp_recv_time) {
        waypoint_timelast_request = tnow;
        send_message(MSG_NEXT_WAYPOINT);
    }

    // stop waypoint receiving if timeout
    if (waypoint_receiving && (tnow - waypoint_timelast_receive) > wp_recv_time+waypoint_receive_timeout) {
        waypoint_receiving = false;
    }
}
Example #5
0
void MavlinkBridge::readFromStream() {
	int counter = 0;
	while (running && connected && read(bridge_tty_fd, &bridge_c, 1) > 0 && counter++ < 1000) {
		if (mavlink_parse_char(MAVLINK_COMM_0, bridge_c, &msg, &status)) {
			if (msg.msgid == MAVLINK_MSG_ID_STATUSTEXT) {
				mavlink_statustext_t s;
				mavlink_msg_statustext_decode(&msg, &s);
				if (strstr(s.text, "recieved") == 0){
					printf("status %s\n", s.text);
				}
			}
			if (msg.msgid == MAVLINK_MSG_ID_ATTITUDE) {
				mavlink_attitude_t at;
				mavlink_msg_attitude_decode(&msg, &at);
				attitude = at;
			}
			if (msg.msgid == MAVLINK_MSG_ID_LOCAL_POSITION_NED) {
				mavlink_local_position_ned_t localPosition_t;
				mavlink_msg_local_position_ned_decode(&msg, &localPosition_t);
				localPosition = localPosition_t;
			}
			if (msg.msgid == MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED) {
				mavlink_position_target_local_ned_t localPositionTarget_t;
				mavlink_msg_position_target_local_ned_decode(&msg, &localPositionTarget_t);
				localPositionTarget = localPositionTarget_t;
			}
		}
	}
}
void mavlink_read(telemetry_data_t *td, uint8_t *buf, int buflen) {
	int i;
	for(i=0; i<buflen; i++) {
		uint8_t c = buf[i];
		if (mavlink_parse_char(0, c, &msg, &status)) {
                	switch (msg.msgid){
                        	case MAVLINK_MSG_ID_GPS_RAW_INT:
					td->fix = mavlink_msg_gps_raw_int_get_fix_type(&msg);
					td->sats = mavlink_msg_gps_raw_int_get_satellites_visible(&msg);
					break;
                                case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
					td->heading = mavlink_msg_global_position_int_get_hdg(&msg)/100.0f;
                                        td->altitude = mavlink_msg_global_position_int_get_relative_alt(&msg)/1000.0f;
                                        td->latitude = mavlink_msg_global_position_int_get_lat(&msg)/10000000.0f;
                                        td->longitude = mavlink_msg_global_position_int_get_lon(&msg)/10000000.0f;
                                        break;
                                case MAVLINK_MSG_ID_ATTITUDE:
					td->roll = mavlink_msg_attitude_get_roll(&msg)*57.2958;
					td->pitch = mavlink_msg_attitude_get_pitch(&msg)*57.2958;
					break;
                                case MAVLINK_MSG_ID_SYS_STATUS:
					td->voltage = mavlink_msg_sys_status_get_voltage_battery(&msg)/1000.0f;
					td->ampere = mavlink_msg_sys_status_get_current_battery(&msg)/100.0f;
                                        break;
                                case MAVLINK_MSG_ID_VFR_HUD:
                                        td->speed = mavlink_msg_vfr_hud_get_groundspeed(&msg)*3.6f;
                                        break;
        	}
	}
  }
}
Example #7
0
static void comm_send_ch(mavlink_channel_t chan, uint8_t c)
{
	mavlink_status_t status;
	if (mavlink_parse_char(chan, c, &last_msg, &status)) {
		print_message(&last_msg);
		chan_counts[chan]++;
		/* channel 0 gets 3 messages per message, because of
		   the channel defaults for _pack() and _encode() */
		if (chan == MAVLINK_COMM_0 && status.current_rx_seq != (uint8_t)(chan_counts[chan]*3)) {
			printf("Channel 0 sequence mismatch error at packet %u (rx_seq=%u)\n", 
			       chan_counts[chan], status.current_rx_seq);
			error_count++;
		} else if (chan > MAVLINK_COMM_0 && status.current_rx_seq != (uint8_t)chan_counts[chan]) {
			printf("Channel %u sequence mismatch error at packet %u (rx_seq=%u)\n", 
			       (unsigned)chan, chan_counts[chan], status.current_rx_seq);
			error_count++;
		}
		if (message_lengths[last_msg.msgid] != last_msg.len) {
			printf("Incorrect message length %u for message %u - expected %u\n", 
			       (unsigned)last_msg.len, (unsigned)last_msg.msgid, message_lengths[last_msg.msgid]);
			error_count++;
		}
	}
	if (status.packet_rx_drop_count != 0) {
		printf("Parse error at packet %u\n", chan_counts[chan]);
		error_count++;
	}
}
void MAVLinkExchanger::receiveMessage(const boost::system::error_code &ec, std::size_t bytes_transferred)
{
	mavlink_status_t status;
	for(auto i = 0; i < bytes_transferred; ++i)
	{
		if(mavlink_parse_char(MAVLINK_COMM_0, receiveBuffer[i], &message, &status))
		{
			receiveQueue.push(message);
		}
	}
	if(tcpPort != nullptr)
	{
		tcpPort->async_read_some(boost::asio::buffer(receiveBuffer), 
		std::bind(&MAVLinkExchanger::receiveMessage, 
		this, 
		std::placeholders::_1,
		std::placeholders::_2));
	}
	else
	{
		serialPort->async_read_some(boost::asio::buffer(receiveBuffer), 
		std::bind(&MAVLinkExchanger::receiveMessage, 
		this, 
		std::placeholders::_1,
		std::placeholders::_2));
	}
	
}
    void receive() {
        mavlink_message_t msg;
        while(get_board()->get_serial()->available())
        {
            uint8_t c = get_board()->get_serial()->read();

            // try to get new message
            if(mavlink_parse_char(MAVLINK_COMM_0,c,&msg,&m_status))
            {
                switch(msg.msgid)
                {

                case MAVLINK_MSG_ID_HIL_CONTROLS:
                {
                    //std::cout << "receiving messages" << std::endl;
                    mavlink_hil_controls_t hil_controls;
                    mavlink_msg_hil_controls_decode(&msg,&hil_controls);
                    std::cout << "roll: " << hil_controls.roll_ailerons << std::endl;
                    std::cout << "pitch: " << hil_controls.pitch_elevator << std::endl;
                    std::cout << "yaw: " << hil_controls.yaw_rudder << std::endl;
                    std::cout << "throttle: " << hil_controls.throttle << std::endl;
                    std::cout << "mode: " << hil_controls.mode << std::endl;
                    std::cout << "nav mode: " << hil_controls.nav_mode << std::endl;
                    break;
                }

                default:
                {
                    std::cout << "received message: " << uint32_t(msg.msgid) << std::endl;
                }

                }
            }
        }
    }
/**
 * Receive data from UART.
 */
static void *
receive_thread(void *arg)
{
  int uart_fd = *((int*)arg);

  const int timeout = 1000;
  uint8_t ch;

  mavlink_message_t msg;

  prctl(PR_SET_NAME, "mavlink offb mod rcv", getpid());

  while (!thread_should_exit)
  {

    struct pollfd fds[] = { {.fd = uart_fd, .events = POLLIN}};

    if (poll(fds, 1, timeout) > 0)
    {
      /* non-blocking read until buffer is empty */
      int nread = 0;

      do
      {
        nread = read(uart_fd, &ch, 1);

        if (mavlink_parse_char(chan, ch, &msg, &status))
        { //parse the char
          /* handle generic messages and commands */
          handle_message(&msg);
        }
      } while (nread > 0);
    }
  }
Example #11
0
void GCSAgent::testMavlink(void)
{
    // Create ros_info
    mavlink_message_t mmsg;
    char host[32];
    int hostId = 0;
    gethostname(host, sizeof(host));
    mavlink_msg_ros_info_pack_chan(1, 1, 0, &mmsg,
                                   hostId,
                                   1,
                                   host,
                                   1);

    send(mmsg);
    //>>>> mavlink_message_t mmsg

    uint8_t data[MAVLINK_MAX_PACKET_LEN + 2 + 7];
    int len = mavlink_msg_to_send_buffer(data, &mmsg);

    //>>>> uint8_t data[]

    mavlink_message_t r;
    mavlink_status_t status;
    for(int x = 0; x < len; x++)
    {
        if(int ok = mavlink_parse_char(0, data[x], &r, &status))
            ROS_INFO("Test passed");
    }
}
Example #12
0
void mavlink_stream_receive(mavlink_stream_t* mavlink_stream) 
{
	uint8_t byte;
	byte_stream_t* stream = mavlink_stream->rx;
	mavlink_received_t* rec = &mavlink_stream->rec;

	int bytes_received=0;
	if(mavlink_stream->msg_available == false)
	{
		bytes_received = stream->bytes_available(stream->data);
		//if (bytes_received>0) print_util_dbg_printf("received %i bytes.\n ", bytes_received);
		while(stream->bytes_available(stream->data) > 0) 
		{
			byte = stream->get(stream->data);
			//print_util_dbg_printf("%x ", byte);
			bytes_received++;
			if(mavlink_parse_char(MAVLINK_COMM_0, byte, &rec->msg, &rec->status)) 
			{
				mavlink_stream->msg_available = true;
				return;
			}
		}
		
	}
}
Example #13
0
/****************************************************************************
 * Private Data
 ****************************************************************************/
static void *receiveloop(void * arg) //runs as a pthread and listens to uart1 ("/dev/ttyS0")
{

	uint8_t ch;

	mavlink_message_t msg;

	while(1) {
		/* blocking read on next byte */
		int nread = read(uart, &ch, 1);


		if (nread > 0 && mavlink_parse_char(chan,ch,&msg,&status)) {//parse the char
			// handle generic messages and commands
			handleMessage(&msg);

			// Handle packet with waypoint component
			mavlink_wpm_message_handler(&msg);

			// Handle packet with parameter component
			mavlink_pm_message_handler(MAVLINK_COMM_0, &msg);
			led_toggle(LED_AMBER);
		}

	}
}
Example #14
0
static void comm_send_ch(mavlink_channel_t chan, uint8_t c)
{
	mavlink_status_t status;
	memset(&status, 0, sizeof(status));
#ifdef MAVLINK_SIGNING_FLAG_SIGN_OUTGOING
	status.signing = &signing_in[chan];
        status.signing_streams = &signing_streams_in;
#endif
	if (mavlink_parse_char(chan, c, &last_msg, &status)) {
		print_message(&last_msg);
		chan_counts[chan]++;
		/* channel 0 gets 3 messages per message, because of
		   the channel defaults for _pack() and _encode() */
		if (chan == MAVLINK_COMM_0 && status.current_rx_seq != (uint8_t)(chan_counts[chan]*3)) {
			printf("Channel 0 sequence mismatch error at packet %u (rx_seq=%u)\n", 
			       chan_counts[chan], status.current_rx_seq);
			error_count++;
		} else if (chan > MAVLINK_COMM_0 && status.current_rx_seq != (uint8_t)chan_counts[chan]) {
			printf("Channel %u sequence mismatch error at packet %u (rx_seq=%u)\n", 
			       (unsigned)chan, chan_counts[chan], status.current_rx_seq);
			error_count++;
		}
                // we only check the lengtth for MAVLink1. In MAVLink2 packets are zero trimmed
		if (mavlink_expected_message_length(&last_msg) > last_msg.len && last_msg.magic == 254) {
			printf("Incorrect message length %u for message %u - expected %u\n", 
			       (unsigned)last_msg.len, (unsigned)last_msg.msgid,
                               mavlink_expected_message_length(&last_msg));
			error_count++;
		}
	}
	if (status.packet_rx_drop_count != 0) {
		printf("Parse error at packet %u\n", chan_counts[chan]);
		error_count++;
	}
}
Example #15
0
void mp_mavlink_transmit(uint8_t ch)
// This is a special version of the routine for testing MAVLink routines
// The incoming serial stream is parsed to reproduce a mavlink message.
// This will then be checked against the original message and results recorded
// using the MAVLINK_ASSERT macro.
{
	mavlink_parse_char(0, ch, &last_msg, &r_mavlink_status);
}
Example #16
0
    void receive(double * y) {
        // receive messages
        mavlink_message_t msg;
        while(_comm->available())
        {

            uint8_t c = 0;
            if (!_comm->read((char*)&c,1)) return;

            // try to get new message
            if(mavlink_parse_char(MAVLINK_COMM_0,c,&msg,&_status))
            {
                switch(msg.msgid)
                {

                    // this packet seems to me more constrictive so I
                    // recommend using rc channels scaled instead
                    case MAVLINK_MSG_ID_HIL_CONTROLS:
                    {
                        //std::cout << "receiving hil controls packet" << std::endl;
                        mavlink_hil_controls_t packet;
                        mavlink_msg_hil_controls_decode(&msg,&packet);
                        y[0] = packet.roll_ailerons;
                        y[1] = packet.pitch_elevator;
                        y[2] = packet.yaw_rudder;
                        y[3] = packet.throttle;
                        y[4] = packet.mode;
                        y[5] = packet.nav_mode;
                        y[6] = 0;
                        y[7] = 0;
                        break;
                    }

                    case MAVLINK_MSG_ID_RC_CHANNELS_SCALED:
                    {
                        //std::cout << "receiving rc channels scaled packet" << std::endl;
                        mavlink_rc_channels_scaled_t packet;
                        mavlink_msg_rc_channels_scaled_decode(&msg,&packet);
                        y[0] = packet.chan1_scaled/1.0e4;
                        y[1] = packet.chan2_scaled/1.0e4;
                        y[2] = packet.chan3_scaled/1.0e4;
                        y[3] = packet.chan4_scaled/1.0e4;
                        y[4] = packet.chan5_scaled/1.0e4;
                        y[5] = packet.chan6_scaled/1.0e4;
                        y[6] = packet.chan7_scaled/1.0e4;
                        y[7] = packet.chan8_scaled/1.0e4;
                        break;
                    } 

                    default:
                    {
                        //std::cout << "received message: " << uint32_t(msg.msgid) << std::endl;
                    }

                }
            }
        }
    }
Example #17
0
void *MavlinkReceiveTask(void *ptr) {
	MavlinkStruct			*Mavlink = (MavlinkStruct *) ptr;
	AttitudeData			*AttitudeDesire = Mavlink->AttitudeDesire;
	AttData			 		 Data, Speed;
	double					 Pitch, Roll, Yaw, Elevation;
	mavlink_message_t		 msg;
	mavlink_status_t		 status;
	mavlink_manual_control_t man_control;
	socklen_t 				 fromlen;
	ssize_t 				 recsize;
	uint8_t 				 buf[BUFFER_LENGTH];
	int 					 i = 0;

	printf("%s : Mavlink Receive démarré\n", __FUNCTION__);
	pthread_barrier_wait(&(MavlinkStartBarrier));

	while (MavlinkActivated) {
		sem_wait(&MavlinkReceiveTimerSem);
		if (MavlinkActivated == 0)
			break;

		memset(buf, 0, BUFFER_LENGTH);
		recsize = recvfrom(Mavlink->sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&Mavlink->gcAddr, &fromlen);
		if (recsize > 0) {
			// Something received - print out all bytes and parse packet
			for (i = 0; i < recsize; ++i) {
				if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status)) {
					// Packet received
				}
			}
			if (msg.msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {
				mavlink_msg_manual_control_decode(&msg, &man_control);
				Pitch		= -((double)man_control.y/1000.0)*PITCH_MAX*M_PI/180.0;
				Roll		= ((double)man_control.x/1000.0)*ROLL_MAX*M_PI/180.0;
				Yaw			= -((double)man_control.r/1000.0)*YAW_MAX*M_PI/180.0;
				Elevation	= HEIGHT_MAX*((double)man_control.z/1000.0);
				Speed.Pitch	    = (Pitch - Data.Pitch)/TS;
				Speed.Roll	    = (Roll - Data.Roll)/TS;
				Speed.Yaw	    = (Yaw - Data.Yaw)/TS;
				Speed.Elevation = (Elevation - Data.Elevation)/TS;
				Data.Pitch	    = Pitch;
				Data.Roll	    = Roll;
				Data.Yaw	    = Yaw;
				Data.Elevation  = Elevation;
				pthread_spin_lock(&(AttitudeDesire->AttitudeLock));
				memcpy((void *) &(AttitudeDesire->Data), (void *) &Data, sizeof(AttData));
				memcpy((void *) &(AttitudeDesire->Speed), (void *) &Speed, sizeof(AttData));
				AttitudeDesire->Throttle = (float)man_control.z/1000;
				pthread_spin_unlock(&(AttitudeDesire->AttitudeLock));
			} else {	// Un message non attendu a ete recu
			}
		}
	}
	printf("%s : Mavlink Receive Arrêté\n", __FUNCTION__);
	pthread_exit(NULL);

}
Example #18
0
void mavlink_receiver_task(void)
{
	uint8_t buffer;

	while(1) {
		buffer = usart3_read();

		mavlink_parse_char(MAVLINK_COMM_0, buffer, &received_msg, &received_status); 
	}
}
 void mavlink_connector::handle_mavlink(char *buffer, int len)
 {
     mavlink_message_t msg;
     mavlink_status_t status;
     for (ssize_t i = 0; i < len; i++) {
         if (mavlink_parse_char(MAVLINK_COMM_1, buffer[i], &msg, &status)) {
             handle_message(&msg);
         }
     }
 }
void mavlink_usb_importer::receiver()
{
    mavlink_message_t msg;
    int lastSequenceNumber = 0;
    int sequenceFail = 0;
    //TODO check seq. number manually
    while( usb_fd >= 0 && !shouldStopReceiver ){
        //check how much bytes are available
        //int bytesAvailable = 0;
        //TODO int clearBufferLimit = 0;
        //ioctl(usb_fd,FIONREAD,&bytesAvailable);
        //logger.error("bytesAvailable")<<bytesAvailable;
        /*//TODO
        if(bytesAvailable> clearBufferLimit){
            tcflush(usb_fd,TCIFLUSH);
        }
        */
        //logger.error("bytesAvailable")<<bytesAvailable;
        // read byte
        char buf;
        ssize_t numBytes = read(usb_fd, &buf, 1);
        if(numBytes == 1)
        {
            if(mavlink_parse_char(0, buf, &msg, &receiverStatus))
            {
                // new message received, put message in temporary buffer
                messageBufferMutex.lock();
                messageBuffer.add(msg);
                messageBufferMutex.unlock();
                logger.debug("receive") << "Received Message!";
                //TODO don't think that packet_rx_drop_count is set
                //logger.error("msg.seq")<<(int)msg.seq;
                if(((int)msg.seq)-lastSequenceNumber>1){
                    logger.error("sequence number failed") << msg.seq-lastSequenceNumber;
                    sequenceFail += lastSequenceNumber-msg.seq -1;
                }
                lastSequenceNumber  = msg.seq;
                if(255 == lastSequenceNumber){
                    lastSequenceNumber = -1;
                }
                if(receiverStatus.buffer_overrun > 0){
                    logger.error("receiver")<<"buffer_overrun: "<<(int) receiverStatus.buffer_overrun;
                }
                if(receiverStatus.packet_rx_drop_count > 0){
                    logger.error("receiver")<<"packet_rx_drop_count: "<<(int) receiverStatus.packet_rx_drop_count<< " received packets"<<receiverStatus.packet_rx_success_count;
                }
                if(receiverStatus.parse_error > 0){
                    logger.error("receiver")<<"parse_error: "<<(int) receiverStatus.parse_error;
                }
            }
        }
    }
    
    logger.warn("receiverThread") << "Terminating receiver thread";
}
Example #21
0
static bool
sol_mavlink_fd_handler(void *data, int fd, uint32_t cond)
{
    struct sol_mavlink *mavlink = data;
    mavlink_message_t msg = { 0 };
    mavlink_status_t status;
    uint8_t buf[MAVLINK_MAX_PACKET_LEN] = { 0 };
    int i, res;

    res = recv(mavlink->fd, buf, MAVLINK_MAX_PACKET_LEN, 0);
    if (res == -1) {
        if (errno == EINTR) {
            SOL_INF("Could not read socket, retrying.");
            return true;
        } else {
            SOL_WRN("Could not read socket. %s",
                sol_util_strerrora(errno));
            return false;
        }
    }

    for (i = 0; i < res; ++i) {
        if (!mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status))
            continue;

        switch (msg.msgid) {
        case MAVLINK_MSG_ID_GPS_RAW_INT:
            sol_mavlink_position_handler(mavlink, &msg);
            break;
        case MAVLINK_MSG_ID_HEARTBEAT:
            sol_mavlink_heartbeat_handler(mavlink, &msg);
            break;
        case MAVLINK_MSG_ID_STATUSTEXT:
            sol_mavlink_statustext_handler(&msg);
            break;
        case MAVLINK_MSG_ID_HOME_POSITION:
            sol_mavlink_home_position_handler(mavlink, &msg);
            break;
        case MAVLINK_MSG_ID_MISSION_ITEM_REACHED:
            sol_mavlink_mission_reached_handler(mavlink);
            break;
        default:
            SOL_INF("Unhandled event, msgid: %d", msg.msgid);
        }
    }

    if (mavlink->status == SOL_MAVLINK_STATUS_FULL_SETUP) {
        mavlink->status = SOL_MAVLINK_STATUS_READY;

        if (CHECK_HANDLER(mavlink, connect))
            HANDLERS(mavlink)->connect((void *)mavlink->data, mavlink);
    }

    return true;
}
Example #22
0
File: msg.c Project: 2thetop/OpenCR
BOOL msg_recv( uint8_t chan, uint8_t data , mavlink_message_t *p_msg, mavlink_status_t *p_status )
{
  BOOL ret = FALSE;

if (mavlink_parse_char(MAVLINK_COMM_0, data, p_msg, p_status) == MAVLINK_FRAMING_OK)
{
  ret = TRUE;
}

  return ret;
}
Example #23
0
void simplegcs_update(mavlink_channel_t chan) {
    mavlink_message_t msg;
    mavlink_status_t status;
    while(comm_get_available(chan)){
        uint8_t c = comm_receive_ch(chan);
        bool newmsg = mavlink_parse_char(chan, c, &msg, &status);
        if (newmsg) {
            handle_message(chan, &msg);
        }
    }
}
Example #24
0
static unsigned int mavlink_receive(struct uart_client *cli, unsigned char *buf, unsigned int len)
{
    mavlink_message_t msg __attribute__ ((aligned(2)));
    mavlink_status_t status;
    unsigned int i = len;
    while (i--) {
        if (mavlink_parse_char(cli->ch, *(buf++), &msg, &status)) {
            mavlink_handle_msg(cli->ch, &msg, &status);
        }
    }
    return len;
}
Example #25
0
BOOL Dialog::msg_recv( uint8_t chan, uint8_t data , mavlink_message_t *p_msg, mavlink_status_t *p_status )
{
    BOOL ret = FALSE;
    ui->led_MavlinkStatus->turnOn();
    if(chan == 0)
    {
        if (mavlink_parse_char(MAVLINK_COMM_0, data, p_msg, p_status) == MAVLINK_FRAMING_OK)
        {
            ret = TRUE;
        }
    }
    else
    {
        if (mavlink_parse_char(MAVLINK_COMM_1, data, p_msg, p_status) == MAVLINK_FRAMING_OK)
        {
            ret = TRUE;
        }
    }

    return ret;
}
Example #26
0
void MavSerialPort::mavRead(QByteArray* ba){
    unsigned char *buf;
    buf = (unsigned char*)ba->data();
    //kernel part of the code
    for(int i = 0 ; i < ba->size(); i++){
        //does it matter if i change it to COMM_0 ?
        msgReceived = mavlink_parse_char(MAVLINK_COMM_1, buf[i], &message, &status);
        if(msgReceived){
            mavDecode(message);
            msgReceived = false;
        }
    }
}
Example #27
0
/**
 * Receive data from UART.
 */
void *
MavlinkReceiver::receive_thread(void *arg)
{
	int uart_fd = _mavlink->get_uart_fd();

	const int timeout = 500;
	uint8_t buf[32];

	mavlink_message_t msg;

	/* set thread name */
	char thread_name[24];
	sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id());
	prctl(PR_SET_NAME, thread_name, getpid());

	_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));

	struct pollfd fds[1];
	fds[0].fd = uart_fd;
	fds[0].events = POLLIN;

	ssize_t nread = 0;

	while (!_mavlink->_task_should_exit) {
		if (poll(fds, 1, timeout) > 0) {

			/* non-blocking read. read may return negative values */
			if ((nread = read(uart_fd, buf, sizeof(buf))) < (ssize_t)sizeof(buf)) {
				/* to avoid reading very small chunks wait for data before reading */
				usleep(1000);
			}

			/* if read failed, this loop won't execute */
			for (ssize_t i = 0; i < nread; i++) {
				if (mavlink_parse_char(_mavlink->get_channel(), buf[i], &msg, &status)) {
					/* handle generic messages and commands */
					handle_message(&msg);

					/* handle packet with waypoint component */
					_mavlink->mavlink_wpm_message_handler(&msg);

					/* handle packet with parameter component */
					_mavlink->mavlink_pm_message_handler(_mavlink->get_channel(), &msg);
				}
			}
		}
	}

	return NULL;
}
Example #28
0
/* Receive a message from the DataLink
 *	This stores the latest message received for use by the getters
 *	This function should be called at least once per loop to ensure all packets
 *	get captured. If running on a PC you should consider threading a call to this
 *	function so it is called regularly in the background
 */
int MAVLink::receiveMessage() {
	mavlink_message_t msg;
	mavlink_status_t status;
	uint8_t buffer[MAVLINK_BUFFER_SIZE];
	if (link == NULL) { return -1; }
	int msg_len = link->receive(MAVLINK_BUFFER_SIZE,(char*)buffer);
	int i;
	for (i=0; i<msg_len; i++) {
		if (mavlink_parse_char(0,buffer[i],&msg,&status)) {
			addMessage(msg);
			return msg.msgid;
		}
	}
	return -2;
}
/**
 * @brief -- check the incoming byte from GCS
 * @return -- NULL
 */
void UAS_comm::updateGcs(){
    mavlink_message_t msg;
    mavlink_status_t status;
    unsigned char c;

    while(_comm_gcs->getIncomingBytes()>0){
        _comm_gcs->fetch(&c);
        if (mavlink_parse_char(chan_gcs, c, &msg, &status)){
            //printf("GCS: message received: %d\n",msg.msgid );
            parseGcsMessage(&msg);
        }
    }

    _gcs_packet_drops += status.packet_rx_drop_count;
}
void mavlink_rx(SerialDevice *dev){

	COMM_FRAME frame;
	frame.direction = CHANNEL_IN;
	frame.channel = CHANNEL_APP_UART;

	int c;

	while((c = serial_getc(dev)) >= 0) {
		if(mavlink_parse_char(CHANNEL_APP_UART, (uint8_t)c, &(frame.mavlink_message), &mavlink_status)){
			// --> deal with received message...
			Mailbox_post(comm_mailbox, &frame, BIOS_NO_WAIT);
		}
	}
}