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;
}
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;
}
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;
}
