int main(int argc, char* argv[])
{
    // serial devices "COM5" or /dev/ttyUSB0 ..
    char serialDevice[256] = "";

    for (int i = 1; i < argc; i++) {
        if (i + 1 != argc) {
            if (strcmp(argv[i], "-s") == 0) {
                strcpy(serialDevice, argv[i + 1]);
                i++;
            }
        }
    }

    MW_TRACE("starting..\n")
    serialLink = MWIserialbuffer_init(serialDevice);

    if (serialLink == NOK) {
        perror("error opening serial port");
        return EXIT_FAILURE;
    }

    // mwi state
    mwiState = calloc(sizeof(*mwiState),sizeof(*mwiState));
    mwiState->callback = &callBack_mwi;

    uint64_t lastFrameRequest = 0;

    printf("currentTime;angx;angy;head;ax;ay;az;gx;gy;gz;magx;magy;magz;mot[0];mot[1];mot[2];mot[3];mot[4];mot[5];rcRoll;rcPitch;rcYaw;rcThrottle;rcAUX1;rcAUX2;rcAUX3;rcAUX4;debug1;debug2;debug3;debug4\n");

    for (;;) {

        currentTime = microsSinceEpoch();

        if ((currentTime - lastFrameRequest) > 1000 * 30) {
            if (initOk == OK) {
                lastFrameRequest = currentTime;
                MWIserialbuffer_askForFrame(serialLink, MSP_RAW_IMU);
                MWIserialbuffer_askForFrame(serialLink, MSP_DEBUG);
                MWIserialbuffer_askForFrame(serialLink, MSP_BAT);
                MWIserialbuffer_askForFrame(serialLink, MSP_ALTITUDE);
                MWIserialbuffer_askForFrame(serialLink, MSP_COMP_GPS);
                MWIserialbuffer_askForFrame(serialLink, MSP_RAW_GPS);
                MWIserialbuffer_askForFrame(serialLink, MSP_RC);
                MWIserialbuffer_askForFrame(serialLink, MSP_MOTOR);
                MWIserialbuffer_askForFrame(serialLink, MSP_SERVO);
                MWIserialbuffer_askForFrame(serialLink, MSP_RAW_IMU);
                MWIserialbuffer_askForFrame(serialLink, MSP_STATUS);
                MWIserialbuffer_askForFrame(serialLink, MSP_ATTITUDE);
            } else {
                MWIserialbuffer_askForFrame(serialLink, MSP_IDENT);
            }
        }

        MWIserialbuffer_readNewFrames(serialLink, mwiState);

        usleep(5000);
    }
}
Exemple #2
0
int gcs_raw_imu(int sock, daisy7_imu data) {
	int bytes_sent;
	mavlink_message_t msg;
	uint8_t buf[512];
	uint16_t len;

	mavlink_msg_raw_imu_pack(1, 200, &msg, microsSinceEpoch(),
			data.acc.raw_x, data.acc.raw_y, data.acc.raw_z,
			data.gyro.raw_x,data.gyro.raw_y,data.gyro.raw_z,
			0,0,0);
	len = mavlink_msg_to_send_buffer(buf, &msg);
	bytes_sent = gcs_udp_send(sock, buf, len);
	return(bytes_sent);
}
Exemple #3
0
void cDataLink::SendAttMsg(float roll, float pitch, float yaw,
                           float rollspeed, float pitchspeed, float yawspeed)
{
    int bytes_sent;
    uint16_t len;

    /* Send attitude */
    mavlink_msg_attitude_pack(1, MAV_COMP_ID_ALL, &m_msg, microsSinceEpoch(),
                              roll, pitch, yaw,
                              rollspeed, pitchspeed, yawspeed);
    len = mavlink_msg_to_send_buffer(m_buf, &m_msg);
    bytes_sent = sendto(sock, m_buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
    (void)bytes_sent; //avoid compiler warning
}
void callBack_mwi(int state)
{
    switch (state) {
        case MSP_IDENT:
            initOk = OK;
            break;
        default:
            currentTime = microsSinceEpoch();
            printf("%ju;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i\n", currentTime, mwiState->angx, mwiState->angy, mwiState->head, mwiState->ax, mwiState->ay, mwiState->az, mwiState->gx, mwiState->gy, mwiState->gz, mwiState->magx,
                    mwiState->magy, mwiState->magz, mwiState->mot[0], mwiState->mot[1], mwiState->mot[2], mwiState->mot[3], mwiState->mot[4], mwiState->mot[5], mwiState->rcRoll, mwiState->rcPitch, mwiState->rcYaw, mwiState->rcThrottle, mwiState->rcAUX1, mwiState->rcAUX2,
                    mwiState->rcAUX3, mwiState->rcAUX4, mwiState->debug[0], mwiState->debug[1], mwiState->debug[2], mwiState->debug[3]);
            break;
    }

}
Exemple #5
0
void cDataLink::SendServoOutputRaw(uint16_t ped_us, uint16_t col_us, uint16_t lon_us,
                                   uint16_t lat_us, uint16_t aux_us, uint16_t mode_us)
{
    int bytes_sent;
    uint16_t len;

    /* Send RC channels */
    {
        mavlink_msg_servo_output_raw_pack(1, MAV_COMP_ID_ALL, &m_msg, microsSinceEpoch(),
                                         uint8_t(1),                     // port
                                         ped_us, col_us, lon_us, lat_us, //1...4
                                         aux_us, mode_us,                //5, 6
                                         uint16_t(65535), uint16_t(65535)); // 6...8, 65535 implies the channel is unused
        len = mavlink_msg_to_send_buffer(m_buf, &m_msg);
        bytes_sent = sendto(sock, m_buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
        (void)bytes_sent; //avoid compiler warning
    }
}
Exemple #6
0
void cDataLink::SendGpsMsg(unsigned short fix_type, long lat, long lon, long alt,
                           long vdop, long hdop,
                           long vel, long course, unsigned short sats)
{
    int bytes_sent;
    uint16_t len;

    /* Send GPS raw int */
    mavlink_msg_gps_raw_int_pack(1, MAV_COMP_ID_GPS, &m_msg, microsSinceEpoch(),
                                 fix_type,          //fix_type
                                 lat, lon, alt,  //lat, lon, alt
                                 vdop, hdop,  // hdop, vdop
                                 vel,      //vel*100
                                 course,       //course deg*100
                                 sats);         //visible satellites
    len = mavlink_msg_to_send_buffer(m_buf, &m_msg);
    bytes_sent = sendto(sock, m_buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
    (void)bytes_sent; //avoid compiler warning
}
Exemple #7
0
void Create_packets(ExtU_ANN_EKF_NMPC_2_T *data,int sock)
{   int i=0;
    int bytes_sent;
    uint8_t buf[BUFFER_LENGTH];
    uint8_t buf1[BUFFER_LENGTH];
    uint8_t buf2[BUFFER_LENGTH];
    mavlink_message_t msg,msg1,msg2;
    uint16_t len;
    mavlink_msg_heartbeat_pack(2, 200, &msg, 2, 12, 65, 0, 3);
    len = mavlink_msg_to_send_buffer(buf, &msg);
    int len1  = strlen(buf);
     bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
    mavlink_msg_attitude_pack(2, 200, &msg, microsSinceEpoch(),data->EulerAnglesmeas.phi, data->EulerAnglesmeas.theta, data->EulerAnglesmeas.psi, data->BodyRatesmeas.P,data->BodyRatesmeas.Q, data->BodyRatesmeas.R);
    len = mavlink_msg_to_send_buffer(buf, &msg);
    len1  = strlen(buf);
     bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
//    mavlink_msg_hil_state_pack(1, 200, &msg, microsSinceEpoch(),data->EulerAngles.phi, data->EulerAngles.theta, data->EulerAngles.psi, data->BodyRatesmeas.P, data->BodyRatesmeas.Q, data->BodyRatesmeas.R,data->GPSPosition.Latitude,data->GPSPosition.Longitude,data->GPSPosition.Altitude,0,0,0,data->Accelerometermeas.Axb,data->Accelerometermeas.Ayb,data->Accelerometermeas.Azb);
   // mavlink_msg_hil_state_pack(2, 200, &msg, microsSinceEpoch(),data->EulerAngles.phi, data->EulerAngles.theta, data->EulerAngles.psi, data->BodyRatesmeas.P, data->BodyRatesmeas.Q, data->BodyRatesmeas.R,39,-95,data->GPSPosition.Altitude,0,0,0,data->Accelerometermeas.Axb,data->Accelerometermeas.Ayb,data->Accelerometermeas.Azb);
    //len = mavlink_msg_to_send_buffer(buf, &msg);
  //  bytes_sent = write(tty_fd1,buf,len);
    mavlink_msg_gps_raw_int_pack(2, 200, &msg, microsSinceEpoch(),2,data->GPSPositionmeas.Latitude,data->GPSPositionmeas.Longitude,data->GPSPositionmeas.Altitude,UINT16_MAX,UINT16_MAX,UINT16_MAX,UINT16_MAX,255);
    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);
		//mavlink_message_t msg;
		mavlink_status_t status,status1;
		mavlink_mission_count_t mission_count;
		int cn;
		recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen);
		if (recsize > 0)
      	        {
			// Something received - print out all bytes and parse packet
			
			
			printf("Bytes Received: %d\nDatagram: ", (int)recsize);
			for (i = 0; i < recsize; ++i)
			{
				//temp = buf[i];
			//	printf("%02x ", (unsigned char)temp);
				if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status))
				{
					// Packet received
					printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid);		
					printf("\n\n Incomming packet\n\n");
		if(msg.msgid ==  44)
		{				
			mavlink_msg_mission_count_decode( &msg, &mission_count);
			printf("\n\n the # of wypts received is %d########\n\n",mission_count.count);
			data->wcn=mission_count.count;
			for(cn=1;cn<=data->wcn;cn++)
			{
				//sendrequest(sock,cn);
				memset(buf1, 0, BUFFER_LENGTH);
				mavlink_msg_mission_request_pack(2, 200, &msg1,255,0,(cn-1));
				len = mavlink_msg_to_send_buffer(buf1, &msg1);
				bytes_sent = sendto(sock, buf1, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
				//printf("\n\n @@@@@ sent successfull\n\n");
				memset(buf2, 0, BUFFER_LENGTH);
				//receive_waypoints(sock,cn);
				while( (recsize1 = recvfrom(sock, (void *)buf2, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen)) <= 0);
	 if (recsize1 > 0)
	
      	{	
		int ii;
		mavlink_mission_item_t mission_item;		
	  	for (ii = 0; ii < recsize1; ++ii)
		{
			if (mavlink_parse_char(MAVLINK_COMM_0, buf2[ii], &msg2, &status1))
			{
				printf("\n\n reading waypoint # %d",(cn-1));
			}
		}//if (msg2.msgid==0)
			//goto loop;
		if(msg2.msgid ==  39)
		{
			mavlink_msg_mission_item_decode(&msg2, &mission_item);
			printf("\n\nthe waypoint # %d is lat:%f lon:%f alt:%f",cn,mission_item.x,mission_item.y,mission_item.z);
			data->lat[cn]=mission_item.x;
			data->alt[cn] =mission_item.z;
			data->lon[cn] = mission_item.y;
			data->WaypointsIN.v[cn] = mission_item.param1;	
			latlon(&ANN_EKF_NMPC_2_U,cn);	
		}
	}else printf("!!!!!!!!\n\n receiving failed \n\n "); 
			} 
			if((cn-1) == mission_count.count)
			{
				//sendack(sock,cn);
				mavlink_msg_mission_ack_pack(2, 200, &msg2,255,0,0);
				memset(buf2, 0, BUFFER_LENGTH);
				len = mavlink_msg_to_send_buffer(buf2, &msg2);
				bytes_sent = sendto(sock, buf2, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
				printf("\n\n ***** sent acknowledgement *****\n\n");
			} 		
		}        // for msg id 44
		if(msg.msgid ==  43)
		{       
			int j; 
			
			//sendcn(sock,cn);
			mavlink_msg_mission_count_pack(2, 200, &msg2,255,0,data->wcn);
			memset(buf2, 0, BUFFER_LENGTH);
			len = mavlink_msg_to_send_buffer(buf2, &msg2);
			bytes_sent = sendto(sock, buf2, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
			memset(buf2, 0, BUFFER_LENGTH);
			for(j=1; j<=data->wcn;j++)
			{
				//receive_request(sock);
				//sendwyp(sock,-95,95);
				mavlink_msg_mission_item_pack(2, 200, &msg2,255,0,(j-1),0,16,0,1,data->WaypointsIN.v[j],0,0,0,data->lat[j],data->lon[j],data->alt[j]);
				len = mavlink_msg_to_send_buffer(buf2, &msg2);
				bytes_sent = sendto(sock, buf2, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
				printf("\n\n @@@@@ sent count successfull\n\n");
			}
			//rec_ack(sock);
		}// for msg id 43
		}	//parse - if
				}//for
			}//else printf("\n\n 12121212not good");//recsize - if
		//	printf("\n");
		
		//memset(buf, 0, BUFFER_LENGTH);
		
		//printf("\n\n !!!@@##$$ end of a loop\n\n");




}//for Createpacket
Exemple #8
0
/***********************************************************************
*	main()
*	initialize the IMU, start all the threads, and wait still something 
*	triggers a shut down
***********************************************************************/
int main(int argc, char* argv[]){
	// initialize cape hardware
	if(initialize_cape()<0){
		blink_red();
		return -1;
	}
	setRED(HIGH);
	setGRN(LOW);
	set_state(UNINITIALIZED);
	
	// set up button handlers first
	// so user can exit by holding pause
	set_pause_pressed_func(&on_pause_press);
	set_mode_unpressed_func(&on_mode_release);
	
	// load data from disk.
	if(load_config(&config)==-1){
		printf("aborting, config file error\n");
		return -1;
	}
	
	// start a thread to slowly sample battery 
	pthread_t  battery_thread;
	pthread_create(&battery_thread, NULL, battery_checker, (void*) NULL);
	
	
	// start listening for RC control from dsm2 radio
	if(config.enable_dsm2){
		if(initialize_dsm2()<0){
				printf("failed to start DSM2\n");
		}
		else{
			pthread_t  dsm2_thread;
			pthread_create(&dsm2_thread, NULL, dsm2_watcher, (void*) NULL);
		}
	}
	
	// // start logging thread if enabled
	// if(config.enable_logging){
		// if(start_log(SAMPLE_RATE_HZ, &cstate.time_us)<0){
			// printf("failed to start log\n");
		// }
		// else{
			// // start new thread to write the file occationally
			// pthread_t  logging_thread;
			// pthread_create(&logging_thread, NULL, log_writer, (void*) NULL);
		// }
	// }
	
	// // first check for user options
	// if(parse_arguments(argc, argv)<0){
		// return -1;
	// }
	
	
	// // start logging thread if enabled
	// if(config.enable_logging){
		// if(start_log(SAMPLE_RATE_HZ, &cstate.time_us)<0){
			// printf("failed to start log\n");
		// }
		// else{
			// // start new thread to write the file occationally
			// pthread_t  logging_thread;
			// pthread_create(&logging_thread, NULL, log_writer, (void*) NULL);
		// }
	// }
	
	// // Start Safety checking thread
	// pthread_create(&safety_thread, NULL, safety_thread_func, (void*) NULL);
	
	
	// Finally start the real-time interrupt driven control thread
	// start IMU with equilibrium set with upright orientation 
	// for MiP with Ethernet pointing relatively up
	signed char orientation[9] = ORIENTATION_FLAT; 
	if(initialize_imu(SAMPLE_RATE_HZ, orientation)){
		// can't talk to IMU, all hope is lost
		// blink red until the user exits
		printf("IMU initialization failed, please reboot\n");
		blink_red();
		cleanup_cape();
		return -1;
	}
	// assigning the interrupt function and stack
	// should be the last step in initialization 
	// to make sure other setup functions don't interfere
	printf("starting core IMU interrupt\n");
	cstate.core_start_time_us = microsSinceEpoch();
	set_imu_interrupt_func(&flight_core);
	// start flight stack to control setpoints
	// this thread is in charge of arming and managing the core
	pthread_t  flight_stack_thread;
	pthread_create(&flight_stack_thread, NULL, flight_stack, (void*) NULL);
	
	printf("\nReady for arming sequence\n");
	set_state(RUNNING);
	
	// start printf_thread if running from a terminal
	// if it was started as a background process then don't bother
	if(isatty(fileno(stdout))){
		pthread_t  printf_thread;
		pthread_create(&printf_thread, NULL, printf_loop, (void*) NULL);
	}
	
	//chill until something exits the program
	while(get_state()!=EXITING){
		usleep(100000);
	}
	
	// cleanup before closing
	//close(sock); 	// mavlink UDP socket
	cleanup_cape();	// de-initialize cape hardware
	return 0;
}
Exemple #9
0
int main(int argc, char* argv[])
{
    
    char help[] = "--help";
    
    
    char target_ip[100];
    
    float position[6] = {};
    int sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
    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;
    
    // 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]);
    }
    
    
    memset(&locAddr, 0, sizeof(locAddr));
    locAddr.sin_family = AF_INET;
    locAddr.sin_addr.s_addr = INADDR_ANY;
    locAddr.sin_port = htons(14551);
    
    /* 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(14550);
    
    
    
    for (;;) 
    {
        
        /*Send Heartbeat */
        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);
        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;
            
            printf("Bytes Received: %d\nDatagram: ", (int)recsize);
            for (i = 0; i < recsize; ++i)
            {
                temp = buf[i];
                printf("%02x ", (unsigned char)temp);
                if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status))
                {
                    // Packet received
                    printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid);
                }
            }
            printf("\n");
        }
        memset(buf, 0, BUFFER_LENGTH);
        sleep(1); // Sleep one second
    }
}
Exemple #10
0
int main(int argc, char* argv[])
{
#if defined( _WINDOZ )
    WSADATA WSAData;
    WSAStartup(MAKEWORD(2,0), &WSAData);
#endif

    // msp paylaod
    payload = calloc(sizeof(*payload), sizeof(*payload));

    // mwi state
    mwiState = calloc(sizeof(*mwiState), sizeof(*mwiState));
    mwiState->callback = &callBack_mwi;

    // mavlink state
    mavlinkState = calloc(sizeof(*mavlinkState), sizeof(*mavlinkState));
    mavlinkState->mwiUavID = 1;
    mavlinkState->mwiAutoPilotType = MAV_AUTOPILOT_GENERIC;
    mavlinkState->mwiFlightMode = MAV_STATE_UNINIT; // initial mode is unknown
    mavlinkState->mwiAirFrametype = MAV_TYPE_GENERIC;
    mavlinkState->autoTelemtry = NOK;
    mavlinkState->baudrate = SERIAL_115200_BAUDRATE;
    mavlinkState->hertz = 30;
    strcpy(mavlinkState->targetIp, "127.0.0.1");
    strcpy(mavlinkState->serialDevice, "/dev/ttyO2");

    // some counters
    uint64_t lastFrameRequest = 0;
    uint64_t lastHeartBeat = 0;
    uint64_t lastReaquestLowPriority = 0;
    uint64_t currentTime = microsSinceEpoch();

    // create a configuration from command line
    if (config(mavlinkState, argc, argv) == NOK) {
        eexit(serialLink);
    }
    // translate argument to something mavlink knows about
    if (mavlinkState->mwiAutoPilotType == TYPE_PX4) {
        mavlinkState->mwiAutoPilotType = MAV_AUTOPILOT_PX4;
    }

    // create our socket
    sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
    sockFSin = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
    sockFSout = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);

    memset(&locAddr, 0, sizeof(locAddr));
    locAddr.sin_family = AF_INET;
    locAddr.sin_port = htons(14551);
    locAddr.sin_addr.s_addr = htonl(INADDR_LOOPBACK);

    memset(&locGSAddr, 0, sizeof(locGSAddr));
    locGSAddr.sin_family = AF_INET;
    locGSAddr.sin_addr.s_addr = htonl(INADDR_LOOPBACK);
    locGSAddr.sin_port = htons(14550); // TODO port number option

    // Bind the socket to port 14551 - necessary to receive packets from qgroundcontrol
    if (NOK != bind(sock, (struct sockaddr *)&locAddr, sizeof(struct sockaddr))) {
        perror("error bind to port 14551 failed");
        eexit(serialLink);
    }

    memset(&locFSAddr, 0, sizeof(locFSAddr));
    locFSAddr.sin_family = AF_INET;
    locFSAddr.sin_addr.s_addr = htonl(INADDR_LOOPBACK);
    locFSAddr.sin_port = htons(15500); // TODO port number option

    memset(&locAddr, 0, sizeof(locAddr));
    locAddr.sin_family = AF_INET;
    locAddr.sin_port = htons(15501);
    locAddr.sin_addr.s_addr = htonl(INADDR_LOOPBACK);

    // Bind the socket to port 15501 - necessary to receive packets from simulator
    if (NOK != bind(sockFSin, (struct sockaddr *)&locAddr, sizeof(struct sockaddr))) {
        perror("error bind to port 15501 failed");
        eexit(serialLink);
    }

    // Attempt to make it non blocking
#if defined(_WINDOZ)
    u_long argi = 1;
    if(ioctlsocket(sock, FIONBIO, &argi )<0) {
        fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno));
        eexit(serialLink);
    }

    argi = 1;
    if(ioctlsocket(sockFSin, FIONBIO, &argi )<0) {
        fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno));
        eexit(serialLink);
    }
    argi = 1;
    if(ioctlsocket(sockFSout, FIONBIO, &argi )<0) {
        fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno));
        eexit(serialLink);
    }
#define SOCKLEN_T_INT(fromlen) ((int*)fromlen)
    int opt = 1;
       if (setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)) < 0){
           printf("IMU setsockopt SO_REUSEADDR");
           eexit(serialLink);
       }
       opt = 1;
       if (setsockopt(sockFSin, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)) < 0){
           printf("IMU setsockopt SO_REUSEADDR");
           eexit(serialLink);
       }
       opt = 1;
       if (setsockopt(sockFSout, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)) < 0){
           printf("IMU setsockopt SO_REUSEADDR");
           eexit(serialLink);
       }
#else
    if (fcntl(sock, F_SETFL, O_NONBLOCK | FASYNC) < 0) {
        fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno));
        close(sock);
        eexit(serialLink);
    }
    if (fcntl(sockFSin, F_SETFL, O_NONBLOCK | FASYNC) < 0) {
        fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno));
        close(sockFSin);
        eexit(serialLink);
    }
    if (fcntl(sockFSout, F_SETFL, O_NONBLOCK | FASYNC) < 0) {
        fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno));
        close(sockFSout);
        eexit(serialLink);
    }
#define SOCKLEN_T_INT(fromlen) fromlen

    int opt = 1;
    if (setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)) < 0)
        err(1, "IMU setsockopt SO_REUSEADDR");
    opt = 1;
    if (setsockopt(sockFSin, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)) < 0)
        err(1, "IMU setsockopt SO_REUSEADDR");
    opt = 1;
    if (setsockopt(sockFSout, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)) < 0)
        err(1, "IMU setsockopt SO_REUSEADDR");
    // end socket
#endif
    // init serial
    serialLink = MWIserialbuffer_init(mavlinkState->serialDevice, mavlinkState->baudrate);
    if (serialLink == NOK) {
        perror("error opening serial port");
        eexit(serialLink);
    }

    MW_TRACE("starting..\n")
    for (;;) {

        currentTime = microsSinceEpoch();

        if (!mavlinkState->autoTelemtry && ((currentTime - lastFrameRequest) > (1000 * (1000 / (uint32_t)(mavlinkState->hertz))))) {
            lastFrameRequest = currentTime;
            if (mwiState->init == OK) {
                if ((currentTime - lastHeartBeat) > 1000 * 500) {
                    // ~ 2hz
                    lastHeartBeat = currentTime;
                    MWIserialbuffer_askForFrame(serialLink, MSP_IDENT, payload);
                    MWIserialbuffer_askForFrame(serialLink, MSP_STATUS, payload);
                }
                if ((currentTime - lastReaquestLowPriority) > 1000 * 90) {
                    // ~10hz
                    lastReaquestLowPriority = currentTime;
                    MWIserialbuffer_askForFrame(serialLink, MSP_ANALOG, payload);
                    MWIserialbuffer_askForFrame(serialLink, MSP_COMP_GPS, payload);
                    MWIserialbuffer_askForFrame(serialLink, MSP_RAW_GPS, payload);
                }
                // ~30 hz
                MWIserialbuffer_askForFrame(serialLink, MSP_ATTITUDE, payload);
                MWIserialbuffer_askForFrame(serialLink, MSP_RAW_IMU, payload);
                MWIserialbuffer_askForFrame(serialLink, MSP_ALTITUDE, payload);
                MWIserialbuffer_askForFrame(serialLink, MSP_RC, payload);
                MWIserialbuffer_askForFrame(serialLink, MSP_MOTOR, payload);
                MWIserialbuffer_askForFrame(serialLink, MSP_SERVO, payload);
                MWIserialbuffer_askForFrame(serialLink, MSP_DEBUG, payload);
            } else {
                // we need boxnames for ARM BOX, HORIZON , ANGLE , GPS ..
                MWIserialbuffer_askForFrame(serialLink, MSP_IDENT, payload);
                MWIserialbuffer_askForFrame(serialLink, MSP_BOXNAMES, payload);
                MWIserialbuffer_askForFrame(serialLink, MSP_RC_TUNING, payload);
                MWIserialbuffer_askForFrame(serialLink, MSP_PID, payload);
            }
            //TODO others if required
            //MSP_COMP_GPS
            //MSP_BAT

            // the ground station is sending rc data
            if (mavlinkState->rcdata.toSend == OK) {
                mavlinkState->rcdata.toSend = NOK;
                payload->length = 0;
                MWIserialbuffer_Payloadwrite16(payload, mavlinkState->rcdata.y);
                MWIserialbuffer_Payloadwrite16(payload, mavlinkState->rcdata.x);
                MWIserialbuffer_Payloadwrite16(payload, mavlinkState->rcdata.r);
                MWIserialbuffer_Payloadwrite16(payload, mavlinkState->rcdata.z);
                MWIserialbuffer_Payloadwrite16(payload, 1500);
                MWIserialbuffer_Payloadwrite16(payload, 1500);
                MWIserialbuffer_Payloadwrite16(payload, 1500);
                MWIserialbuffer_Payloadwrite16(payload, 1500);

                MWIserialbuffer_askForFrame(serialLink, MSP_SET_RAW_RC, payload);
            }

        }

        // fast rate
        annex();

        // we dont use thread ..
        usleep(1);
    }
}
/******************************************************************
* 	balance_core()
*	discrete-time balance controller operated off IMU interrupt
*	Called at SAMPLE_RATE_HZ
*******************************************************************/
int balance_core(){
	// local variables only in memory scope of balance_core
	static int D1_saturation_counter = 0; 
	float compensated_D1_output = 0;
	float dutyL = 0;
	float dutyR = 0;
	static log_entry_t new_log_entry;
	float output_scale; //battery voltage/nominal voltage
	
	// if an IMU packet read failed, ignore and just return
	// the mpu9150_read function may print it's own warnings
	if (mpu9150_read(&mpu) != 0){
		return -1;
	}
	
	/**************************************************************
	*	STATE_ESTIMATION
	*	read sensors and compute the state regardless of if the 
	*	controller is ARMED or DISARMED
	***************************************************************/
	// angle theta is positive in the direction of forward tip
	// add mounting angle of BBB
	cstate.theta[2] = cstate.theta[1]; cstate.theta[1] = cstate.theta[0];
	cstate.theta[0] = mpu.fusedEuler[VEC3_X] + config.bb_mount_angle; 
	cstate.current_theta = cstate.theta[0];
	cstate.d_theta = (cstate.theta[0]-cstate.theta[1])/DT;
	
	// collect encoder positions
	cstate.wheelAngleR = -(get_encoder_pos(config.encoder_channel_R) * 2*PI) \
							/(config.gearbox * config.encoder_res);
	cstate.wheelAngleL = (get_encoder_pos(config.encoder_channel_L) * 2*PI)	\
							/(config.gearbox * config.encoder_res);
	
	// log phi estimate
	// wheel angle is relative to body, 
	// add theta body angle to get absolute wheel position
	cstate.phi[2] = cstate.phi[1]; cstate.phi[1] = cstate.phi[0];
	cstate.phi[0] = ((cstate.wheelAngleL + cstate.wheelAngleR)/2) +cstate.current_theta; 
	cstate.current_phi = cstate.phi[0];
	cstate.d_phi = (cstate.phi[0]-cstate.phi[1])/DT;
	
	// body turning estimation
	cstate.gamma[2] = cstate.gamma[1]; cstate.gamma[1] = cstate.phi[0];
	cstate.gamma[0]=(cstate.wheelAngleL-cstate.wheelAngleR) \
				* (config.wheel_radius/config.track_width);
	cstate.d_gamma = (cstate.gamma[0]-cstate.gamma[1])/DT;
	cstate.current_gamma = cstate.gamma[0];
	
	// output scaling
	output_scale =  cstate.vBatt/config.v_nominal;
	
	/*************************************************************
	*	Control based on the robotics_library defined state variable
	*	PAUSED: make sure the controller stays DISARMED
	*	RUNNING: Normal operation of controller.
	*		- check for tipover
	*		- wait for MiP to be within config.start_angle of 
	*			upright
	*		- choose mode from setpoint.core_mode
	*		- evaluate difference equation and check saturation
	*		- actuate motors
	***************************************************************/
	switch (get_state()){
	
	// make sure things are off if program is closing
	case EXITING:
		disable_motors();
		return 0;
	
	// if the controller is somehow still ARMED, disarm it
	case PAUSED:
		if(setpoint.arm_state==ARMED){
			disarm_controller();
		}
		break;
	
	// normal operating mode
	case RUNNING:
		// exit if the controller was not armed properly
		if(setpoint.arm_state==DISARMED){
			return 0;
		}
		
		// check for a tipover before anything else
		if(fabs(cstate.current_theta)>config.tip_angle){
			disarm_controller();
			printf("tip detected \n");
			break;
		}
		
		/**********************************************************
		*	POSITION Phi controller
		*	feedback control of wheel angle Phi by outputting a 
		*	reference theta body angle. This is controller D2 in 
		*	config
		***********************************************************/
		if(setpoint.core_mode == POSITION){
			
			// move the position set points based on user input
			if(setpoint.phi_dot != 0.0){
				//setpoint.phi == cstate.current_phi + setpoint.phi_dot*DT;
				setpoint.phi += setpoint.phi_dot * DT;
			}
			
			
			// march the different equation terms for the input Phi Error
			// and the output theta reference angle
			cstate.ePhi[2] = cstate.ePhi[1]; 
			cstate.ePhi[1] = cstate.ePhi[0];
			cstate.ePhi[0] = setpoint.phi-cstate.current_phi;
	
			cstate.theta_ref[2] = cstate.theta_ref[1];
			cstate.theta_ref[1] = cstate.theta_ref[0];
			
			// evaluate D2 difference equation
			cstate.theta_ref[0] = config.K_D2*(						\
								config.numD2_2 * cstate.ePhi[2] 	\
							+ config.numD2_1 * cstate.ePhi[1] 		\
							+ config.numD2_0 * cstate.ePhi[0])		\
							-(config.denD2_2 * cstate.theta_ref[2] 	\
							+ config.denD2_1 * cstate.theta_ref[1]);
						
			//check saturation of outer loop theta reference output signal
			saturate_float(&cstate.theta_ref[0],-config.theta_ref_max,config.theta_ref_max);
			setpoint.theta = cstate.theta_ref[0];
		}
		
		// evaluate inner loop controller D1z
		cstate.eTheta[2] = cstate.eTheta[1]; 
		cstate.eTheta[1] = cstate.eTheta[0];
		cstate.eTheta[0] = setpoint.theta - cstate.current_theta;
		cstate.u[2] = cstate.u[1];
		cstate.u[1] = cstate.u[0];
		cstate.u[0] = \
				config.K_D1 * (config.numD1_0 * cstate.eTheta[0]	\
								+	config.numD1_1 * cstate.eTheta[1] 	\
								+	config.numD1_2 * cstate.eTheta[2])	\
								-  (config.denD1_1 * cstate.u[1] 		\
								+	config.denD1_2 * cstate.u[2]); 		
		
		// check saturation of inner loop knowing that right after
		// this control will be scaled by battery voltage
		if(saturate_float(&cstate.u[0], -output_scale, output_scale)){
			D1_saturation_counter ++;
			if(D1_saturation_counter > SAMPLE_RATE_HZ*config.pickup_detection_time){
				printf("inner loop controller saturated\n");
				disarm_controller();
				D1_saturation_counter = 0;
				break;
			}
		}
		else{
			D1_saturation_counter = 0;
		}
		cstate.current_u = cstate.u[0];
		
		// scale output to compensate for battery charge level
		compensated_D1_output = cstate.u[0] / output_scale;
		
		// // integrate the reference theta to correct for imbalance or sensor
		// // only if standing relatively still with zero phi reference
		// // to-do, wait for stillness for a time period before integrating
		// if(setpoint.phi==0 && fabs(cstate.phi_dot)<2){
				// state.thetaTrim += (config.kTrim*cstate.theta_ref[0]) * DT;
		// }
		
		//steering controller
		// move the controller set points based on user input
		setpoint.gamma += setpoint.gamma_dot * DT;
		cstate.egamma[1] = cstate.egamma[0];
		cstate.egamma[0] = setpoint.gamma - cstate.current_gamma;
		cstate.duty_split = config.KP_steer*(cstate.egamma[0]	\
				+config.KD_steer*(cstate.egamma[0]-cstate.egamma[1]));
		
		// if the steering input would saturate a motor, reduce
		// the steering input to prevent compromising balance input
		if(fabs(compensated_D1_output)+fabs(cstate.duty_split) > 1){
			if(cstate.duty_split > 0){
				cstate.duty_split = 1-fabs(compensated_D1_output);
			}
			else cstate.duty_split = -(1-fabs(compensated_D1_output));
		}	
		
		// add D1 balance controller and steering control
		dutyL  = compensated_D1_output - cstate.duty_split;
		dutyR = compensated_D1_output + cstate.duty_split;	
		
		// send to motors
		// one motor is flipped on chassis so reverse duty to L
		set_motor(config.motor_channel_L,-dutyL); 
		set_motor(config.motor_channel_R,dutyR); 
		cstate.time_us = microsSinceEpoch();
		
		// pass new information to the log with add_to_buffer
		// this only puts information in memory, doesn't
		// write to disk immediately
		if(config.enable_logging){
			new_log_entry.time_us	= cstate.time_us-core_start_time_us;
			new_log_entry.theta		= cstate.current_theta;
			new_log_entry.theta_ref	= setpoint.theta;
			new_log_entry.phi 		= cstate.current_phi; 
			new_log_entry.u 		= cstate.current_u;
			add_to_buffer(new_log_entry);
		}
		
		// end of normal balancing routine
		// last_state will be updated beginning of next interrupt
		break;
		
		default:
			break; // nothing to do if UNINITIALIZED
	}
	return 0;
}
/******************************************************************
*	main()
*	initialize the IMU, start all the threads, and wait still
*	something triggers a shut down
*******************************************************************/
int main(){
	initialize_cape();
	set_led(RED,HIGH);
	set_led(GREEN,LOW);
	set_state(UNINITIALIZED);
	
	// set up button handlers first
	// so user can exit by holding pause
	set_pause_pressed_func(&on_pause_press);
	set_mode_unpressed_func(&on_mode_release);
	
	// load data from disk.
	if(load_config(&config)==-1){
		printf("aborting, config file error\n");
		return -1;
	}


	
	// start a thread to slowly sample battery 
	pthread_t  battery_thread;
	pthread_create(&battery_thread, NULL, battery_checker, (void*) NULL);
	
	// start printf_thread if running from a terminal
	// if it was started as a background process then don't bother
	if(isatty(fileno(stdout))){
		pthread_t  printf_thread;
		pthread_create(&printf_thread, NULL, printf_loop, (void*) NULL);
	}
	
	// start listening for RC control from dsm2 radio
	if(config.enable_dsm2){
		initialize_dsm2();
		pthread_t  dsm2_thread;
		pthread_create(&dsm2_thread, NULL, dsm2_listener, (void*) NULL);
	}
	
	// start mavlink if enabled
	if(config.enable_mavlink_listening || config.enable_mavlink_listening){
		char target_ip[16];
		strcpy(target_ip, DEFAULT_MAV_ADDRESS);
		// open a udp port for mavlink
		// sock and gcAddr are global variables needed to send and receive
		gcAddr = initialize_mavlink_udp(target_ip, udp_sock);
		
		if(udp_sock != NULL){ 
			printf("WARNING: continuing without mavlink enabled\n");
		}
		else {
			if(config.enable_mavlink_listening){
				// start a thread listening for incoming packets
				pthread_t  mav_listen_thread;
				pthread_create(&mav_listen_thread, NULL, mavlink_listener, (void*) NULL);
				printf("Listening for Packets\n");
			}
			if(config.enable_mavlink_transmitting){
				// Start thread sending heartbeat and IMU attitude packets
				pthread_t  mav_send_thread;
				pthread_create(&mav_send_thread, NULL, mavlink_sender, (void*) NULL);
				printf("Transmitting Heartbeat Packets\n");
			}
		}
	}
	
	// start logging thread if enabled
	if(config.enable_logging){
		if(start_log(SAMPLE_RATE_HZ, &cstate.time_us)<0){
			printf("failed to start log\n");
		}
		else{
			// start new thread to write the file occationally
			pthread_t  logging_thread;
			pthread_create(&logging_thread, NULL, log_writer, (void*) NULL);
		}
	}
	
	// Finally start the real-time interrupt driven control thread
	// start IMU with equilibrium set with upright orientation 
	// for MiP with Ethernet pointing relatively up
	signed char orientation[9] = ORIENTATION_UPRIGHT; 
	if(initialize_imu(SAMPLE_RATE_HZ, orientation)){
		// can't talk to IMU, all hope is lost
		// blink red until the user exits
		blink_red();
		return -1;
	}
	// this should be the last step in initialization 
	// to make sure other setup functions don't interfere
	printf("starting core IMU interrupt\n");
	core_start_time_us = microsSinceEpoch();
	set_imu_interrupt_func(&balance_core);
	
	// start balance stack to control setpoints
	pthread_t  balance_stack_thread;
	pthread_create(&balance_stack_thread, NULL, balance_stack, (void*) NULL);
	
	printf("\nHold your MIP upright to begin balancing\n");
	set_state(RUNNING);
	
	// chill until something exits the program
	while(get_state()!=EXITING){
		usleep(100000);
	}
	
	// close(*udp_sock); 	// close network socket
	cleanup_cape(); // always end with cleanup to shut down cleanly
	return 0;
}
Exemple #13
0
int main(int argc, char* argv[])
{	
	// Initialize MAVLink
	mavlink_wpm_init(&wpm);
	mavlink_system.sysid = 5;
	mavlink_system.compid = 20;
	mavlink_pm_reset_params(&pm);
	
	int32_t ground_distance;
	uint32_t time_ms;
	
	
	
	// Create socket
	sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
	
	// 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]);
    }
	
	
	memset(&locAddr, 0, sizeof(locAddr));
	locAddr.sin_family = AF_INET;
	locAddr.sin_addr.s_addr = INADDR_ANY;
	locAddr.sin_port = htons(14551);
	
	/* 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(14550);
	
	
	printf("MAVLINK MISSION LIBRARY EXAMPLE PROCESS INITIALIZATION DONE, RUNNING..\n");
	
	
	for (;;) 
    {
		bytes_sent = 0;
		
		/* Send Heartbeat */
		mavlink_msg_heartbeat_pack(mavlink_system.sysid, 200, &msg, MAV_TYPE_HELICOPTER, MAV_AUTOPILOT_GENERIC, 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 Status */
		mavlink_msg_sys_status_pack(1, 200, &msg, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE, 500, 7500, 0, 0, 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(mavlink_system.sysid, 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 global position */
		if (hilEnabled)
		{
			mavlink_msg_global_position_int_pack(mavlink_system.sysid, 200, &msg, time_ms, latitude, longitude, altitude, ground_distance, speedx, speedy, speedz, (yaw/M_PI)*180*100);
			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(mavlink_system.sysid, 200, &msg, microsSinceEpoch(), roll, pitch, yaw, rollspeed, pitchspeed, yawspeed);
		len = mavlink_msg_to_send_buffer(buf, &msg);
		bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
		
		/* Send HIL outputs */
		float roll_ailerons = 0;   // -1 .. 1
		float pitch_elevator = 0.2;  // -1 .. 1
		float yaw_rudder = 0.1;      // -1 .. 1
		float throttle = 0.9;      //  0 .. 1
		mavlink_msg_hil_controls_pack_chan(mavlink_system.sysid, mavlink_system.compid, MAVLINK_COMM_0, &msg, microsSinceEpoch(), roll_ailerons, pitch_elevator, yaw_rudder, throttle, mavlink_system.mode, mavlink_system.nav_mode, 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));
		
		memset(buf, 0, BUFFER_LENGTH);
		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;
			
			printf("Bytes Received: %d\nDatagram: ", (int)recsize);
			for (i = 0; i < recsize; ++i)
			{
				temp = buf[i];
				printf("%02x ", (unsigned char)temp);
				if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status))
				{
					// Packet received
					printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid);
					
					// Handle packet with waypoint component
					mavlink_wpm_message_handler(&msg);
					
					// Handle packet with parameter component
					mavlink_pm_message_handler(MAVLINK_COMM_0, &msg);
					
					// Print HIL values sent to system
					if (msg.msgid == MAVLINK_MSG_ID_HIL_STATE)
					{
						mavlink_hil_state_t hil;
						mavlink_msg_hil_state_decode(&msg, &hil);
						printf("Received HIL state:\n");
						printf("R: %f P: %f Y: %f\n", hil.roll, hil.pitch, hil.yaw);
						roll = hil.roll;
						pitch = hil.pitch;
						yaw = hil.yaw;
						rollspeed = hil.rollspeed;
						pitchspeed = hil.pitchspeed;
						yawspeed = hil.yawspeed;
						speedx = hil.vx;
						speedy = hil.vy;
						speedz = hil.vz;
						latitude = hil.lat;
						longitude = hil.lon;
						altitude = hil.alt;
						hilEnabled = true;
					}
				}
			}
			printf("\n");
		}
		memset(buf, 0, BUFFER_LENGTH);
		usleep(10000); // Sleep 10 ms
		
		
		// Send one parameter
		mavlink_pm_queued_send();
    }
}
Exemple #14
0
int main(int argc, char* argv[])
{	
	// Initialize MAVLink
	mavlink_wpm_init(&wpm);
	mavlink_system.sysid = 1;
	mavlink_system.compid = MAV_COMP_ID_WAYPOINTPLANNER;
	
	
	
	// Create socket
	sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
	
	// 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]);
    }
	
	
	memset(&locAddr, 0, sizeof(locAddr));
	locAddr.sin_family = AF_INET;
	locAddr.sin_addr.s_addr = INADDR_ANY;
	locAddr.sin_port = htons(14551);
	
	/* 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(14550);
	
	
	printf("MAVLINK MISSION LIBRARY EXAMPLE PROCESS INITIALIZATION DONE, RUNNING..\n");
	
	
	for (;;) 
    {
		
		/*Send Heartbeat */
		mavlink_msg_heartbeat_pack(mavlink_system.sysid, 200, &msg, MAV_HELICOPTER, MAV_CLASS_GENERIC);
		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, MAV_MODE_GUIDED, MAV_NAV_HOLD, MAV_STATE_ACTIVE, 500, 7500, 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_pack(mavlink_system.sysid, 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(mavlink_system.sysid, 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);
		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;
			
			printf("Bytes Received: %d\nDatagram: ", (int)recsize);
			for (i = 0; i < recsize; ++i)
			{
				temp = buf[i];
				printf("%02x ", (unsigned char)temp);
				if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status))
				{
					// Packet received
					printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid);
					
					// Handle packet with waypoint component
					mavlink_wpm_message_handler(&msg);
					
					// Handle packet with parameter component
				}
			}
			printf("\n");
		}
		memset(buf, 0, BUFFER_LENGTH);
		usleep(50000); // Sleep one second
    }
}