void
MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
{
mavlink_radio_status_t rstatus;
mavlink_msg_radio_status_decode(msg, &rstatus);
struct telemetry_status_s tstatus;
memset(&tstatus, 0, sizeof(tstatus));
tstatus.timestamp = hrt_absolute_time();
tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO;
tstatus.rssi = rstatus.rssi;
tstatus.remote_rssi = rstatus.remrssi;
tstatus.txbuf = rstatus.txbuf;
tstatus.noise = rstatus.noise;
tstatus.remote_noise = rstatus.remnoise;
tstatus.rxerrors = rstatus.rxerrors;
tstatus.fixed = rstatus.fixed;
if (_telemetry_status_pub < 0) {
_telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
} else {
orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
}
}
void _MavlinkInterface::handleMessages()
{
Time_Stamps this_timestamps;
mavlink_message_t message;
int nMsgHandled;
nMsgHandled = 0;
//Handle Message while new message is received
while (readMessage(message))
{
// Note this doesn't handle multiple message sources.
current_messages.sysid = message.sysid;
current_messages.compid = message.compid;
system_id = current_messages.sysid;
autopilot_id = current_messages.compid;
// Handle Message ID
switch (message.msgid)
{
case MAVLINK_MSG_ID_HEARTBEAT:
{
// printf("MAVLINK_MSG_ID_HEARTBEAT\n");
mavlink_msg_heartbeat_decode(&message,
&(current_messages.heartbeat));
current_messages.time_stamps.heartbeat = get_time_usec();
this_timestamps.heartbeat = current_messages.time_stamps.heartbeat;
break;
}
case MAVLINK_MSG_ID_SYS_STATUS:
{
// printf("MAVLINK_MSG_ID_SYS_STATUS\n");
mavlink_msg_sys_status_decode(&message,
&(current_messages.sys_status));
current_messages.time_stamps.sys_status = get_time_usec();
this_timestamps.sys_status =
current_messages.time_stamps.sys_status;
break;
}
case MAVLINK_MSG_ID_BATTERY_STATUS:
{
// printf("MAVLINK_MSG_ID_BATTERY_STATUS\n");
mavlink_msg_battery_status_decode(&message,
&(current_messages.battery_status));
current_messages.time_stamps.battery_status = get_time_usec();
this_timestamps.battery_status =
current_messages.time_stamps.battery_status;
break;
}
case MAVLINK_MSG_ID_RADIO_STATUS:
{
// printf("MAVLINK_MSG_ID_RADIO_STATUS\n");
mavlink_msg_radio_status_decode(&message,
&(current_messages.radio_status));
current_messages.time_stamps.radio_status = get_time_usec();
this_timestamps.radio_status =
current_messages.time_stamps.radio_status;
break;
}
case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
{
// printf("MAVLINK_MSG_ID_LOCAL_POSITION_NED\n");
mavlink_msg_local_position_ned_decode(&message,
&(current_messages.local_position_ned));
current_messages.time_stamps.local_position_ned = get_time_usec();
this_timestamps.local_position_ned =
current_messages.time_stamps.local_position_ned;
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
{
// printf("MAVLINK_MSG_ID_GLOBAL_POSITION_INT\n");
mavlink_msg_global_position_int_decode(&message,
&(current_messages.global_position_int));
current_messages.time_stamps.global_position_int = get_time_usec();
this_timestamps.global_position_int =
current_messages.time_stamps.global_position_int;
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
{
// printf("MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED\n");
mavlink_msg_position_target_local_ned_decode(&message,
&(current_messages.position_target_local_ned));
current_messages.time_stamps.position_target_local_ned =
get_time_usec();
this_timestamps.position_target_local_ned =
current_messages.time_stamps.position_target_local_ned;
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT:
{
// printf("MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT\n");
mavlink_msg_position_target_global_int_decode(&message,
&(current_messages.position_target_global_int));
current_messages.time_stamps.position_target_global_int =
get_time_usec();
this_timestamps.position_target_global_int =
current_messages.time_stamps.position_target_global_int;
break;
}
case MAVLINK_MSG_ID_HIGHRES_IMU:
{
// printf("MAVLINK_MSG_ID_HIGHRES_IMU\n");
mavlink_msg_highres_imu_decode(&message,
&(current_messages.highres_imu));
current_messages.time_stamps.highres_imu = get_time_usec();
this_timestamps.highres_imu =
current_messages.time_stamps.highres_imu;
break;
}
case MAVLINK_MSG_ID_ATTITUDE:
{
// printf("MAVLINK_MSG_ID_ATTITUDE\n");
mavlink_msg_attitude_decode(&message, &(current_messages.attitude));
current_messages.time_stamps.attitude = get_time_usec();
this_timestamps.attitude = current_messages.time_stamps.attitude;
break;
}
default:
{
// printf("Warning, did not handle message id %i\n", message.msgid);
break;
}
} // end: switch msgid
if(++nMsgHandled >= NUM_MSG_HANDLE)return;
}
}
// ------------------------------------------------------------------------------
// Read Messages
// ------------------------------------------------------------------------------
void
Autopilot_Interface::
read_messages(FILE *fd)
{
bool success; // receive success flag
bool received_all = false; // receive only one message
Time_Stamps this_timestamps;
// fprintf(fd," xacc yacc zacc xgyro ygyro zgyro xmag ymag zmag altitude temperature\n");
// Blocking wait for new data
while ( !received_all and !time_to_exit )
{
// ----------------------------------------------------------------------
// READ MESSAGE
// ----------------------------------------------------------------------
mavlink_message_t message;
success = serial_port->read_message(message);
// printf("messageid = %d\n", message.msgid);
printf("flag = %d ch = %d \n",receive_flag,receive_ch);
// ----------------------------------------------------------------------
// HANDLE MESSAGE
// ----------------------------------------------------------------------
if( success )
{
// Store message sysid and compid.
// Note this doesn't handle multiple message sources.
current_messages.sysid = message.sysid;
current_messages.compid = message.compid;
// Handle Message ID
switch (message.msgid)
{
case MAVLINK_MSG_ID_HEARTBEAT:
{
//printf("MAVLINK_MSG_ID_HEARTBEAT\n");
mavlink_msg_heartbeat_decode(&message, &(current_messages.heartbeat));
current_messages.time_stamps.heartbeat = get_time_usec();
this_timestamps.heartbeat = current_messages.time_stamps.heartbeat;
break;
}
case MAVLINK_MSG_ID_SYS_STATUS:
{
//printf("MAVLINK_MSG_ID_SYS_STATUS\n");
mavlink_msg_sys_status_decode(&message, &(current_messages.sys_status));
current_messages.time_stamps.sys_status = get_time_usec();
this_timestamps.sys_status = current_messages.time_stamps.sys_status;
break;
}
case MAVLINK_MSG_ID_BATTERY_STATUS:
{
//printf("MAVLINK_MSG_ID_BATTERY_STATUS\n");
mavlink_msg_battery_status_decode(&message, &(current_messages.battery_status));
current_messages.time_stamps.battery_status = get_time_usec();
this_timestamps.battery_status = current_messages.time_stamps.battery_status;
break;
}
case MAVLINK_MSG_ID_RADIO_STATUS:
{
//printf("MAVLINK_MSG_ID_RADIO_STATUS\n");
mavlink_msg_radio_status_decode(&message, &(current_messages.radio_status));
current_messages.time_stamps.radio_status = get_time_usec();
this_timestamps.radio_status = current_messages.time_stamps.radio_status;
break;
}
case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
{
//printf("MAVLINK_MSG_ID_LOCAL_POSITION_NED\n");
mavlink_msg_local_position_ned_decode(&message, &(current_messages.local_position_ned));
//fprintf(fd,"x = %f y = %f z = %f \n",current_messages.local_position_ned.x,current_messages.local_position_ned.y,current_messages.local_position_ned.z);
current_messages.time_stamps.local_position_ned = get_time_usec();
this_timestamps.local_position_ned = current_messages.time_stamps.local_position_ned;
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
{
//printf("MAVLINK_MSG_ID_GLOBAL_POSITION_INT\n");
mavlink_msg_global_position_int_decode(&message, &(current_messages.global_position_int));
current_messages.time_stamps.global_position_int = get_time_usec();
this_timestamps.global_position_int = current_messages.time_stamps.global_position_int;
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
{
//printf("MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED\n");
mavlink_msg_position_target_local_ned_decode(&message, &(current_messages.position_target_local_ned));
current_messages.time_stamps.position_target_local_ned = get_time_usec();
this_timestamps.position_target_local_ned = current_messages.time_stamps.position_target_local_ned;
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT:
{
//printf("MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT\n");
mavlink_msg_position_target_global_int_decode(&message, &(current_messages.position_target_global_int));
fprintf(fd, "lat_int = %d\n", current_messages.position_target_global_int.lat_int);
current_messages.time_stamps.position_target_global_int = get_time_usec();
this_timestamps.position_target_global_int = current_messages.time_stamps.position_target_global_int;
break;
}
case MAVLINK_MSG_ID_HIGHRES_IMU:
{
//printf("MAVLINK_MSG_ID_HIGHRES_IMU\n");
mavlink_msg_highres_imu_decode(&message, &(current_messages.highres_imu));
// fprintf(fd,"acc: %f %f %f ",current_messages.highres_imu.xacc,current_messages.highres_imu.yacc,current_messages.highres_imu.zacc);
// fprintf(fd,"gyro: %f %f %f ",current_messages.highres_imu.xgyro,current_messages.highres_imu.ygyro,current_messages.highres_imu.zgyro);
// fprintf(fd,"mag: %f %f %f ",current_messages.highres_imu.xmag,current_messages.highres_imu.ymag,current_messages.highres_imu.zmag);
// fprintf(fd,"alt: %f %f\n",current_messages.highres_imu.pressure_alt,current_messages.highres_imu.temperature);
if (receive_flag && receive_ch == 10) {
fprintf(fd,"acc: %f %f %f ",current_messages.highres_imu.xacc,current_messages.highres_imu.yacc,current_messages.highres_imu.zacc);
fprintf(fd,"gyro: %f %f %f ",current_messages.highres_imu.xgyro,current_messages.highres_imu.ygyro,current_messages.highres_imu.zgyro);
fprintf(fd,"mag: %f %f %f ",current_messages.highres_imu.xmag,current_messages.highres_imu.ymag,current_messages.highres_imu.zmag);
fprintf(fd,"alt: %f %f\n",current_messages.highres_imu.pressure_alt,current_messages.highres_imu.temperature);
receive_flag = false;
}
current_messages.time_stamps.highres_imu = get_time_usec();
this_timestamps.highres_imu = current_messages.time_stamps.highres_imu;
break;
}
case MAVLINK_MSG_ID_GPS_RAW_INT:
{
mavlink_msg_gps_raw_int_decode(&message, &(current_messages.gps_raw_int));
fprintf(fd, "lat = %d lon = %d alt = %d ", current_messages.gps_raw_int.lat,current_messages.gps_raw_int.lon,current_messages.gps_raw_int.alt);
fprintf(fd, "vel = %d cog = %d satellites_visible = %d\n", current_messages.gps_raw_int.vel,current_messages.gps_raw_int.cog,current_messages.gps_raw_int.satellites_visible);
if (receive_flag && receive_ch == 10) {
fprintf(fd, "lat = %d lon = %d alt = %d ", current_messages.gps_raw_int.lat,current_messages.gps_raw_int.lon,current_messages.gps_raw_int.alt);
fprintf(fd, "vel = %d cog = %d satellites_visible = %d\n", current_messages.gps_raw_int.vel,current_messages.gps_raw_int.cog,current_messages.gps_raw_int.satellites_visible);
receive_flag = false;
}
break;
}
case MAVLINK_MSG_ID_ATTITUDE:
{
//printf("MAVLINK_MSG_ID_ATTITUDE\n");
mavlink_msg_attitude_decode(&message, &(current_messages.attitude));
current_messages.time_stamps.attitude = get_time_usec();
this_timestamps.attitude = current_messages.time_stamps.attitude;
break;
}
default:
{
// printf("Warning, did not handle message id %i\n",message.msgid);
break;
}
} // end: switch msgid
} // end: if read message
// Check for receipt of all items
received_all =
this_timestamps.heartbeat &&
// this_timestamps.battery_status &&
// this_timestamps.radio_status &&
// this_timestamps.local_position_ned &&
// this_timestamps.global_position_int &&
// this_timestamps.position_target_local_ned &&
// this_timestamps.position_target_global_int &&
// this_timestamps.highres_imu &&
// this_timestamps.attitude &&
this_timestamps.sys_status
;
// give the write thread time to use the port
if ( writing_status > false ) {
usleep(100); // look for components of batches at 10kHz
}
} // end: while not received all
return;
}
// ------------------------------------------------------------------------------
// Read Messages
// ------------------------------------------------------------------------------
void
Autopilot_Interface::
read_messages()
{
bool success; // receive success flag
bool received_all = false; // receive only one message
Time_Stamps this_timestamps;
// Blocking wait for new data
while ( not received_all and not time_to_exit )
{
// ----------------------------------------------------------------------
// READ MESSAGE
// ----------------------------------------------------------------------
mavlink_message_t message;
success = serial_port->read_message(message);
// ----------------------------------------------------------------------
// HANDLE MESSAGE
// ----------------------------------------------------------------------
if( success )
{
// Store message sysid and compid.
// Note this doesn't handle multiple message sources.
current_messages.sysid = message.sysid;
current_messages.compid = message.compid;
// Handle Message ID
switch (message.msgid)
{
case MAVLINK_MSG_ID_HEARTBEAT:
{
//printf("MAVLINK_MSG_ID_HEARTBEAT\n");
mavlink_msg_heartbeat_decode(&message, &(current_messages.heartbeat));
current_messages.time_stamps.heartbeat = get_time_usec();
this_timestamps.heartbeat = current_messages.time_stamps.heartbeat;
break;
}
case MAVLINK_MSG_ID_SYS_STATUS:
{
//printf("MAVLINK_MSG_ID_SYS_STATUS\n");
mavlink_msg_sys_status_decode(&message, &(current_messages.sys_status));
current_messages.time_stamps.sys_status = get_time_usec();
this_timestamps.sys_status = current_messages.time_stamps.sys_status;
break;
}
case MAVLINK_MSG_ID_BATTERY_STATUS:
{
//printf("MAVLINK_MSG_ID_BATTERY_STATUS\n");
mavlink_msg_battery_status_decode(&message, &(current_messages.battery_status));
current_messages.time_stamps.battery_status = get_time_usec();
this_timestamps.battery_status = current_messages.time_stamps.battery_status;
break;
}
case MAVLINK_MSG_ID_RADIO_STATUS:
{
//printf("MAVLINK_MSG_ID_RADIO_STATUS\n");
mavlink_msg_radio_status_decode(&message, &(current_messages.radio_status));
current_messages.time_stamps.radio_status = get_time_usec();
this_timestamps.radio_status = current_messages.time_stamps.radio_status;
break;
}
case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
{
//printf("MAVLINK_MSG_ID_LOCAL_POSITION_NED\n");
mavlink_msg_local_position_ned_decode(&message, &(current_messages.local_position_ned));
current_messages.time_stamps.local_position_ned = get_time_usec();
this_timestamps.local_position_ned = current_messages.time_stamps.local_position_ned;
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
{
//printf("MAVLINK_MSG_ID_GLOBAL_POSITION_INT\n");
mavlink_msg_global_position_int_decode(&message, &(current_messages.global_position_int));
current_messages.time_stamps.global_position_int = get_time_usec();
this_timestamps.global_position_int = current_messages.time_stamps.global_position_int;
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
{
//printf("MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED\n");
mavlink_msg_position_target_local_ned_decode(&message, &(current_messages.position_target_local_ned));
current_messages.time_stamps.position_target_local_ned = get_time_usec();
this_timestamps.position_target_local_ned = current_messages.time_stamps.position_target_local_ned;
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT:
{
//printf("MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT\n");
mavlink_msg_position_target_global_int_decode(&message, &(current_messages.position_target_global_int));
current_messages.time_stamps.position_target_global_int = get_time_usec();
this_timestamps.position_target_global_int = current_messages.time_stamps.position_target_global_int;
break;
}
case MAVLINK_MSG_ID_HIGHRES_IMU:
{
//printf("MAVLINK_MSG_ID_HIGHRES_IMU\n");
mavlink_msg_highres_imu_decode(&message, &(current_messages.highres_imu));
current_messages.time_stamps.highres_imu = get_time_usec();
this_timestamps.highres_imu = current_messages.time_stamps.highres_imu;
break;
}
case MAVLINK_MSG_ID_ATTITUDE:
{
//printf("MAVLINK_MSG_ID_ATTITUDE\n");
mavlink_msg_attitude_decode(&message, &(current_messages.attitude));
current_messages.time_stamps.attitude = get_time_usec();
this_timestamps.attitude = current_messages.time_stamps.attitude;
break;
}
default:
{
// printf("Warning, did not handle message id %i\n",message.msgid);
break;
}
} // end: switch msgid
} // end: if read message
// Check for receipt of all items
received_all =
this_timestamps.heartbeat &&
this_timestamps.sys_status &&
// this_timestamps.battery_status &&
// this_timestamps.radio_status &&
this_timestamps.local_position_ned &&
// this_timestamps.global_position_int &&
// this_timestamps.position_target_local_ned &&
this_timestamps.position_target_global_int &&
this_timestamps.highres_imu &&
this_timestamps.attitude ;
// give the write thread time to use the port
if ( writing_status > false )
usleep(100); // look for components of batches at 10kHz
} // end: while not received all
return;
}
// ------------------------------------------------------------------------------
// Read Messages
// ------------------------------------------------------------------------------
void
Autopilot_Interface::
read_messages()
{
int success = 0; // receive success flag
bool received_all = false; // receive only one message
Time_Stamps this_timestamps;
int err_counter = 0;
struct timespec diff_r;
struct timespec aft_read;
struct timespec bef_read;
struct timespec receive_time;
struct timespec longRelNanoSleep;
longRelNanoSleep.tv_sec = 0;
longRelNanoSleep.tv_nsec = 100000;
struct timespec RelNanoSleep;
RelNanoSleep.tv_sec = 0;
RelNanoSleep.tv_nsec = 10;
// Blocking wait for new data
// pthread_mutex_lock(&mut);
// if (blocked == 0 && writing_status)
// {
// blocked = 1;
// pthread_cond_wait(&cond,&mut);
// blocked = 0;
// }
// else
// {
// pthread_cond_signal(&cond);
// }
// clock_gettime(CLOCK_REALTIME,&bef_read);
// clock_gettime(CLOCK_REALTIME,&aft_read);
// timespec_sub(&diff_r,&aft_read,&bef_read);
// int64_t diff_r_us = diff_r.tv_sec*1e6 + diff_r.tv_nsec/1e3;
// printf("Read Time: %ld \n",diff_r_us);
//
// pthread_mutex_unlock(&mut);
mavlink_message_t message;
while ( (success != 1) && (!time_to_exit) )
{
// ----------------------------------------------------------------------
// READ ONE BYTE AT TIME UNTIL COMPLETE MESSAGE IS RECEIVED
// ----------------------------------------------------------------------
success = serial_port->read_message(message); //Have I obtained a complete message?
if ( success == -1) // Errors...retry...
{
err_counter++;
if (err_counter == 3) // To many void read
{
fprintf(stderr,"ERRORS!\n");
break;
}
}
}
err_counter = 0;
if (success == -1) return;
// ----------------------------------------------------------------------
// HANDLE MESSAGE
// ----------------------------------------------------------------------
//if( success )
// Store message sysid and compid.
// Note this doesn't handle multiple message sources.
current_messages.sysid = message.sysid;
current_messages.compid = message.compid;
// Handle Message ID
switch (message.msgid)
{
case MAVLINK_MSG_ID_HEARTBEAT:
{
//printf("MAVLINK_MSG_ID_HEARTBEAT\n");
mavlink_msg_heartbeat_decode(&message, &(current_messages.heartbeat));
base_mode = current_messages.heartbeat.base_mode;
mav_type = current_messages.heartbeat.type;
system_status = current_messages.heartbeat.system_status;
custom_mode = current_messages.heartbeat.custom_mode;
//printf("base_mode = %u\n",base_mode);
current_messages.time_stamps.heartbeat = get_time_usec();
this_timestamps.heartbeat = current_messages.time_stamps.heartbeat;
heartbeat_count++;
//printf("IMU Sensors Timestamp %u\n",current_messages.time_stamps.highres_imu/1000);
if ( (current_messages.time_stamps.heartbeat - read_heartbeat_old) > 10000000 )
{
printf("HEARTBEAT frequency : %d \n",heartbeat_count/10);
heartbeat_count = 0;
read_heartbeat_old = current_messages.time_stamps.heartbeat;
}
break;
}
case MAVLINK_MSG_ID_SYS_STATUS:
{
//printf("MAVLINK_MSG_ID_SYS_STATUS\n");
mavlink_msg_sys_status_decode(&message, &(current_messages.sys_status));
current_messages.time_stamps.sys_status = get_time_usec();
this_timestamps.sys_status = current_messages.time_stamps.sys_status;
break;
}
case MAVLINK_MSG_ID_BATTERY_STATUS:
{
//printf("MAVLINK_MSG_ID_BATTERY_STATUS\n");
mavlink_msg_battery_status_decode(&message, &(current_messages.battery_status));
current_messages.time_stamps.battery_status = get_time_usec();
this_timestamps.battery_status = current_messages.time_stamps.battery_status;
break;
}
case MAVLINK_MSG_ID_RADIO_STATUS:
{
//printf("MAVLINK_MSG_ID_RADIO_STATUS\n");
mavlink_msg_radio_status_decode(&message, &(current_messages.radio_status));
current_messages.time_stamps.radio_status = get_time_usec();
this_timestamps.radio_status = current_messages.time_stamps.radio_status;
break;
}
case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
{
//printf("MAVLINK_MSG_ID_LOCAL_POSITION_NED\n");
mavlink_msg_local_position_ned_decode(&message, &(current_messages.local_position_ned));
current_messages.time_stamps.local_position_ned = get_time_usec();
this_timestamps.local_position_ned = current_messages.time_stamps.local_position_ned;
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
{
//printf("MAVLINK_MSG_ID_GLOBAL_POSITION_INT\n");
mavlink_msg_global_position_int_decode(&message, &(current_messages.global_position_int));
current_messages.time_stamps.global_position_int = get_time_usec();
this_timestamps.global_position_int = current_messages.time_stamps.global_position_int;
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
{
//printf("MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED\n");
mavlink_msg_position_target_local_ned_decode(&message, &(current_messages.position_target_local_ned));
current_messages.time_stamps.position_target_local_ned = get_time_usec();
this_timestamps.position_target_local_ned = current_messages.time_stamps.position_target_local_ned;
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT:
{
//printf("MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT\n");
mavlink_msg_position_target_global_int_decode(&message, &(current_messages.position_target_global_int));
current_messages.time_stamps.position_target_global_int = get_time_usec();
this_timestamps.position_target_global_int = current_messages.time_stamps.position_target_global_int;
break;
}
case MAVLINK_MSG_ID_HIGHRES_IMU:
{
//printf("MAVLINK_MSG_ID_HIGHRES_IMU\n");
clock_gettime(CLOCK_REALTIME,&receive_time);
mavlink_msg_highres_imu_decode(&message, &(current_messages.highres_imu));
current_messages.time_stamps.highres_imu = get_time_usec();
this_timestamps.highres_imu = current_messages.time_stamps.highres_imu;
highres_imu_count++;
//printf("IMU Sensors Timestamp %u\n",current_messages.time_stamps.highres_imu/1000);
if ( (current_messages.time_stamps.highres_imu - read_highres_imu_old) > 10000000 )
{
printf("HIGHRES_IMU frequency : %d Hz\n",highres_imu_count/10);
highres_imu_count = 0;
read_highres_imu_old = current_messages.time_stamps.highres_imu;
}
//fprintf(logfile_rd,"HIGHRES_IMU %d, %ld \n",receive_time.tv_sec, receive_time.tv_nsec);
break;
}
case MAVLINK_MSG_ID_ATTITUDE:
{
//printf("MAVLINK_MSG_ID_ATTITUDE\n");
mavlink_msg_attitude_decode(&message, &(current_messages.attitude));
current_messages.time_stamps.attitude = get_time_usec();
this_timestamps.attitude = current_messages.time_stamps.attitude;
break;
}
case MAVLINK_MSG_ID_HIL_CONTROLS:
{
//printf("MAVLINK_MSG_ID_HIL_CONTROLS\n");
mavlink_msg_hil_controls_decode(&message, &(current_messages.hil_controls));
current_messages.time_stamps.hil_controls = get_time_usec();
this_timestamps.hil_controls = current_messages.time_stamps.hil_controls;
float data[4] = {current_messages.hil_controls.roll_ailerons,
current_messages.hil_controls.pitch_elevator,
current_messages.hil_controls.yaw_rudder,
current_messages.hil_controls.throttle};
int bsent = udp_mtl->send_bytes((char *)data,sizeof(float[4]));
hil_controls_count++;
//printf("IMU Sensors Timestamp %u\n",current_messages.time_stamps.highres_imu/1000);
if ( (current_messages.time_stamps.hil_controls - read_hil_controls_old) > 10000000 )
{
printf("HIL_CONTROLS frequency : %d Hz\n",hil_controls_count/10);
hil_controls_count = 0;
read_hil_controls_old = current_messages.time_stamps.hil_controls;
printf("%0.2f | ",current_messages.hil_controls.roll_ailerons);
printf("%0.2f | ",current_messages.hil_controls.pitch_elevator);
printf("%0.2f | ",current_messages.hil_controls.yaw_rudder);
printf("%0.2f | \n",current_messages.hil_controls.throttle);
}
//fprintf(logfile_rd,"HIL_CONTROLS %d, %ld \n",receive_time.tv_sec, receive_time.tv_nsec);
break;
}
default:
{
// printf("Warning, did not handle message id %i\n",message.msgid);
break;
}
} // end: switch msgid
return;
}
static void
handle_message(mavlink_message_t *msg)
{
if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
mavlink_command_long_t cmd_mavlink;
mavlink_msg_command_long_decode(msg, &cmd_mavlink);
if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid)
|| (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
//check for MAVLINK terminate command
if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
/* This is the link shutdown command, terminate mavlink */
printf("[mavlink] Terminating .. \n");
fflush(stdout);
usleep(50000);
/* terminate other threads and this thread */
thread_should_exit = true;
} else {
/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
vcmd.param1 = cmd_mavlink.param1;
vcmd.param2 = cmd_mavlink.param2;
vcmd.param3 = cmd_mavlink.param3;
vcmd.param4 = cmd_mavlink.param4;
vcmd.param5 = cmd_mavlink.param5;
vcmd.param6 = cmd_mavlink.param6;
vcmd.param7 = cmd_mavlink.param7;
// XXX do proper translation
vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command;
vcmd.target_system = cmd_mavlink.target_system;
vcmd.target_component = cmd_mavlink.target_component;
vcmd.source_system = msg->sysid;
vcmd.source_component = msg->compid;
vcmd.confirmation = cmd_mavlink.confirmation;
/* check if topic is advertised */
if (cmd_pub <= 0) {
cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
} else {
/* publish */
orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
}
}
}
}
if (msg->msgid == MAVLINK_MSG_ID_OPTICAL_FLOW) {
mavlink_optical_flow_t flow;
mavlink_msg_optical_flow_decode(msg, &flow);
struct optical_flow_s f;
f.timestamp = flow.time_usec;
f.flow_raw_x = flow.flow_x;
f.flow_raw_y = flow.flow_y;
f.flow_comp_x_m = flow.flow_comp_m_x;
f.flow_comp_y_m = flow.flow_comp_m_y;
f.ground_distance_m = flow.ground_distance;
f.quality = flow.quality;
f.sensor_id = flow.sensor_id;
/* check if topic is advertised */
if (flow_pub <= 0) {
flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
} else {
/* publish */
orb_publish(ORB_ID(optical_flow), flow_pub, &f);
}
}
if (msg->msgid == MAVLINK_MSG_ID_SET_MODE) {
/* Set mode on request */
mavlink_set_mode_t new_mode;
mavlink_msg_set_mode_decode(msg, &new_mode);
union px4_custom_mode custom_mode;
custom_mode.data = new_mode.custom_mode;
/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
vcmd.param1 = new_mode.base_mode;
vcmd.param2 = custom_mode.main_mode;
vcmd.param3 = 0;
vcmd.param4 = 0;
vcmd.param5 = 0;
vcmd.param6 = 0;
vcmd.param7 = 0;
vcmd.command = VEHICLE_CMD_DO_SET_MODE;
vcmd.target_system = new_mode.target_system;
vcmd.target_component = MAV_COMP_ID_ALL;
vcmd.source_system = msg->sysid;
vcmd.source_component = msg->compid;
vcmd.confirmation = 1;
/* check if topic is advertised */
if (cmd_pub <= 0) {
cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
} else {
/* create command */
orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
}
}
/* Handle Vicon position estimates */
if (msg->msgid == MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE) {
mavlink_vicon_position_estimate_t pos;
mavlink_msg_vicon_position_estimate_decode(msg, &pos);
vicon_position.timestamp = hrt_absolute_time();
vicon_position.x = pos.x;
vicon_position.y = pos.y;
vicon_position.z = pos.z;
vicon_position.roll = pos.roll;
vicon_position.pitch = pos.pitch;
vicon_position.yaw = pos.yaw;
if (vicon_position_pub <= 0) {
vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position);
} else {
orb_publish(ORB_ID(vehicle_vicon_position), vicon_position_pub, &vicon_position);
}
}
/* Handle quadrotor motor setpoints */
if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST) {
mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint;
mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint);
if (mavlink_system.sysid < 4) {
/* switch to a receiving link mode */
gcs_link = false;
/*
* rate control mode - defined by MAVLink
*/
uint8_t ml_mode = 0;
bool ml_armed = false;
switch (quad_motors_setpoint.mode) {
case 0:
ml_armed = false;
break;
case 1:
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES;
ml_armed = true;
break;
case 2:
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE;
ml_armed = true;
break;
case 3:
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY;
break;
case 4:
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION;
break;
}
offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX;
offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / (float)INT16_MAX;
offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / (float)INT16_MAX;
offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX;
if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) {
ml_armed = false;
}
offboard_control_sp.armed = ml_armed;
offboard_control_sp.mode = static_cast<enum OFFBOARD_CONTROL_MODE>(ml_mode);
offboard_control_sp.timestamp = hrt_absolute_time();
/* check if topic has to be advertised */
if (offboard_control_sp_pub <= 0) {
offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
} else {
/* Publish */
orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp);
}
}
}
/* handle status updates of the radio */
if (msg->msgid == MAVLINK_MSG_ID_RADIO_STATUS) {
struct telemetry_status_s tstatus;
mavlink_radio_status_t rstatus;
mavlink_msg_radio_status_decode(msg, &rstatus);
/* publish telemetry status topic */
tstatus.timestamp = hrt_absolute_time();
tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO;
tstatus.rssi = rstatus.rssi;
tstatus.remote_rssi = rstatus.remrssi;
tstatus.txbuf = rstatus.txbuf;
tstatus.noise = rstatus.noise;
tstatus.remote_noise = rstatus.remnoise;
tstatus.rxerrors = rstatus.rxerrors;
tstatus.fixed = rstatus.fixed;
if (telemetry_status_pub == 0) {
telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
} else {
orb_publish(ORB_ID(telemetry_status), telemetry_status_pub, &tstatus);
}
}
/*
* Only decode hil messages in HIL mode.
*
* The HIL mode is enabled by the HIL bit flag
* in the system mode. Either send a set mode
* COMMAND_LONG message or a SET_MODE message
*/
if (mavlink_hil_enabled) {
uint64_t timestamp = hrt_absolute_time();
if (msg->msgid == MAVLINK_MSG_ID_HIL_SENSOR) {
mavlink_hil_sensor_t imu;
mavlink_msg_hil_sensor_decode(msg, &imu);
/* sensors general */
hil_sensors.timestamp = hrt_absolute_time();
/* hil gyro */
static const float mrad2rad = 1.0e-3f;
hil_sensors.gyro_counter = hil_counter;
hil_sensors.gyro_raw[0] = imu.xgyro / mrad2rad;
hil_sensors.gyro_raw[1] = imu.ygyro / mrad2rad;
hil_sensors.gyro_raw[2] = imu.zgyro / mrad2rad;
hil_sensors.gyro_rad_s[0] = imu.xgyro;
hil_sensors.gyro_rad_s[1] = imu.ygyro;
hil_sensors.gyro_rad_s[2] = imu.zgyro;
/* accelerometer */
hil_sensors.accelerometer_counter = hil_counter;
static const float mg2ms2 = 9.8f / 1000.0f;
hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2;
hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2;
hil_sensors.accelerometer_raw[2] = imu.zacc / mg2ms2;
hil_sensors.accelerometer_m_s2[0] = imu.xacc;
hil_sensors.accelerometer_m_s2[1] = imu.yacc;
hil_sensors.accelerometer_m_s2[2] = imu.zacc;
hil_sensors.accelerometer_mode = 0; // TODO what is this?
hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16
/* adc */
hil_sensors.adc_voltage_v[0] = 0.0f;
hil_sensors.adc_voltage_v[1] = 0.0f;
hil_sensors.adc_voltage_v[2] = 0.0f;
/* magnetometer */
float mga2ga = 1.0e-3f;
hil_sensors.magnetometer_counter = hil_counter;
hil_sensors.magnetometer_raw[0] = imu.xmag / mga2ga;
hil_sensors.magnetometer_raw[1] = imu.ymag / mga2ga;
hil_sensors.magnetometer_raw[2] = imu.zmag / mga2ga;
hil_sensors.magnetometer_ga[0] = imu.xmag;
hil_sensors.magnetometer_ga[1] = imu.ymag;
hil_sensors.magnetometer_ga[2] = imu.zmag;
hil_sensors.magnetometer_range_ga = 32.7f; // int16
hil_sensors.magnetometer_mode = 0; // TODO what is this
hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f;
/* baro */
hil_sensors.baro_pres_mbar = imu.abs_pressure;
hil_sensors.baro_alt_meter = imu.pressure_alt;
hil_sensors.baro_temp_celcius = imu.temperature;
hil_sensors.gyro_counter = hil_counter;
hil_sensors.magnetometer_counter = hil_counter;
hil_sensors.accelerometer_counter = hil_counter;
/* differential pressure */
hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa
hil_sensors.differential_pressure_counter = hil_counter;
/* airspeed from differential pressure, ambient pressure and temp */
struct airspeed_s airspeed;
float ias = calc_indicated_airspeed(hil_sensors.differential_pressure_pa);
// XXX need to fix this
float tas = ias;
airspeed.timestamp = hrt_absolute_time();
airspeed.indicated_airspeed_m_s = ias;
airspeed.true_airspeed_m_s = tas;
if (pub_hil_airspeed < 0) {
pub_hil_airspeed = orb_advertise(ORB_ID(airspeed), &airspeed);
} else {
orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed);
}
//warnx("SENSOR: IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s);
/* individual sensor publications */
struct gyro_report gyro;
gyro.x_raw = imu.xgyro / mrad2rad;
gyro.y_raw = imu.ygyro / mrad2rad;
gyro.z_raw = imu.zgyro / mrad2rad;
gyro.x = imu.xgyro;
gyro.y = imu.ygyro;
gyro.z = imu.zgyro;
gyro.temperature = imu.temperature;
gyro.timestamp = hrt_absolute_time();
if (pub_hil_gyro < 0) {
pub_hil_gyro = orb_advertise(ORB_ID(sensor_gyro), &gyro);
} else {
orb_publish(ORB_ID(sensor_gyro), pub_hil_gyro, &gyro);
}
struct accel_report accel;
accel.x_raw = imu.xacc / mg2ms2;
accel.y_raw = imu.yacc / mg2ms2;
accel.z_raw = imu.zacc / mg2ms2;
accel.x = imu.xacc;
accel.y = imu.yacc;
accel.z = imu.zacc;
accel.temperature = imu.temperature;
accel.timestamp = hrt_absolute_time();
if (pub_hil_accel < 0) {
pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel);
} else {
orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel);
}
struct mag_report mag;
mag.x_raw = imu.xmag / mga2ga;
mag.y_raw = imu.ymag / mga2ga;
mag.z_raw = imu.zmag / mga2ga;
mag.x = imu.xmag;
mag.y = imu.ymag;
mag.z = imu.zmag;
mag.timestamp = hrt_absolute_time();
if (pub_hil_mag < 0) {
pub_hil_mag = orb_advertise(ORB_ID(sensor_mag), &mag);
} else {
orb_publish(ORB_ID(sensor_mag), pub_hil_mag, &mag);
}
struct baro_report baro;
baro.pressure = imu.abs_pressure;
baro.altitude = imu.pressure_alt;
baro.temperature = imu.temperature;
baro.timestamp = hrt_absolute_time();
if (pub_hil_baro < 0) {
pub_hil_baro = orb_advertise(ORB_ID(sensor_baro), &baro);
} else {
orb_publish(ORB_ID(sensor_baro), pub_hil_baro, &baro);
}
/* publish combined sensor topic */
if (pub_hil_sensors > 0) {
orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
} else {
pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
}
/* fill in HIL battery status */
hil_battery_status.timestamp = hrt_absolute_time();
hil_battery_status.voltage_v = 11.1f;
hil_battery_status.current_a = 10.0f;
/* lazily publish the battery voltage */
if (pub_hil_battery > 0) {
orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status);
} else {
pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
}
// increment counters
hil_counter++;
hil_frames++;
// output
if ((timestamp - old_timestamp) > 10000000) {
printf("receiving hil sensor at %d hz\n", hil_frames / 10);
old_timestamp = timestamp;
hil_frames = 0;
}
}
if (msg->msgid == MAVLINK_MSG_ID_HIL_GPS) {
mavlink_hil_gps_t gps;
mavlink_msg_hil_gps_decode(msg, &gps);
/* gps */
hil_gps.timestamp_position = gps.time_usec;
hil_gps.time_gps_usec = gps.time_usec;
hil_gps.lat = gps.lat;
hil_gps.lon = gps.lon;
hil_gps.alt = gps.alt;
hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m
hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m
hil_gps.s_variance_m_s = 5.0f;
hil_gps.p_variance_m = hil_gps.eph_m * hil_gps.eph_m;
hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s
/* gps.cog is in degrees 0..360 * 100, heading is -PI..+PI */
float heading_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f;
/* go back to -PI..PI */
if (heading_rad > M_PI_F)
heading_rad -= 2.0f * M_PI_F;
hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m
hil_gps.vel_e_m_s = gps.ve * 1e-2f; // from cm to m
hil_gps.vel_d_m_s = gps.vd * 1e-2f; // from cm to m
hil_gps.vel_ned_valid = true;
/* COG (course over ground) is spec'ed as -PI..+PI */
hil_gps.cog_rad = heading_rad;
hil_gps.fix_type = gps.fix_type;
hil_gps.satellites_visible = gps.satellites_visible;
/* publish GPS measurement data */
if (pub_hil_gps > 0) {
orb_publish(ORB_ID(vehicle_gps_position), pub_hil_gps, &hil_gps);
} else {
pub_hil_gps = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
}
}
if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE_QUATERNION) {
mavlink_hil_state_quaternion_t hil_state;
mavlink_msg_hil_state_quaternion_decode(msg, &hil_state);
struct airspeed_s airspeed;
airspeed.timestamp = hrt_absolute_time();
airspeed.indicated_airspeed_m_s = hil_state.ind_airspeed * 1e-2f;
airspeed.true_airspeed_m_s = hil_state.true_airspeed * 1e-2f;
if (pub_hil_airspeed < 0) {
pub_hil_airspeed = orb_advertise(ORB_ID(airspeed), &airspeed);
} else {
orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed);
}
uint64_t timestamp = hrt_absolute_time();
// publish global position
if (pub_hil_global_pos > 0) {
orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos);
// global position packet
hil_global_pos.timestamp = timestamp;
hil_global_pos.valid = true;
hil_global_pos.lat = hil_state.lat;
hil_global_pos.lon = hil_state.lon;
hil_global_pos.alt = hil_state.alt / 1000.0f;
hil_global_pos.vx = hil_state.vx / 100.0f;
hil_global_pos.vy = hil_state.vy / 100.0f;
hil_global_pos.vz = hil_state.vz / 100.0f;
} else {
pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
}
// publish local position
if (pub_hil_local_pos > 0) {
float x;
float y;
bool landed = hil_state.alt/1000.0f < (alt0 + 0.1); // XXX improve?
double lat = hil_state.lat*1e-7;
double lon = hil_state.lon*1e-7;
map_projection_project(lat, lon, &x, &y);
hil_local_pos.timestamp = timestamp;
hil_local_pos.xy_valid = true;
hil_local_pos.z_valid = true;
hil_local_pos.v_xy_valid = true;
hil_local_pos.v_z_valid = true;
hil_local_pos.x = x;
hil_local_pos.y = y;
hil_local_pos.z = alt0 - hil_state.alt/1000.0f;
hil_local_pos.vx = hil_state.vx/100.0f;
hil_local_pos.vy = hil_state.vy/100.0f;
hil_local_pos.vz = hil_state.vz/100.0f;
hil_local_pos.yaw = hil_attitude.yaw;
hil_local_pos.xy_global = true;
hil_local_pos.z_global = true;
hil_local_pos.ref_timestamp = timestamp;
hil_local_pos.ref_lat = hil_state.lat;
hil_local_pos.ref_lon = hil_state.lon;
hil_local_pos.ref_alt = alt0;
hil_local_pos.landed = landed;
orb_publish(ORB_ID(vehicle_local_position), pub_hil_local_pos, &hil_local_pos);
} else {
pub_hil_local_pos = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos);
lat0 = hil_state.lat;
lon0 = hil_state.lon;
alt0 = hil_state.alt / 1000.0f;
map_projection_init(hil_state.lat, hil_state.lon);
}
/* Calculate Rotation Matrix */
math::Quaternion q(hil_state.attitude_quaternion);
math::Dcm C_nb(q);
math::EulerAngles euler(C_nb);
/* set rotation matrix */
for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
hil_attitude.R[i][j] = C_nb(i, j);
hil_attitude.R_valid = true;
/* set quaternion */
hil_attitude.q[0] = q(0);
hil_attitude.q[1] = q(1);
hil_attitude.q[2] = q(2);
hil_attitude.q[3] = q(3);
hil_attitude.q_valid = true;
hil_attitude.roll = euler.getPhi();
hil_attitude.pitch = euler.getTheta();
hil_attitude.yaw = euler.getPsi();
hil_attitude.rollspeed = hil_state.rollspeed;
hil_attitude.pitchspeed = hil_state.pitchspeed;
hil_attitude.yawspeed = hil_state.yawspeed;
/* set timestamp and notify processes (broadcast) */
hil_attitude.timestamp = hrt_absolute_time();
if (pub_hil_attitude > 0) {
orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude);
} else {
pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
}
struct accel_report accel;
accel.x_raw = hil_state.xacc / 9.81f * 1e3f;
accel.y_raw = hil_state.yacc / 9.81f * 1e3f;
accel.z_raw = hil_state.zacc / 9.81f * 1e3f;
accel.x = hil_state.xacc;
accel.y = hil_state.yacc;
accel.z = hil_state.zacc;
accel.temperature = 25.0f;
accel.timestamp = hrt_absolute_time();
if (pub_hil_accel < 0) {
pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel);
} else {
orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel);
}
/* fill in HIL battery status */
hil_battery_status.timestamp = hrt_absolute_time();
hil_battery_status.voltage_v = 11.1f;
hil_battery_status.current_a = 10.0f;
/* lazily publish the battery voltage */
if (pub_hil_battery > 0) {
orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status);
} else {
pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
}
}
if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {
mavlink_manual_control_t man;
mavlink_msg_manual_control_decode(msg, &man);
struct rc_channels_s rc_hil;
memset(&rc_hil, 0, sizeof(rc_hil));
static orb_advert_t rc_pub = 0;
rc_hil.timestamp = hrt_absolute_time();
rc_hil.chan_count = 4;
rc_hil.chan[0].scaled = man.x / 1000.0f;
rc_hil.chan[1].scaled = man.y / 1000.0f;
rc_hil.chan[2].scaled = man.r / 1000.0f;
rc_hil.chan[3].scaled = man.z / 1000.0f;
struct manual_control_setpoint_s mc;
static orb_advert_t mc_pub = 0;
int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
/* get a copy first, to prevent altering values that are not sent by the mavlink command */
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &mc);
mc.timestamp = rc_hil.timestamp;
mc.roll = man.x / 1000.0f;
mc.pitch = man.y / 1000.0f;
mc.yaw = man.r / 1000.0f;
mc.throttle = man.z / 1000.0f;
/* fake RC channels with manual control input from simulator */
if (rc_pub == 0) {
rc_pub = orb_advertise(ORB_ID(rc_channels), &rc_hil);
} else {
orb_publish(ORB_ID(rc_channels), rc_pub, &rc_hil);
}
if (mc_pub == 0) {
mc_pub = orb_advertise(ORB_ID(manual_control_setpoint), &mc);
} else {
orb_publish(ORB_ID(manual_control_setpoint), mc_pub, &mc);
}
}
}
}
void _Mavlink::handleMessages()
{
mavlink_message_t message;
int nMsgHandled = 0;
//Handle Message while new message is received
while (readMessage(message))
{
// Note this doesn't handle multiple message sources.
m_msg.sysid = message.sysid;
m_msg.compid = message.compid;
// Handle Message ID
switch (message.msgid)
{
case MAVLINK_MSG_ID_HEARTBEAT:
{
LOG(INFO)<<"-> MAVLINK_MSG_ID_HEARTBEAT";
mavlink_msg_heartbeat_decode(&message, &(m_msg.heartbeat));
m_msg.time_stamps.heartbeat = get_time_usec();
if (m_msg.heartbeat.type != MAV_TYPE_GCS)
{
m_systemID = m_msg.sysid;
m_targetComponentID = m_msg.compid;
LOG_I("-> SYSTEM_ID:"<<m_systemID
<<" COMPONENT_ID:"<<m_componentID
<<" TARGET_COMPONENT_ID:"<<m_targetComponentID);
}
else
{
LOG_I("->HEARTBEAT FROM MAV_TYPE_GCS");
}
break;
}
case MAVLINK_MSG_ID_SYS_STATUS:
{
LOG_I("-> MAVLINK_MSG_ID_SYS_STATUS");
mavlink_msg_sys_status_decode(&message,
&(m_msg.sys_status));
m_msg.time_stamps.sys_status = get_time_usec();
break;
}
case MAVLINK_MSG_ID_BATTERY_STATUS:
{
LOG_I("-> MAVLINK_MSG_ID_BATTERY_STATUS");
mavlink_msg_battery_status_decode(&message,
&(m_msg.battery_status));
m_msg.time_stamps.battery_status = get_time_usec();
break;
}
case MAVLINK_MSG_ID_RADIO_STATUS:
{
LOG_I("-> MAVLINK_MSG_ID_RADIO_STATUS");
mavlink_msg_radio_status_decode(&message,
&(m_msg.radio_status));
m_msg.time_stamps.radio_status = get_time_usec();
break;
}
case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
{
LOG_I("-> MAVLINK_MSG_ID_LOCAL_POSITION_NED");
mavlink_msg_local_position_ned_decode(&message,
&(m_msg.local_position_ned));
m_msg.time_stamps.local_position_ned = get_time_usec();
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
{
LOG_I("-> MAVLINK_MSG_ID_GLOBAL_POSITION_INT");
mavlink_msg_global_position_int_decode(&message,
&(m_msg.global_position_int));
m_msg.time_stamps.global_position_int = get_time_usec();
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
{
LOG_I("-> MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED");
mavlink_msg_position_target_local_ned_decode(&message,
&(m_msg.position_target_local_ned));
m_msg.time_stamps.position_target_local_ned =
get_time_usec();
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT:
{
LOG_I("-> MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT");
mavlink_msg_position_target_global_int_decode(&message,
&(m_msg.position_target_global_int));
m_msg.time_stamps.position_target_global_int =
get_time_usec();
break;
}
case MAVLINK_MSG_ID_HIGHRES_IMU:
{
LOG_I("-> MAVLINK_MSG_ID_HIGHRES_IMU");
mavlink_msg_highres_imu_decode(&message,
&(m_msg.highres_imu));
m_msg.time_stamps.highres_imu = get_time_usec();
break;
}
case MAVLINK_MSG_ID_ATTITUDE:
{
LOG_I("-> MAVLINK_MSG_ID_ATTITUDE");
mavlink_msg_attitude_decode(&message, &(m_msg.attitude));
m_msg.time_stamps.attitude = get_time_usec();
break;
}
case MAVLINK_MSG_ID_COMMAND_ACK:
{
mavlink_msg_command_ack_decode(&message,
&(m_msg.command_ack));
m_msg.time_stamps.attitude = get_time_usec();
LOG_I("-> MAVLINK_MSG_ID_COMMAND_ACK:"<<m_msg.command_ack.result);
break;
}
default:
{
LOG_I("-> UNKNOWN MSG_ID:"<<message.msgid);
break;
}
}
if (++nMsgHandled >= NUM_MSG_HANDLE)
return;
}
}
// ------------------------------------------------------------------------------
// Read Messages
// ------------------------------------------------------------------------------
void
Autopilot_Interface::
read_messages()
{
bool success; // receive success flag
bool received_all = false; // receive only one message
Time_Stamps this_timestamps;
// Blocking wait for new data
while ( not received_all and not time_to_exit )
{
// ----------------------------------------------------------------------
// READ MESSAGE
// ----------------------------------------------------------------------
mavlink_message_t message;
success = serial_port->read_message(message);
// ----------------------------------------------------------------------
// HANDLE MESSAGE
// ----------------------------------------------------------------------
if( success )
{
// Store message sysid and compid.
// Note this doesn't handle multiple message sources.
current_messages.sysid = message.sysid;
current_messages.compid = message.compid;
// Handle Message ID
switch (message.msgid)
{
case MAVLINK_MSG_ID_HEARTBEAT:
{
std::cout << "MAVLINK_MSG_ID_HEARTBEAT" << std::endl;
mavlink_msg_heartbeat_decode(&message, &(current_messages.heartbeat));
current_messages.time_stamps.heartbeat = get_time_usec();
this_timestamps.heartbeat = current_messages.time_stamps.heartbeat;
break;
}
case MAVLINK_MSG_ID_SYS_STATUS:
{
//std::cout << "MAVLINK_MSG_ID_SYS_STATUS" << std::endl;
mavlink_msg_sys_status_decode(&message, &(current_messages.sys_status));
current_messages.time_stamps.sys_status = get_time_usec();
this_timestamps.sys_status = current_messages.time_stamps.sys_status;
break;
}
case MAVLINK_MSG_ID_BATTERY_STATUS:
{
std::cout << "MAVLINK_MSG_ID_BATTERY_STATUS" << std::endl;
mavlink_msg_battery_status_decode(&message, &(current_messages.battery_status));
current_messages.time_stamps.battery_status = get_time_usec();
this_timestamps.battery_status = current_messages.time_stamps.battery_status;
break;
}
case MAVLINK_MSG_ID_RADIO_STATUS:
{
std::cout << "MAVLINK_MSG_ID_RADIO_STATUS" << std::endl;
mavlink_msg_radio_status_decode(&message, &(current_messages.radio_status));
current_messages.time_stamps.radio_status = get_time_usec();
this_timestamps.radio_status = current_messages.time_stamps.radio_status;
break;
}
case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
{
std::cout << "MAVLINK_MSG_ID_LOCAL_POSITION_NED" << std::endl;
mavlink_msg_local_position_ned_decode(&message, &(current_messages.local_position_ned));
current_messages.time_stamps.local_position_ned = get_time_usec();
this_timestamps.local_position_ned = current_messages.time_stamps.local_position_ned;
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
{
std::cout << "MAVLINK_MSG_ID_GLOBAL_POSITION_INT" << std::endl;
mavlink_msg_global_position_int_decode(&message, &(current_messages.global_position_int));
current_messages.time_stamps.global_position_int = get_time_usec();
this_timestamps.global_position_int = current_messages.time_stamps.global_position_int;
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
{
std::cout << "MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED" << std::endl;
mavlink_msg_position_target_local_ned_decode(&message, &(current_messages.position_target_local_ned));
current_messages.time_stamps.position_target_local_ned = get_time_usec();
this_timestamps.position_target_local_ned = current_messages.time_stamps.position_target_local_ned;
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT:
{
std::cout << "MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT" << std::endl;
mavlink_msg_position_target_global_int_decode(&message, &(current_messages.position_target_global_int));
current_messages.time_stamps.position_target_global_int = get_time_usec();
this_timestamps.position_target_global_int = current_messages.time_stamps.position_target_global_int;
break;
}
case MAVLINK_MSG_ID_HIGHRES_IMU:
{
std::cout << "MAVLINK_MSG_ID_HIGHRES_IMU" << std::endl;
mavlink_msg_highres_imu_decode(&message, &(current_messages.highres_imu));
current_messages.time_stamps.highres_imu = get_time_usec();
this_timestamps.highres_imu = current_messages.time_stamps.highres_imu;
break;
}
case MAVLINK_MSG_ID_ATTITUDE:
{
vector4d quaternion;
std::cout << "MAVLINK_MSG_ID_ATTITUDE" << std::endl;
mavlink_msg_attitude_decode(&message, &(current_messages.attitude));
current_messages.time_stamps.attitude = get_time_usec();
this_timestamps.attitude = current_messages.time_stamps.attitude;
quaternion_from_euler(&quaternion, current_messages.attitude.roll,
current_messages.attitude.pitch, current_messages.attitude.yaw);
std::cout <<
"\troll: " << current_messages.attitude.roll <<
"\tpitch: " << current_messages.attitude.pitch <<
"\tyaw: " << current_messages.attitude.yaw <<
std::endl;
std::cout <<
"\trollspeed: " << current_messages.attitude.rollspeed <<
"\tpitchspeed: " << current_messages.attitude.pitchspeed <<
"\tyawspeed: " << current_messages.attitude.yawspeed <<
std::endl;
std::cout <<
"\tqx: " << quaternion.x <<
"\tqy: " << quaternion.y <<
"\tqz: " << quaternion.z <<
"\tqw: " << quaternion.w <<
std::endl;
break;
}
case MAVLINK_MSG_ID_DEBUG:
{
std::cout << "MAVLINK_MSG_ID_DEBUG" << std::endl;
/* mavlink_msg_debug_decode(&message, &(current_messages.attitude));
current_messages.time_stamps.attitude = get_time_usec();
this_timestamps.attitude = current_messages.time_stamps.attitude;*/
break;
}
case MAVLINK_MSG_ID_STATUSTEXT:
{
std::cout << "MAVLINK_MSG_ID_STATUSTEXT: ";
mavlink_msg_statustext_decode(&message, &(current_messages.statustext));
current_messages.statustext.text[50] = 0;
printf("%d - '%s'\n", current_messages.statustext.severity, current_messages.statustext.text);
current_messages.time_stamps.statustext = get_time_usec();
this_timestamps.statustext = current_messages.time_stamps.statustext;
break;
}
case MAVLINK_MSG_ID_RAW_IMU:
{
std::cout << "MAVLINK_MSG_ID_RAW_IMU:" << std::endl;
mavlink_msg_raw_imu_decode(&message, &(current_messages.raw_imu));
std::cout << "\tacc :\t" << current_messages.raw_imu.xacc <<
"\t" << current_messages.raw_imu.yacc <<
"\t" << current_messages.raw_imu.zacc <<
std::endl;
std::cout << "\tgyro:\t" << current_messages.raw_imu.xgyro <<
"\t" << current_messages.raw_imu.ygyro <<
"\t" << current_messages.raw_imu.zgyro <<
std::endl;
std::cout << "\tmag:\t" << current_messages.raw_imu.xmag <<
"\t" << current_messages.raw_imu.ymag <<
"\t" << current_messages.raw_imu.zmag <<
std::endl;
current_messages.time_stamps.raw_imu = get_time_usec();
this_timestamps.raw_imu = current_messages.time_stamps.raw_imu;
break;
}
case MAVLINK_MSG_ID_GPS_RAW_INT:
{
std::cout << "MAVLINK_MSG_ID_GPS_RAW_INT:" << std::endl;
mavlink_msg_gps_raw_int_decode(&message, &(current_messages.gps_raw_int));
current_messages.time_stamps.gps_raw_int = get_time_usec();
this_timestamps.gps_raw_int = current_messages.time_stamps.gps_raw_int;
std::cout <<
"\tlat: " << current_messages.gps_raw_int.lat <<
"\tlon: " << current_messages.gps_raw_int.lon <<
"\talt: " << current_messages.gps_raw_int.alt <<
std::endl;
std::cout <<
"\teph: " << current_messages.gps_raw_int.eph <<
"\tepv: " << current_messages.gps_raw_int.epv <<
std::endl;
std::cout <<
"\tvel: " << current_messages.gps_raw_int.vel <<
"\tcog: " << current_messages.gps_raw_int.cog <<
std::endl;
std::cout <<
"\tfix: " << (int)current_messages.gps_raw_int.fix_type <<
"\tsat: " << (int)current_messages.gps_raw_int.satellites_visible <<
std::endl;
break;
}
case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT:
{
vector4d quaternion;
std::cout << "MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT:" << std::endl;
mavlink_msg_nav_controller_output_decode(&message, &(current_messages.nav_controller_output));
current_messages.time_stamps.nav_controller_output = get_time_usec();
this_timestamps.nav_controller_output = current_messages.time_stamps.nav_controller_output;
quaternion_from_euler(&quaternion, current_messages.nav_controller_output.nav_roll,
current_messages.nav_controller_output.nav_pitch, current_messages.nav_controller_output.nav_bearing);
std::cout <<
"\tnav_roll: " << current_messages.nav_controller_output.nav_roll <<
"\tnav_pitch: " << current_messages.nav_controller_output.nav_pitch <<
"\tnav_bearing: " << current_messages.nav_controller_output.nav_bearing <<
std::endl;
std::cout <<
"\tqx: " << quaternion.x <<
"\tqy: " << quaternion.y <<
"\tqz: " << quaternion.z <<
"\tqw: " << quaternion.w <<
std::endl;
std::cout <<
"\ttarget_bearing: " << current_messages.nav_controller_output.target_bearing <<
"\twp_dist: " << current_messages.nav_controller_output.wp_dist <<
std::endl;
std::cout <<
"\talt_error: " << current_messages.nav_controller_output.alt_error <<
"\taspd_error: " << current_messages.nav_controller_output.aspd_error <<
"\txtrack_error: " << current_messages.nav_controller_output.xtrack_error <<
std::endl;
break;
}
case MAVLINK_MSG_ID_SCALED_PRESSURE:
{
std::cout << "MAVLINK_MSG_ID_SCALED_PRESSURE:" << std::endl;
mavlink_msg_scaled_pressure_decode(&message, &(current_messages.scaled_pressure));
current_messages.time_stamps.scaled_pressure = get_time_usec();
this_timestamps.scaled_pressure = current_messages.time_stamps.scaled_pressure;
std::cout <<
"\tpress_abs: " << current_messages.scaled_pressure.press_abs <<
"\tpress_diff: " << current_messages.scaled_pressure.press_diff <<
"\ttemperature: " << ((double)current_messages.scaled_pressure.temperature / 100.0) <<
std::endl;
break;
}
case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
{
std::cout << "MAVLINK_MSG_ID_RC_CHANNELS_RAW:" << std::endl;
mavlink_msg_rc_channels_raw_decode(&message, &(current_messages.rc_channels_raw));
current_messages.time_stamps.rc_channels_raw = get_time_usec();
this_timestamps.rc_channels_raw = current_messages.time_stamps.rc_channels_raw;
std::cout <<
"\tport: " << (int)current_messages.rc_channels_raw.port <<
"\tchan1_raw: " << current_messages.rc_channels_raw.chan1_raw <<
"\tchan2_raw: " << current_messages.rc_channels_raw.chan2_raw <<
"\tchan3_raw: " << current_messages.rc_channels_raw.chan3_raw <<
"\tchan4_raw: " << current_messages.rc_channels_raw.chan4_raw <<
"\tchan5_raw: " << current_messages.rc_channels_raw.chan5_raw <<
"\tchan6_raw: " << current_messages.rc_channels_raw.chan6_raw <<
"\tchan7_raw: " << current_messages.rc_channels_raw.chan7_raw <<
"\tchan8_raw: " << current_messages.rc_channels_raw.chan8_raw <<
"\trssi: " << (int)current_messages.rc_channels_raw.rssi <<
std::endl;
break;
}
case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:
{
std::cout << "MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:" << std::endl;
mavlink_msg_servo_output_raw_decode(&message, &(current_messages.servo_output_raw));
current_messages.time_stamps.servo_output_raw = get_time_usec();
this_timestamps.servo_output_raw = current_messages.time_stamps.servo_output_raw;
std::cout <<
"\tport: " << (int)current_messages.servo_output_raw.port <<
"\tservo1_raw: " << current_messages.servo_output_raw.servo1_raw <<
"\tservo2_raw: " << current_messages.servo_output_raw.servo2_raw <<
"\tservo3_raw: " << current_messages.servo_output_raw.servo3_raw <<
"\tservo4_raw: " << current_messages.servo_output_raw.servo4_raw <<
"\tservo5_raw: " << current_messages.servo_output_raw.servo5_raw <<
"\tservo6_raw: " << current_messages.servo_output_raw.servo6_raw <<
"\tservo7_raw: " << current_messages.servo_output_raw.servo7_raw <<
"\tservo8_raw: " << current_messages.servo_output_raw.servo8_raw <<
std::endl;
break;
}
case MAVLINK_MSG_ID_VFR_HUD:
{
std::cout << "MAVLINK_MSG_ID_VFR_HUD:" << std::endl;
mavlink_msg_vfr_hud_decode(&message, &(current_messages.vfr_hud));
current_messages.time_stamps.vfr_hud = get_time_usec();
this_timestamps.vfr_hud = current_messages.time_stamps.vfr_hud;
std::cout <<
"\tairspeed: " << current_messages.vfr_hud.airspeed <<
"\tgroundspeed: " << current_messages.vfr_hud.groundspeed <<
"\theading: " << current_messages.vfr_hud.heading <<
std::endl;
std::cout <<
"\tthrottle: " << current_messages.vfr_hud.throttle <<
"\talt: " << current_messages.vfr_hud.alt <<
"\tclimb: " << current_messages.vfr_hud.climb <<
std::endl;
break;
}
case MAVLINK_MSG_ID_MISSION_CURRENT:
{
std::cout << "MAVLINK_MSG_ID_MISSION_CURRENT: ";
mavlink_msg_mission_current_decode(&message, &(current_messages.mission_current));
current_messages.time_stamps.mission_current = get_time_usec();
this_timestamps.mission_current = current_messages.time_stamps.mission_current;
std::cout <<
"seq: " << current_messages.mission_current.seq <<
std::endl;
break;
}
default:
{
printf("Warning, did not handle message id %i\n",message.msgid);
break;
}
} // end: switch msgid
} // end: if read message
// Check for receipt of all items
received_all =
this_timestamps.heartbeat &&
this_timestamps.sys_status &&
// this_timestamps.battery_status &&
// this_timestamps.radio_status &&
this_timestamps.local_position_ned &&
// this_timestamps.global_position_int &&
// this_timestamps.position_target_local_ned &&
this_timestamps.position_target_global_int &&
this_timestamps.highres_imu &&
this_timestamps.attitude ;
// give the write thread time to use the port
if ( writing_status > false )
usleep(100); // look for components of batches at 10kHz
} // end: while not received all
return;
}
void handle_radio_status(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
mavlink_radio_status_t rst;
mavlink_msg_radio_status_decode(msg, &rst);
has_radio_status = true;
handle_message(rst, sysid, compid);
}
/**
* This method parses all incoming bytes and constructs a MAVLink packet.
* It can handle multiple links in parallel, as each link has it's own buffer/
* parsing state machine.
* @param link The interface to read from
* @see LinkInterface
**/
void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
{
// receiveMutex.lock();
mavlink_message_t message;
mavlink_status_t status;
// Cache the link ID for common use.
int linkId = link->getId();
static int mavlink09Count = 0;
static int nonmavlinkCount = 0;
static bool decodedFirstPacket = false;
static bool warnedUser = false;
static bool checkedUserNonMavlink = false;
static bool warnedUserNonMavlink = false;
// FIXME: Add check for if link->getId() >= MAVLINK_COMM_NUM_BUFFERS
for (int position = 0; position < b.size(); position++) {
unsigned int decodeState = mavlink_parse_char(linkId, (uint8_t)(b[position]), &message, &status);
if ((uint8_t)b[position] == 0x55) mavlink09Count++;
if ((mavlink09Count > 100) && !decodedFirstPacket && !warnedUser)
{
warnedUser = true;
// Obviously the user tries to use a 0.9 autopilot
// with QGroundControl built for version 1.0
emit protocolStatusMessage(tr("MAVLink Protocol"), tr("There is a MAVLink Version or Baud Rate Mismatch. "
"Your MAVLink device seems to use the deprecated version 0.9, while QGroundControl only supports version 1.0+. "
"Please upgrade the MAVLink version of your autopilot. "
"If your autopilot is using version 1.0, check if the baud rates of QGroundControl and your autopilot are the same."));
}
if (decodeState == 0 && !decodedFirstPacket)
{
nonmavlinkCount++;
if (nonmavlinkCount > 2000 && !warnedUserNonMavlink)
{
//2000 bytes with no mavlink message. Are we connected to a mavlink capable device?
if (!checkedUserNonMavlink)
{
link->requestReset();
checkedUserNonMavlink = true;
}
else
{
warnedUserNonMavlink = true;
emit protocolStatusMessage(tr("MAVLink Protocol"), tr("There is a MAVLink Version or Baud Rate Mismatch. "
"Please check if the baud rates of QGroundControl and your autopilot are the same."));
}
}
}
if (decodeState == 1)
{
decodedFirstPacket = true;
if (message.msgid == MAVLINK_MSG_ID_PING)
{
// process ping requests (tgt_system and tgt_comp must be zero)
mavlink_ping_t ping;
mavlink_msg_ping_decode(&message, &ping);
if (!ping.target_system && !ping.target_component)
{
mavlink_message_t msg;
mavlink_msg_ping_pack(getSystemId(), getComponentId(), &msg, ping.time_usec, ping.seq, message.sysid, message.compid);
sendMessage(msg);
}
}
if (message.msgid == MAVLINK_MSG_ID_RADIO_STATUS)
{
// process telemetry status message
mavlink_radio_status_t rstatus;
mavlink_msg_radio_status_decode(&message, &rstatus);
emit radioStatusChanged(link, rstatus.rxerrors, rstatus.fixed, rstatus.rssi, rstatus.remrssi,
rstatus.txbuf, rstatus.noise, rstatus.remnoise);
}
// Log data
if (!_logSuspendError && !_logSuspendReplay && _tempLogFile.isOpen()) {
uint8_t buf[MAVLINK_MAX_PACKET_LEN + sizeof(quint64)];
// Write the uint64 time in microseconds in big endian format before the message.
// This timestamp is saved in UTC time. We are only saving in ms precision because
// getting more than this isn't possible with Qt without a ton of extra code.
quint64 time = (quint64)QDateTime::currentMSecsSinceEpoch() * 1000;
qToBigEndian(time, buf);
// Then write the message to the buffer
int len = mavlink_msg_to_send_buffer(buf + sizeof(quint64), &message);
// Determine how many bytes were written by adding the timestamp size to the message size
len += sizeof(quint64);
// Now write this timestamp/message pair to the log.
QByteArray b((const char*)buf, len);
if (_tempLogFile.write(b) != len)
{
// If there's an error logging data, raise an alert and stop logging.
emit protocolStatusMessage(tr("MAVLink Protocol"), tr("MAVLink Logging failed. Could not write to file %1, logging disabled.").arg(_tempLogFile.fileName()));
_stopLogging();
_logSuspendError = true;
}
}
// ORDER MATTERS HERE!
// If the matching UAS object does not yet exist, it has to be created
// before emitting the packetReceived signal
UASInterface* uas = UASManager::instance()->getUASForId(message.sysid);
// Check and (if necessary) create UAS object
if (uas == NULL && message.msgid == MAVLINK_MSG_ID_HEARTBEAT)
{
// ORDER MATTERS HERE!
// The UAS object has first to be created and connected,
// only then the rest of the application can be made aware
// of its existence, as it only then can send and receive
// it's first messages.
// Check if the UAS has the same id like this system
if (message.sysid == getSystemId())
{
emit protocolStatusMessage(tr("MAVLink Protocol"), tr("Warning: A second system is using the same system id (%1)").arg(getSystemId()));
}
// Create a new UAS based on the heartbeat received
// Todo dynamically load plugin at run-time for MAV
// WIKISEARCH:AUTOPILOT_TYPE_INSTANTIATION
// First create new UAS object
// Decode heartbeat message
mavlink_heartbeat_t heartbeat;
// Reset version field to 0
heartbeat.mavlink_version = 0;
mavlink_msg_heartbeat_decode(&message, &heartbeat);
// Check if the UAS has a different protocol version
if (m_enable_version_check && (heartbeat.mavlink_version != MAVLINK_VERSION))
{
// Bring up dialog to inform user
if (!versionMismatchIgnore)
{
emit protocolStatusMessage(tr("MAVLink Protocol"), tr("The MAVLink protocol version on the MAV and QGroundControl mismatch! "
"It is unsafe to use different MAVLink versions. "
"QGroundControl therefore refuses to connect to system %1, which sends MAVLink version %2 (QGroundControl uses version %3).").arg(message.sysid).arg(heartbeat.mavlink_version).arg(MAVLINK_VERSION));
versionMismatchIgnore = true;
}
// Ignore this message and continue gracefully
continue;
}
// Create a new UAS object
uas = QGCMAVLinkUASFactory::createUAS(this, link, message.sysid, &heartbeat);
}
// Increase receive counter
totalReceiveCounter[linkId]++;
currReceiveCounter[linkId]++;
// Determine what the next expected sequence number is, accounting for
// never having seen a message for this system/component pair.
int lastSeq = lastIndex[message.sysid][message.compid];
int expectedSeq = (lastSeq == -1) ? message.seq : (lastSeq + 1);
// And if we didn't encounter that sequence number, record the error
if (message.seq != expectedSeq)
{
// Determine how many messages were skipped
int lostMessages = message.seq - expectedSeq;
// Out of order messages or wraparound can cause this, but we just ignore these conditions for simplicity
if (lostMessages < 0)
{
lostMessages = 0;
}
// And log how many were lost for all time and just this timestep
totalLossCounter[linkId] += lostMessages;
currLossCounter[linkId] += lostMessages;
}
// And update the last sequence number for this system/component pair
lastIndex[message.sysid][message.compid] = expectedSeq;
// Update on every 32th packet
if ((totalReceiveCounter[linkId] & 0x1F) == 0)
{
// Calculate new loss ratio
// Receive loss
float receiveLoss = (double)currLossCounter[linkId] / (double)(currReceiveCounter[linkId] + currLossCounter[linkId]);
receiveLoss *= 100.0f;
currLossCounter[linkId] = 0;
currReceiveCounter[linkId] = 0;
emit receiveLossChanged(message.sysid, receiveLoss);
}
// The packet is emitted as a whole, as it is only 255 - 261 bytes short
// kind of inefficient, but no issue for a groundstation pc.
// It buys as reentrancy for the whole code over all threads
emit messageReceived(link, message);
// Multiplex message if enabled
if (m_multiplexingEnabled)
{
// Get all links connected to this unit
QList<LinkInterface*> links = _linkMgr->getLinks();
// Emit message on all links that are currently connected
foreach(LinkInterface* currLink, links)
{
// Only forward this message to the other links,
// not the link the message was received on
if (currLink != link) sendMessage(currLink, message, message.sysid, message.compid);
}
}
}
}
// ------------------------------------------------------------------------------
// Read Messages
// ------------------------------------------------------------------------------
void Autopilot_Interface::read_messages() {
bool success; // receive success flag
bool received_all = false; // receive only one message
Time_Stamps this_timestamps;
// Blocking wait for new data
while (!received_all and !time_to_exit) {
// ----------------------------------------------------------------------
// READ MESSAGE
// ----------------------------------------------------------------------
mavlink_message_t message;
success = serial_port->read_message(message);
// ----------------------------------------------------------------------
// HANDLE MESSAGE
// ----------------------------------------------------------------------
if (success) {
// Store message sysid and compid.
// Note this doesn't handle multiple message sources.
current_messages.sysid = message.sysid;
current_messages.compid = message.compid;
// Handle Message ID
switch (message.msgid) {
case MAVLINK_MSG_ID_HEARTBEAT: {
printf("MAVLINK_MSG_ID_HEARTBEAT\n");
mavlink_msg_heartbeat_decode(&message, &(current_messages.heartbeat));
current_messages.time_stamps.heartbeat = get_time_usec();
this_timestamps.heartbeat = current_messages.time_stamps.heartbeat;
break;
}
case MAVLINK_MSG_ID_SYS_STATUS: {
printf("MAVLINK_MSG_ID_SYS_STATUS\n");
mavlink_msg_sys_status_decode(&message,
&(current_messages.sys_status));
current_messages.time_stamps.sys_status = get_time_usec();
this_timestamps.sys_status = current_messages.time_stamps.sys_status;
printf("Battery life remaining: %i\n",
current_messages.sys_status.battery_remaining);
break;
}
// this is not being received
case MAVLINK_MSG_ID_BATTERY_STATUS: {
printf("MAVLINK_MSG_ID_BATTERY_STATUS\n");
mavlink_msg_battery_status_decode(&message,
&(current_messages.battery_status));
current_messages.time_stamps.battery_status = get_time_usec();
this_timestamps.battery_status =
current_messages.time_stamps.battery_status;
printf("Battery life remaining: %i\n",
current_messages.battery_status.battery_remaining);
break;
}
case MAVLINK_MSG_ID_RADIO_STATUS: {
printf("MAVLINK_MSG_ID_RADIO_STATUS\n");
mavlink_msg_radio_status_decode(&message,
&(current_messages.radio_status));
current_messages.time_stamps.radio_status = get_time_usec();
this_timestamps.radio_status =
current_messages.time_stamps.radio_status;
break;
}
case MAVLINK_MSG_ID_LOCAL_POSITION_NED: {
printf("MAVLINK_MSG_ID_LOCAL_POSITION_NED\n");
mavlink_msg_local_position_ned_decode(
&message, &(current_messages.local_position_ned));
printf(" pos (GPS): %f %f %f (m)\n",
current_messages.local_position_ned.x,
current_messages.local_position_ned.y,
current_messages.local_position_ned.z);
current_messages.time_stamps.local_position_ned = get_time_usec();
this_timestamps.local_position_ned =
current_messages.time_stamps.local_position_ned;
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
printf("MAVLINK_MSG_ID_GLOBAL_POSITION_INT\n");
mavlink_msg_global_position_int_decode(
&message, &(current_messages.global_position_int));
current_messages.time_stamps.global_position_int = get_time_usec();
this_timestamps.global_position_int =
current_messages.time_stamps.global_position_int;
mavlink_global_position_int_t gpos =
current_messages.global_position_int;
printf(" pos (GPS): %f %f %f (m)\n", gpos.lat, gpos.lon,
gpos.alt);
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED: {
printf("MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED\n");
mavlink_msg_position_target_local_ned_decode(
&message, &(current_messages.position_target_local_ned));
current_messages.time_stamps.position_target_local_ned =
get_time_usec();
this_timestamps.position_target_local_ned =
current_messages.time_stamps.position_target_local_ned;
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT: {
printf("MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT\n");
mavlink_msg_position_target_global_int_decode(
&message, &(current_messages.position_target_global_int));
current_messages.time_stamps.position_target_global_int =
get_time_usec();
this_timestamps.position_target_global_int =
current_messages.time_stamps.position_target_global_int;
break;
}
case MAVLINK_MSG_ID_HIGHRES_IMU: {
printf("MAVLINK_MSG_ID_HIGHRES_IMU\n");
mavlink_msg_highres_imu_decode(&message,
&(current_messages.highres_imu));
current_messages.time_stamps.highres_imu = get_time_usec();
this_timestamps.highres_imu =
current_messages.time_stamps.highres_imu;
mavlink_highres_imu_t imu = current_messages.highres_imu;
printf(" ap time: %llu \n", imu.time_usec);
printf(" acc (NED): % f % f % f (m/s^2)\n", imu.xacc, imu.yacc,
imu.zacc);
printf(" gyro (NED): % f % f % f (rad/s)\n", imu.xgyro, imu.ygyro,
imu.zgyro);
printf(" mag (NED): % f % f % f (Ga)\n", imu.xmag, imu.ymag,
imu.zmag);
printf(" baro: %f (mBar) \n", imu.abs_pressure);
printf(" altitude: %f (m) \n", imu.pressure_alt);
printf(" temperature: %f C \n", imu.temperature);
break;
}
case MAVLINK_MSG_ID_ATTITUDE: {
printf("### MAVLINK_MSG_ID_ATTITUDE\n");
mavlink_msg_attitude_decode(&message, &(current_messages.attitude));
current_messages.time_stamps.attitude = get_time_usec();
this_timestamps.attitude = current_messages.time_stamps.attitude;
mavlink_attitude_t att = current_messages.attitude;
printf("### roll: %f pitch: %f yaw: %f\n", att.roll, att.pitch,
att.yaw);
break;
}
default: {
// printf("Warning, did not handle message id %i\n",message.msgid);
break;
}
} // end: switch msgid
} // end: if read message
// Check for receipt of all items
received_all =
this_timestamps.heartbeat &&
// this_timestamps.battery_status &&
// this_timestamps.radio_status &&
// this_timestamps.local_position_ned &&
// this_timestamps.global_position_int &&
// this_timestamps.position_target_local_ned
//&&
// this_timestamps.position_target_global_int
//&&
// this_timestamps.highres_imu &&
// this_timestamps.attitude &&
this_timestamps.sys_status;
// give the write thread time to use the port
if (writing_status > false) {
usleep(100); // look for components of batches at 10kHz
}
} // end: while not received all
return;
}