void handle_gps_raw_int(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) { mavlink_gps_raw_int_t raw_gps; mavlink_msg_gps_raw_int_decode(msg, &raw_gps); auto fix = boost::make_shared<sensor_msgs::NavSatFix>(); fix->header = uas->synchronized_header(frame_id, raw_gps.time_usec); fix->status.service = sensor_msgs::NavSatStatus::SERVICE_GPS; if (raw_gps.fix_type > 2) fix->status.status = sensor_msgs::NavSatStatus::STATUS_FIX; else { ROS_WARN_THROTTLE_NAMED(30, "global_position", "GP: No GPS fix"); fix->status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX; } fill_lla(raw_gps, fix); float eph = (raw_gps.eph != UINT16_MAX) ? raw_gps.eph / 1E2F : NAN; float epv = (raw_gps.epv != UINT16_MAX) ? raw_gps.epv / 1E2F : NAN; if (!isnan(eph)) { const double hdop = eph; // From nmea_navsat_driver fix->position_covariance[0 + 0] = \ fix->position_covariance[3 + 1] = std::pow(hdop, 2); fix->position_covariance[6 + 2] = std::pow(2 * hdop, 2); fix->position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_APPROXIMATED; } else { fill_unknown_cov(fix); } // store & publish uas->update_gps_fix_epts(fix, eph, epv, raw_gps.fix_type, raw_gps.satellites_visible); raw_fix_pub.publish(fix); if (raw_gps.vel != UINT16_MAX && raw_gps.cog != UINT16_MAX) { double speed = raw_gps.vel / 1E2; // m/s double course = angles::from_degrees(raw_gps.cog / 1E2); // rad auto vel = boost::make_shared<geometry_msgs::TwistStamped>(); vel->header.frame_id = frame_id; vel->header.stamp = fix->header.stamp; // From nmea_navsat_driver vel->twist.linear.x = speed * std::sin(course); vel->twist.linear.y = speed * std::cos(course); raw_vel_pub.publish(vel); } }
void MavlinkReceiver::handle_message_gps_raw(mavlink_message_t *msg) { //if(!if_hil) if_hil= true; mavlink_gps_raw_int_t gps_raw; mavlink_msg_gps_raw_int_decode(msg,&gps_raw); printf("gps_raw received:lat:%.4f,lon:%.4f,alt:%.1f,cog:%.2f,vel:%.4f\n",gps_raw.lat/(float)1E7,gps_raw.lon/(float)1e7,gps_raw.alt/(float)1e3,gps_raw.cog/(float)1e2,gps_raw.vel/(float)1e2); //send to ros uascode::GlobalPos global_pos; global_pos.lat= gps_raw.lat/(float)1e7; global_pos.lon= gps_raw.lon/(float)1e7; global_pos.alt= gps_raw.alt/(float)1e3; global_pos.cog= gps_raw.cog/(float)1e2*M_PI/180.; global_pos.speed= gps_raw.vel/(float)1e2; pub_pos.publish(global_pos); }
// ------------------------------------------------------------------------------ // 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; }
void message_rx_cb(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) { switch (msg->msgid) { case MAVLINK_MSG_ID_GPS_RAW_INT: { mavlink_gps_raw_int_t raw_gps; mavlink_msg_gps_raw_int_decode(msg, &raw_gps); sensor_msgs::NavSatFixPtr fix(new sensor_msgs::NavSatFix); geometry_msgs::TwistStampedPtr vel(new geometry_msgs::TwistStamped); gps_diag.set_gps_raw(raw_gps); if (raw_gps.fix_type < 2) { ROS_WARN_THROTTLE_NAMED(60, "gps", "GPS: no fix"); return; } fix->status.service = sensor_msgs::NavSatStatus::SERVICE_GPS; if (raw_gps.fix_type == 2 || raw_gps.fix_type == 3) fix->status.status = sensor_msgs::NavSatStatus::STATUS_FIX; else fix->status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX; fix->latitude = raw_gps.lat / 1E7; // deg fix->longitude = raw_gps.lon / 1E7; // deg fix->altitude = raw_gps.alt / 1E3; // m if (raw_gps.eph != UINT16_MAX) { double hdop = raw_gps.eph / 1E2; double hdop2 = std::pow(hdop, 2); // TODO: Check // From nmea_navsat_driver fix->position_covariance[0] = hdop2; fix->position_covariance[4] = hdop2; fix->position_covariance[8] = pow(2 * hdop, 2); fix->position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_APPROXIMATED; } else { fix->position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN; } fix->header.frame_id = frame_id; fix->header.stamp = ros::Time::now(); fix_pub.publish(fix); if (raw_gps.vel != UINT16_MAX && raw_gps.cog != UINT16_MAX) { double speed = raw_gps.vel / 1E2; // m/s double course = raw_gps.cog / 1E2; // deg // From nmea_navsat_driver vel->twist.linear.x = speed * std::sin(course); vel->twist.linear.y = speed * std::cos(course); vel->header.frame_id = frame_id; vel->header.stamp = fix->header.stamp; vel_pub.publish(vel); } } break; case MAVLINK_MSG_ID_GPS_STATUS: { // TODO: not supported by APM:Plane, // no standard ROS messages mavlink_gps_status_t gps_stat; mavlink_msg_gps_status_decode(msg, &gps_stat); ROS_DEBUG_NAMED("gps", "GPS stat sat visible: %d", gps_stat.satellites_visible); } break; case MAVLINK_MSG_ID_SYSTEM_TIME: { mavlink_system_time_t mtime; mavlink_msg_system_time_decode(msg, &mtime); if (mtime.time_unix_usec == 0) { ROS_WARN_THROTTLE_NAMED(60, "gps", "Wrong system time. Is GPS Ok? (boot_ms: %u)", mtime.time_boot_ms); return; } sensor_msgs::TimeReferencePtr time(new sensor_msgs::TimeReference); ros::Time time_ref( mtime.time_unix_usec / 1000000, // t_sec (mtime.time_unix_usec % 1000000) * 1000); // t_nsec time->source = time_ref_source; time->time_ref = time_ref; time->header.frame_id = time_ref_source; time->header.stamp = ros::Time::now(); time_ref_pub.publish(time); } break; }; }
void ofxMavlink::threadedFunction() { while( isThreadRunning() != 0 ) { // static unsigned int imu_receive_counter = uint8_t cp; mavlink_message_t message; mavlink_status_t status; uint8_t msgReceived = false; if (read(fd, &cp, 1) > 0) { // Check if a message could be decoded, return the message in case yes msgReceived = mavlink_parse_char(MAVLINK_COMM_1, cp, &message, &status); if (status.packet_rx_drop_count > 0) { if(bDebug) ofLogWarning("ofxMavlink") << "ERROR: DROPPED " << status.packet_rx_drop_count <<" PACKETS:" << " " << cp; } } else { if(bDebug) ofLogError("ofxMavlink") << "ERROR: Could not read from fd " << fd; } // If a message could be decoded, handle it if(msgReceived) { msgcount++; if (bDebug) { fprintf(stderr,"Received serial data: "); unsigned int i; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; unsigned int messageLength = mavlink_msg_to_send_buffer(buffer, &message); if (messageLength > MAVLINK_MAX_PACKET_LEN) { fprintf(stderr, "\nFATAL ERROR: MESSAGE LENGTH IS LARGER THAN BUFFER SIZE\n"); } else { for (i=0; i<messageLength; i++) { unsigned char v=buffer[i]; fprintf(stderr,"%02x ", v); } fprintf(stderr,"\n"); } } //if(bDebug) ofLogNotice("ofxMavlink") << "Received message from serial with ID #" << message.msgid << " (sys:" << message.sysid << "|comp:" << message.compid << "):\n"; switch (message.msgid) { case MAVLINK_MSG_ID_COMMAND_ACK: { mavlink_command_ack_t ack; mavlink_msg_command_ack_decode(&message, &ack); switch (ack.result) { case MAV_RESULT_ACCEPTED: { ofLogNotice("ofxMavlink") << "SUCCESS: Executed Command: " << ack.command; } break; case MAV_RESULT_TEMPORARILY_REJECTED: { ofLogError("ofxMavlink") << "FAILURE: Temporarily rejected Command: " << ack.command; } break; case MAV_RESULT_DENIED: { ofLogError("ofxMavlink") << "FAILURE: Denied Command: " << ack.command; } break; case MAV_RESULT_UNSUPPORTED: { ofLogError("ofxMavlink") << "FAILURE: Unsupported Command: " << ack.command; } break; case MAV_RESULT_FAILED: { ofLogError("ofxMavlink") << "FAILURE: Failed Command: " << ack.command; } break; } break; } // case MAVLINK_MSG_ID_COMMAND_ACK: // { // mavlink_command_ack_t ack; // mavlink_msg_command_ack_decode(&message,&ack); // cout << "received CMD_ACK command=" << ack.command << " result=" << ack.result << endl; // // if(ack.command == MAV_CMD_COMPONENT_ARM_DISARM) { // cout << "Acknowledge arm/disarm command"; // // } // if(ack.result == MAV_CMD_ACK_OK) { // cout << "received CMD_ACK_OK" << endl; // } // break; // } case MAVLINK_MSG_ID_RAW_IMU: { mavlink_raw_imu_t imu; mavlink_msg_raw_imu_decode(&message, &imu); lock(); time_usec = imu.time_usec; unlock(); ofLogVerbose("ofxMavlink") << "-- RAW_IMU message received --" << endl; ofLogVerbose("ofxMavlink") << "\t time: (us) " << imu.time_usec << endl; ofLogVerbose("ofxMavlink") << "\t acc:" << imu.xacc << " " << imu.yacc << " " << imu.zacc << endl; ofLogVerbose("ofxMavlink") << "\t gyro: (rad/s)" << imu.xgyro << " " << imu.ygyro << " " << imu.zgyro << endl; ofLogVerbose("ofxMavlink") << "\t mag: (Ga)" << imu.xmag << " " << imu.ymag << " " << imu.zmag << endl; break; } case MAVLINK_MSG_ID_GPS_RAW_INT: { ofLogNotice("ofxMavlink") << "-- GPS RAW INT message received --" << endl; mavlink_gps_raw_int_t packet; mavlink_msg_gps_raw_int_decode(&message, &packet); if (packet.fix_type > 2) { lock(); latitude = packet.lat/(double)1E7; longitude = packet.lon/(double)1E7; altitude = packet.alt/1000.0; unlock(); ofLogNotice("ofxMavlink") << "Altitude:" << packet.alt/1000.0 << "latitude:" << (float)packet.lat / 10000000.0 << " longitude:" << (float)packet.lon / 10000000.0 << " "; ofLogNotice("ofxMavlink") << "Satellites visible:" << packet.satellites_visible << " GPS fix:" << packet.fix_type << endl; // if (packet.lat != 0.0) { // // latitude = (float)packet.lat; // longitude = (float)packet.lon; // altitude = (float)packet.alt; // //ModelData.speed = (float)packet.vel / 100.0; // //ModelData.numSat = packet.satellites_visible; // gpsfix = packet.fix_type; } break; } case MAVLINK_MSG_ID_HEARTBEAT: { mavlink_heartbeat_t packet; mavlink_msg_heartbeat_decode(&message, &packet); int droneType = packet.type; int autoPilot = packet.autopilot; // cout << "base mode:" << packet.base_mode << " custom mode:" << packet.custom_mode << endl; // if (packet.base_mode == MAV_MODE_MANUAL_ARMED) { // //ModelData.mode = MODEL_MODE_MANUAL; // cout << "Manual Mode" << endl; // } else if (packet.base_mode == 128 + 64 + 16) { // //ModelData.mode = MODEL_MODE_RTL; // cout << "RTL Mode" << endl; // } else if (packet.base_mode == 128 + 16) { // //ModelData.mode = MODEL_MODE_POSHOLD; // cout << "Poshold Mode" << endl; // } else if (packet.base_mode == 128 + 4) { // //ModelData.mode = MODEL_MODE_MISSION; // cout << "Mission Mode" << endl; // } // if(packet.base_mode == MAV_MODE_STABILIZE_DISARMED) // { // cout << "Stablilize disarmed mode" << endl; // } ofLogNotice("ofxMavlink") << "-- Heartbeat -- sysId:" << message.sysid << " compId:" << message.compid << " drone type:" << droneType << " autoPilot:" << autoPilot; // if(droneType == MAV_TYPE_QUADROTOR) cout << " Quadrotor"; else cout << droneType; // cout << " Autopilot:"; // if(autoPilot == MAV_AUTOPILOT_ARDUPILOTMEGA) cout << " ArduPilotMega"; else cout << autoPilot; // cout << endl; if (message.sysid != 0xff) { lock(); targetId = message.sysid; compId = message.compid; //cout << "compid = " << message.compid << " sysid =" << message.sysid; unlock(); } break; } default: break; } } } }
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); } } } }
static void downstream_handle_gps(mavlink_message_t* msg) { mavlink_gps_raw_int_t pkt; mavlink_msg_gps_raw_int_decode(msg, &pkt); sm.on_downstream_gps_raw_int(&pkt); }
//24 void MavSerialPort::gps_raw_int_handler(){ qDebug() << "MAVLINK_MSG_ID_GPS_RAW_INT\n"; mavlink_msg_gps_raw_int_decode(&message, &gps_raw_int); }
// ------------------------------------------------------------------------------ // Read Messages // ------------------------------------------------------------------------------ void Autopilot_Interface:: read_messages() { bool success; // receive success flag bool received_all = false; // receive only one message Time_Stamps this_timestamps; // Blocking wait for new data while ( not received_all and not time_to_exit ) { // ---------------------------------------------------------------------- // READ MESSAGE // ---------------------------------------------------------------------- mavlink_message_t message; success = serial_port->read_message(message); // ---------------------------------------------------------------------- // HANDLE MESSAGE // ---------------------------------------------------------------------- if( success ) { // Store message sysid and compid. // Note this doesn't handle multiple message sources. current_messages.sysid = message.sysid; current_messages.compid = message.compid; // Handle Message ID switch (message.msgid) { case MAVLINK_MSG_ID_HEARTBEAT: { std::cout << "MAVLINK_MSG_ID_HEARTBEAT" << std::endl; mavlink_msg_heartbeat_decode(&message, &(current_messages.heartbeat)); current_messages.time_stamps.heartbeat = get_time_usec(); this_timestamps.heartbeat = current_messages.time_stamps.heartbeat; break; } case MAVLINK_MSG_ID_SYS_STATUS: { //std::cout << "MAVLINK_MSG_ID_SYS_STATUS" << std::endl; mavlink_msg_sys_status_decode(&message, &(current_messages.sys_status)); current_messages.time_stamps.sys_status = get_time_usec(); this_timestamps.sys_status = current_messages.time_stamps.sys_status; break; } case MAVLINK_MSG_ID_BATTERY_STATUS: { std::cout << "MAVLINK_MSG_ID_BATTERY_STATUS" << std::endl; mavlink_msg_battery_status_decode(&message, &(current_messages.battery_status)); current_messages.time_stamps.battery_status = get_time_usec(); this_timestamps.battery_status = current_messages.time_stamps.battery_status; break; } case MAVLINK_MSG_ID_RADIO_STATUS: { std::cout << "MAVLINK_MSG_ID_RADIO_STATUS" << std::endl; mavlink_msg_radio_status_decode(&message, &(current_messages.radio_status)); current_messages.time_stamps.radio_status = get_time_usec(); this_timestamps.radio_status = current_messages.time_stamps.radio_status; break; } case MAVLINK_MSG_ID_LOCAL_POSITION_NED: { std::cout << "MAVLINK_MSG_ID_LOCAL_POSITION_NED" << std::endl; mavlink_msg_local_position_ned_decode(&message, &(current_messages.local_position_ned)); current_messages.time_stamps.local_position_ned = get_time_usec(); this_timestamps.local_position_ned = current_messages.time_stamps.local_position_ned; break; } case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: { std::cout << "MAVLINK_MSG_ID_GLOBAL_POSITION_INT" << std::endl; mavlink_msg_global_position_int_decode(&message, &(current_messages.global_position_int)); current_messages.time_stamps.global_position_int = get_time_usec(); this_timestamps.global_position_int = current_messages.time_stamps.global_position_int; break; } case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED: { std::cout << "MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED" << std::endl; mavlink_msg_position_target_local_ned_decode(&message, &(current_messages.position_target_local_ned)); current_messages.time_stamps.position_target_local_ned = get_time_usec(); this_timestamps.position_target_local_ned = current_messages.time_stamps.position_target_local_ned; break; } case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT: { std::cout << "MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT" << std::endl; mavlink_msg_position_target_global_int_decode(&message, &(current_messages.position_target_global_int)); current_messages.time_stamps.position_target_global_int = get_time_usec(); this_timestamps.position_target_global_int = current_messages.time_stamps.position_target_global_int; break; } case MAVLINK_MSG_ID_HIGHRES_IMU: { std::cout << "MAVLINK_MSG_ID_HIGHRES_IMU" << std::endl; mavlink_msg_highres_imu_decode(&message, &(current_messages.highres_imu)); current_messages.time_stamps.highres_imu = get_time_usec(); this_timestamps.highres_imu = current_messages.time_stamps.highres_imu; break; } case MAVLINK_MSG_ID_ATTITUDE: { vector4d quaternion; std::cout << "MAVLINK_MSG_ID_ATTITUDE" << std::endl; mavlink_msg_attitude_decode(&message, &(current_messages.attitude)); current_messages.time_stamps.attitude = get_time_usec(); this_timestamps.attitude = current_messages.time_stamps.attitude; quaternion_from_euler(&quaternion, current_messages.attitude.roll, current_messages.attitude.pitch, current_messages.attitude.yaw); std::cout << "\troll: " << current_messages.attitude.roll << "\tpitch: " << current_messages.attitude.pitch << "\tyaw: " << current_messages.attitude.yaw << std::endl; std::cout << "\trollspeed: " << current_messages.attitude.rollspeed << "\tpitchspeed: " << current_messages.attitude.pitchspeed << "\tyawspeed: " << current_messages.attitude.yawspeed << std::endl; std::cout << "\tqx: " << quaternion.x << "\tqy: " << quaternion.y << "\tqz: " << quaternion.z << "\tqw: " << quaternion.w << std::endl; break; } case MAVLINK_MSG_ID_DEBUG: { std::cout << "MAVLINK_MSG_ID_DEBUG" << std::endl; /* mavlink_msg_debug_decode(&message, &(current_messages.attitude)); current_messages.time_stamps.attitude = get_time_usec(); this_timestamps.attitude = current_messages.time_stamps.attitude;*/ break; } case MAVLINK_MSG_ID_STATUSTEXT: { std::cout << "MAVLINK_MSG_ID_STATUSTEXT: "; mavlink_msg_statustext_decode(&message, &(current_messages.statustext)); current_messages.statustext.text[50] = 0; printf("%d - '%s'\n", current_messages.statustext.severity, current_messages.statustext.text); current_messages.time_stamps.statustext = get_time_usec(); this_timestamps.statustext = current_messages.time_stamps.statustext; break; } case MAVLINK_MSG_ID_RAW_IMU: { std::cout << "MAVLINK_MSG_ID_RAW_IMU:" << std::endl; mavlink_msg_raw_imu_decode(&message, &(current_messages.raw_imu)); std::cout << "\tacc :\t" << current_messages.raw_imu.xacc << "\t" << current_messages.raw_imu.yacc << "\t" << current_messages.raw_imu.zacc << std::endl; std::cout << "\tgyro:\t" << current_messages.raw_imu.xgyro << "\t" << current_messages.raw_imu.ygyro << "\t" << current_messages.raw_imu.zgyro << std::endl; std::cout << "\tmag:\t" << current_messages.raw_imu.xmag << "\t" << current_messages.raw_imu.ymag << "\t" << current_messages.raw_imu.zmag << std::endl; current_messages.time_stamps.raw_imu = get_time_usec(); this_timestamps.raw_imu = current_messages.time_stamps.raw_imu; break; } case MAVLINK_MSG_ID_GPS_RAW_INT: { std::cout << "MAVLINK_MSG_ID_GPS_RAW_INT:" << std::endl; mavlink_msg_gps_raw_int_decode(&message, &(current_messages.gps_raw_int)); current_messages.time_stamps.gps_raw_int = get_time_usec(); this_timestamps.gps_raw_int = current_messages.time_stamps.gps_raw_int; std::cout << "\tlat: " << current_messages.gps_raw_int.lat << "\tlon: " << current_messages.gps_raw_int.lon << "\talt: " << current_messages.gps_raw_int.alt << std::endl; std::cout << "\teph: " << current_messages.gps_raw_int.eph << "\tepv: " << current_messages.gps_raw_int.epv << std::endl; std::cout << "\tvel: " << current_messages.gps_raw_int.vel << "\tcog: " << current_messages.gps_raw_int.cog << std::endl; std::cout << "\tfix: " << (int)current_messages.gps_raw_int.fix_type << "\tsat: " << (int)current_messages.gps_raw_int.satellites_visible << std::endl; break; } case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: { vector4d quaternion; std::cout << "MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT:" << std::endl; mavlink_msg_nav_controller_output_decode(&message, &(current_messages.nav_controller_output)); current_messages.time_stamps.nav_controller_output = get_time_usec(); this_timestamps.nav_controller_output = current_messages.time_stamps.nav_controller_output; quaternion_from_euler(&quaternion, current_messages.nav_controller_output.nav_roll, current_messages.nav_controller_output.nav_pitch, current_messages.nav_controller_output.nav_bearing); std::cout << "\tnav_roll: " << current_messages.nav_controller_output.nav_roll << "\tnav_pitch: " << current_messages.nav_controller_output.nav_pitch << "\tnav_bearing: " << current_messages.nav_controller_output.nav_bearing << std::endl; std::cout << "\tqx: " << quaternion.x << "\tqy: " << quaternion.y << "\tqz: " << quaternion.z << "\tqw: " << quaternion.w << std::endl; std::cout << "\ttarget_bearing: " << current_messages.nav_controller_output.target_bearing << "\twp_dist: " << current_messages.nav_controller_output.wp_dist << std::endl; std::cout << "\talt_error: " << current_messages.nav_controller_output.alt_error << "\taspd_error: " << current_messages.nav_controller_output.aspd_error << "\txtrack_error: " << current_messages.nav_controller_output.xtrack_error << std::endl; break; } case MAVLINK_MSG_ID_SCALED_PRESSURE: { std::cout << "MAVLINK_MSG_ID_SCALED_PRESSURE:" << std::endl; mavlink_msg_scaled_pressure_decode(&message, &(current_messages.scaled_pressure)); current_messages.time_stamps.scaled_pressure = get_time_usec(); this_timestamps.scaled_pressure = current_messages.time_stamps.scaled_pressure; std::cout << "\tpress_abs: " << current_messages.scaled_pressure.press_abs << "\tpress_diff: " << current_messages.scaled_pressure.press_diff << "\ttemperature: " << ((double)current_messages.scaled_pressure.temperature / 100.0) << std::endl; break; } case MAVLINK_MSG_ID_RC_CHANNELS_RAW: { std::cout << "MAVLINK_MSG_ID_RC_CHANNELS_RAW:" << std::endl; mavlink_msg_rc_channels_raw_decode(&message, &(current_messages.rc_channels_raw)); current_messages.time_stamps.rc_channels_raw = get_time_usec(); this_timestamps.rc_channels_raw = current_messages.time_stamps.rc_channels_raw; std::cout << "\tport: " << (int)current_messages.rc_channels_raw.port << "\tchan1_raw: " << current_messages.rc_channels_raw.chan1_raw << "\tchan2_raw: " << current_messages.rc_channels_raw.chan2_raw << "\tchan3_raw: " << current_messages.rc_channels_raw.chan3_raw << "\tchan4_raw: " << current_messages.rc_channels_raw.chan4_raw << "\tchan5_raw: " << current_messages.rc_channels_raw.chan5_raw << "\tchan6_raw: " << current_messages.rc_channels_raw.chan6_raw << "\tchan7_raw: " << current_messages.rc_channels_raw.chan7_raw << "\tchan8_raw: " << current_messages.rc_channels_raw.chan8_raw << "\trssi: " << (int)current_messages.rc_channels_raw.rssi << std::endl; break; } case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: { std::cout << "MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:" << std::endl; mavlink_msg_servo_output_raw_decode(&message, &(current_messages.servo_output_raw)); current_messages.time_stamps.servo_output_raw = get_time_usec(); this_timestamps.servo_output_raw = current_messages.time_stamps.servo_output_raw; std::cout << "\tport: " << (int)current_messages.servo_output_raw.port << "\tservo1_raw: " << current_messages.servo_output_raw.servo1_raw << "\tservo2_raw: " << current_messages.servo_output_raw.servo2_raw << "\tservo3_raw: " << current_messages.servo_output_raw.servo3_raw << "\tservo4_raw: " << current_messages.servo_output_raw.servo4_raw << "\tservo5_raw: " << current_messages.servo_output_raw.servo5_raw << "\tservo6_raw: " << current_messages.servo_output_raw.servo6_raw << "\tservo7_raw: " << current_messages.servo_output_raw.servo7_raw << "\tservo8_raw: " << current_messages.servo_output_raw.servo8_raw << std::endl; break; } case MAVLINK_MSG_ID_VFR_HUD: { std::cout << "MAVLINK_MSG_ID_VFR_HUD:" << std::endl; mavlink_msg_vfr_hud_decode(&message, &(current_messages.vfr_hud)); current_messages.time_stamps.vfr_hud = get_time_usec(); this_timestamps.vfr_hud = current_messages.time_stamps.vfr_hud; std::cout << "\tairspeed: " << current_messages.vfr_hud.airspeed << "\tgroundspeed: " << current_messages.vfr_hud.groundspeed << "\theading: " << current_messages.vfr_hud.heading << std::endl; std::cout << "\tthrottle: " << current_messages.vfr_hud.throttle << "\talt: " << current_messages.vfr_hud.alt << "\tclimb: " << current_messages.vfr_hud.climb << std::endl; break; } case MAVLINK_MSG_ID_MISSION_CURRENT: { std::cout << "MAVLINK_MSG_ID_MISSION_CURRENT: "; mavlink_msg_mission_current_decode(&message, &(current_messages.mission_current)); current_messages.time_stamps.mission_current = get_time_usec(); this_timestamps.mission_current = current_messages.time_stamps.mission_current; std::cout << "seq: " << current_messages.mission_current.seq << std::endl; break; } default: { printf("Warning, did not handle message id %i\n",message.msgid); break; } } // end: switch msgid } // end: if read message // Check for receipt of all items received_all = this_timestamps.heartbeat && this_timestamps.sys_status && // this_timestamps.battery_status && // this_timestamps.radio_status && this_timestamps.local_position_ned && // this_timestamps.global_position_int && // this_timestamps.position_target_local_ned && this_timestamps.position_target_global_int && this_timestamps.highres_imu && this_timestamps.attitude ; // give the write thread time to use the port if ( writing_status > false ) usleep(100); // look for components of batches at 10kHz } // end: while not received all return; }
void ArduPilot::readData(){ mavlink_msgs::Mavlink ros_mav_msg; mavlink_message_t msg; mavlink_status_t status; uint8_t byte; while(my_serial_.available()>0){ my_serial_.read(&byte,1); if(mavlink_parse_char(MAVLINK_COMM_0,byte,&msg,&status)){ clock_gettime(CLOCK_MONOTONIC, ¤t_time_); createROSFromMavlink(&msg,&ros_mav_msg); ros_mav_msg.header.stamp.sec = current_time_.tv_sec; ros_mav_msg.header.stamp.nsec = current_time_.tv_nsec; mav_pub_.publish(ros_mav_msg); switch(msg.msgid){ case MAVLINK_MSG_ID_HEARTBEAT: { } break; case MAVLINK_MSG_ID_RAW_IMU: // msg ID: 27 { if(imu_pub_.getNumSubscribers()>0){ mavlink_raw_imu_t imu; mavlink_msg_raw_imu_decode(&msg,&imu); ros_imu_msg_.header.stamp.sec = current_time_.tv_sec; ros_imu_msg_.header.stamp.nsec = current_time_.tv_nsec; //ros_imu_msg_.header.stamp = ros::Time.now(); ros_imu_msg_.linear_acceleration.x = 9.81*(imu.xacc/1000); ros_imu_msg_.linear_acceleration.y = 9.81*(imu.yacc/1000); ros_imu_msg_.linear_acceleration.z = 9.81*(imu.zacc/1000); ros_imu_msg_.angular_velocity.x = imu.xgyro/1000; ros_imu_msg_.angular_velocity.y = imu.ygyro/1000; ros_imu_msg_.angular_velocity.z = imu.zgyro/1000; imu_pub_.publish(ros_imu_msg_); } } break; case MAVLINK_MSG_ID_GPS_RAW_INT: //msg ID: 24 { if(gps_pub_.getNumSubscribers()>0){ mavlink_gps_raw_int_t gps; mavlink_msg_gps_raw_int_decode(&msg,&gps); ros_gps_msg_.header.stamp.sec = current_time_.tv_sec; ros_gps_msg_.header.stamp.nsec = current_time_.tv_nsec; ros_gps_msg_.latitude = gps.lat/1E7; ros_gps_msg_.longitude = gps.lon/1E7; ros_gps_msg_.altitude = gps.alt/1E3; gps_pub_.publish(ros_gps_msg_); gps_common::LLtoUTM(gps.lat,gps.lon, north_, east_, utm_zone_); ros_pose_msg_.position.x = north_; ros_pose_msg_.position.y = east_; ros_pose_msg_.position.z = -gps.alt/1E7; } } break; case MAVLINK_MSG_ID_ATTITUDE: //msg ID: 30 { if(pose_pub_.getNumSubscribers()>0){ mavlink_attitude_t att; mavlink_msg_attitude_decode(&msg,&att); ros_att_msg_.header.stamp.sec = current_time_.tv_sec; ros_att_msg_.header.stamp.nsec = current_time_.tv_nsec; ros_att_msg_.orientation.x = att.roll; ros_att_msg_.orientation.y = att.pitch; ros_att_msg_.orientation.z = att.yaw; att_pub_.publish(ros_att_msg_); ros_pose_msg_.orientation.x = att.roll; ros_pose_msg_.orientation.y = att.pitch; ros_pose_msg_.orientation.z = att.yaw; pose_pub_.publish(ros_pose_msg_); } } break; case MAVLINK_MSG_ID_AHRS: //msg ID: 163 { mavlink_ahrs_t ahrs; } break; case MAVLINK_MSG_ID_RC_CHANNELS_RAW: //msg ID: 35 { if(rc_pub_.getNumSubscribers()>0){ mavlink_rc_channels_raw_t rc; ros_rc_msg_.time_boot_ms = rc.time_boot_ms; ros_rc_msg_.chan1_raw = rc.chan1_raw; ros_rc_msg_.chan2_raw = rc.chan2_raw; ros_rc_msg_.chan3_raw = rc.chan3_raw; ros_rc_msg_.chan4_raw = rc.chan4_raw; ros_rc_msg_.chan5_raw = rc.chan5_raw; ros_rc_msg_.chan6_raw = rc.chan6_raw; ros_rc_msg_.chan7_raw = rc.chan7_raw; ros_rc_msg_.chan8_raw = rc.chan8_raw; ros_rc_msg_.rssi = rc.rssi; rc_pub_.publish(ros_rc_msg_); } } break; /* case MAVLINK_MSG_ID_G: //msg ID: 28 { mavlink_raw_pressure_t baro; } break; */ case MAVLINK_MSG_ID_RAW_PRESSURE: //msg ID: 28 { mavlink_raw_pressure_t baro; } break; case MAVLINK_MSG_ID_COMMAND_LONG: // EXECUTE ACTION break; default: //Do nothing break; } } } // my_serial.flush(); }
// ------------------------------------------------------------------------------ // 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; 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; } case MAVLINK_MSG_ID_GPS_RAW_INT: { //printf("MAVLINK_MSG_ID_GPS_RAW_INT\n"); mavlink_msg_gps_raw_int_decode(&message, &(current_messages)); current_messages.time_stamps.gps_raw= get_time_usec(); this_timestamps.gps_raw= current_messages.time_stamps.gps_raw; 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; }
void MAVLink_Message_Handler::handle_message(uint64_t timestamp, mavlink_message_t &msg) { // ::fprintf(stderr, "msg.msgid=%u\n", msg.msgid); switch(msg.msgid) { case MAVLINK_MSG_ID_AHRS2: { mavlink_ahrs2_t decoded; mavlink_msg_ahrs2_decode(&msg, &decoded); handle_decoded_message(timestamp, decoded); break; } case MAVLINK_MSG_ID_ATTITUDE: { mavlink_attitude_t decoded; mavlink_msg_attitude_decode(&msg, &decoded); handle_decoded_message(timestamp, decoded); break; } case MAVLINK_MSG_ID_EKF_STATUS_REPORT: { mavlink_ekf_status_report_t decoded; mavlink_msg_ekf_status_report_decode(&msg, &decoded); handle_decoded_message(timestamp, decoded); break; } case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: { mavlink_global_position_int_t decoded; mavlink_msg_global_position_int_decode(&msg, &decoded); handle_decoded_message(timestamp, decoded); break; } case MAVLINK_MSG_ID_GPS_RAW_INT: { mavlink_gps_raw_int_t decoded; mavlink_msg_gps_raw_int_decode(&msg, &decoded); handle_decoded_message(timestamp, decoded); break; } case MAVLINK_MSG_ID_HEARTBEAT: { mavlink_heartbeat_t decoded; mavlink_msg_heartbeat_decode(&msg, &decoded); handle_decoded_message(timestamp, decoded); break; } case MAVLINK_MSG_ID_MOUNT_STATUS: { mavlink_mount_status_t decoded; mavlink_msg_mount_status_decode(&msg, &decoded); handle_decoded_message(timestamp, decoded); break; } case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: { mavlink_nav_controller_output_t decoded; mavlink_msg_nav_controller_output_decode(&msg, &decoded); handle_decoded_message(timestamp, decoded); break; } case MAVLINK_MSG_ID_PARAM_VALUE: { mavlink_param_value_t decoded; mavlink_msg_param_value_decode(&msg, &decoded); handle_decoded_message(timestamp, decoded); break; } case MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK: { mavlink_remote_log_data_block_t decoded; mavlink_msg_remote_log_data_block_decode(&msg, &decoded); handle_decoded_message(timestamp, decoded); break; } case MAVLINK_MSG_ID_SCALED_PRESSURE: { mavlink_scaled_pressure_t decoded; mavlink_msg_scaled_pressure_decode(&msg, &decoded); handle_decoded_message(timestamp, decoded); break; } case MAVLINK_MSG_ID_SCALED_PRESSURE2: { mavlink_scaled_pressure2_t decoded; mavlink_msg_scaled_pressure2_decode(&msg, &decoded); handle_decoded_message(timestamp, decoded); break; } case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: { mavlink_servo_output_raw_t decoded; mavlink_msg_servo_output_raw_decode(&msg, &decoded); handle_decoded_message(timestamp, decoded); break; } case MAVLINK_MSG_ID_STATUSTEXT: { mavlink_statustext_t decoded; mavlink_msg_statustext_decode(&msg, &decoded); handle_decoded_message(timestamp, decoded); break; } case MAVLINK_MSG_ID_SYS_STATUS: { mavlink_sys_status_t decoded; mavlink_msg_sys_status_decode(&msg, &decoded); handle_decoded_message(timestamp, decoded); break; } case MAVLINK_MSG_ID_SYSTEM_TIME: { mavlink_system_time_t decoded; mavlink_msg_system_time_decode(&msg, &decoded); handle_decoded_message(timestamp, decoded); break; } case MAVLINK_MSG_ID_VFR_HUD: { mavlink_vfr_hud_t decoded; mavlink_msg_vfr_hud_decode(&msg, &decoded); handle_decoded_message(timestamp, decoded); break; } } }
void mavlink_handleMessage(mavlink_message_t* msg) { mavlink_message_t msg2; char sysmsg_str[1024]; switch (msg->msgid) { case MAVLINK_MSG_ID_HEARTBEAT: { mavlink_heartbeat_t packet; mavlink_msg_heartbeat_decode(msg, &packet); droneType = packet.type; autoPilot = packet.autopilot; if (packet.base_mode == MAV_MODE_MANUAL_ARMED) { ModelData.mode = MODEL_MODE_MANUAL; } else if (packet.base_mode == 128 + 64 + 16) { ModelData.mode = MODEL_MODE_RTL; } else if (packet.base_mode == 128 + 16) { ModelData.mode = MODEL_MODE_POSHOLD; } else if (packet.base_mode == 128 + 4) { ModelData.mode = MODEL_MODE_MISSION; } if (packet.system_status == MAV_STATE_ACTIVE) { ModelData.armed = MODEL_ARMED; } else { ModelData.armed = MODEL_DISARMED; } // SDL_Log("Heartbeat: %i, %i, %i\n", ModelData.armed, ModelData.mode, ModelData.status); ModelData.heartbeat = 100; // sprintf(sysmsg_str, "Heartbeat: %i", (int)time(0)); if ((*msg).sysid != 0xff) { ModelData.sysid = (*msg).sysid; ModelData.compid = (*msg).compid; if (mavlink_maxparam == 0) { mavlink_start_feeds(); } } redraw_flag = 1; break; } case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: { mavlink_rc_channels_scaled_t packet; mavlink_msg_rc_channels_scaled_decode(msg, &packet); // SDL_Log("Radio: %i,%i,%i\n", packet.chan1_scaled, packet.chan2_scaled, packet.chan3_scaled); /* if ((int)packet.chan6_scaled > 1000) { mode = MODE_MISSION; } else if ((int)packet.chan6_scaled < -1000) { mode = MODE_MANUEL; } else { mode = MODE_POSHOLD; } if ((int)packet.chan7_scaled > 1000) { mode = MODE_RTL; } else if ((int)packet.chan7_scaled < -1000) { mode = MODE_SETHOME; } */ ModelData.radio[0] = (int)packet.chan1_scaled / 100.0; ModelData.radio[1] = (int)packet.chan2_scaled / 100.0; ModelData.radio[2] = (int)packet.chan3_scaled / 100.0; ModelData.radio[3] = (int)packet.chan4_scaled / 100.0; ModelData.radio[4] = (int)packet.chan5_scaled / 100.0; ModelData.radio[5] = (int)packet.chan6_scaled / 100.0; ModelData.radio[6] = (int)packet.chan7_scaled / 100.0; ModelData.radio[7] = (int)packet.chan8_scaled / 100.0; redraw_flag = 1; break; } case MAVLINK_MSG_ID_SCALED_PRESSURE: { mavlink_scaled_pressure_t packet; mavlink_msg_scaled_pressure_decode(msg, &packet); // SDL_Log("BAR;%i;%0.2f;%0.2f;%0.2f\n", time(0), packet.press_abs, packet.press_diff, packet.temperature / 100.0); // redraw_flag = 1; break; } case MAVLINK_MSG_ID_ATTITUDE: { mavlink_attitude_t packet; mavlink_msg_attitude_decode(msg, &packet); ModelData.roll = toDeg(packet.roll); ModelData.pitch = toDeg(packet.pitch); if (toDeg(packet.yaw) < 0.0) { ModelData.yaw = 360.0 + toDeg(packet.yaw); } else { ModelData.yaw = toDeg(packet.yaw); } mavlink_update_yaw = 1; // SDL_Log("ATT;%i;%0.2f;%0.2f;%0.2f\n", time(0), toDeg(packet.roll), toDeg(packet.pitch), toDeg(packet.yaw)); redraw_flag = 1; break; } case MAVLINK_MSG_ID_SCALED_IMU: { // SDL_Log("SCALED_IMU\n"); break; } case MAVLINK_MSG_ID_GPS_RAW_INT: { mavlink_gps_raw_int_t packet; mavlink_msg_gps_raw_int_decode(msg, &packet); if (packet.lat != 0.0) { GPS_found = 1; ModelData.p_lat = (float)packet.lat / 10000000.0; ModelData.p_long = (float)packet.lon / 10000000.0; ModelData.p_alt = (float)packet.alt / 1000.0; ModelData.speed = (float)packet.vel / 100.0; ModelData.numSat = packet.satellites_visible; ModelData.gpsfix = packet.fix_type; redraw_flag = 1; } break; } case MAVLINK_MSG_ID_RC_CHANNELS_RAW: { // SDL_Log("RC_CHANNELS_RAW\n"); break; } case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: { // SDL_Log("SERVO_OUTPUT_RAW\n"); break; } case MAVLINK_MSG_ID_SYS_STATUS: { mavlink_sys_status_t packet; mavlink_msg_sys_status_decode(msg, &packet); // SDL_Log("%0.1f %%, %0.3f V)\n", packet.load / 10.0, packet.voltage_battery / 1000.0); ModelData.voltage = packet.voltage_battery / 1000.0; ModelData.load = packet.load / 10.0; redraw_flag = 1; break; } case MAVLINK_MSG_ID_STATUSTEXT: { mavlink_statustext_t packet; mavlink_msg_statustext_decode(msg, &packet); SDL_Log("mavlink: ## %s ##\n", packet.text); sys_message((char *)packet.text); redraw_flag = 1; break; } case MAVLINK_MSG_ID_PARAM_VALUE: { mavlink_param_value_t packet; mavlink_msg_param_value_decode(msg, &packet); char var[101]; uint16_t n1 = 0; uint16_t n2 = 0; for (n1 = 0; n1 < strlen(packet.param_id); n1++) { if (packet.param_id[n1] != 9 && packet.param_id[n1] != ' ' && packet.param_id[n1] != '\t') { var[n2++] = packet.param_id[n1]; } } var[n2++] = 0; // MAV_VAR_FLOAT=0, /* 32 bit float | */ // MAV_VAR_UINT8=1, /* 8 bit unsigned integer | */ // MAV_VAR_INT8=2, /* 8 bit signed integer | */ // MAV_VAR_UINT16=3, /* 16 bit unsigned integer | */ // MAV_VAR_INT16=4, /* 16 bit signed integer | */ // MAV_VAR_UINT32=5, /* 32 bit unsigned integer | */ // MAV_VAR_INT32=6, /* 32 bit signed integer | */ sprintf(sysmsg_str, "PARAM_VALUE (%i/%i): #%s# = %f (Type: %i)", packet.param_index + 1, packet.param_count, var, packet.param_value, packet.param_type); SDL_Log("mavlink: %s\n", sysmsg_str); sys_message(sysmsg_str); mavlink_maxparam = packet.param_count; mavlink_timeout = 0; mavlink_set_value(var, packet.param_value, packet.param_type, packet.param_index); if (packet.param_index + 1 == packet.param_count || packet.param_index % 10 == 0) { mavlink_param_xml_meta_load(); } redraw_flag = 1; break; } case MAVLINK_MSG_ID_MISSION_COUNT: { mavlink_mission_count_t packet; mavlink_msg_mission_count_decode(msg, &packet); sprintf(sysmsg_str, "MISSION_COUNT: %i\n", packet.count); sys_message(sysmsg_str); mission_max = packet.count; if (mission_max > 0) { mavlink_msg_mission_request_pack(127, 0, &msg2, ModelData.sysid, ModelData.compid, 0); mavlink_send_message(&msg2); } redraw_flag = 1; break; } case MAVLINK_MSG_ID_MISSION_ACK: { SDL_Log("mavlink: Mission-Transfer ACK\n"); break; } case MAVLINK_MSG_ID_MISSION_REQUEST: { mavlink_mission_request_t packet; mavlink_msg_mission_request_decode(msg, &packet); uint16_t id = packet.seq; uint16_t id2 = packet.seq; uint16_t type = 0; if (ModelData.teletype == TELETYPE_MEGAPIRATE_NG || ModelData.teletype == TELETYPE_ARDUPILOT) { if (id2 > 0) { id2 = id2 - 1; } else { SDL_Log("mavlink: WORKAROUND: first WP == HOME ?\n"); } } sprintf(sysmsg_str, "sending Waypoint (%i): %s\n", id, WayPoints[1 + id2].name); sys_message(sysmsg_str); if (strcmp(WayPoints[1 + id2].command, "WAYPOINT") == 0) { SDL_Log("mavlink: Type: MAV_CMD_NAV_WAYPOINT\n"); type = MAV_CMD_NAV_WAYPOINT; } else if (strcmp(WayPoints[1 + id2].command, "RTL") == 0) { SDL_Log("mavlink: Type: MAV_CMD_NAV_RETURN_TO_LAUNCH\n"); type = MAV_CMD_NAV_RETURN_TO_LAUNCH; } else if (strcmp(WayPoints[1 + id2].command, "LAND") == 0) { SDL_Log("mavlink: Type: MAV_CMD_NAV_LAND\n"); type = MAV_CMD_NAV_LAND; } else if (strcmp(WayPoints[1 + id2].command, "TAKEOFF") == 0) { SDL_Log("mavlink: Type: MAV_CMD_NAV_TAKEOFF\n"); type = MAV_CMD_NAV_TAKEOFF; } else { SDL_Log("mavlink: Type: UNKNOWN\n"); type = MAV_CMD_NAV_WAYPOINT; } sprintf(sysmsg_str, "SENDING MISSION_ITEM: %i: %f, %f, %f\n", id, WayPoints[1 + id2].p_lat, WayPoints[1 + id2].p_long, WayPoints[1 + id2].p_alt); SDL_Log("mavlink: %s\n", sysmsg_str); mavlink_msg_mission_item_pack(127, 0, &msg2, ModelData.sysid, ModelData.compid, id, 0, type, 0.0, 0.0, WayPoints[1 + id2].radius, WayPoints[1 + id2].wait, WayPoints[1 + id2].orbit, WayPoints[1 + id2].yaw, WayPoints[1 + id2].p_lat, WayPoints[1 + id2].p_long, WayPoints[1 + id2].p_alt); mavlink_send_message(&msg2); /* mavlink_msg_mission_item_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.seq , packet1.frame , packet1.command , packet1.current , packet1.autocontinue , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 , packet1.x , packet1.y , packet1.z ); float param1; ///< PARAM1 / For NAV command MISSIONs: Radius in which the MISSION is accepted as reached, in meters float param2; ///< PARAM2 / For NAV command MISSIONs: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds float param3; ///< PARAM3 / For LOITER command MISSIONs: Orbit to circle around the MISSION, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. float param4; ///< PARAM4 / For NAV and LOITER command MISSIONs: Yaw orientation in degrees, [0..360] 0 = NORTH float x; ///< PARAM5 / local: x position, global: latitude float y; ///< PARAM6 / y position: global: longitude float z; ///< PARAM7 / z position: global: altitude uint16_t seq; ///< Sequence uint16_t command; ///< The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs uint8_t target_system; ///< System ID uint8_t target_component; ///< Component ID uint8_t frame; ///< The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h uint8_t current; ///< false:0, true:1 uint8_t autocontinue; ///< autocontinue to next wp */ redraw_flag = 1; break; } case MAVLINK_MSG_ID_MISSION_ITEM: { mavlink_mission_item_t packet; mavlink_msg_mission_item_decode(msg, &packet); sprintf(sysmsg_str, "RECEIVED MISSION_ITEM: %i/%i: %f, %f, %f (%i)\n", packet.seq, mission_max, packet.x, packet.y, packet.z, packet.frame); SDL_Log("mavlink: %s\n", sysmsg_str); sys_message(sysmsg_str); if (packet.seq < mission_max - 1) { mavlink_msg_mission_request_pack(127, 0, &msg2, ModelData.sysid, ModelData.compid, packet.seq + 1); mavlink_send_message(&msg2); } else { mavlink_msg_mission_ack_pack(127, 0, &msg2, ModelData.sysid, ModelData.compid, 15); mavlink_send_message(&msg2); } if (ModelData.teletype == TELETYPE_MEGAPIRATE_NG || ModelData.teletype == TELETYPE_ARDUPILOT) { if (packet.seq > 0) { packet.seq = packet.seq - 1; } else { SDL_Log("mavlink: WORKAROUND: ignore first WP\n"); break; } } SDL_Log("mavlink: getting WP(%i): %f, %f\n", packet.seq, packet.x, packet.y); switch (packet.command) { case MAV_CMD_NAV_WAYPOINT: { strcpy(WayPoints[1 + packet.seq].command, "WAYPOINT"); break; } case MAV_CMD_NAV_LOITER_UNLIM: { strcpy(WayPoints[1 + packet.seq].command, "LOITER_UNLIM"); break; } case MAV_CMD_NAV_LOITER_TURNS: { strcpy(WayPoints[1 + packet.seq].command, "LOITER_TURNS"); break; } case MAV_CMD_NAV_LOITER_TIME: { strcpy(WayPoints[1 + packet.seq].command, "LOITER_TIME"); break; } case MAV_CMD_NAV_RETURN_TO_LAUNCH: { strcpy(WayPoints[1 + packet.seq].command, "RTL"); break; } case MAV_CMD_NAV_LAND: { strcpy(WayPoints[1 + packet.seq].command, "LAND"); break; } case MAV_CMD_NAV_TAKEOFF: { strcpy(WayPoints[1 + packet.seq].command, "TAKEOFF"); break; } default: { sprintf(WayPoints[1 + packet.seq].command, "CMD:%i", packet.command); break; } } if (packet.x == 0.0) { packet.x = 0.00001; } if (packet.y == 0.0) { packet.y = 0.00001; } if (packet.z == 0.0) { packet.z = 0.00001; } WayPoints[1 + packet.seq].p_lat = packet.x; WayPoints[1 + packet.seq].p_long = packet.y; WayPoints[1 + packet.seq].p_alt = packet.z; WayPoints[1 + packet.seq].yaw = packet.param4; sprintf(WayPoints[1 + packet.seq].name, "WP%i", packet.seq + 1); WayPoints[1 + packet.seq + 1].p_lat = 0.0; WayPoints[1 + packet.seq + 1].p_long = 0.0; WayPoints[1 + packet.seq + 1].p_alt = 0.0; WayPoints[1 + packet.seq + 1].yaw = 0.0; WayPoints[1 + packet.seq + 1].name[0] = 0; WayPoints[1 + packet.seq + 1].command[0] = 0; /* float param1; ///< PARAM1 / For NAV command MISSIONs: Radius in which the MISSION is accepted as reached, in meters float param2; ///< PARAM2 / For NAV command MISSIONs: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds float param3; ///< PARAM3 / For LOITER command MISSIONs: Orbit to circle around the MISSION, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. float param4; ///< PARAM4 / For NAV and LOITER command MISSIONs: Yaw orientation in degrees, [0..360] 0 = NORTH float x; ///< PARAM5 / local: x position, global: latitude float y; ///< PARAM6 / y position: global: longitude float z; ///< PARAM7 / z position: global: altitude uint16_t seq; ///< Sequence uint16_t command; ///< The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs uint8_t target_system; ///< System ID uint8_t target_component; ///< Component ID uint8_t frame; ///< The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h uint8_t current; ///< false:0, true:1 uint8_t autocontinue; ///< autocontinue to next wp GCS_MAVLink/message_definitions_v1.0/common.xml: <entry value="0" name="MAV_FRAME_GLOBAL"> GCS_MAVLink/message_definitions_v1.0/common.xml: <entry value="1" name="MAV_FRAME_LOCAL_NED"> GCS_MAVLink/message_definitions_v1.0/common.xml: <entry value="2" name="MAV_FRAME_MISSION"> GCS_MAVLink/message_definitions_v1.0/common.xml: <entry value="3" name="MAV_FRAME_GLOBAL_RELATIVE_ALT"> GCS_MAVLink/message_definitions_v1.0/common.xml: <entry value="4" name="MAV_FRAME_LOCAL_ENU"> */ redraw_flag = 1; break; } case MAVLINK_MSG_ID_MISSION_CURRENT: { mavlink_mission_current_t packet; mavlink_msg_mission_current_decode(msg, &packet); // SDL_Log("mavlink: ## Active_WP %f ##\n", packet.seq); uav_active_waypoint = (uint8_t)packet.seq; break; } case MAVLINK_MSG_ID_RAW_IMU: { mavlink_raw_imu_t packet; mavlink_msg_raw_imu_decode(msg, &packet); /* SDL_Log("## IMU_RAW_ACC_X %i ##\n", packet.xacc); SDL_Log("## IMU_RAW_ACC_Y %i ##\n", packet.yacc); SDL_Log("## IMU_RAW_ACC_Z %i ##\n", packet.zacc); SDL_Log("## IMU_RAW_GYRO_X %i ##\n", packet.xgyro); SDL_Log("## IMU_RAW_GYRO_Y %i ##\n", packet.ygyro); SDL_Log("## IMU_RAW_GYRO_Z %i ##\n", packet.zgyro); SDL_Log("## IMU_RAW_MAG_X %i ##\n", packet.xmag); SDL_Log("## IMU_RAW_MAG_Y %i ##\n", packet.ymag); SDL_Log("## IMU_RAW_MAG_Z %i ##\n", packet.zmag); */ ModelData.acc_x = (float)packet.xacc / 1000.0; ModelData.acc_y = (float)packet.yacc / 1000.0; ModelData.acc_z = (float)packet.zacc / 1000.0; ModelData.gyro_x = (float)packet.zgyro; ModelData.gyro_y = (float)packet.zgyro; ModelData.gyro_z = (float)packet.zgyro; redraw_flag = 1; break; } case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: { mavlink_nav_controller_output_t packet; mavlink_msg_nav_controller_output_decode(msg, &packet); /* nav_roll nav_pitch alt_error aspd_error xtrack_error nav_bearing target_bearing wp_dist */ break; } case MAVLINK_MSG_ID_VFR_HUD: { mavlink_vfr_hud_t packet; mavlink_msg_vfr_hud_decode(msg, &packet); // SDL_Log("## pa %f ##\n", packet.airspeed); // SDL_Log("## pg %f ##\n", packet.groundspeed); // SDL_Log("## palt %f ##\n", packet.alt); if (GPS_found == 0) { ModelData.p_alt = packet.alt; } // SDL_Log("## pc %f ##\n", packet.climb); // SDL_Log("## ph %i ##\n", packet.heading); // SDL_Log("## pt %i ##\n", packet.throttle); break; } case MAVLINK_MSG_ID_RADIO: { mavlink_radio_t packet; mavlink_msg_radio_decode(msg, &packet); SDL_Log("mavlink: ## rxerrors %i ##\n", packet.rxerrors); SDL_Log("mavlink: ## fixed %i ##\n", packet.fixed); SDL_Log("mavlink: ## rssi %i ##\n", packet.rssi); SDL_Log("mavlink: ## remrssi %i ##\n", packet.remrssi); SDL_Log("mavlink: ## txbuf %i ##\n", packet.txbuf); SDL_Log("mavlink: ## noise %i ##\n", packet.noise); SDL_Log("mavlink: ## remnoise %i ##\n", packet.remnoise); break; } default: { // SDL_Log(" ## MSG_ID == %i ##\n", msg->msgid); break; } } }