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 ""
// }
}
}
}
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
}
/**
* 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);
}
}
}
}
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;
}
}
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;
}
}
}
}
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);
}
}
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");
}
}
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;
}
}
}
}
/****************************************************************************
* 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);
}
}
}
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++;
}
}
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);
}
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;
}
}
}
}
}
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);
}
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";
}
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;
}
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;
}
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);
}
}
}
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;
}
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;
}
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;
}
}
}
/**
* 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;
}
/* 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);
}
}
}