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 _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; }
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; } }
//85 void MavSerialPort::position_target_local_ned_handler(){ // qDebug() << "MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_END\n"; mavlink_msg_position_target_local_ned_decode(&message, &position_target_local_ned); }
// ------------------------------------------------------------------------------ // 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; }
// ------------------------------------------------------------------------------ // 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; }