/* We eavesdrop on MAVLINK_MSG_ID_GLOBAL_POSITION_INT and MAVLINK_MSG_ID_SCALED_PRESSUREs */ void GCS_MAVLINK_Tracker::packetReceived(const mavlink_status_t &status, mavlink_message_t &msg) { // return immediately if sysid doesn't match our target sysid if ((tracker.g.sysid_target != 0) && (tracker.g.sysid_target != msg.sysid)) { return; } switch (msg.msgid) { case MAVLINK_MSG_ID_HEARTBEAT: { mavlink_check_target(msg); break; } case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: { // decode mavlink_global_position_int_t packet; mavlink_msg_global_position_int_decode(&msg, &packet); tracker.tracking_update_position(packet); break; } case MAVLINK_MSG_ID_SCALED_PRESSURE: { // decode mavlink_scaled_pressure_t packet; mavlink_msg_scaled_pressure_decode(&msg, &packet); tracker.tracking_update_pressure(packet); break; } } GCS_MAVLINK::packetReceived(status, msg); }
/* We eavesdrop on MAVLINK_MSG_ID_GLOBAL_POSITION_INT and MAVLINK_MSG_ID_SCALED_PRESSUREs */ void Tracker::mavlink_snoop(const mavlink_message_t* msg) { // return immediately if sysid doesn't match our target sysid if ((g.sysid_target != 0) && (g.sysid_target != msg->sysid)) { return; } switch (msg->msgid) { case MAVLINK_MSG_ID_HEARTBEAT: { mavlink_check_target(msg); break; } case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: { // decode mavlink_global_position_int_t packet; mavlink_msg_global_position_int_decode(msg, &packet); tracking_update_position(packet); break; } case MAVLINK_MSG_ID_SCALED_PRESSURE: { // decode mavlink_scaled_pressure_t packet; mavlink_msg_scaled_pressure_decode(msg, &packet); tracking_update_pressure(packet); break; } } }
void handle_scaled_pressure(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) { if (has_hr_imu) return; mavlink_scaled_pressure_t press; mavlink_msg_scaled_pressure_decode(msg, &press); std_msgs::Header header; header.stamp = ros::Time::now(); header.frame_id = frame_id; sensor_msgs::TemperaturePtr temp_msg = boost::make_shared<sensor_msgs::Temperature>(); temp_msg->temperature = press.temperature / 100.0; temp_msg->header = header; temp_pub.publish(temp_msg); sensor_msgs::FluidPressurePtr atmp_msg = boost::make_shared<sensor_msgs::FluidPressure>(); atmp_msg->fluid_pressure = press.press_abs * 100.0; atmp_msg->header = header; press_pub.publish(atmp_msg); }
void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) { switch (msg->msgid) { // If we are currently operating as a proxy for a remote, // alas we have to look inside each packet to see if its for us or for the remote case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: { handle_request_data_stream(msg, true); break; } case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { handle_param_request_list(msg); break; } case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { handle_param_request_read(msg); break; } case MAVLINK_MSG_ID_PARAM_SET: { handle_param_set(msg, NULL); break; } case MAVLINK_MSG_ID_HEARTBEAT: break; case MAVLINK_MSG_ID_COMMAND_LONG: { // decode mavlink_command_long_t packet; mavlink_msg_command_long_decode(msg, &packet); uint8_t result = MAV_RESULT_UNSUPPORTED; // do command send_text_P(SEVERITY_LOW,PSTR("command received: ")); switch(packet.command) { case MAV_CMD_PREFLIGHT_CALIBRATION: { if (is_equal(packet.param1,1.0f)) { tracker.ins.init_gyro(); if (tracker.ins.gyro_calibrated_ok_all()) { tracker.ahrs.reset_gyro_drift(); result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; } } if (is_equal(packet.param3,1.0f)) { tracker.init_barometer(); // zero the altitude difference on next baro update tracker.nav_status.need_altitude_calibration = true; } if (is_equal(packet.param4,1.0f)) { // Cant trim radio } else if (is_equal(packet.param5,1.0f)) { float trim_roll, trim_pitch; AP_InertialSensor_UserInteract_MAVLink interact(this); if(tracker.ins.calibrate_accel(&interact, trim_roll, trim_pitch)) { // reset ahrs's trim to suggested values from calibration routine tracker.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0)); } } else if (is_equal(packet.param5,2.0f)) { // accel trim float trim_roll, trim_pitch; if(tracker.ins.calibrate_trim(trim_roll, trim_pitch)) { // reset ahrs's trim to suggested values from calibration routine tracker.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0)); result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; } } result = MAV_RESULT_ACCEPTED; break; } case MAV_CMD_COMPONENT_ARM_DISARM: if (packet.target_component == MAV_COMP_ID_SYSTEM_CONTROL) { if (is_equal(packet.param1,1.0f)) { tracker.arm_servos(); result = MAV_RESULT_ACCEPTED; } else if (is_zero(packet.param1)) { tracker.disarm_servos(); result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_UNSUPPORTED; } } else { result = MAV_RESULT_UNSUPPORTED; } break; case MAV_CMD_DO_SET_MODE: switch ((uint16_t)packet.param1) { case MAV_MODE_MANUAL_ARMED: case MAV_MODE_MANUAL_DISARMED: tracker.set_mode(MANUAL); result = MAV_RESULT_ACCEPTED; break; case MAV_MODE_AUTO_ARMED: case MAV_MODE_AUTO_DISARMED: tracker.set_mode(AUTO); result = MAV_RESULT_ACCEPTED; break; default: result = MAV_RESULT_UNSUPPORTED; } break; case MAV_CMD_DO_SET_SERVO: if (tracker.servo_test_set_servo(packet.param1, packet.param2)) { result = MAV_RESULT_ACCEPTED; } break; // mavproxy/mavutil sends this when auto command is entered case MAV_CMD_MISSION_START: tracker.set_mode(AUTO); result = MAV_RESULT_ACCEPTED; break; case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { if (is_equal(packet.param1,1.0f) || is_equal(packet.param1,3.0f)) { // when packet.param1 == 3 we reboot to hold in bootloader hal.scheduler->reboot(is_equal(packet.param1,3.0f)); result = MAV_RESULT_ACCEPTED; } break; } case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: { if (is_equal(packet.param1,1.0f)) { tracker.gcs[chan-MAVLINK_COMM_0].send_autopilot_version(); result = MAV_RESULT_ACCEPTED; } break; } default: break; } mavlink_msg_command_ack_send( chan, packet.command, result); break; } // When mavproxy 'wp sethome' case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST: { // decode mavlink_mission_write_partial_list_t packet; mavlink_msg_mission_write_partial_list_decode(msg, &packet); if (packet.start_index == 0) { // New home at wp index 0. Ask for it waypoint_receiving = true; waypoint_request_i = 0; waypoint_request_last = 0; send_message(MSG_NEXT_WAYPOINT); waypoint_receiving = true; } break; } // XXX receive a WP from GCS and store in EEPROM if it is HOME case MAVLINK_MSG_ID_MISSION_ITEM: { // decode mavlink_mission_item_t packet; uint8_t result = MAV_MISSION_ACCEPTED; mavlink_msg_mission_item_decode(msg, &packet); struct Location tell_command = {}; switch (packet.frame) { case MAV_FRAME_MISSION: case MAV_FRAME_GLOBAL: { tell_command.lat = 1.0e7f*packet.x; // in as DD converted to * t7 tell_command.lng = 1.0e7f*packet.y; // in as DD converted to * t7 tell_command.alt = packet.z*1.0e2f; // in as m converted to cm tell_command.options = 0; // absolute altitude break; } #ifdef MAV_FRAME_LOCAL_NED case MAV_FRAME_LOCAL_NED: // local (relative to home position) { tell_command.lat = 1.0e7f*ToDeg(packet.x/ (RADIUS_OF_EARTH*cosf(ToRad(home.lat/1.0e7f)))) + home.lat; tell_command.lng = 1.0e7f*ToDeg(packet.y/RADIUS_OF_EARTH) + home.lng; tell_command.alt = -packet.z*1.0e2f; tell_command.options = MASK_OPTIONS_RELATIVE_ALT; break; } #endif #ifdef MAV_FRAME_LOCAL case MAV_FRAME_LOCAL: // local (relative to home position) { tell_command.lat = 1.0e7f*ToDeg(packet.x/ (RADIUS_OF_EARTH*cosf(ToRad(home.lat/1.0e7f)))) + home.lat; tell_command.lng = 1.0e7f*ToDeg(packet.y/RADIUS_OF_EARTH) + home.lng; tell_command.alt = packet.z*1.0e2f; tell_command.options = MASK_OPTIONS_RELATIVE_ALT; break; } #endif case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude { tell_command.lat = 1.0e7f * packet.x; // in as DD converted to * t7 tell_command.lng = 1.0e7f * packet.y; // in as DD converted to * t7 tell_command.alt = packet.z * 1.0e2f; tell_command.options = MASK_OPTIONS_RELATIVE_ALT; // store altitude relative!! Always!! break; } default: result = MAV_MISSION_UNSUPPORTED_FRAME; break; } if (result != MAV_MISSION_ACCEPTED) goto mission_failed; // Check if receiving waypoints (mission upload expected) if (!waypoint_receiving) { result = MAV_MISSION_ERROR; goto mission_failed; } // check if this is the HOME wp if (packet.seq == 0) { tracker.set_home(tell_command); // New home in EEPROM send_text_P(SEVERITY_LOW,PSTR("new HOME received")); waypoint_receiving = false; } mission_failed: // we are rejecting the mission/waypoint mavlink_msg_mission_ack_send( chan, msg->sysid, msg->compid, result); break; } case MAVLINK_MSG_ID_MANUAL_CONTROL: { mavlink_manual_control_t packet; mavlink_msg_manual_control_decode(msg, &packet); tracker.tracking_manual_control(packet); break; } case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: { // decode mavlink_global_position_int_t packet; mavlink_msg_global_position_int_decode(msg, &packet); tracker.tracking_update_position(packet); break; } case MAVLINK_MSG_ID_SCALED_PRESSURE: { // decode mavlink_scaled_pressure_t packet; mavlink_msg_scaled_pressure_decode(msg, &packet); tracker.tracking_update_pressure(packet); break; } case MAVLINK_MSG_ID_SET_MODE: { handle_set_mode(msg, FUNCTOR_BIND(&tracker, &Tracker::mavlink_set_mode, bool, uint8_t)); break; } #if HAL_CPU_CLASS > HAL_CPU_CLASS_16 case MAVLINK_MSG_ID_SERIAL_CONTROL: handle_serial_control(msg, tracker.gps); break; case MAVLINK_MSG_ID_GPS_INJECT_DATA: handle_gps_inject(msg, tracker.gps); break; #endif case MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST: tracker.gcs[chan-MAVLINK_COMM_0].send_autopilot_version(); break; } // end switch } // end handle mavlink
void GCS_MAVLINK_Tracker::handleMessage(mavlink_message_t* msg) { switch (msg->msgid) { case MAVLINK_MSG_ID_HEARTBEAT: break; // When mavproxy 'wp sethome' case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST: { // decode mavlink_mission_write_partial_list_t packet; mavlink_msg_mission_write_partial_list_decode(msg, &packet); if (packet.start_index == 0) { // New home at wp index 0. Ask for it waypoint_receiving = true; waypoint_request_i = 0; waypoint_request_last = 0; send_message(MSG_NEXT_WAYPOINT); } break; } // XXX receive a WP from GCS and store in EEPROM if it is HOME case MAVLINK_MSG_ID_MISSION_ITEM: { // decode mavlink_mission_item_t packet; MAV_MISSION_RESULT result = MAV_MISSION_ACCEPTED; mavlink_msg_mission_item_decode(msg, &packet); struct Location tell_command = {}; switch (packet.frame) { case MAV_FRAME_MISSION: case MAV_FRAME_GLOBAL: { tell_command.lat = 1.0e7f*packet.x; // in as DD converted to * t7 tell_command.lng = 1.0e7f*packet.y; // in as DD converted to * t7 tell_command.alt = packet.z*1.0e2f; // in as m converted to cm tell_command.options = 0; // absolute altitude break; } #ifdef MAV_FRAME_LOCAL_NED case MAV_FRAME_LOCAL_NED: // local (relative to home position) { tell_command.lat = 1.0e7f*ToDeg(packet.x/ (RADIUS_OF_EARTH*cosf(ToRad(home.lat/1.0e7f)))) + home.lat; tell_command.lng = 1.0e7f*ToDeg(packet.y/RADIUS_OF_EARTH) + home.lng; tell_command.alt = -packet.z*1.0e2f; tell_command.options = MASK_OPTIONS_RELATIVE_ALT; break; } #endif #ifdef MAV_FRAME_LOCAL case MAV_FRAME_LOCAL: // local (relative to home position) { tell_command.lat = 1.0e7f*ToDeg(packet.x/ (RADIUS_OF_EARTH*cosf(ToRad(home.lat/1.0e7f)))) + home.lat; tell_command.lng = 1.0e7f*ToDeg(packet.y/RADIUS_OF_EARTH) + home.lng; tell_command.alt = packet.z*1.0e2f; tell_command.options = MASK_OPTIONS_RELATIVE_ALT; break; } #endif case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude { tell_command.lat = 1.0e7f * packet.x; // in as DD converted to * t7 tell_command.lng = 1.0e7f * packet.y; // in as DD converted to * t7 tell_command.alt = packet.z * 1.0e2f; tell_command.options = MASK_OPTIONS_RELATIVE_ALT; // store altitude relative!! Always!! break; } default: result = MAV_MISSION_UNSUPPORTED_FRAME; break; } if (result != MAV_MISSION_ACCEPTED) goto mission_failed; // Check if receiving waypoints (mission upload expected) if (!waypoint_receiving) { result = MAV_MISSION_ERROR; goto mission_failed; } // check if this is the HOME wp if (packet.seq == 0) { tracker.set_home(tell_command); // New home in EEPROM send_text(MAV_SEVERITY_INFO,"New HOME received"); waypoint_receiving = false; } mission_failed: // we are rejecting the mission/waypoint mavlink_msg_mission_ack_send( chan, msg->sysid, msg->compid, result, MAV_MISSION_TYPE_MISSION); break; } case MAVLINK_MSG_ID_MANUAL_CONTROL: { mavlink_manual_control_t packet; mavlink_msg_manual_control_decode(msg, &packet); tracker.tracking_manual_control(packet); break; } case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: { // decode mavlink_global_position_int_t packet; mavlink_msg_global_position_int_decode(msg, &packet); tracker.tracking_update_position(packet); break; } case MAVLINK_MSG_ID_SCALED_PRESSURE: { // decode mavlink_scaled_pressure_t packet; mavlink_msg_scaled_pressure_decode(msg, &packet); tracker.tracking_update_pressure(packet); break; } default: handle_common_message(msg); break; } // end switch } // end handle mavlink
// ------------------------------------------------------------------------------ // 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 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; } } }