int show_help()
{
printf("Choose unit test: \n");
uint test_cnt = sizeof(g_tests)/sizeof(struct unit_test_desc);
for (uint i = 0; i < test_cnt; i++) {
printf("%d- %s (%s)\n", i, g_tests[i].desc, g_tests[i].name);
}
printf("q- quit\n");
char r = util_getch();
if (r == 'q') {
return -1;
} else if (r >= '0' && r <= '9') {
char rs[2];
rs[0] = (char)r;
rs[1] = 0;
return atoi(rs);
} else {
return -1;
}
}
// Handle user input
void checkKeypress(){
int c=tolower(util_getch());
if(c=='q') {
stopLoop = true;
}
if(c=='n') { // Ignore navdata
printf("Ignoring Navdata\n");
ignore_navdata = 1;
}
if(c=='1') {
printf("\rCounterclockwise pulse\n");
pulseCCW(pulseDuration);
}
if(c=='2') {
printf("\rClockwise pulse\n");
pulseCW(pulseDuration);
}
if(c=='3') {
smallPulseCCW(.1);
}
if(c=='4') {
smallPulseCW(.1);
}
if(c=='5') {
printf("\rRun All Motors 50% ");
throttle1 = .01;
throttle2 = .01;
throttle3 = .01;
throttle4 = .01;
mot_Run(throttle1,throttle2,throttle3,throttle4);
}
if(c==',') {
// printf("\rThrottle down ");
// if(throttle1>step) throttle1 -= step;
// if(throttle2>step) throttle2 -= step;
// if(throttle3>step) throttle3 -= step;
// if(throttle4>step) throttle4 -= step;
// mot_Run(throttle1,throttle2,throttle3,throttle4);
angleGlobal--;
printf("a:%i\n",angleGlobal);
}
if(c=='j') {
printf("\rClockwise ");
if(throttle1==.40) throttle1 = .80; else throttle1 = .40;
if(throttle3==.25) throttle3 = .50; else throttle3 = .25;
mot_Run(throttle1,throttle2,throttle3,throttle4);
}
if(c=='k') {
printf("\rCounterclockwise ");
if(throttle2==.25) throttle2 = .50; else throttle2 = .25;
if(throttle4==.25) throttle4 = .50; else throttle4 = .25;
mot_Run(throttle1,throttle2,throttle3,throttle4);
}
if(c=='.') {
// printf("\rThrottle up %f ",throttle2+step);
// if(throttle1>0) throttle1 += step;
// if(throttle2>0) throttle2 += step;
// if(throttle3>0) throttle3 += step;
// if(throttle4>0) throttle4 += step;
// mot_Run(throttle1,throttle2,throttle3,throttle4);
angleGlobal++;
printf("a:%i\n",angleGlobal);
}
if(c==' ') {
printf("\rStop ");
mot_Stop();
}
if(c=='a') {
printf("\rLeds off ");
mot_SetLeds(MOT_LEDOFF,MOT_LEDOFF,MOT_LEDOFF,MOT_LEDOFF);
}
if(c=='s') {
waitToStart = false;
//printf("\rLeds green ");
//mot_SetLeds(MOT_LEDGREEN,MOT_LEDGREEN,MOT_LEDGREEN,MOT_LEDGREEN);
}
if(c=='d') {
printf("\rLeds orange ");
mot_SetLeds(MOT_LEDORANGE,MOT_LEDORANGE,MOT_LEDORANGE,MOT_LEDORANGE);
}
if(c=='f') {
printf("\rLeds red ");
mot_SetLeds(MOT_LEDRED,MOT_LEDRED,MOT_LEDRED,MOT_LEDRED);
}
if(c=='s') {
waitToStart = false;
//printf("\rLeds green ");
//mot_SetLeds(MOT_LEDGREEN,MOT_LEDGREEN,MOT_LEDGREEN,MOT_LEDGREEN);
}
if(c=='p') {
waitToStart = true;
}
}
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;
}