void ml_logger_default_write_sys_status(ml_logger_t* mll, mavlink_message_t* msg)
{
    /**

        uint32_t onboard_control_sensors_present; ///< Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
        uint32_t onboard_control_sensors_enabled; ///< Bitmask showing which onboard controllers and sensors are enabled:  Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
        uint32_t onboard_control_sensors_health; ///< Bitmask showing which onboard controllers and sensors are operational or have an error:  Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
        uint16_t load; ///< Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000
        uint16_t voltage_battery; ///< Battery voltage, in millivolts (1 = 1 millivolt)
        int16_t current_battery; ///< Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current
        uint16_t drop_rate_comm; ///< Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
        uint16_t errors_comm; ///< Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
        uint16_t errors_count1; ///< Autopilot-specific errors
        uint16_t errors_count2; ///< Autopilot-specific errors
        uint16_t errors_count3; ///< Autopilot-specific errors
        uint16_t errors_count4; ///< Autopilot-specific errors
        int8_t battery_remaining; ///< Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery

    **/


    if (msg->msgid == MAVLINK_MSG_ID_SYS_STATUS)
    {
        mavlink_sys_status_t stat;
        mavlink_msg_sys_status_decode(msg, &stat);

        fprintf(mll->files.sys_status.fs, "%12" PRIu64 " %10d %10d %10d %5d %5d %5d %5d %5d %5d %5d %5d %5d %3d\n",
               utils_us_since_epoch(),
               stat.onboard_control_sensors_present,
               stat.onboard_control_sensors_enabled,
               stat.onboard_control_sensors_health,
               stat.load,
               stat.voltage_battery,
               stat.current_battery,
               stat.drop_rate_comm,
               stat.errors_comm,
               stat.errors_count1,
               stat.errors_count2,
               stat.errors_count3,
               stat.errors_count4,
               stat.battery_remaining
                );
        fflush(mll->files.sys_status.fs);
    }
}
Пример #2
0
    void handle_sys_status(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
        mavlink_sys_status_t stat;
        mavlink_msg_sys_status_decode(msg, &stat);

        float volt = stat.voltage_battery / 1000.0f;	// mV
        float curr = stat.current_battery / 100.0f;	// 10 mA or -1
        float rem = stat.battery_remaining / 100.0f;	// or -1

        auto batt_msg = boost::make_shared<mavros::BatteryStatus>();
        batt_msg->header.stamp = ros::Time::now();
        batt_msg->voltage = volt;
        batt_msg->current = curr;
        batt_msg->remaining = rem;

        sys_diag.set(stat);
        batt_diag.set(volt, curr, rem);
        batt_pub.publish(batt_msg);
    }
Пример #3
0
void _MavlinkInterface::handleMessages()
{
	Time_Stamps this_timestamps;
	mavlink_message_t message;
	int nMsgHandled;

	nMsgHandled = 0;

	//Handle Message while new message is received
	while (readMessage(message))
	{
		// Note this doesn't handle multiple message sources.
		current_messages.sysid = message.sysid;
		current_messages.compid = message.compid;

		system_id = current_messages.sysid;
		autopilot_id = current_messages.compid;

		// Handle Message ID
		switch (message.msgid)
		{

		case MAVLINK_MSG_ID_HEARTBEAT:
		{
//			printf("MAVLINK_MSG_ID_HEARTBEAT\n");
			mavlink_msg_heartbeat_decode(&message,
					&(current_messages.heartbeat));
			current_messages.time_stamps.heartbeat = get_time_usec();
			this_timestamps.heartbeat = current_messages.time_stamps.heartbeat;
			break;
		}

		case MAVLINK_MSG_ID_SYS_STATUS:
		{
//			printf("MAVLINK_MSG_ID_SYS_STATUS\n");
			mavlink_msg_sys_status_decode(&message,
					&(current_messages.sys_status));
			current_messages.time_stamps.sys_status = get_time_usec();
			this_timestamps.sys_status =
					current_messages.time_stamps.sys_status;
			break;
		}

		case MAVLINK_MSG_ID_BATTERY_STATUS:
		{
//			printf("MAVLINK_MSG_ID_BATTERY_STATUS\n");
			mavlink_msg_battery_status_decode(&message,
					&(current_messages.battery_status));
			current_messages.time_stamps.battery_status = get_time_usec();
			this_timestamps.battery_status =
					current_messages.time_stamps.battery_status;
			break;
		}

		case MAVLINK_MSG_ID_RADIO_STATUS:
		{
//			printf("MAVLINK_MSG_ID_RADIO_STATUS\n");
			mavlink_msg_radio_status_decode(&message,
					&(current_messages.radio_status));
			current_messages.time_stamps.radio_status = get_time_usec();
			this_timestamps.radio_status =
					current_messages.time_stamps.radio_status;
			break;
		}

		case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
		{
//			printf("MAVLINK_MSG_ID_LOCAL_POSITION_NED\n");
			mavlink_msg_local_position_ned_decode(&message,
					&(current_messages.local_position_ned));
			current_messages.time_stamps.local_position_ned = get_time_usec();
			this_timestamps.local_position_ned =
					current_messages.time_stamps.local_position_ned;
			break;
		}

		case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
		{
//			printf("MAVLINK_MSG_ID_GLOBAL_POSITION_INT\n");
			mavlink_msg_global_position_int_decode(&message,
					&(current_messages.global_position_int));
			current_messages.time_stamps.global_position_int = get_time_usec();
			this_timestamps.global_position_int =
					current_messages.time_stamps.global_position_int;
			break;
		}

		case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
		{
//			printf("MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED\n");
			mavlink_msg_position_target_local_ned_decode(&message,
					&(current_messages.position_target_local_ned));
			current_messages.time_stamps.position_target_local_ned =
					get_time_usec();
			this_timestamps.position_target_local_ned =
					current_messages.time_stamps.position_target_local_ned;
			break;
		}

		case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT:
		{
//			printf("MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT\n");
			mavlink_msg_position_target_global_int_decode(&message,
					&(current_messages.position_target_global_int));
			current_messages.time_stamps.position_target_global_int =
					get_time_usec();
			this_timestamps.position_target_global_int =
					current_messages.time_stamps.position_target_global_int;
			break;
		}

		case MAVLINK_MSG_ID_HIGHRES_IMU:
		{
//			printf("MAVLINK_MSG_ID_HIGHRES_IMU\n");
			mavlink_msg_highres_imu_decode(&message,
					&(current_messages.highres_imu));
			current_messages.time_stamps.highres_imu = get_time_usec();
			this_timestamps.highres_imu =
					current_messages.time_stamps.highres_imu;
			break;
		}

		case MAVLINK_MSG_ID_ATTITUDE:
		{
//			printf("MAVLINK_MSG_ID_ATTITUDE\n");
			mavlink_msg_attitude_decode(&message, &(current_messages.attitude));
			current_messages.time_stamps.attitude = get_time_usec();
			this_timestamps.attitude = current_messages.time_stamps.attitude;
			break;
		}

		default:
		{
//			printf("Warning, did not handle message id %i\n", message.msgid);
			break;
		}

		} // end: switch msgid


		if(++nMsgHandled >= NUM_MSG_HANDLE)return;
	}

}
// ------------------------------------------------------------------------------
//   Read Messages
// ------------------------------------------------------------------------------
void
Autopilot_Interface::
read_messages(FILE *fd)
{
	bool success;               // receive success flag
	bool received_all = false;  // receive only one message
	Time_Stamps this_timestamps;

//	fprintf(fd,"   xacc	    yacc	zacc	 xgyro  ygyro	  zgyro	    xmag	   ymag	  zmag	altitude	temperature\n");

	// Blocking wait for new data
	while ( !received_all and !time_to_exit )
	{
		// ----------------------------------------------------------------------
		//   READ MESSAGE
		// ----------------------------------------------------------------------
		mavlink_message_t message;
		success = serial_port->read_message(message);
		// printf("messageid = %d\n", message.msgid);
		printf("flag = %d     ch = %d  \n",receive_flag,receive_ch);
		// ----------------------------------------------------------------------
		//   HANDLE MESSAGE
		// ----------------------------------------------------------------------
		if( success )
		{

			// Store message sysid and compid.
			// Note this doesn't handle multiple message sources.
			current_messages.sysid  = message.sysid;
			current_messages.compid = message.compid;

			// Handle Message ID
			switch (message.msgid)
			{

				case MAVLINK_MSG_ID_HEARTBEAT:
				{
					//printf("MAVLINK_MSG_ID_HEARTBEAT\n");
					mavlink_msg_heartbeat_decode(&message, &(current_messages.heartbeat));
					current_messages.time_stamps.heartbeat = get_time_usec();
					this_timestamps.heartbeat = current_messages.time_stamps.heartbeat;
					break;
				}

				case MAVLINK_MSG_ID_SYS_STATUS:
				{
					//printf("MAVLINK_MSG_ID_SYS_STATUS\n");
					mavlink_msg_sys_status_decode(&message, &(current_messages.sys_status));
					current_messages.time_stamps.sys_status = get_time_usec();
					this_timestamps.sys_status = current_messages.time_stamps.sys_status;
					break;
				}

				case MAVLINK_MSG_ID_BATTERY_STATUS:
				{
					//printf("MAVLINK_MSG_ID_BATTERY_STATUS\n");
					mavlink_msg_battery_status_decode(&message, &(current_messages.battery_status));
					current_messages.time_stamps.battery_status = get_time_usec();
					this_timestamps.battery_status = current_messages.time_stamps.battery_status;
					break;
				}

				case MAVLINK_MSG_ID_RADIO_STATUS:
				{
					//printf("MAVLINK_MSG_ID_RADIO_STATUS\n");
					mavlink_msg_radio_status_decode(&message, &(current_messages.radio_status));
					current_messages.time_stamps.radio_status = get_time_usec();
					this_timestamps.radio_status = current_messages.time_stamps.radio_status;
					break;
				}

				case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
				{
					//printf("MAVLINK_MSG_ID_LOCAL_POSITION_NED\n");
					mavlink_msg_local_position_ned_decode(&message, &(current_messages.local_position_ned));
					//fprintf(fd,"x = %f y = %f z = %f \n",current_messages.local_position_ned.x,current_messages.local_position_ned.y,current_messages.local_position_ned.z);
					current_messages.time_stamps.local_position_ned = get_time_usec();
					this_timestamps.local_position_ned = current_messages.time_stamps.local_position_ned;
					break;
				}

				case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
				{
					//printf("MAVLINK_MSG_ID_GLOBAL_POSITION_INT\n");
					mavlink_msg_global_position_int_decode(&message, &(current_messages.global_position_int));
					current_messages.time_stamps.global_position_int = get_time_usec();
					this_timestamps.global_position_int = current_messages.time_stamps.global_position_int;
					break;
				}

				case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
				{
					//printf("MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED\n");
					mavlink_msg_position_target_local_ned_decode(&message, &(current_messages.position_target_local_ned));
					current_messages.time_stamps.position_target_local_ned = get_time_usec();
					this_timestamps.position_target_local_ned = current_messages.time_stamps.position_target_local_ned;
					break;
				}

				case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT:
				{
					//printf("MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT\n");
					mavlink_msg_position_target_global_int_decode(&message, &(current_messages.position_target_global_int));
					fprintf(fd, "lat_int = %d\n", current_messages.position_target_global_int.lat_int);
					current_messages.time_stamps.position_target_global_int = get_time_usec();
					this_timestamps.position_target_global_int = current_messages.time_stamps.position_target_global_int;
					break;
				}

				case MAVLINK_MSG_ID_HIGHRES_IMU:
				{
					//printf("MAVLINK_MSG_ID_HIGHRES_IMU\n");
					mavlink_msg_highres_imu_decode(&message, &(current_messages.highres_imu));
					// fprintf(fd,"acc: %f %f %f ",current_messages.highres_imu.xacc,current_messages.highres_imu.yacc,current_messages.highres_imu.zacc);
					// fprintf(fd,"gyro: %f %f	%f ",current_messages.highres_imu.xgyro,current_messages.highres_imu.ygyro,current_messages.highres_imu.zgyro);
					// fprintf(fd,"mag: %f %f %f ",current_messages.highres_imu.xmag,current_messages.highres_imu.ymag,current_messages.highres_imu.zmag);
					// fprintf(fd,"alt: %f %f\n",current_messages.highres_imu.pressure_alt,current_messages.highres_imu.temperature);
					if (receive_flag && receive_ch == 10) {
						fprintf(fd,"acc: %f %f %f ",current_messages.highres_imu.xacc,current_messages.highres_imu.yacc,current_messages.highres_imu.zacc);
						fprintf(fd,"gyro: %f %f	%f ",current_messages.highres_imu.xgyro,current_messages.highres_imu.ygyro,current_messages.highres_imu.zgyro);
						fprintf(fd,"mag: %f %f %f ",current_messages.highres_imu.xmag,current_messages.highres_imu.ymag,current_messages.highres_imu.zmag);
						fprintf(fd,"alt: %f %f\n",current_messages.highres_imu.pressure_alt,current_messages.highres_imu.temperature);
						receive_flag = false;
					}
					current_messages.time_stamps.highres_imu = get_time_usec();
					this_timestamps.highres_imu = current_messages.time_stamps.highres_imu;
					break;
				}
				case MAVLINK_MSG_ID_GPS_RAW_INT:
				{
					mavlink_msg_gps_raw_int_decode(&message, &(current_messages.gps_raw_int));
					fprintf(fd, "lat = %d	lon = %d	 alt = %d		", current_messages.gps_raw_int.lat,current_messages.gps_raw_int.lon,current_messages.gps_raw_int.alt);
					fprintf(fd, "vel = %d	cog = %d	 satellites_visible = %d\n", current_messages.gps_raw_int.vel,current_messages.gps_raw_int.cog,current_messages.gps_raw_int.satellites_visible);
					if (receive_flag && receive_ch == 10) {
						fprintf(fd, "lat = %d	lon = %d	 alt = %d		", current_messages.gps_raw_int.lat,current_messages.gps_raw_int.lon,current_messages.gps_raw_int.alt);
						fprintf(fd, "vel = %d	cog = %d	 satellites_visible = %d\n", current_messages.gps_raw_int.vel,current_messages.gps_raw_int.cog,current_messages.gps_raw_int.satellites_visible);
						receive_flag = false;
					}
					break;
				}

				case MAVLINK_MSG_ID_ATTITUDE:
				{
					//printf("MAVLINK_MSG_ID_ATTITUDE\n");
					mavlink_msg_attitude_decode(&message, &(current_messages.attitude));
					current_messages.time_stamps.attitude = get_time_usec();
					this_timestamps.attitude = current_messages.time_stamps.attitude;
					break;
				}

				default:
				{
					// printf("Warning, did not handle message id %i\n",message.msgid);
					break;
				}


			} // end: switch msgid

		} // end: if read message

		// Check for receipt of all items
		received_all =
				this_timestamps.heartbeat                  &&
//				this_timestamps.battery_status             &&
//				this_timestamps.radio_status               &&
//				this_timestamps.local_position_ned         &&
//				this_timestamps.global_position_int        &&
//				this_timestamps.position_target_local_ned  &&
//				this_timestamps.position_target_global_int &&
//  				this_timestamps.highres_imu                &&
//				this_timestamps.attitude                   &&
				this_timestamps.sys_status
				;

		// give the write thread time to use the port
		if ( writing_status > false ) {
			usleep(100); // look for components of batches at 10kHz
		}

	} // end: while not received all

	return;
}
void Pixhawk_Interface::
read_msg()
{
	bool success;
	//printf("come first\n");
	mavlink_message_t msg;
	success = serial_port->read_message(msg);
	//printf("come here\n");
	if (success)
	{

		switch (msg.msgid)
		{
			case MAVLINK_MSG_ID_HEARTBEAT:
			{
				if (msg.sysid == 1)
				{
					static int count = 0;

					//send back an heartbeat
					//mavlink_message_t msg_heartbeat;
					//send_heartbeat(msg_heartbeat);

					//print out
					printf("<3 beat %d, sys_ID: %d\n", count, msg.sysid);
					count++;
				}
				break;
			}

			case MAVLINK_MSG_ID_SYS_STATUS:
			{
				mavlink_sys_status_t sys_status;
				mavlink_msg_sys_status_decode(&msg, &sys_status);

				//print

				break;
			}

			case COMMAND_ACK:	//COMMAND_ACK #77
			{
				mavlink_command_ack_t ack;
				mavlink_msg_command_ack_decode(&msg, &ack);

				//print out
				printf("Received command: %d, result: %d\n", ack.command, ack.result);

				break;
			}

			case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
			{
				//dont need to do anything
				break;
			}

			case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
			{
				mavlink_rc_channels_raw_t rc_raw;
				mavlink_msg_rc_channels_raw_decode(&msg, &rc_raw);

				printf("rc1 %d, rc2 %d, rc3 %d, rc4 %d\n", rc_raw.chan1_raw,rc_raw.chan2_raw,rc_raw.chan3_raw,rc_raw.chan4_raw);

				break;
			}

			case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:	//#36 from data stream RC_CHANNELS
			{

				break;
			}

			default:
			{
				/*
				 * 125: POWER_STATUS
				 * 253: STATUTEXT
				 */

				//print
				//printf("Other messages - msg ID: %d\n", msg.msgid);
				break;
			}
		}

	}

}// end read_msg
Пример #6
0
// ------------------------------------------------------------------------------
//   Read Messages
// ------------------------------------------------------------------------------
void
Autopilot_Interface::
read_messages()
{
	bool success;               // receive success flag
	bool received_all = false;  // receive only one message
	Time_Stamps this_timestamps;

	// Blocking wait for new data
	while ( not received_all and not time_to_exit )
	{
		// ----------------------------------------------------------------------
		//   READ MESSAGE
		// ----------------------------------------------------------------------
		mavlink_message_t message;
		success = serial_port->read_message(message);

		// ----------------------------------------------------------------------
		//   HANDLE MESSAGE
		// ----------------------------------------------------------------------
		if( success )
		{

			// Store message sysid and compid.
			// Note this doesn't handle multiple message sources.
			current_messages.sysid  = message.sysid;
			current_messages.compid = message.compid;

			// Handle Message ID
			switch (message.msgid)
			{

				case MAVLINK_MSG_ID_HEARTBEAT:
				{
					//printf("MAVLINK_MSG_ID_HEARTBEAT\n");
					mavlink_msg_heartbeat_decode(&message, &(current_messages.heartbeat));
					current_messages.time_stamps.heartbeat = get_time_usec();
					this_timestamps.heartbeat = current_messages.time_stamps.heartbeat;
					break;
				}

				case MAVLINK_MSG_ID_SYS_STATUS:
				{
					//printf("MAVLINK_MSG_ID_SYS_STATUS\n");
					mavlink_msg_sys_status_decode(&message, &(current_messages.sys_status));
					current_messages.time_stamps.sys_status = get_time_usec();
					this_timestamps.sys_status = current_messages.time_stamps.sys_status;
					break;
				}

				case MAVLINK_MSG_ID_BATTERY_STATUS:
				{
					//printf("MAVLINK_MSG_ID_BATTERY_STATUS\n");
					mavlink_msg_battery_status_decode(&message, &(current_messages.battery_status));
					current_messages.time_stamps.battery_status = get_time_usec();
					this_timestamps.battery_status = current_messages.time_stamps.battery_status;
					break;
				}

				case MAVLINK_MSG_ID_RADIO_STATUS:
				{
					//printf("MAVLINK_MSG_ID_RADIO_STATUS\n");
					mavlink_msg_radio_status_decode(&message, &(current_messages.radio_status));
					current_messages.time_stamps.radio_status = get_time_usec();
					this_timestamps.radio_status = current_messages.time_stamps.radio_status;
					break;
				}

				case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
				{
					//printf("MAVLINK_MSG_ID_LOCAL_POSITION_NED\n");
					mavlink_msg_local_position_ned_decode(&message, &(current_messages.local_position_ned));
					current_messages.time_stamps.local_position_ned = get_time_usec();
					this_timestamps.local_position_ned = current_messages.time_stamps.local_position_ned;
					break;
				}

				case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
				{
					//printf("MAVLINK_MSG_ID_GLOBAL_POSITION_INT\n");
					mavlink_msg_global_position_int_decode(&message, &(current_messages.global_position_int));
					current_messages.time_stamps.global_position_int = get_time_usec();
					this_timestamps.global_position_int = current_messages.time_stamps.global_position_int;
					break;
				}

				case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
				{
					//printf("MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED\n");
					mavlink_msg_position_target_local_ned_decode(&message, &(current_messages.position_target_local_ned));
					current_messages.time_stamps.position_target_local_ned = get_time_usec();
					this_timestamps.position_target_local_ned = current_messages.time_stamps.position_target_local_ned;
					break;
				}

				case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT:
				{
					//printf("MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT\n");
					mavlink_msg_position_target_global_int_decode(&message, &(current_messages.position_target_global_int));
					current_messages.time_stamps.position_target_global_int = get_time_usec();
					this_timestamps.position_target_global_int = current_messages.time_stamps.position_target_global_int;
					break;
				}

				case MAVLINK_MSG_ID_HIGHRES_IMU:
				{
					//printf("MAVLINK_MSG_ID_HIGHRES_IMU\n");
					mavlink_msg_highres_imu_decode(&message, &(current_messages.highres_imu));
					current_messages.time_stamps.highres_imu = get_time_usec();
					this_timestamps.highres_imu = current_messages.time_stamps.highres_imu;
					break;
				}

				case MAVLINK_MSG_ID_ATTITUDE:
				{
					//printf("MAVLINK_MSG_ID_ATTITUDE\n");
					mavlink_msg_attitude_decode(&message, &(current_messages.attitude));
					current_messages.time_stamps.attitude = get_time_usec();
					this_timestamps.attitude = current_messages.time_stamps.attitude;
					break;
				}

				default:
				{
					// printf("Warning, did not handle message id %i\n",message.msgid);
					break;
				}


			} // end: switch msgid

		} // end: if read message

		// Check for receipt of all items
		received_all =
				this_timestamps.heartbeat                  &&
				this_timestamps.sys_status                 &&
//				this_timestamps.battery_status             &&
//				this_timestamps.radio_status               &&
				this_timestamps.local_position_ned         &&
//				this_timestamps.global_position_int        &&
//				this_timestamps.position_target_local_ned  &&
				this_timestamps.position_target_global_int &&
				this_timestamps.highres_imu                &&
				this_timestamps.attitude                   ;

		// give the write thread time to use the port
		if ( writing_status > false )
			usleep(100); // look for components of batches at 10kHz

	} // end: while not received all

	return;
}
Пример #7
0
void _Mavlink::handleMessages()
{
	mavlink_message_t message;
	int nMsgHandled = 0;

	//Handle Message while new message is received
	while (readMessage(message))
	{
		// Note this doesn't handle multiple message sources.
		m_msg.sysid = message.sysid;
		m_msg.compid = message.compid;

		// Handle Message ID
		switch (message.msgid)
		{

		case MAVLINK_MSG_ID_HEARTBEAT:
		{
			LOG(INFO)<<"-> MAVLINK_MSG_ID_HEARTBEAT";
			mavlink_msg_heartbeat_decode(&message, &(m_msg.heartbeat));
			m_msg.time_stamps.heartbeat = get_time_usec();

			if (m_msg.heartbeat.type != MAV_TYPE_GCS)
			{
				m_systemID = m_msg.sysid;
				m_targetComponentID = m_msg.compid;

				LOG_I("-> SYSTEM_ID:"<<m_systemID
				<<" COMPONENT_ID:"<<m_componentID
				<<" TARGET_COMPONENT_ID:"<<m_targetComponentID);
			}
			else
			{
				LOG_I("->HEARTBEAT FROM MAV_TYPE_GCS");
			}
			break;
		}

		case MAVLINK_MSG_ID_SYS_STATUS:
		{
			LOG_I("-> MAVLINK_MSG_ID_SYS_STATUS");
			mavlink_msg_sys_status_decode(&message,
					&(m_msg.sys_status));
			m_msg.time_stamps.sys_status = get_time_usec();
			break;
		}

		case MAVLINK_MSG_ID_BATTERY_STATUS:
		{
			LOG_I("-> MAVLINK_MSG_ID_BATTERY_STATUS");
			mavlink_msg_battery_status_decode(&message,
					&(m_msg.battery_status));
			m_msg.time_stamps.battery_status = get_time_usec();
			break;
		}

		case MAVLINK_MSG_ID_RADIO_STATUS:
		{
			LOG_I("-> MAVLINK_MSG_ID_RADIO_STATUS");
			mavlink_msg_radio_status_decode(&message,
					&(m_msg.radio_status));
			m_msg.time_stamps.radio_status = get_time_usec();
			break;
		}

		case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
		{
			LOG_I("-> MAVLINK_MSG_ID_LOCAL_POSITION_NED");
			mavlink_msg_local_position_ned_decode(&message,
					&(m_msg.local_position_ned));
			m_msg.time_stamps.local_position_ned = get_time_usec();
			break;
		}

		case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
		{
			LOG_I("-> MAVLINK_MSG_ID_GLOBAL_POSITION_INT");
			mavlink_msg_global_position_int_decode(&message,
					&(m_msg.global_position_int));
			m_msg.time_stamps.global_position_int = get_time_usec();
			break;
		}

		case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
		{
			LOG_I("-> MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED");
			mavlink_msg_position_target_local_ned_decode(&message,
					&(m_msg.position_target_local_ned));
			m_msg.time_stamps.position_target_local_ned =
			get_time_usec();
			break;
		}

		case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT:
		{
			LOG_I("-> MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT");
			mavlink_msg_position_target_global_int_decode(&message,
					&(m_msg.position_target_global_int));
			m_msg.time_stamps.position_target_global_int =
			get_time_usec();
			break;
		}

		case MAVLINK_MSG_ID_HIGHRES_IMU:
		{
			LOG_I("-> MAVLINK_MSG_ID_HIGHRES_IMU");
			mavlink_msg_highres_imu_decode(&message,
					&(m_msg.highres_imu));
			m_msg.time_stamps.highres_imu = get_time_usec();
			break;
		}

		case MAVLINK_MSG_ID_ATTITUDE:
		{
			LOG_I("-> MAVLINK_MSG_ID_ATTITUDE");
			mavlink_msg_attitude_decode(&message, &(m_msg.attitude));
			m_msg.time_stamps.attitude = get_time_usec();
			break;
		}

		case MAVLINK_MSG_ID_COMMAND_ACK:
		{
			mavlink_msg_command_ack_decode(&message,
					&(m_msg.command_ack));
			m_msg.time_stamps.attitude = get_time_usec();

			LOG_I("-> MAVLINK_MSG_ID_COMMAND_ACK:"<<m_msg.command_ack.result);
			break;
		}

		default:
		{
			LOG_I("-> UNKNOWN MSG_ID:"<<message.msgid);
			break;
		}

	}

		if (++nMsgHandled >= NUM_MSG_HANDLE)
			return;
	}

}
Пример #8
0
// ------------------------------------------------------------------------------
//   Read Messages
// ------------------------------------------------------------------------------
void
Autopilot_Interface::
read_messages()
{
  int success = 0;               // receive success flag
  bool received_all = false;  // receive only one message
  Time_Stamps this_timestamps;

  int err_counter = 0;

  struct timespec diff_r;

  struct timespec aft_read;
  struct timespec bef_read;

  struct timespec receive_time;

  struct timespec longRelNanoSleep;
  longRelNanoSleep.tv_sec = 0;
  longRelNanoSleep.tv_nsec = 100000;
  
  struct timespec RelNanoSleep;
  RelNanoSleep.tv_sec = 0;
  RelNanoSleep.tv_nsec = 10;

  // Blocking wait for new data
  // pthread_mutex_lock(&mut);
  // if (blocked == 0 && writing_status)
  //  {
  //    blocked = 1;
  //    pthread_cond_wait(&cond,&mut);
  //    blocked = 0;
  //  }
  //  else
  //  {
  //    pthread_cond_signal(&cond);
  //  }
  //  clock_gettime(CLOCK_REALTIME,&bef_read);
  //  clock_gettime(CLOCK_REALTIME,&aft_read);
  //  timespec_sub(&diff_r,&aft_read,&bef_read);
  //  int64_t diff_r_us = diff_r.tv_sec*1e6 + diff_r.tv_nsec/1e3;
  //  printf("Read Time: %ld \n",diff_r_us);
  //
  //  pthread_mutex_unlock(&mut);
  
  mavlink_message_t message;
  
  while ( (success != 1) && (!time_to_exit) )
    {
      // ----------------------------------------------------------------------
      //   READ ONE BYTE AT TIME UNTIL COMPLETE MESSAGE IS RECEIVED
      // ----------------------------------------------------------------------
      success = serial_port->read_message(message);  //Have I obtained a complete message?
      if ( success == -1) // Errors...retry...
	{
	  err_counter++;
	  if (err_counter == 3) // To many void read
	    {
	      fprintf(stderr,"ERRORS!\n");
	      break;
	    }
	}
    }
  err_counter = 0;
  
  if (success == -1) return;
	
  // ----------------------------------------------------------------------
  //   HANDLE MESSAGE
  // ----------------------------------------------------------------------
  //if( success )
  
	  
  // Store message sysid and compid.
  // Note this doesn't handle multiple message sources.
  current_messages.sysid  = message.sysid;
  current_messages.compid = message.compid;
  
  // Handle Message ID
  switch (message.msgid)
    {

    case MAVLINK_MSG_ID_HEARTBEAT:
      {
	//printf("MAVLINK_MSG_ID_HEARTBEAT\n");
	mavlink_msg_heartbeat_decode(&message, &(current_messages.heartbeat));
	base_mode = current_messages.heartbeat.base_mode;
	mav_type = current_messages.heartbeat.type;
	system_status = current_messages.heartbeat.system_status;
	custom_mode = current_messages.heartbeat.custom_mode;
	//printf("base_mode = %u\n",base_mode);
	current_messages.time_stamps.heartbeat = get_time_usec();
	this_timestamps.heartbeat = current_messages.time_stamps.heartbeat;
	heartbeat_count++;
	//printf("IMU Sensors Timestamp %u\n",current_messages.time_stamps.highres_imu/1000);
	if ( (current_messages.time_stamps.heartbeat - read_heartbeat_old) > 10000000 )
	  {
	    printf("HEARTBEAT frequency :   %d \n",heartbeat_count/10);
	    heartbeat_count = 0;
	    read_heartbeat_old = current_messages.time_stamps.heartbeat;
	  }
	break;
      }

    case MAVLINK_MSG_ID_SYS_STATUS:
      {
	//printf("MAVLINK_MSG_ID_SYS_STATUS\n");
	mavlink_msg_sys_status_decode(&message, &(current_messages.sys_status));
	current_messages.time_stamps.sys_status = get_time_usec();
	this_timestamps.sys_status = current_messages.time_stamps.sys_status;
	break;
      }

    case MAVLINK_MSG_ID_BATTERY_STATUS:
      {
	//printf("MAVLINK_MSG_ID_BATTERY_STATUS\n");
	mavlink_msg_battery_status_decode(&message, &(current_messages.battery_status));
	current_messages.time_stamps.battery_status = get_time_usec();
	this_timestamps.battery_status = current_messages.time_stamps.battery_status;
	break;
      }

    case MAVLINK_MSG_ID_RADIO_STATUS:
      {
	//printf("MAVLINK_MSG_ID_RADIO_STATUS\n");
	mavlink_msg_radio_status_decode(&message, &(current_messages.radio_status));
	current_messages.time_stamps.radio_status = get_time_usec();
	this_timestamps.radio_status = current_messages.time_stamps.radio_status;
	break;
      }

    case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
      {
	//printf("MAVLINK_MSG_ID_LOCAL_POSITION_NED\n");
	mavlink_msg_local_position_ned_decode(&message, &(current_messages.local_position_ned));
	current_messages.time_stamps.local_position_ned = get_time_usec();
	this_timestamps.local_position_ned = current_messages.time_stamps.local_position_ned;
	break;
      }

    case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
      {
	//printf("MAVLINK_MSG_ID_GLOBAL_POSITION_INT\n");
	mavlink_msg_global_position_int_decode(&message, &(current_messages.global_position_int));
	current_messages.time_stamps.global_position_int = get_time_usec();
	this_timestamps.global_position_int = current_messages.time_stamps.global_position_int;
	break;
      }

    case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
      {
	//printf("MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED\n");
	mavlink_msg_position_target_local_ned_decode(&message, &(current_messages.position_target_local_ned));
	current_messages.time_stamps.position_target_local_ned = get_time_usec();
	this_timestamps.position_target_local_ned = current_messages.time_stamps.position_target_local_ned;
	break;
      }

    case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT:
      {
	//printf("MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT\n");
	mavlink_msg_position_target_global_int_decode(&message, &(current_messages.position_target_global_int));
	current_messages.time_stamps.position_target_global_int = get_time_usec();
	this_timestamps.position_target_global_int = current_messages.time_stamps.position_target_global_int;
	break;
      }

    case MAVLINK_MSG_ID_HIGHRES_IMU:
      {
	//printf("MAVLINK_MSG_ID_HIGHRES_IMU\n");
	clock_gettime(CLOCK_REALTIME,&receive_time);
	mavlink_msg_highres_imu_decode(&message, &(current_messages.highres_imu));
	current_messages.time_stamps.highres_imu = get_time_usec();
	this_timestamps.highres_imu = current_messages.time_stamps.highres_imu;
	highres_imu_count++;
	//printf("IMU Sensors Timestamp %u\n",current_messages.time_stamps.highres_imu/1000);
	if ( (current_messages.time_stamps.highres_imu - read_highres_imu_old) > 10000000 )
	  {
	    printf("HIGHRES_IMU frequency :   %d Hz\n",highres_imu_count/10);
	    highres_imu_count = 0;
	    read_highres_imu_old = current_messages.time_stamps.highres_imu;
	  }
	//fprintf(logfile_rd,"HIGHRES_IMU %d, %ld \n",receive_time.tv_sec, receive_time.tv_nsec);
	break;
      }

    case MAVLINK_MSG_ID_ATTITUDE:
      {
	//printf("MAVLINK_MSG_ID_ATTITUDE\n");
	mavlink_msg_attitude_decode(&message, &(current_messages.attitude));
	current_messages.time_stamps.attitude = get_time_usec();
	this_timestamps.attitude = current_messages.time_stamps.attitude;
	break;
      }

    case MAVLINK_MSG_ID_HIL_CONTROLS:
      {
	//printf("MAVLINK_MSG_ID_HIL_CONTROLS\n");
	mavlink_msg_hil_controls_decode(&message, &(current_messages.hil_controls));
	current_messages.time_stamps.hil_controls = get_time_usec();
	this_timestamps.hil_controls = current_messages.time_stamps.hil_controls;
	float data[4] = {current_messages.hil_controls.roll_ailerons,
			 current_messages.hil_controls.pitch_elevator,
			 current_messages.hil_controls.yaw_rudder,
			 current_messages.hil_controls.throttle};
	int bsent = udp_mtl->send_bytes((char *)data,sizeof(float[4]));
	hil_controls_count++;
	//printf("IMU Sensors Timestamp %u\n",current_messages.time_stamps.highres_imu/1000);
	if ( (current_messages.time_stamps.hil_controls - read_hil_controls_old) > 10000000 )
	  {
	    printf("HIL_CONTROLS frequency :   %d Hz\n",hil_controls_count/10);
	    hil_controls_count = 0;
	    read_hil_controls_old = current_messages.time_stamps.hil_controls;
	    printf("%0.2f | ",current_messages.hil_controls.roll_ailerons); 
	    printf("%0.2f | ",current_messages.hil_controls.pitch_elevator);
	    printf("%0.2f | ",current_messages.hil_controls.yaw_rudder);
	    printf("%0.2f | \n",current_messages.hil_controls.throttle);
	  }
	//fprintf(logfile_rd,"HIL_CONTROLS %d, %ld \n",receive_time.tv_sec, receive_time.tv_nsec);
	break;
      }

    default:
      {
	// printf("Warning, did not handle message id %i\n",message.msgid);
	break;
      }


    } // end: switch msgid
  
return;
}
Пример #9
0
//1
void MavSerialPort::sys_status_handler(){
  //  qDebug() << "MAVLINK_MSG_ID_SYS_STATUS\n";
    mavlink_msg_sys_status_decode(&message, &sys_status);

    emit batteryChanged();
}
Пример #10
0
//1
void MavSerialPort::sys_status_handler(){
    mavlink_msg_sys_status_decode(&message, &sys_status);
    emit batteryChanged(sys_status.voltage_battery);
}
// ------------------------------------------------------------------------------
//   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;
    }
    }
}
Пример #13
0
void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
{
    switch (msg->msgid) {

    case MAVLINK_MSG_ID_HEARTBEAT: {    // MAV ID: 0
        // We keep track of the last time we received a heartbeat from our GCS for failsafe purposes
        if (msg->sysid != sub.g.sysid_my_gcs) {
            break;
        }
        sub.failsafe.last_heartbeat_ms = AP_HAL::millis();
        break;
    }

    case MAVLINK_MSG_ID_MANUAL_CONTROL: {     // MAV ID: 69
        if (msg->sysid != sub.g.sysid_my_gcs) {
            break;    // Only accept control from our gcs
        }
        mavlink_manual_control_t packet;
        mavlink_msg_manual_control_decode(msg, &packet);

        if (packet.target != sub.g.sysid_this_mav) {
            break; // only accept control aimed at us
        }

        sub.transform_manual_control_to_rc_override(packet.x,packet.y,packet.z,packet.r,packet.buttons);

        sub.failsafe.last_pilot_input_ms = AP_HAL::millis();
        // a RC override message is considered to be a 'heartbeat' from the ground station for failsafe purposes
        sub.failsafe.last_heartbeat_ms = AP_HAL::millis();
        break;
    }

    case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: {     // MAV ID: 70
        // allow override of RC input
        if (msg->sysid != sub.g.sysid_my_gcs) {
            break;    // Only accept control from our gcs
        }

        uint32_t tnow = AP_HAL::millis();

        mavlink_rc_channels_override_t packet;
        mavlink_msg_rc_channels_override_decode(msg, &packet);

        RC_Channels::set_override(0, packet.chan1_raw, tnow);
        RC_Channels::set_override(1, packet.chan2_raw, tnow);
        RC_Channels::set_override(2, packet.chan3_raw, tnow);
        RC_Channels::set_override(3, packet.chan4_raw, tnow);
        RC_Channels::set_override(4, packet.chan5_raw, tnow);
        RC_Channels::set_override(5, packet.chan6_raw, tnow);
        RC_Channels::set_override(6, packet.chan7_raw, tnow);
        RC_Channels::set_override(7, packet.chan8_raw, tnow);

        sub.failsafe.last_pilot_input_ms = tnow;
        // a RC override message is considered to be a 'heartbeat' from the ground station for failsafe purposes
        sub.failsafe.last_heartbeat_ms = tnow;
        break;
    }

    case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: { // MAV ID: 82
        // decode packet
        mavlink_set_attitude_target_t packet;
        mavlink_msg_set_attitude_target_decode(msg, &packet);

        // ensure type_mask specifies to use attitude
        // the thrust can be used from the altitude hold
        if (packet.type_mask & (1<<6)) {
            sub.set_attitude_target_no_gps = {AP_HAL::millis(), packet};
        }

        // ensure type_mask specifies to use attitude and thrust
        if ((packet.type_mask & ((1<<7)|(1<<6))) != 0) {
            break;
        }

        // convert thrust to climb rate
        packet.thrust = constrain_float(packet.thrust, 0.0f, 1.0f);
        float climb_rate_cms = 0.0f;
        if (is_equal(packet.thrust, 0.5f)) {
            climb_rate_cms = 0.0f;
        } else if (packet.thrust > 0.5f) {
            // climb at up to WPNAV_SPEED_UP
            climb_rate_cms = (packet.thrust - 0.5f) * 2.0f * sub.wp_nav.get_default_speed_up();
        } else {
            // descend at up to WPNAV_SPEED_DN
            climb_rate_cms = (packet.thrust - 0.5f) * 2.0f * fabsf(sub.wp_nav.get_default_speed_down());
        }
        sub.guided_set_angle(Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]), climb_rate_cms);
        break;
    }

    case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: {   // MAV ID: 84
        // decode packet
        mavlink_set_position_target_local_ned_t packet;
        mavlink_msg_set_position_target_local_ned_decode(msg, &packet);

        // exit if vehicle is not in Guided mode or Auto-Guided mode
        if ((sub.control_mode != GUIDED) && !(sub.control_mode == AUTO && sub.auto_mode == Auto_NavGuided)) {
            break;
        }

        // check for supported coordinate frames
        if (packet.coordinate_frame != MAV_FRAME_LOCAL_NED &&
                packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED &&
                packet.coordinate_frame != MAV_FRAME_BODY_NED &&
                packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED) {
            break;
        }

        bool pos_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
        bool vel_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
        bool acc_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;

        /*
         * for future use:
         * bool force           = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_FORCE;
         * bool yaw_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
         * bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
         */

        // prepare position
        Vector3f pos_vector;
        if (!pos_ignore) {
            // convert to cm
            pos_vector = Vector3f(packet.x * 100.0f, packet.y * 100.0f, -packet.z * 100.0f);
            // rotate to body-frame if necessary
            if (packet.coordinate_frame == MAV_FRAME_BODY_NED ||
                    packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
                sub.rotate_body_frame_to_NE(pos_vector.x, pos_vector.y);
            }
            // add body offset if necessary
            if (packet.coordinate_frame == MAV_FRAME_LOCAL_OFFSET_NED ||
                    packet.coordinate_frame == MAV_FRAME_BODY_NED ||
                    packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
                pos_vector += sub.inertial_nav.get_position();
            } else {
                // convert from alt-above-home to alt-above-ekf-origin
                pos_vector.z = sub.pv_alt_above_origin(pos_vector.z);
            }
        }

        // prepare velocity
        Vector3f vel_vector;
        if (!vel_ignore) {
            // convert to cm
            vel_vector = Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f);
            // rotate to body-frame if necessary
            if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
                sub.rotate_body_frame_to_NE(vel_vector.x, vel_vector.y);
            }
        }

        // send request
        if (!pos_ignore && !vel_ignore && acc_ignore) {
            sub.guided_set_destination_posvel(pos_vector, vel_vector);
        } else if (pos_ignore && !vel_ignore && acc_ignore) {
            sub.guided_set_velocity(vel_vector);
        } else if (!pos_ignore && vel_ignore && acc_ignore) {
            sub.guided_set_destination(pos_vector);
        }

        break;
    }

    case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT: {  // MAV ID: 86
        // decode packet
        mavlink_set_position_target_global_int_t packet;
        mavlink_msg_set_position_target_global_int_decode(msg, &packet);

        // exit if vehicle is not in Guided mode or Auto-Guided mode
        if ((sub.control_mode != GUIDED) && !(sub.control_mode == AUTO && sub.auto_mode == Auto_NavGuided)) {
            break;
        }

        // check for supported coordinate frames
        if (packet.coordinate_frame != MAV_FRAME_GLOBAL &&
                packet.coordinate_frame != MAV_FRAME_GLOBAL_INT &&
                packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT && // solo shot manager incorrectly sends RELATIVE_ALT instead of RELATIVE_ALT_INT
                packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT &&
                packet.coordinate_frame != MAV_FRAME_GLOBAL_TERRAIN_ALT &&
                packet.coordinate_frame != MAV_FRAME_GLOBAL_TERRAIN_ALT_INT) {
            break;
        }

        bool pos_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
        bool vel_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
        bool acc_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;

        /*
         * for future use:
         * bool force           = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_FORCE;
         * bool yaw_ignore      = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
         * bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
         */

        Vector3f pos_neu_cm;  // position (North, East, Up coordinates) in centimeters

        if (!pos_ignore) {
            // sanity check location
            if (!check_latlng(packet.lat_int, packet.lon_int)) {
                break;
            }
            Location loc;
            loc.lat = packet.lat_int;
            loc.lng = packet.lon_int;
            loc.alt = packet.alt*100;
            switch (packet.coordinate_frame) {
            case MAV_FRAME_GLOBAL_RELATIVE_ALT: // solo shot manager incorrectly sends RELATIVE_ALT instead of RELATIVE_ALT_INT
            case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT:
                loc.relative_alt = true;
                loc.terrain_alt = false;
                break;
            case MAV_FRAME_GLOBAL_TERRAIN_ALT:
            case MAV_FRAME_GLOBAL_TERRAIN_ALT_INT:
                loc.relative_alt = true;
                loc.terrain_alt = true;
                break;
            case MAV_FRAME_GLOBAL:
            case MAV_FRAME_GLOBAL_INT:
            default:
                loc.relative_alt = false;
                loc.terrain_alt = false;
                break;
            }
            pos_neu_cm = sub.pv_location_to_vector(loc);
        }

        if (!pos_ignore && !vel_ignore && acc_ignore) {
            sub.guided_set_destination_posvel(pos_neu_cm, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f));
        } else if (pos_ignore && !vel_ignore && acc_ignore) {
            sub.guided_set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f));
        } else if (!pos_ignore && vel_ignore && acc_ignore) {
            sub.guided_set_destination(pos_neu_cm);
        }

        break;
    }

    case MAVLINK_MSG_ID_DISTANCE_SENSOR: {
        sub.rangefinder.handle_msg(msg);
        break;
    }

#if AC_FENCE == ENABLED
        // send or receive fence points with GCS
    case MAVLINK_MSG_ID_FENCE_POINT:            // MAV ID: 160
    case MAVLINK_MSG_ID_FENCE_FETCH_POINT:
        sub.fence.handle_msg(*this, msg);
        break;
#endif // AC_FENCE == ENABLED

    case MAVLINK_MSG_ID_TERRAIN_DATA:
    case MAVLINK_MSG_ID_TERRAIN_CHECK:
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
        sub.terrain.handle_data(chan, msg);
#endif
        break;

    case MAVLINK_MSG_ID_SET_HOME_POSITION: {
        mavlink_set_home_position_t packet;
        mavlink_msg_set_home_position_decode(msg, &packet);
        if ((packet.latitude == 0) && (packet.longitude == 0) && (packet.altitude == 0)) {
            sub.set_home_to_current_location(true);
        } else {
            // sanity check location
            if (!check_latlng(packet.latitude, packet.longitude)) {
                break;
            }
            Location new_home_loc;
            new_home_loc.lat = packet.latitude;
            new_home_loc.lng = packet.longitude;
            new_home_loc.alt = packet.altitude / 10;
            if (sub.far_from_EKF_origin(new_home_loc)) {
                break;
            }
            sub.set_home(new_home_loc, true);
        }
        break;
    }

    // This adds support for leak detectors in a separate enclosure
    // connected to a mavlink enabled subsystem
    case MAVLINK_MSG_ID_SYS_STATUS: {
        uint32_t MAV_SENSOR_WATER = 0x20000000;
        mavlink_sys_status_t packet;
        mavlink_msg_sys_status_decode(msg, &packet);
        if ((packet.onboard_control_sensors_enabled & MAV_SENSOR_WATER) && !(packet.onboard_control_sensors_health & MAV_SENSOR_WATER)) {
            sub.leak_detector.set_detect();
        }
    }
        break;

    default:
        handle_common_message(msg);
        break;
    }     // end switch
} // end handle mavlink
Пример #14
0
// ------------------------------------------------------------------------------
//   Read Messages
// ------------------------------------------------------------------------------
void Autopilot_Interface::read_messages() {
  bool success;               // receive success flag
  bool received_all = false;  // receive only one message
  Time_Stamps this_timestamps;

  // Blocking wait for new data
  while (!received_all and !time_to_exit) {
    // ----------------------------------------------------------------------
    //   READ MESSAGE
    // ----------------------------------------------------------------------
    mavlink_message_t message;
    success = serial_port->read_message(message);

    // ----------------------------------------------------------------------
    //   HANDLE MESSAGE
    // ----------------------------------------------------------------------
    if (success) {
      // Store message sysid and compid.
      // Note this doesn't handle multiple message sources.
      current_messages.sysid = message.sysid;
      current_messages.compid = message.compid;

      // Handle Message ID
      switch (message.msgid) {
        case MAVLINK_MSG_ID_HEARTBEAT: {
          printf("MAVLINK_MSG_ID_HEARTBEAT\n");
          mavlink_msg_heartbeat_decode(&message, &(current_messages.heartbeat));
          current_messages.time_stamps.heartbeat = get_time_usec();
          this_timestamps.heartbeat = current_messages.time_stamps.heartbeat;
          break;
        }

        case MAVLINK_MSG_ID_SYS_STATUS: {
          printf("MAVLINK_MSG_ID_SYS_STATUS\n");
          mavlink_msg_sys_status_decode(&message,
                                        &(current_messages.sys_status));
          current_messages.time_stamps.sys_status = get_time_usec();
          this_timestamps.sys_status = current_messages.time_stamps.sys_status;

          printf("Battery life remaining: %i\n",
                 current_messages.sys_status.battery_remaining);
          break;
        }

        // this is not being received
        case MAVLINK_MSG_ID_BATTERY_STATUS: {
          printf("MAVLINK_MSG_ID_BATTERY_STATUS\n");
          mavlink_msg_battery_status_decode(&message,
                                            &(current_messages.battery_status));
          current_messages.time_stamps.battery_status = get_time_usec();
          this_timestamps.battery_status =
              current_messages.time_stamps.battery_status;

          printf("Battery life remaining: %i\n",
                 current_messages.battery_status.battery_remaining);
          break;
        }

        case MAVLINK_MSG_ID_RADIO_STATUS: {
          printf("MAVLINK_MSG_ID_RADIO_STATUS\n");
          mavlink_msg_radio_status_decode(&message,
                                          &(current_messages.radio_status));
          current_messages.time_stamps.radio_status = get_time_usec();
          this_timestamps.radio_status =
              current_messages.time_stamps.radio_status;
          break;
        }

        case MAVLINK_MSG_ID_LOCAL_POSITION_NED: {
          printf("MAVLINK_MSG_ID_LOCAL_POSITION_NED\n");
          mavlink_msg_local_position_ned_decode(
              &message, &(current_messages.local_position_ned));
          printf("    pos  (GPS):  %f %f %f (m)\n",
                 current_messages.local_position_ned.x,
                 current_messages.local_position_ned.y,
                 current_messages.local_position_ned.z);
          current_messages.time_stamps.local_position_ned = get_time_usec();
          this_timestamps.local_position_ned =
              current_messages.time_stamps.local_position_ned;
          break;
        }

        case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
          printf("MAVLINK_MSG_ID_GLOBAL_POSITION_INT\n");
          mavlink_msg_global_position_int_decode(
              &message, &(current_messages.global_position_int));
          current_messages.time_stamps.global_position_int = get_time_usec();
          this_timestamps.global_position_int =
              current_messages.time_stamps.global_position_int;

          mavlink_global_position_int_t gpos =
              current_messages.global_position_int;
          printf("    pos  (GPS):  %f %f %f (m)\n", gpos.lat, gpos.lon,
                 gpos.alt);

          break;
        }

        case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED: {
          printf("MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED\n");
          mavlink_msg_position_target_local_ned_decode(
              &message, &(current_messages.position_target_local_ned));
          current_messages.time_stamps.position_target_local_ned =
              get_time_usec();
          this_timestamps.position_target_local_ned =
              current_messages.time_stamps.position_target_local_ned;
          break;
        }

        case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT: {
          printf("MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT\n");
          mavlink_msg_position_target_global_int_decode(
              &message, &(current_messages.position_target_global_int));
          current_messages.time_stamps.position_target_global_int =
              get_time_usec();
          this_timestamps.position_target_global_int =
              current_messages.time_stamps.position_target_global_int;
          break;
        }

        case MAVLINK_MSG_ID_HIGHRES_IMU: {
          printf("MAVLINK_MSG_ID_HIGHRES_IMU\n");
          mavlink_msg_highres_imu_decode(&message,
                                         &(current_messages.highres_imu));
          current_messages.time_stamps.highres_imu = get_time_usec();
          this_timestamps.highres_imu =
              current_messages.time_stamps.highres_imu;

          mavlink_highres_imu_t imu = current_messages.highres_imu;
          printf("    ap time:     %llu \n", imu.time_usec);
          printf("    acc  (NED):  % f % f % f (m/s^2)\n", imu.xacc, imu.yacc,
                 imu.zacc);
          printf("    gyro (NED):  % f % f % f (rad/s)\n", imu.xgyro, imu.ygyro,
                 imu.zgyro);
          printf("    mag  (NED):  % f % f % f (Ga)\n", imu.xmag, imu.ymag,
                 imu.zmag);
          printf("    baro:        %f (mBar) \n", imu.abs_pressure);
          printf("    altitude:    %f (m) \n", imu.pressure_alt);
          printf("    temperature: %f C \n", imu.temperature);
          break;
        }

        case MAVLINK_MSG_ID_ATTITUDE: {
          printf("### MAVLINK_MSG_ID_ATTITUDE\n");
          mavlink_msg_attitude_decode(&message, &(current_messages.attitude));
          current_messages.time_stamps.attitude = get_time_usec();
          this_timestamps.attitude = current_messages.time_stamps.attitude;

          mavlink_attitude_t att = current_messages.attitude;
          printf("###   roll: %f  pitch: %f  yaw: %f\n", att.roll, att.pitch,
                 att.yaw);
          break;
        }

        default: {
          // printf("Warning, did not handle message id %i\n",message.msgid);
          break;
        }

      }  // end: switch msgid

    }  // end: if read message

    // Check for receipt of all items
    received_all =
        this_timestamps.heartbeat &&
        //				this_timestamps.battery_status &&
        //				this_timestamps.radio_status &&
        //				this_timestamps.local_position_ned &&
        //				this_timestamps.global_position_int &&
        //				this_timestamps.position_target_local_ned
        //&&
        //				this_timestamps.position_target_global_int
        //&&
        //				this_timestamps.highres_imu &&
        //				this_timestamps.attitude &&
        this_timestamps.sys_status;

    // give the write thread time to use the port
    if (writing_status > false) {
      usleep(100);  // look for components of batches at 10kHz
    }

  }  // end: while not received all

  return;
}
Пример #15
0
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;
		}
	}
}