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

        float airspeed; ///< Current airspeed in m/s
        float groundspeed; ///< Current ground speed in m/s
        float alt; ///< Current altitude (MSL), in meters
        float climb; ///< Current climb rate in meters/second
        int16_t heading; ///< Current heading in degrees, in compass units (0..360, 0=north)
        uint16_t throttle; ///< Current throttle setting in integer percent, 0 to 100

    **/


    if (msg->msgid == MAVLINK_MSG_ID_VFR_HUD)
    {
        mavlink_vfr_hud_t vh;
        mavlink_msg_vfr_hud_decode(msg, &vh);

        fprintf(mll->files.vfr_hud.fs, "%12" PRIu64 " %9.6f %9.6f %9.6f %9.6f %5d %5d\n",
               utils_us_since_epoch(),
                vh.airspeed,
                vh.groundspeed,
                vh.alt,
                vh.climb,
                vh.heading,
                vh.throttle
                );
        fflush(mll->files.vfr_hud.fs);
    }
}
Example #2
0
int main(int argc, char* argv[])
{
 
	char help[] = "--help";
 
 	// IP 使用的是数组来存储
	char target_ip[100];
 
	float position[6] = {};
	int sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
	// socket的地址
	struct sockaddr_in gcAddr; 
	struct sockaddr_in locAddr;
	//struct sockaddr_in fromAddr;
	uint8_t buf[BUFFER_LENGTH];
	ssize_t recsize;
	socklen_t fromlen;
	int bytes_sent;
	mavlink_message_t msg;
	uint16_t len;
	int i = 0;
	//int success = 0;
	unsigned int temp = 0;



	// channel
	int chan1_raw;
	int chan2_raw;
	int chan3_raw;
	int chan4_raw;
	int chan5_raw;
	int chan6_raw;
	int chan7_raw;
	int chan8_raw;
	int osd_rssi;

	// pitch roll yaw
	int osd_pitch;
	int osd_roll;
	int osd_yaw;

 
	// Check if --help flag was used
	if ((argc == 2) && (strcmp(argv[1], help) == 0))
    {
		printf("\n");
		printf("\tUsage:\n\n");
		printf("\t");
		printf("%s", argv[0]);
		printf(" <ip address of QGroundControl>\n");
		printf("\tDefault for localhost: udp-server 127.0.0.1\n\n");
		exit(EXIT_FAILURE);
    }
 
 
	// Change the target ip if parameter was given
	strcpy(target_ip, "127.0.0.1");
	if (argc == 2)
    {
		strcpy(target_ip, argv[1]);
    }
 
 	// 配置socket的地址,这是本地的地址
	memset(&locAddr, 0, sizeof(locAddr));
	locAddr.sin_family = AF_INET;
	locAddr.sin_addr.s_addr = INADDR_ANY;
	locAddr.sin_port = htons(14550);
 
	/* Bind the socket to port 14551 - necessary to receive packets from qgroundcontrol */ 
	if (-1 == bind(sock,(struct sockaddr *)&locAddr, sizeof(struct sockaddr)))
    {
		perror("error bind failed");
		close(sock);
		exit(EXIT_FAILURE);
    } 
 
	/* Attempt to make it non blocking */
	if (fcntl(sock, F_SETFL, O_NONBLOCK | FASYNC) < 0)
    {
		fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno));
		close(sock);
		exit(EXIT_FAILURE);
    }
 
 	// 这是远端的地址,也就是模拟飞机的地址
	memset(&gcAddr, 0, sizeof(gcAddr));
	gcAddr.sin_family = AF_INET;
	gcAddr.sin_addr.s_addr = inet_addr(target_ip);
	gcAddr.sin_port = htons(14551);
 
	for (;;) 
    {
 

		// 这里有发送的代码模块, 发送的时候,并没有控制发送的速度
		 /*Send Heartbeat */
    	 // 1,system_id component_id
		 mavlink_msg_heartbeat_pack(1, 200, &msg, MAV_TYPE_HELICOPTER, MAV_AUTOPILOT_GENERIC, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE);
		 len = mavlink_msg_to_send_buffer(buf, &msg);
		 bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
 
		// /* Send Status */
		// mavlink_msg_sys_status_pack(1, 200, &msg, 0, 0, 0, 500, 11000, -1, -1, 0, 0, 0, 0, 0, 0);
		// len = mavlink_msg_to_send_buffer(buf, &msg);
		// bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in));
 
		//  Send Local Position 
		// mavlink_msg_local_position_ned_pack(1, 200, &msg, microsSinceEpoch(), 
		// 								position[0], position[1], position[2],
		// 								position[3], position[4], position[5]);
		// len = mavlink_msg_to_send_buffer(buf, &msg);
		// bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
 
		// /* Send attitude */
		// mavlink_msg_attitude_pack(1, 200, &msg, microsSinceEpoch(), 1.2, 1.7, 3.14, 0.01, 0.02, 0.03);
		// len = mavlink_msg_to_send_buffer(buf, &msg);
		// bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
 
 		
		memset(buf, 0, BUFFER_LENGTH);
		// linux socket函数,头文件#include <arpa/inet.h>
		recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen);
		if (recsize > 0)
      	{
			// Something received - print out all bytes and parse packet
			mavlink_message_t msg;
			mavlink_status_t status;
			mavlink_attitude_t attitude;
			mavlink_vfr_hud_t hud;

 			#if SHOW
			printf("Bytes Received: %d\nDatagram: ", (int)recsize);
			#endif
			for (i = 0; i < recsize; ++i)
			{
				// temp 是一个byteBUF的形式
				temp = buf[i];
				#if SHOW
				printf("%02x ", (unsigned char)temp);
				#endif
				if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status))
				{
					#if SHOW
					printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid);
					#endif
					
					if(msg.msgid== MAVLINK_MSG_ID_ATTITUDE){
						mavlink_msg_attitude_decode(&msg,&attitude);
				    	//printf(" Received attitude packet: roll %f, pitch %f, yaw %f \n",attitude.roll,attitude.pitch,attitude.yaw);
					}
					if(msg.msgid == MAVLINK_MSG_ID_RC_CHANNELS_RAW){
						chan1_raw = mavlink_msg_rc_channels_raw_get_chan1_raw(&msg);
                   		chan2_raw = mavlink_msg_rc_channels_raw_get_chan2_raw(&msg);
                   		chan3_raw = mavlink_msg_rc_channels_raw_get_chan3_raw(&msg);
                   		chan4_raw = mavlink_msg_rc_channels_raw_get_chan4_raw(&msg);
						chan5_raw = mavlink_msg_rc_channels_raw_get_chan5_raw(&msg);
	                    chan6_raw = mavlink_msg_rc_channels_raw_get_chan6_raw(&msg);
	                    chan7_raw = mavlink_msg_rc_channels_raw_get_chan7_raw(&msg);
	                    chan8_raw = mavlink_msg_rc_channels_raw_get_chan8_raw(&msg);
	                    osd_rssi = mavlink_msg_rc_channels_raw_get_rssi(&msg);
	                    printf("chan1: %d, chan2: %d,chan3: %d, chan4: %d\n",chan1_raw,chan2_raw,chan3_raw,chan4_raw);
                	}
                	if(msg.msgid == MAVLINK_MSG_ID_ATTITUDE){
	                    //osd_pitch = ToDeg(mavlink_msg_attitude_get_pitch(&msg));
	                    //osd_roll = ToDeg(mavlink_msg_attitude_get_roll(&msg));
	                    //osd_yaw = ToDeg(mavlink_msg_attitude_get_yaw(&msg));
	                    //printf("pitch: %d,roll: %d,yaw: %d\n",osd_pitch,osd_roll,osd_yaw );
                	}
                	if(msg.msgid == MAVLINK_MSG_ID_VFR_HUD){
                		  // 解码
                		  mavlink_msg_vfr_hud_decode(&msg,&hud);     
	                    // osd_airspeed = mavlink_msg_vfr_hud_get_airspeed(&msg);
	                    // osd_groundspeed = mavlink_msg_vfr_hud_get_groundspeed(&msg);
	                    // osd_heading = mavlink_msg_vfr_hud_get_heading(&msg); // 0..360 deg, 0=north
	                    // osd_throttle = (uint8_t)mavlink_msg_vfr_hud_get_throttle(&msg);
	                    // osd_alt = mavlink_msg_vfr_hud_get_alt(&msg);
	                    // osd_climb = mavlink_msg_vfr_hud_get_climb(&msg);
	                    // printf("airspeed: %d,osd_throttle %d,osd_alt: %d\n",osd_airspeed,osd_throttle,osd_alt);
	                    printf("airspeed: %f,hrottle %d,alt: %f\n",hud.airspeed,hud.throttle,hud.alt);
                	}
				}
				
			}
			#if SHOW
			printf("\n");
			#endif
		}

		memset(buf, 0, BUFFER_LENGTH);
		usleep(1000); // Sleep one second
    }
}
Example #3
0
//74
void MavSerialPort::vfr_hud_handler(){
  //  qDebug() << "MAVLINK_MSG_ID_VFR_HUD\n";
    mavlink_msg_vfr_hud_decode(&message, &vfr_hud);

}
// ------------------------------------------------------------------------------
//   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;
    }
    }
}
Example #6
0
/*Mainloop*/
void loop() {
	/*Auswertung des anstehenden Aufgaben im Scheduler*/
	int i; unsigned char new_value = 0; unsigned char new_heading = 0; 
	for (i = 0; i < MAX_TASKS; i++) {
		if (scheduler[i].task == NOTHING || get_current_millis() < scheduler[i].tv_msec) {continue;}
		switch (scheduler[i].task) {
			case NOTHING: break;
			case MEASURE:
				/*Fordert den als Parameter uebergebenen Sensor auf eine Messung zu starten und bestimmt den Lese-Zeitpunkt*/			
				srf[scheduler[i].param - SE0_ADDRESS].measure();
				schedule_add((long)srf_speed[scheduler[i].param - SE0_ADDRESS], READ_MEASURE, scheduler[i].param);
				scheduler[i].task = NOTHING;
				break;
			case READ_MEASURE: {
				unsigned short index = scheduler[i].param - SE0_ADDRESS;
				/*Liest die Messdaten des als Parameter uebergebenen Sensors aus und veranlasst erneute Messung*/
				srf[index].read_it(get_current_millis());
				schedule_add((long)srf[index].get_delay(), MEASURE, scheduler[i].param);
				if(state == HOLD_STILL || state == MEASURE_ENVIRONMENT || state == GET_ANCHOR) {
					nvalue[index] = 1;
				}
				/*if (state == HOLD_STILL || state == HEAD_TO_MIDDLE || state = HTM_DELAY) {
					nvalue2[scheduler[i].param - SE0_ADDRESS] = 1;
				}*/
				new_value = scheduler[i].param;
				
				/*Korrektur der Messwerte bei Schieflage des Copters bei glaubhaften Winkeln*/
				if ((index == 0 || index == 1) && (abs(current_roll_rad) < MAX_ROLL_ANGLE))		srf[index].set_mean((unsigned short)(srf[index].get_mean()*cos(current_roll_rad)));
				else if ((index == 2 || index == 3) && (abs(current_pitch_rad) < MAX_PITCH_ANGLE))	srf[index].set_mean((unsigned short)(srf[index].get_mean()*cos(current_roll_rad)));
				
				/*Korrektur der Messwerte, wenn sich ein Sensor im Nahbereich (< SE_MIN_DISTANCE) befindet*/
				static unsigned char 	rm_state[SE_COUNT];
				static short		rm_min[SE_COUNT];
				if (state != IDLE && state != INIT && state != DELAY) {
					if (rm_state[index] == 0 && srf[index].get_mean() < SE_MIN_DISTANCE) {
						rm_state[index] = 1;
						if (index == 0 || index == 2)	rm_min[index] = srf[index+1].get_mean() - SE_MIN_DIFF;
						else				rm_min[index] = srf[index-1].get_mean() - SE_MIN_DIFF;
						srf[index].set_mean(SE_MIN);
					} else if (rm_state[index] == 1) {
						if ((index == 0 || index == 2) && srf[index+1].get_mean() < rm_min[index]) {
							rm_state[index] = 0;
						} else if ((index == 1 || index == 3) && srf[index-1].get_mean() < rm_min[index]) {
							rm_state[index] = 0;
						} else {
							srf[index].set_mean(SE_MIN);
						}
					}
				}
				if (rm_state[0] == 1 && rm_state[1] == 1) {rm_state[0] = 0; rm_state[1] = 0;}
				if (rm_state[2] == 1 && rm_state[3] == 1) {rm_state[2] = 0; rm_state[3] = 0;}
				
				if (state == HOLD_STILL || state ==  GET_ANCHOR || state == HTM_DELAY) {
					if (nvalue2[0] == 1 && nvalue2[1] == 1 && nvalue2[2] == 1 && nvalue2[3] == 1) {
						slam_insert_v((srf[0].get_cm_per_s() + srf[1].get_cm_per_s())/2,(srf[2].get_cm_per_s() + srf[3].get_cm_per_s())/2,current_heading,get_current_millis());
						nvalue2[0] = 0; nvalue2[1] = 0; nvalue2[2] = 0; nvalue2[3] = 0;
					}
				}
				
				/*Speichert den gelesenen Messwert in der entsprechenden Log-Datei*/
				#if LOG > 0
				FILE *fd;
				if (scheduler[i].param == SE0_ADDRESS) 	fd = fd_112;
				else if (scheduler[i].param == SE1_ADDRESS) 	fd = fd_113;
				else if (scheduler[i].param == SE2_ADDRESS) 	fd = fd_114;
				else						fd = fd_115;
				fprintf(fd,"%lu\t%u\t%u\n", srf[scheduler[i].param - SE0_ADDRESS].get_msec(), srf[scheduler[i].param - SE0_ADDRESS].get_data(), srf[scheduler[i].param - SE0_ADDRESS].get_mean());
				#endif
				scheduler[i].task = NOTHING;
				break;
			}
			case SAVE_LOG:
				/*Sichert die bisher geloggten Daten*/
				#if LOG > 0
				fclose(fd_112);
				fclose(fd_113);
				fclose(fd_114);
				fclose(fd_115);
				fclose(fd_data);
				char tmp_dir[32]; strcpy(tmp_dir,log_dir);
				fd_112 = fopen(strcat(tmp_dir,"/112"), "a"); strcpy(tmp_dir,log_dir);
				fd_113 = fopen(strcat(tmp_dir,"/113"), "a"); strcpy(tmp_dir,log_dir);
				fd_114 = fopen(strcat(tmp_dir,"/114"), "a"); strcpy(tmp_dir,log_dir);
				fd_115 = fopen(strcat(tmp_dir,"/115"), "a"); strcpy(tmp_dir,log_dir);
				fd_data= fopen(strcat(tmp_dir,"/data"),"a"); strcpy(tmp_dir,log_dir);
				schedule_add(LOG_SPEED,SAVE_LOG,0);
				#endif
				scheduler[i].task = NOTHING;
				break;   
			case CHECK_STILL:
				/*Prueft auf Stillstand des Copters*/
				if (still && state == HOLD_STILL) {
					if (scheduler[i].param < CHECK_STILL_COUNT) {
						schedule_add(CHECK_STILL_SPEED,CHECK_STILL,scheduler[i].param+1);
					} else {
						state = MEASURE_ENVIRONMENT;
						still = 0;
						schedule_add(CHECK_STILL_SPEED,CHECK_STILL, 0);
					}
				} else {
					schedule_add(CHECK_STILL_SPEED,CHECK_STILL,0);
				}
				scheduler[i].task = NOTHING;
				break;
			case CHANGE_STATE:
				/*Ändert den aktuellen Status zum als Parameter übergebenen Status*/
				state = (states)scheduler[i].param;
				if ((states)scheduler[i].param == HOLD_STILL) hs_state = 0;
				scheduler[i].task = NOTHING;
				break;
			case SHOW_ME: {
				/*Schreibt Messwerte und Neigungswinkel auf das Terminal*/
				char st[16];
				switch (state) {
					case INIT: 			sprintf(st, "INIT"); 		break;
					case MEASURE_ENVIRONMENT: 	sprintf(st, "ME"); 		break;
					case IDLE: 			sprintf(st, "IDLE"); 		break;
					case HOLD_STILL: 		sprintf(st, "HOLD_STILL"); 	break;
					case GET_ALIGNMENT: 		sprintf(st, "GET_ALIGNMENT"); 	break;
					case GET_ANCHOR: 		sprintf(st, "GET_ANCHOR"); 	break;
					case DELAY:			sprintf(st, "DELAY");		break;
					default: 			sprintf(st, "???"); 		break;
				}
					
				if (roll > 9 || roll < 0) 	printf("roll: %d\tpitch: %d\tyaw: %d\theading: %d\tdhead: %d\tSE0: %d\tSE1: %d\tSE2: %d\tSE3:%d\tstate: %s\n", roll, pitch, yaw, current_heading, desired_heading, srf[0].get_mean(), srf[1].get_mean(), srf[2].get_mean(), srf[3].get_mean(),st);
				else 				printf("roll: %d\t\tpitch: %d\tyaw: %d\theading: %d\tdhead. %d\tSE0: %d\tSE1: %d\tSE2: %d\tSE3:%d\tstate: %s\n", roll, pitch, yaw, current_heading, desired_heading, srf[0].get_mean(), srf[1].get_mean(), srf[2].get_mean(), srf[3].get_mean(),st);
				schedule_add(100,SHOW_ME,0);
				scheduler[i].task = NOTHING;
				
				/*Mavlinkkommunikation mit QGroundcontrol (basierend auf Based on http://qgroundcontrol.org/dev/mavlink_linux_integration_tutorial)*/
				mavlink_message_t msg;
				uint16_t len;
				uint8_t buf[MAVLINK_MAX_PACKET_LEN];
				
				static int i; i++;
				if (i == 10) {
					i = 0;
					mavlink_msg_heartbeat_pack(0, 0, &msg, 0, 0, 0, 0, 0);
					len = mavlink_msg_to_send_buffer(buf, &msg);
					sendto(s, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
				}
				mavlink_msg_debug_vect_pack(0,0,&msg,"DEBUG",0,roll,srf[0].get_mean(),srf[0].get_data());
				len = mavlink_msg_to_send_buffer(buf, &msg);
				sendto(s, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
				break;
			}
			default: if(WARNINGS){perror("LOOP: Unbekannte Aufgabe im Scheduler.");} break; 
		}
	}

	/*Auswertung der Daten, die an der seriellen Schnittstelle anliegen.*/
	struct timeval tv; tv.tv_sec = 0; tv.tv_usec = 0;
	/*Prueft, ob Daten an der seriellen Schnittstelle anliegen*/
	FD_ZERO(&tty_fdset);
	FD_SET(tty_fd, &tty_fdset);
	if (select(tty_fd+1, &tty_fdset, NULL, NULL, &tv) == -1) {perror("LOOP: select() fehlgeschlagen"); exit(1);}

	/*Liest ein einzelnes Byte von der seriellen Schnittstelle und prueft, ob ein Mavlinkpaket vervollstaendigt wurde*/
	if (FD_ISSET(tty_fd, &tty_fdset)) {
		static mavlink_message_t msg;
		static mavlink_status_t status;
		char c[1];
		if (read(tty_fd,c,1) == -1) {if(WARNINGS){perror("LOOP: Fehler beim Lesen aus " TTY_DEVICE);}}
		#if DEBUG_LEVEL > 2
		printf("%#x\n",c[0]);
		#endif
		if (mavlink_parse_char(MAVLINK_COMM_0,(uint8_t) *c, &msg, &status)) {
			#if DEBUG_LEVEL > 1
			printf("%u Mavlinkpaket empfangen: %d\n", get_current_millis(), msg.msgid);
			#endif
			//printf("%lu Mavlinkpaket empfangen: %d\n", get_current_millis(), msg.msgid);
			
			switch (msg.msgid) {
				case MAVLINK_MSG_ID_HEARTBEAT:
					/*Beim Empfang des ersten Heartbeats wird zunaechst ein eventuell vorhandener Datenstream gekuendigt und ein neuer Datenstream abonnemiert*/
					mavlink_heartbeat_t hb;
					mavlink_msg_heartbeat_decode(&msg,&hb);
					if (!first_heartbeat) {
						first_heartbeat = 1;
						request_data_stream(MAV_DATA_STREAM_ALL,0,0);
					}
					//if (state == IDLE) {
					if (state == IDLE && (hb.custom_mode == 13 /*EXT_CTRL*/ || desktop_build)) {
						std::cout << "State changed from IDLE to INIT.\n";
						state = INIT;
						schedule_add(2000,CHANGE_STATE,(int)HOLD_STILL);
						request_data_stream(MAV_DATA_STREAM_EXTRA2/*VFR_HUD*/,DATA_STREAM_SPEED,1);
						init_state = 1;
					}
					break;
				case MAVLINK_MSG_ID_VFR_HUD:
					/*Speichert bei Empfang von HUD-Daten die aktuelle Ausrichtung des Copters*/
					mavlink_vfr_hud_t hud_data;
					mavlink_msg_vfr_hud_decode(&msg, &hud_data);
					old_heading = current_heading;
					current_heading = hud_data.heading;
					new_heading = 1;
					break;
				case MAVLINK_MSG_ID_ATTITUDE:
					mavlink_attitude_t adata;
					mavlink_msg_attitude_decode(&msg,&adata);
					old_heading = current_heading;
					current_heading = ((short)lround(DEG(adata.yaw))+360)%360;
					current_pitch_rad = adata.pitch;
					current_roll_rad = adata.roll;
					new_heading = 1;
					break;
				case MAVLINK_MSG_ID_RAW_IMU:
					/*Speichert die Beschleunigungen auf der x- und y-Achse*/
					mavlink_raw_imu_t raw_imu;
					mavlink_msg_raw_imu_decode(&msg,&raw_imu);
					xacc = raw_imu.xacc;
					yacc = raw_imu.yacc;
					//slam_insert_acc(xacc,yacc,current_heading,get_current_millis());
					std::cout << "xacc: " << xacc << " yacc: " << yacc << "\n";
					break;
				case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
					/*Prüft, ob eine Umstellung auf externe Kontrolle erfolgt ist*/
					/*if (state != IDLE) {break;}
					mavlink_rc_channels_raw_t rc;
					mavlink_msg_rc_channels_raw_decode(&msg,&rc);
					if (desktop_build) init_state = 1; rc.chan5_raw = MODE_SWITCH_RANGE_UP-1;
					if (init_state && rc.chan5_raw < MODE_SWITCH_RANGE_UP && rc.chan5_raw > MODE_SWITCH_RANGE_DOWN) {
						std::cout << "State changed from IDLE to INIT.\n";
						schedule_add(0,CHANGE_STATE,(int)INIT);
						schedule_add(2000,CHANGE_STATE,(int)HOLD_STILL);
						request_data_stream(MAV_DATA_STREAM_RC_CHANNELS,0,0);
						request_data_stream(MAV_DATA_STREAM_EXTRA2,DATA_STREAM_SPEED,1);
						request_data_stream(MAV_DATA_STREAM_RAW_SENSORS,DATA_STREAM_SPEED,1);
					}
					if(rc.chan5_raw > MODE_SWITCH_RANGE_UP || rc.chan5_raw < MODE_SWITCH_RANGE_DOWN) {
						init_state = 1;
					}*/
					break;
				default: break;
			}
		}
	}		

	/*Berechnung der vorgeschlagenen Werte fuer roll, pitch und yaw an Hand der neuen Messwerte*/
	if (state != INIT && !new_value && !new_heading) {/*Wenn keine neuen Daten vorhanden sind -> Abarbeitung überspringen*/return;}
	switch (state) {	
		case IDLE: /*Wartet auf Aktivierung der externen Kontrolle*/	break;
		case INIT:
			std::cout << "INIT\n";
			/*Initialisiert Abarbeitung, wenn externe Kontrolle zum ersten Mal aktiviert wurde*/
			schedule_add(5000,CHECK_STILL,0);
			#if LOG > 0
			schedule_add(LOG_SPEED,SAVE_LOG,0);
			//schedule_add(SLAM_SPEED,SAVE_SLAM,0);
			#endif
			for (int i = 0; i < SE_COUNT; i++) {
				if ((int)pow(2,i) & ENABLED_SENSORS) {
					schedule_add(0, MEASURE, SE0_ADDRESS + i);
				}
			}
			slam_init(current_heading,get_current_millis());
			state = DELAY;
			break;
		case HOLD_STILL:
			if(1) {
				static short set_point[SE_COUNT];
				if (hs_state == 0) {
					hs_state = 1;
					set_point[0] = (srf[0].get_mean() + srf[1].get_mean())/2;
					set_point[1] = set_point[0];
					set_point[2] = (srf[2].get_mean() + srf[3].get_mean())/2;
					set_point[3] = set_point[2];
					//for (int i = 0; i < SE_COUNT; i++) {
					//	set_point[i] = srf[i].get_mean();
					//	std::cout << "Set_Point " << i << ":" << set_point[i] << "\n";
					//}
					/*for (int i = 0; i < SE_COUNT; i++) {
						if (set_point[i] < 100 || set_point[i] > 0) {
							if (i == 0 || i == 2) {
								set_point[i+1] -= (100 - set_point[i]);
							} else {
								set_point[i-1] -= (100 - set_point[i]);
							}
							set_point[i] = 100;
						}
					}*/
					for (int i = 0; i < SE_COUNT; i++) {
						std::cout << "Set_Point " << i << ":" << set_point[i] << "\n";
					}
				}
				/*Veranlasst den Copter auf Grundlage der Änderungen der Messwerte still zu stehen*/
				if (!new_value) break;
				if (nvalue[0] == 1 && nvalue[1] == 1) {
					if (srf[0].get_mean() == SE_MIN)	roll = between<short>(pid_roll.get(0 - srf[1].get_mean() + set_point[1],srf[1].get_msec_diff()),-max_roll,max_roll);
					else if (srf[1].get_mean() == SE_MIN) 	roll = between<short>(pid_roll.get(srf[0].get_mean() - set_point[0],srf[0].get_msec_diff()),-max_roll,max_roll);
					else 					roll = between<short>(pid_roll.get(((srf[0].get_mean() - set_point[0]) - (srf[1].get_mean() - set_point[1]))/2,(srf[0].get_msec_diff() + srf[1].get_msec_diff())/2),-max_roll,max_roll);
					nvalue[0] = 0; nvalue[1] = 0;
				}
				if (nvalue[2] == 1 && nvalue[3] == 1) {
					if (srf[2].get_mean() == SE_MIN)	pitch = between<short>(pid_pitch.get(0 - srf[3].get_mean() + set_point[3],srf[3].get_msec_diff()),-max_pitch,max_pitch);
					else if (srf[3].get_mean() == SE_MIN) 	pitch = between<short>(pid_pitch.get(srf[2].get_mean() - set_point[2],srf[2].get_msec_diff()),-max_pitch,max_pitch);
					else 					pitch = between<short>(pid_pitch.get(((srf[2].get_mean() - set_point[2]) - (srf[3].get_mean() - set_point[3]))/2,(srf[2].get_msec_diff() + srf[3].get_msec_diff())/2),-max_pitch,max_pitch);
					nvalue[2] = 0; nvalue[3] = 0;
				}
				yaw = 0;
				send_ext_ctrl();
				/*Prüfung auf Stillstand*/
				if (abs(roll) <= STILL_FAK_ROLL*max_roll/100 && abs(pitch) <= STILL_FAK_PITCH*max_pitch/100) {
					//still = 1; 
					still = 0; /*FIXME*/
				} else {
					still = 0;
				}
			}
		break;
		case MEASURE_ENVIRONMENT:
			/*Vermisst die Umgebung durch eine Drehung um 360/SE_COUNT Grad*/
			if (breakpoint == 1) {state = HOLD_STILL; hs_state= 0; break;}
			if (abs(xacc) > HOLD_STILL_MAX_XACC || abs(yacc) > HOLD_STILL_MAX_YACC) {/*Wurde der Copter zu stark bewegt: Abbruch*/state = HOLD_STILL; hs_state = 0; std::cout << "State changed to HOLD_STILL\n"; break;}
			if (first_heading == -1) {
				/*Speichert die anfaengliche Ausrichtung und veranlasst den Copter sich zu drehen*/
				std::cout << "State changed to MEASURE_ENVIRONMENT\n";
				first_heading = current_heading;
				roll = 0; pitch = 0; rotation = 0; yaw = rotation_angle_sign*CONST_YAW;
				send_ext_ctrl();
			}
			//if (new_value) slam_insert_measurement(srf[new_value-SE0_ADDRESS].get_mean(),(current_heading + (360/SE_COUNT)*(new_value-SE0_ADDRESS))%360);
			/*Berechnung der aktuellen Drehung*/
			if (new_heading) {
				if (abs(current_heading - old_heading) > 180/SE_COUNT)	rotation += current_heading - old_heading - sign(current_heading - old_heading)*360;
				else 							rotation += current_heading - old_heading;
				for(int i = 0; i < SE_COUNT; i++) {
					if(nvalue[i] == 1) {
						env[(current_heading + srf[i].get_alignment()*(360/SE_COUNT))%360] = srf[i].get_mean();
						nvalue[i] = 0;
					}
				}
				//if (abs(rotation) >= 360/SE_COUNT) {
				if (abs(rotation) >= rotation_angle) {
					/*Copter hat sich ausreichend gedreht, Abbruch der Vermessung*/
					//state = HEAD_TO_MIDDLE;
					state = GET_ALIGNMENT;
					roll = 0; pitch = 0; yaw = 0;
					send_ext_ctrl();
					rotation_angle_sign = sign(rotation);
				}
			}
			break;
		case GET_ALIGNMENT: {
			/*Bestimme für jeden Sensor die Minimalentfernung der letzten Drehung*/
			/*short min_v[SE_COUNT];
			short min_h[SE_COUNT];
			for (int i = 0; i < SE_COUNT; i++) {
				min_v[i] = 0;
				min_h[i] = 0;
			}*/
			short min_v = 0;
			for (int i = 0; i < rotation_angle; i++) {
				if (min_v < env[(first_heading + rotation_angle_sign*i + 360 + srf[align_se].get_alignment()*(360/SE_COUNT))%360]) {
					min_v = env[(first_heading + rotation_angle_sign*i + 360 + srf[align_se].get_alignment()*(360/SE_COUNT))%360];
					desired_heading = (first_heading + rotation_angle_sign*i + 360 + srf[align_se].get_alignment()*(360/SE_COUNT))%360;
				}
				/*for (int j = 0; j < SE_COUNT; j++) {
					if (min_v[j] < env[(first_heading + rotation_angle_sign*i + j*360/SE_COUNT + 360)%360]) {
						min_v[j] = env[(first_heading + rotation_angle_sign*i + j*360/SE_COUNT + 360)%360];
						min_h[j] = (first_heading + rotation_angle_sign*i + 360)%360;
					}
				}*/
			}
			//desired_heading = round((float)(min_h[0]+min_h[1]+min_h[2]+min_h[3])/4);
			/*Bestimmung der künftigen Drehrichtung*/
			for (int k = 0; k < rotation_angle/2; k++) {
				/*Wenn Minimum in der ersten Hälfte der Drehung gefunden wurde, Wechsel der Drehrichtung bzw. Abbruch*/
				if (desired_heading == (first_heading+rotation_angle_sign*k)%360) {
					rotation_angle_sign *= -1;
					if (init_state == 2) init_state = 3;
					break;
				}
			}
			/*Cleanup*/
			first_heading = -1; yaw = 0; pid_roll.reset(); pid_pitch.reset();
			for(int i = 0; i < 360; i++) env[i] = 0;
			
			if (init_state == 1) 	init_state = 2;
			if (init_state == 3) {
				state = GET_ANCHOR;
				pid_yaw.reset();
				pid_yaw.set(HTM_YAW_KP,HTM_YAW_TN,HTM_YAW_TV);
				pid_yaw.set_target(0);
				roll = 0; pitch = 0; yaw = 0;
				std::cout << "State changed to GET_ANCHOR\n";
				tv_old_heading = get_current_millis();
			} else {
				state = HOLD_STILL;
				hs_state = 0;
			}
			break;
		}
		case GET_ANCHOR:
			/*Copter versucht sich stabil auf ANCHOR_DISTANCE und desired_heading zu stellen*/
			if (new_heading) {
				short heading_diff;
				/*Berechnung der Differenz zwischen aktueller Ausrichtung und gewünschter Ausrichtung*/
				if (abs(desired_heading - current_heading) > 180) {
					heading_diff = desired_heading - current_heading - sign(desired_heading - current_heading)*360;
				} else {
					heading_diff = desired_heading - current_heading;
				}
				yaw = pid_yaw.get(heading_diff,get_current_millis() - tv_old_heading);
				tv_old_heading = get_current_millis();
			}
			if (new_value) {
				roll = 0; pitch = 0;
				if (nvalue[align_se] == 1) {
					if (align_se == 0)	roll   = between<short>(pid_roll_anc.get(srf[align_se].get_mean(), srf[align_se].get_msec_diff()), -max_roll,  max_roll);
					else if (align_se == 1)	roll   = between<short>(pid_roll_anc.get((-1)*srf[align_se].get_mean(), srf[align_se].get_msec_diff()), -max_roll,  max_roll);
					else if (align_se == 2)	pitch  = between<short>(pid_pitch_anc.get(srf[align_se].get_mean(), srf[align_se].get_msec_diff()), -max_pitch,  max_pitch);
					else if (align_se == 3)	pitch  = between<short>(pid_pitch_anc.get((-1)*srf[align_se].get_mean(), srf[align_se].get_msec_diff()), -max_pitch,  max_pitch);
					nvalue[align_se] = 0;
				}
				if 	(srf[0].get_mean() < 30) 	roll = 1000;
				else if (srf[1].get_mean() < 30)	roll = -1000;
				else if (srf[2].get_mean() < 30)	pitch = 1000;
				else if (srf[3].get_mean() < 30)	pitch = -1000;
				if (abs(srf[0].get_mean() - ANCHOR_DISTANCE) < 5) {
					for (int i = 0; i < SE_COUNT; i++) nvalue[i] = 0;
					/*state = MOVE_BACKWARD*/
					/*TODO Neuausrichtung des Copters?*/
				}
			}
			if (breakpoint == 3) 		{roll = 0; pitch = 0; send_ext_ctrl();}
			else if (breakpoint != 2) 	{send_ext_ctrl();}
			break;
		case DELAY: 	break;
		case HTM_DELAY:	break;
		default:	break;
	}
	#if LOG > 0
	fprintf(fd_data,"%lu\t%d\t%d\t%d\t%d\t%d\t%f\t%f\n", (unsigned long)get_current_millis(), roll, pitch, yaw, current_heading, state, current_roll_rad, current_pitch_rad);
	#endif	
}
Example #7
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;
		}
	}
}