void pulseCW(float t){
//mot_Run(.01,0,.01,0);
if(cWSwitcher) mot_Run(0,0,.01,0);
else mot_Run(.01,0,0,0);
cWSwitcher = !cWSwitcher;
usleep(t*1000000);
mot_Run(0,0,0,0);
}
// Small clockwise motor pulse
void smallPulseCW(float t){ //starts with a pulse, then lowers speed
float min_duration = 1;
float max_duration = 1.5;
// Lower bound the pulse time
if (t < 0.8) t = min_duration;
//mot_Run(.01,0,.01,0);
if(cWSwitcher) mot_Run(0,0,.01,0);
else mot_Run(.01,0,0,0);
cWSwitcher = !cWSwitcher;
if( t < max_duration) usleep(t*1000000);
else usleep(max_duration*1000000);
mot_Run(0,0,0,0);
//mot_Run(.01,0,.01,0);
}
// Small counterclockwise motor pulse
void smallPulseCCW(float t){
float min_duration = 1;
float max_duration = 1.5;
// Lower bound the pulse time
if (t < 0.8) t = min_duration;
//mot_Run(0,.01,0,.01);
if(cCWSwitcher) mot_Run(0,0,0,0.01);
else mot_Run(0,0.01,0,0);
cCWSwitcher = !cCWSwitcher;
if( t < max_duration) usleep(t*1000000);
else usleep(max_duration*1000000);
mot_Run(0,0,0,0);
//mot_Run(0,.01,0,.01);
}
// Use navigation data to set motors
void checkNavdata (nav_struct * nav)
{
// Get navdata sample
int rc = nav_GetSample(nav);
if(rc) {
printf("ERROR: nav_GetSample return code=%d\n",rc);
}
// Print Navdata
nav_Print(nav);
// Shut off motors if z-axis accelerometer is outside thresholds
if (nav->az >= 0.3 && ignore_navdata==0) {
throttle1 = 0.01;
throttle2 = 0.01;
throttle3 = 0.01;
throttle4 = 0.01;
mot_Run(throttle1,throttle2,throttle3,throttle4);
}
}
// 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;
}
}
// Counterclockwise
void turnOnCCW(){
mot_Run(0,.01,0,.01);
}
// Clockwise
void turnOnCW(){
mot_Run(.01,0,.01,0);
}
// Pulse motors such that drone moves counterclockwise
void pulseCCW(float t){
mot_Run(0,.01,0,.01);
usleep(t*1000000);
mot_Run(0,0,0,0);
}
void *ctl_thread_main(void* data)
{
int cnt;
int rc;
while(1) {
rc = att_GetSample(&att);
if(!rc) break;
if(rc!=1) printf("ctl_thread_main: att_GetSample return code=%d",rc);
}
while(1) {
//get sample
while(1) {
rc = att_GetSample(&att); //non blocking call
if(!rc) break; //got a sample
if(rc!=1) printf("ctl_thread_main: att_GetSample return code=%d",rc);
}
// video_GrabImage(&vid, img_new);
//process
// video_blocksum(img_old, img_new, &dx, &dy);
// x+=dx;
// y+=dy;
float motor[4];
if(setpoint.h==0.00) {
//motors off
adj_roll = 0;
adj_pitch = 0;
adj_h = 0;
adj_x = 0;
adj_y = 0;
adj_yaw = 0;
throttle = 0;
}else{
//flying, calc pid controller corrections
adj_roll = pid_CalcD(&pid_roll, setpoint.roll - att.roll, att.dt, att.gx); //err positive = need to roll right
adj_pitch = pid_CalcD(&pid_pitch, setpoint.pitch - att.pitch, att.dt, att.gy); //err positive = need to pitch down
adj_yaw = pid_CalcD(&pid_yaw, setpoint.yaw - att.yaw, att.dt, att.gz); //err positive = need to increase yaw to the left
adj_h = pid_CalcD(&pid_h, setpoint.h - att.h, att.dt, att.hv); //err positive = need to increase height
// adj_x = pid_Calc(&pid_x, 0 - x, att.dt);
// adj_y = pid_Calc(&pid_y, 0 - y, att.dt);
throttle = setpoint.throttle_hover + adj_h;
if(throttle < setpoint.throttle_min) throttle = setpoint.throttle_min;
if(throttle > setpoint.throttle_max) throttle = setpoint.throttle_max;
}
// printf("%f %f -- %f %f\n",x,y, adj_x, adj_y);
// adj_pitch += adj_y;
// adj_roll += adj_x;
//convert pid adjustments to motor values
motor[0] = throttle +adj_roll -adj_pitch +adj_yaw;
motor[1] = throttle -adj_roll -adj_pitch -adj_yaw;
motor[2] = throttle -adj_roll +adj_pitch +adj_yaw;
motor[3] = throttle +adj_roll +adj_pitch -adj_yaw;
//send to motors
mot_Run(motor[0],motor[1],motor[2],motor[3]);
//blink leds
cnt++;
if((cnt%200)==0)
mot_SetLeds(MOT_LEDGREEN,MOT_LEDGREEN,MOT_LEDGREEN,MOT_LEDGREEN);
else if((cnt%200)==100)
mot_SetLeds(0,0,0,0);
//send UDP nav log packet
//navLog_Send();
//yield to other threads
pthread_yield();
}
}
void *ctl_thread_main(void* data)
{
int cnt;
int rc;
//printf("run pthread");
//navdata_update(&ahrsdata); //get data from thread
while(1) {
// get navdata
navdata_update(&ahrsdata); //get data from thread
float motor[4];
if(setpoint.h<=0.02) {
//motors off
adj_roll = 0;
adj_pitch = 0;
adj_h = 0;
adj_yaw = 0;
throttle = 0;
}else{
//attitude pid controller
adj_roll = pid_Calc(&pid_roll, setpoint.roll - ahrsdata.roll, ahrsdata.dt);
adj_pitch = pid_Calc(&pid_pitch, setpoint.pitch - ahrsdata.pitch, ahrsdata.dt);
adj_yaw = setpoint.yaw;
throttle = setpoint.h;
if(throttle < setpoint.throttle_min) throttle = setpoint.throttle_min;
if(throttle > setpoint.throttle_max) throttle = setpoint.throttle_max;
}
// Convert pid adjustments to motor values
motor[0] = throttle +adj_roll -adj_pitch +adj_yaw;
motor[1] = throttle -adj_roll -adj_pitch -adj_yaw;
motor[2] = throttle -adj_roll +adj_pitch +adj_yaw;
motor[3] = throttle +adj_roll +adj_pitch -adj_yaw;
float motor_min = 0.125;
if(motor[0] < motor_min) motor[0] = motor_min;
if(motor[1] < motor_min) motor[1] = motor_min;
if(motor[2] < motor_min) motor[2] = motor_min;
if(motor[3] < motor_min) motor[3] = motor_min;
ahrsdata.motor1 = motor[0];
ahrsdata.motor2 = motor[1];
ahrsdata.motor3 = motor[2];
ahrsdata.motor4 = motor[3];
//printf("motor: %.5f,%.5f,%.5f,%.5f\n",motor[0],motor[1],motor[2],motor[3]);
//printf("throttle = %.5f, adj_roll = %0.5f, adj_pitch = %0.5f, adj_yaw = %0.5f \n", throttle,adj_roll,adj_pitch,adj_yaw);
//printf("throttle = %.5f, adj_roll = %0.5f, adj_pitch = %0.5f, motor: %.5f,%.5f,%.5f,%.5f \n", throttle,adj_roll,adj_pitch,motor[0],motor[1],motor[2],motor[3]);
//send to motors
mot_Run(motor[0],motor[1],motor[2],motor[3]); // Comment when you do not want motors rotate
//blink leds
cnt++;
if((cnt%200)==0)
mot_SetLeds(MOT_LEDGREEN,MOT_LEDGREEN,MOT_LEDGREEN,MOT_LEDGREEN);
else if((cnt%200)==100)
mot_SetLeds(0,0,0,0);
// Send UDP nav log packet
navLog_Send();
// Yield to other threads
pthread_yield();
}
}