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;
}
void ctl_SetSetpointDiff(float pitch, float roll, float yaw, float h)
{
ctl_SetSetpoint(pitch+setpoint.pitch, setpoint.pitch+pitch, yaw+setpoint.yaw, h+setpoint.h);
}
int main()
{
//wait for udp packet on port 7777
//udp_struct udpCmd;
// udpServer_Init(&udpCmd,7777,1/*blocking*/);
// char buf[1024];
float roll = 0;
float pitch = 0;
float yaw = 0;
float height = 0;
struct object_detect_struct od;
int rc = system("(../../bin/program.elf ${PELF_ARGS}; gpio 181 -d ho 1) &");
sleep(7);
printf("Return code from program.elf = %i\n", rc);
//kill program.elf
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
while(ctl_Init(NULL))
{
printf("Flat trim failed. Retry in 2 seconds...\n");
sleep(2);
}
printf("ctl_Init completed\n");
//rc = object_detect_init(&od);
//if (rc)
// return rc;
//height = 0.5;
ctl_SetSetpoint(roll,pitch,yaw,height);
sleep(6);
int counter = 0;
int keyPress = -1;
printf("Starting...\n");
//main loop
while(keyPress==-1)
{
keyPress = util_getch();
//object_detect_getSample(&od);
//yaw -= (float)od.locX/(4*1700);
height += (float)od.locY/(4*2000);
//printf("roll: %f pitch: %f yaw: %f height: %f\n",roll,pitch,yaw,height);
//ctl_SetSetpoint(roll,pitch,yaw,height);
}
//printf("Framerate: %f frames per second\n", (double)numFrames/(util_timestamp()-timeStart));
//printf("Landing...\n");
ctl_SetSetpoint(roll,pitch,yaw,0);
sleep(6);
//object_detect_close();
ctl_Close();
printf("\nDone...\n");
return 0;
}