//105 void MavSerialPort::highres_imu_handler(){ // qDebug() << "MAVLINK_MSG_HIGHRES_IMU"; mavlink_msg_highres_imu_decode(&message, &highres_imu); emit IMUChanged(); // qDebug() << "imu: " << highres_imu.xgyro << " " << highres_imu.ygyro << " " << highres_imu.zgyro << endl; }
void handle_highres_imu(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) { mavlink_highres_imu_t imu_hr; mavlink_msg_highres_imu_decode(msg, &imu_hr); ROS_INFO_COND_NAMED(!has_hr_imu, "imu", "High resolution IMU detected!"); has_hr_imu = true; std_msgs::Header header; header.stamp = ros::Time::now(); header.frame_id = frame_id; /* imu/data_raw filled by HR IMU */ if (imu_hr.fields_updated & ((7<<3)|(7<<0))) { sensor_msgs::ImuPtr imu_msg = boost::make_shared<sensor_msgs::Imu>(); fill_imu_msg_raw(imu_msg, imu_hr.xgyro, -imu_hr.ygyro, -imu_hr.zgyro, imu_hr.xacc, -imu_hr.yacc, -imu_hr.zacc); imu_msg->header = header; imu_raw_pub.publish(imu_msg); } if (imu_hr.fields_updated & (7<<6)) { sensor_msgs::MagneticFieldPtr magn_msg = boost::make_shared<sensor_msgs::MagneticField>(); // Convert from local NED plane to ENU magn_msg->magnetic_field.x = imu_hr.ymag * GAUSS_TO_TESLA; magn_msg->magnetic_field.y = imu_hr.xmag * GAUSS_TO_TESLA; magn_msg->magnetic_field.z = -imu_hr.zmag * GAUSS_TO_TESLA; magn_msg->magnetic_field_covariance = magnetic_cov; magn_msg->header = header; magn_pub.publish(magn_msg); } if (imu_hr.fields_updated & (1<<9)) { sensor_msgs::FluidPressurePtr atmp_msg = boost::make_shared<sensor_msgs::FluidPressure>(); atmp_msg->fluid_pressure = imu_hr.abs_pressure * MILLIBAR_TO_PASCAL; atmp_msg->header = header; press_pub.publish(atmp_msg); } if (imu_hr.fields_updated & (1<<12)) { sensor_msgs::TemperaturePtr temp_msg = boost::make_shared<sensor_msgs::Temperature>(); temp_msg->temperature = imu_hr.temperature; temp_msg->header = header; temp_pub.publish(temp_msg); } }
void ml_logger_default_write_highres_imu(ml_logger_t* mll, mavlink_message_t* msg) { /** uint64_t time_usec; ///< Timestamp (microseconds, synced to UNIX time or since system boot) float xacc; ///< X acceleration (m/s^2) float yacc; ///< Y acceleration (m/s^2) float zacc; ///< Z acceleration (m/s^2) float xgyro; ///< Angular speed around X axis (rad / sec) float ygyro; ///< Angular speed around Y axis (rad / sec) float zgyro; ///< Angular speed around Z axis (rad / sec) float xmag; ///< X Magnetic field (Gauss) float ymag; ///< Y Magnetic field (Gauss) float zmag; ///< Z Magnetic field (Gauss) float abs_pressure; ///< Absolute pressure in millibar float diff_pressure; ///< Differential pressure in millibar float pressure_alt; ///< Altitude calculated from pressure float temperature; ///< Temperature in degrees celsius **/ if (msg->msgid == MAVLINK_MSG_ID_HIGHRES_IMU) { mavlink_highres_imu_t imu; mavlink_msg_highres_imu_decode(msg, &imu); fprintf(mll->files.highres_imu.fs, "%12" PRIu64 " %12" PRIu64 " %9.6f %9.6f %9.6f %9.6f %9.6f %9.6f %9.6f %9.6f %9.6f %9.6f %9.6f\n", utils_us_since_epoch(), imu.time_usec, imu.xacc, imu.yacc, imu.zacc, imu.xgyro, imu.ygyro, imu.zgyro, imu.xmag, imu.ymag, imu.zmag, imu.abs_pressure, imu.pressure_alt ); fflush(mll->files.highres_imu.fs); } }
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; } }
//105 void MavSerialPort::highres_imu_handler(){ // qDebug() << "MAVLINK_MSG_HIGHRES_IMU"; mavlink_msg_highres_imu_decode(&message, &highres_imu); //emit IMUChanged(); }
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; vcmd.command = 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); } /* 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); /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ vcmd.param1 = new_mode.base_mode; vcmd.param2 = new_mode.custom_mode; vcmd.param3 = 0; vcmd.param4 = 0; vcmd.param5 = 0; vcmd.param6 = 0; vcmd.param7 = 0; vcmd.command = MAV_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 = 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); } } } /* * 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(); /* TODO, set ground_press/ temp during calib */ static const float ground_press = 1013.25f; // mbar static const float ground_tempC = 21.0f; static const float ground_alt = 0.0f; static const float T0 = 273.15; static const float R = 287.05f; static const float g = 9.806f; if (msg->msgid == MAVLINK_MSG_ID_RAW_IMU) { mavlink_raw_imu_t imu; mavlink_msg_raw_imu_decode(msg, &imu); /* packet counter */ static uint16_t hil_counter = 0; static uint16_t hil_frames = 0; static uint64_t old_timestamp = 0; /* sensors general */ hil_sensors.timestamp = imu.time_usec; /* hil gyro */ static const float mrad2rad = 1.0e-3f; hil_sensors.gyro_counter = hil_counter; hil_sensors.gyro_raw[0] = imu.xgyro; hil_sensors.gyro_raw[1] = imu.ygyro; hil_sensors.gyro_raw[2] = imu.zgyro; hil_sensors.gyro_rad_s[0] = imu.xgyro * mrad2rad; hil_sensors.gyro_rad_s[1] = imu.ygyro * mrad2rad; hil_sensors.gyro_rad_s[2] = imu.zgyro * mrad2rad; /* accelerometer */ hil_sensors.accelerometer_counter = hil_counter; static const float mg2ms2 = 9.8f / 1000.0f; hil_sensors.accelerometer_raw[0] = imu.xacc; hil_sensors.accelerometer_raw[1] = imu.yacc; hil_sensors.accelerometer_raw[2] = imu.zacc; hil_sensors.accelerometer_m_s2[0] = mg2ms2 * imu.xacc; hil_sensors.accelerometer_m_s2[1] = mg2ms2 * imu.yacc; hil_sensors.accelerometer_m_s2[2] = mg2ms2 * 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; hil_sensors.adc_voltage_v[1] = 0; hil_sensors.adc_voltage_v[2] = 0; /* magnetometer */ float mga2ga = 1.0e-3f; hil_sensors.magnetometer_counter = hil_counter; hil_sensors.magnetometer_raw[0] = imu.xmag; hil_sensors.magnetometer_raw[1] = imu.ymag; hil_sensors.magnetometer_raw[2] = imu.zmag; hil_sensors.magnetometer_ga[0] = imu.xmag * mga2ga; hil_sensors.magnetometer_ga[1] = imu.ymag * mga2ga; hil_sensors.magnetometer_ga[2] = imu.zmag * mga2ga; 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; /* publish */ orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors); // increment counters hil_counter += 1 ; hil_frames += 1 ; // output if ((timestamp - old_timestamp) > 10000000) { printf("receiving hil imu at %d hz\n", hil_frames/10); old_timestamp = timestamp; hil_frames = 0; } } if (msg->msgid == MAVLINK_MSG_ID_HIGHRES_IMU) { mavlink_highres_imu_t imu; mavlink_msg_highres_imu_decode(msg, &imu); /* packet counter */ static uint16_t hil_counter = 0; static uint16_t hil_frames = 0; static uint64_t old_timestamp = 0; /* sensors general */ hil_sensors.timestamp = imu.time_usec; /* 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; hil_sensors.adc_voltage_v[1] = 0; hil_sensors.adc_voltage_v[2] = 0; /* 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; hil_sensors.baro_pres_mbar = imu.abs_pressure; float tempC = imu.temperature; float tempAvgK = T0 + (tempC + ground_tempC) / 2.0f; float h = ground_alt + (R / g) * tempAvgK * logf(ground_press / imu.abs_pressure); hil_sensors.baro_alt_meter = h; hil_sensors.baro_temp_celcius = imu.temperature; /* publish */ orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors); // increment counters hil_counter += 1 ; hil_frames += 1 ; // output if ((timestamp - old_timestamp) > 10000000) { printf("receiving hil imu at %d hz\n", hil_frames/10); old_timestamp = timestamp; hil_frames = 0; } } if (msg->msgid == MAVLINK_MSG_ID_GPS_RAW_INT) { mavlink_gps_raw_int_t gps; mavlink_msg_gps_raw_int_decode(msg, &gps); /* packet counter */ static uint16_t hil_counter = 0; static uint16_t hil_frames = 0; static uint64_t old_timestamp = 0; /* gps */ hil_gps.timestamp_position = gps.time_usec; // hil_gps.counter = hil_counter++; 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.counter_pos_valid = hil_counter++; 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 = 100; // XXX 100 m/s variance? hil_gps.p_variance_m = 100; // XXX 100 m variance? hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s hil_gps.vel_n_m_s = (float)gps.vel * 1e-2f * cosf(gps.cog * M_DEG_TO_RAD_F * 1e-2f); hil_gps.vel_e_m_s = (float)gps.vel * 1e-2f * sinf(gps.cog * M_DEG_TO_RAD_F * 1e-2f); hil_gps.vel_d_m_s = 0.0f; hil_gps.cog_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f; // from deg*100 to rad hil_gps.fix_type = gps.fix_type; hil_gps.satellites_visible = gps.satellites_visible; /* publish */ orb_publish(ORB_ID(vehicle_gps_position), pub_hil_gps, &hil_gps); // increment counters hil_counter += 1 ; hil_frames += 1 ; // output if ((timestamp - old_timestamp) > 10000000) { printf("receiving hil gps at %d hz\n", hil_frames/10); old_timestamp = timestamp; hil_frames = 0; } } if (msg->msgid == MAVLINK_MSG_ID_RAW_PRESSURE) { mavlink_raw_pressure_t press; mavlink_msg_raw_pressure_decode(msg, &press); /* packet counter */ static uint16_t hil_counter = 0; static uint16_t hil_frames = 0; static uint64_t old_timestamp = 0; /* sensors general */ hil_sensors.timestamp = press.time_usec; /* baro */ float tempC = press.temperature / 100.0f; float tempAvgK = T0 + (tempC + ground_tempC) / 2.0f; float h = ground_alt + (R / g) * tempAvgK * logf(ground_press / press.press_abs); hil_sensors.baro_counter = hil_counter; hil_sensors.baro_pres_mbar = press.press_abs; hil_sensors.baro_alt_meter = h; hil_sensors.baro_temp_celcius = tempC; /* publish */ orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors); // increment counters hil_counter += 1 ; hil_frames += 1 ; // output if ((timestamp - old_timestamp) > 10000000) { printf("receiving hil pressure at %d hz\n", hil_frames/10); old_timestamp = timestamp; hil_frames = 0; } } if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE) { mavlink_hil_state_t hil_state; mavlink_msg_hil_state_decode(msg, &hil_state); /* Calculate Rotation Matrix */ //TODO: better clarification which app does this, atm we have a ekf for quadrotors which does this, but there is no such thing if fly in fixed wing mode if (mavlink_system.type == MAV_TYPE_FIXED_WING) { //TODO: assuming low pitch and roll values for now hil_attitude.R[0][0] = cosf(hil_state.yaw); hil_attitude.R[0][1] = sinf(hil_state.yaw); hil_attitude.R[0][2] = 0.0f; hil_attitude.R[1][0] = -sinf(hil_state.yaw); hil_attitude.R[1][1] = cosf(hil_state.yaw); hil_attitude.R[1][2] = 0.0f; hil_attitude.R[2][0] = 0.0f; hil_attitude.R[2][1] = 0.0f; hil_attitude.R[2][2] = 1.0f; hil_attitude.R_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; /* set timestamp and notify processes (broadcast) */ hil_global_pos.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos); hil_attitude.roll = hil_state.roll; hil_attitude.pitch = hil_state.pitch; hil_attitude.yaw = hil_state.yaw; 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(); orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude); } 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); } } } }
// ------------------------------------------------------------------------------ // 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; }