コード例 #1
0
ファイル: main_fly.c プロジェクト: djtrance/ardrone
int main()
{
  printf("'fly' version 1.00 - Copyright (C) 2011 Hugo Perquin - http://blog.perquin.com\n");
  //wait for udp packet on port 7777
  struct udp_struct udpCmd;
  udpServer_Init(&udpCmd,7777,1/*blocking*/);
  char buf[1025];
  printf("Waiting for UDP wakeup on port 7777\n");
  int bufcnt=udpServer_Receive(&udpCmd, buf, 1024);
  if(bufcnt<=0) return 1;
  buf[bufcnt]=0;
  printf("UDP wakeup received from %s\n",inet_ntoa(udpCmd.si_other.sin_addr));

  //kill program.elf
  int rc = system("/usr/bin/killall program.elf > /dev/null 2>&1");
  printf("killall program.elf -> returncode=%d  (0=killed,256=not found)\n",rc);	
  
  //init controller
  ctl_Init(inet_ntoa(udpCmd.si_other.sin_addr));
  printf("ctl_Init completed\n");

  //main loop	
  while(1) { 
    //wait for next packet on cmd port
    bufcnt=udpServer_Receive(&udpCmd, buf, 1024);
    if(bufcnt<=0) continue;
    printf("Received a packet of %d bytes\n",bufcnt);
    buf[bufcnt]=0;
    
    //split tokens
    int i=0;
    char delims[] = ",";
    char *result = NULL;
    result = strtok( buf, delims );
    printf("Command token is %s\n",result);
    if(strcmp(result,"s")) continue;
    result = strtok( NULL, delims );
    float val[4];
    while( i<4 && result != NULL ) {
      val[i]=atof(result);
      //printf( "->token%d is \"%s\" %f\n", i, result, val[i] );
      result = strtok( NULL, delims );
      i++;
    }
    if(i==4) {
      printf("set:%f,%f,%f,%f\n", val[0],val[1],val[2],val[3] );
      ctl_SetSetpoint(val[0],val[1],val[2],val[3]);
    } else {
      printf("Unable to parse: %s\n",buf);
    }

  }
  ctl_Close();
  printf("\nDone...\n");
  return 0;

}
コード例 #2
0
ファイル: main_udpserver.c プロジェクト: djtrance/ardrone
int main(void)
{
  struct udp_struct udp;

  int msglen;
  char buf[512];
  
  if(udpServer_Init(&udp,9930,1)) diep("udpServer_Init");
  
  for (int i=0; i<10; i++) {
    int cnt=0;
    do {
      msglen = udpServer_Receive(&udp, buf, 512);
      cnt++;
    } while(msglen<=0);
    printf("Received at cnt=%d -> '%s'\n\n", cnt, buf);
  }
  
  udpClient_Close(&udp);
  return 0;
}
コード例 #3
0
int main(int argc, char **argv)
{
  udp_struct udp;
  ardrone_islab::Navdataislab navdata;

  int msglen;
  char buf[512];
  char * pch;
  ardrone_islab::Navdataislab navdataformat;
  ros::init(argc, argv, "ardrone_ground_node_server");
  ros::NodeHandle n;
  ros::Rate loop_rate(300);
  
  ros::Publisher navdata_pub = n.advertise<ardrone_islab::Navdataislab>("ardrone/navdata",1000);
  
  printf("check1");
  // Broadcast a simple log message
  ROS_INFO_STREAM("Start ROS node!");
  
  if(udpServer_Init(&udp,9930,0)) diep("udpServer_Init");
  printf("check2");
  while (ros::ok())
  {

	int cnt=0;
   	do {
      		msglen = udpServer_Receive(&udp, buf, 512);
      		cnt++;
    	} while(msglen<=0);
 
        pch = strtok (buf," ,");
        int countpart = 0;
	
	//printf("%d",len(buf));
  	while (countpart<=42)
  	{
 		if (countpart == 0) navdata.tm = atof(pch);
		if (countpart == 1) navdata.tm_pre = atof(pch);
		if (countpart == 2) navdata.frame = atof(pch);
  		if (countpart == 3) navdata.pitch_w = atof(pch);
  		if (countpart == 4) navdata.pitch_a = atof(pch);
  		if (countpart == 5) navdata.pitch = atof(pch);
  		if (countpart == 6) navdata.roll_w = atof(pch);
  		if (countpart == 7) navdata.roll_a = atof(pch);
  		if (countpart == 8) navdata.roll = atof(pch);
  		if (countpart == 9) navdata.yaw_w = atof(pch);
  		if (countpart == 10) navdata.yaw_m = atof(pch);
  		if (countpart == 11) navdata.yaw = atof(pch);
  		if (countpart == 12) navdata.dt = atof(pch);
		if (countpart == 13) navdata.dt2 = atof(pch);
  		if (countpart == 14) navdata.q_est[0] = atof(pch);
  		if (countpart == 15) navdata.q_est[1] = atof(pch);
  		if (countpart == 16) navdata.q_est[2] = atof(pch);
  		if (countpart == 17) navdata.q_est[3] = atof(pch);
  		if (countpart == 18) navdata.b_est[0] = atof(pch);
  		if (countpart == 19) navdata.b_est[1] = atof(pch);
  		if (countpart == 20) navdata.b_est[2] = atof(pch);

  		if (countpart == 21) navdata.ts = atof(pch);
  		if (countpart == 22) navdata.hraw = atof(pch);
  		if (countpart == 23) navdata.h_meas = atof(pch);
  		if (countpart == 24) navdata.ax = atof(pch);         
  		if (countpart == 25) navdata.ay = atof(pch);               
  		if (countpart == 26) navdata.az = atof(pch);  
  		if (countpart == 27) navdata.wx = atof(pch);        
  		if (countpart == 28) navdata.wy = atof(pch);                  
  		if (countpart == 29) navdata.wz = atof(pch);
  		if (countpart == 30) navdata.magX = atof(pch);
  		if (countpart == 31) navdata.magY = atof(pch);
  		if (countpart == 32) navdata.magZ = atof(pch);
  		if (countpart == 33) navdata.pressure = atof(pch);
  		if (countpart == 34) navdata.rotX = atof(pch);
  		if (countpart == 35) navdata.rotY = atof(pch);
  		if (countpart == 36) navdata.rotZ = atof(pch);
  		if (countpart == 37) navdata.altd = atof(pch);
	        if (countpart == 38) navdata.motor1 = atof(pch);
		if (countpart == 39) navdata.motor2 = atof(pch);
		if (countpart == 40) navdata.motor3 = atof(pch);
		if (countpart == 41) navdata.motor4 = atof(pch);
		countpart ++;
    		pch = strtok (NULL, " ,");
  	}
	
	//printf(" %.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%f,%f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f ", navdata.tm,navdata.pitch_w,navdata.pitch_a,navdata.pitch,navdata.roll_w,navdata.roll_a,navdata.roll,navdata.yaw_w,navdata.yaw_m,navdata.yaw,  navdata.dt,navdata.ts,navdata.hraw,navdata.h_meas,navdata.ax,navdata.ay,navdata.az,navdata.wx,navdata.wy,navdata.wz,navdata.magX,navdata.magY,navdata.magZ,navdata.pressure,navdata.rotX,navdata.rotY,navdata.rotZ,navdata.altd,navdata.motor1,navdata.motor2,navdata.motor3,navdata.motor4);
 	navdata.header.stamp = ros::Time::now(); 	
 	navdata_pub.publish(navdata);	
        //printf(" %.3f", navdata.tm);
   	ros::spinOnce();
  	loop_rate.sleep();        
  }

  udpClient_Close(&udp);
  return 0;
}