===== Zagi ===== At present the plan is for a system as so.. {{projects:ukhas_glider_project:scheme3.png|}} ==== Main code (Zagi.c) ==== //main code //-Laurenceb //////////////////////////////////////////////////////////////// #include // include I/O definitions (port names, pin names, etc) #include // include interrupt support #include //#include #include #include #include #include #include #include "init.h" #include "attitude.h" #define altitudelimit 4000 //4km cutdown #define timelimit 600 //10 minutes #define Deg2rad M_PI/180 #define Rad2deg 180/M_PI #define tweakingfactor 0.002 #define batteryinput 4 #define batteryfactor 8/1024 void wiggleservo(void); void cutdown(char channel,char time); void updategpsdata(void); char *datastring(float k, float Target, attitude_type *ourattitude); float getbatteryvoltage(void); u08 cutdownrulecheck(void); u08 checksum(char *c_buf); char EEMEM windeast[1020]; char EEMEM windnorth[1020]; float targetnorth; float targeteast; int time; //time char *datastring(float k, float Target, attitude_type *ourattitude) { static char outputbuffer[80]; char longitude; char latitude; if (gpsGetInfo()->PosLLA.lon.f>0) { longitude='E'; } else { longitude='W'; } if (gpsGetInfo()->PosLLA.lat.f>0) { latitude='N'; } else { latitude='S'; } sprintf(outputbuffer,"%.6f,%c,%.6f,%c,%.1f,M,%.1f,%.1f,%c,%.1f,%.1f,%.2f,",gpsGetInfo()->PosLLA.lat.f, latitude,gpsGetInfo()->PosLLA.lon.f,longitude,gpsGetInfo()->PosLLA.alt.f,k,Target,ourattitude->status, ourattitude->pitch,ourattitude->roll,getbatteryvoltage()); sprintf(outputbuffer,"%s%X",outputbuffer,checksum(outputbuffer)); return (outputbuffer); } float getbatteryvoltage() { return (a2dConvert10bit(batteryinput)*batteryfactor); } u08 checksum(char *c_buf) { u08 i; u08 sum=0; u08 size=strlen(c_buf); for(i=0 ; iPosLLA.alt.f>altitudelimit) { stdout=&mystdio0; printf("CUTDOWN, Altitude limit\r\n"); stdout=&mystdio1; printf("CUTDOWN, Altitude limit\r\n"); return 1; } if(time>timelimit) { stdout=&mystdio0; printf("CUTDOWN, Time limit\r\n"); stdout=&mystdio1; printf("CUTDOWN, Time limit\r\n"); return 2; } return 0; } void cutdown(char channel,char time) { if(channel==1) { PORTD|=(1<<7); _delay_ms(1000*time); PORTD&=~(1<<7);; } if(channel==2) { PORTD|=(1<<6); _delay_ms(1000*time); PORTD&=~(1<<6);; } } void updategpsdata() { static u16 updatecounterENU; static u16 updatecounterHS; static u16 updatecounterLLA; while(gpsGetInfo()->PosLLA.updates<=updatecounterLLA || gpsGetInfo()->VelENU.updates<=updatecounterENU || gpsGetInfo()->VelHS.updates<=updatecounterHS) //while we still have some old data { nmeaProcess(Rxbuff0); } //look for new data updatecounterLLA=gpsGetInfo()->PosLLA.updates; //set the update counters correctly updatecounterENU=gpsGetInfo()->VelENU.updates; updatecounterHS=gpsGetInfo()->VelHS.updates; } int main() { uint16_t mem; int Heightupto,L; float K,Windx_total,Windy_total,X,Y,Target,North,East,Wind_y,Wind_x; init(); printf("Back in main\n"); printf("enter target north\n"); scanf("%f\r\n",&targetnorth); printf("%f\n Ok, now target east\n",targetnorth); scanf("%f\r\n",&targeteast); printf("%f\n",targeteast); disablegroundcontrol(); timerDetach(TIMER0OVERFLOW_INT); // stop low level guidance on timer0 printf("Killed lowlevel guidance and groundcontrol\n now dumping eeprom"); timer1PWMASet (trimAbias); timer1PWMBSet (trimBbias); for (mem=0;mem<2096;mem++) { printf("Record number %d %d",mem,(int)eeprom_read_byte(&windeast[mem])); mem++; printf(" %d\n",(int)eeprom_read_byte(&windeast[mem])); } time=0; //use i to check time while(!cutdownrulecheck()) { time++; updategpsdata(); Windx_total += gpsGetInfo()->VelENU.east.f; //add it to our total wind Windy_total += gpsGetInfo()->VelENU.north.f; L++; //incrament the denominator for our averaging K = gpsGetInfo()->PosLLA.alt.f / 50 ; //find the altitude in 50m incraments if ((int)K > Heightupto) //do we now fall into another (higher) 100m incrament ? { Heightupto++; windeast[Heightupto] = Windx_total/L + 127; //we are storing as a signed byte windnorth[Heightupto] = Windy_total/L + 127; //we find the average and shove it in the eeprom L = 0; } wiggleservos(); if(cutdownrulecheck()) {break;} } cutdown(1,12); reinitlowlevel(); while(1) { updategpsdata(); East = targeteast - gpsGetInfo()->PosLLA.lon.f; North=gpsGetInfo()->PosLLA.lat.f; K = cos(Deg2rad*North); East = East * K; //distance to target in equatorial degree units North = targetnorth - North; Target = Rad2deg*atan(East/North); if(North < 0) //direction to target { Target = Target - 180; } if( Target < -180) { Target = Target + 360; //gets it in +-180 degree range } X = gpsGetInfo()->VelENU.east.f-Wind_x; //wind compensation in ENU frame Y = gpsGetInfo()->VelENU.north.f-Wind_y; if(Y == 0) { X = 0; //don't divide by 0 !!! } else { X = X / Y; } K = Rad2deg*atan(X); if (Y < 0) { K = K - 180; } //all to keep us in +-180 degree range if(K < -180) { K = K + 360; //K is now our air vector heading } K = K - Target; //k is now our heading offset (from now on is just for if (K >180) { K = K - 360; } //all to keep us in +-180 degree range if(K < -180) { K = K + 360; } if (K > 0) { PORTG&=~(1<<3); PORTG|=(1<<4); //left/right indictor: bicolour LED } else { PORTG&=~(1<<4); PORTG|=(1<<3); } tweakfactor=K*tweakingfactor; L=(int)gpsGetInfo()->PosLLA.alt.f/50; Wind_x=(float)windeast[L]-127; Wind_y=(float)windnorth[L]-127; if(L<5) { enablegroundcontrol(); //groundcontrol enabled for smooth landing } stdout=&mystdio0; if (bit_is_set(PIND,4)) //CTS from radio {printf("UKHAS, %s\r\n",datastring(K,Target,getattitude()));} stdout=&mystdio1; printf("UKHAS, %s\r\n",datastring(K,Target,getattitude())); } return(1); } ==== Initialisation code (Init.c, Init.h) ==== // initialisation code // - Laurenceb #include "init.h" int get_char0(FILE* stream) { char c=0; while(!c) {c=bufferGetFromFront(Rxbuff0);} //getchar gets the stream, loop until we get something return c; } int get_char1(FILE* stream) { char c=0; while(!c) {c=bufferGetFromFront(Rxbuff1);} //getchar gets the stream, loop until we get something return c; } int put_char1(char c, FILE *stream) { while(!bufferAddToEnd(Txbuff1,c)); //send the character return 0; //means ok? } int put_char0(char c, FILE *stream) { while(!bufferAddToEnd(Txbuff0,c)); //send the character return 0; //means ok? } void init() { char teststr[15]; sbi(DDRD,5); //led is output sbi(DDRB,5); sbi(DDRB,6); //pwm outputs sbi(DDRG,3); //direction led sbi(DDRG,4); sbi(DDRD,6); //cutdowns sbi(DDRD,7); sbi(PORTD,4); uartInit(); //init uarts uartSetBaudRate(0,4800); //GPS+radio uartSetBaudRate(1,4800); // Debug link to pc Rxbuff0 = uartGetRxBuffer(0); Txbuff0 = uartGetTxBuffer(0); Rxbuff1 = uartGetRxBuffer(1); Txbuff1 = uartGetTxBuffer(1); uartSendTxBuffer(0); //turn on uart0 uartSendTxBuffer(1); //turn on uart1 //TXpointer0=Txbuff0->dataptr; // we will use this to talk to the radio //NO NO NO use the add to buffer function !!!!! //RXpointer1=Rxbuff1->dataptr; //now we can use sscanf to get data from the buffers - maybe, better to redirect stdio //TXpointer1=Txbuff1->dataptr; //not used as yet //rprintfInit(uart1SendByte); // configure rprintf to use UART1 for output FILE mystdio1 = FDEV_SETUP_STREAM(put_char1, get_char1, _FDEV_SETUP_RW); //sets up usart as our file stream FILE mystdio0 = FDEV_SETUP_STREAM(put_char0, get_char0, _FDEV_SETUP_RW); //so we can printf to the radio stdout = &mystdio1; //set our stdio out function stdin = &mystdio1; // set our stdio in funciton sei(); // so we can use buffering printf("Hello, Uart setup complete\r\n"); // send "hello world" message nmeaInit(); printf("Checking GPS recieve buffer\r\n"); while(nmeaProcess(Rxbuff0)!=0); //loop until we get valid data printf("GPS recieve buffer ok\r\n"); while(gpsGetInfo()->PosLLA.alt.f==0) //look for non zero altitude { printf("%d%s\r\n",nmeaProcess(Rxbuff0),nmeaGetPacketBuffer()); } printf("GPS lock ok, testing radio\r\n"); stdout = &mystdio0; //set our stdio out function to uart0 (radio) printf("Hello world\n"); //sends output to radio buffer stdout = &mystdio1; //back to normal (PC) printf("Ok, now firing up the timers\r\n"); // TODO modify uart2.c to check Radio CTS timerInit(); timer0SetPrescaler(TIMER_CLK_DIV1024); timer1SetPrescaler(TIMER_CLK_DIV8); //prescale all the timers, use timer3 not timer2 outb(TCCR3B, (inb(TCCR3B) & ~TIMER_PRESCALE_MASK) | TIMER_CLK_DIV64); //prescale timer3 by 64 and start it outb(TCNT3H, 0); // reset TCNT3 outb(TCNT3L, 0); // set up timer3 manually, but disable the overflow interrupt cbi(TIMSK, TOIE3); // its not supported by procyon, check this as its copied from timer1 timer1PWMInitICR(40000); // set pwm top count gives 20ms a2dInit(); printf("turning ground control on\r\n"); enablegroundcontrol(); //enable pwm capture printf("test ground control function\r\n"); printf("type something if you are ok to continue with setup\r\n"); scanf("%s\r",teststr); printf("you said: %s\r\n",teststr); printf("ok, test the thermopile guidance\r\n"); disablegroundcontrol(); //we are now in full low level guidance printf("entered thermopile guidance\r\n"); printf("initialisation complete, bye\r\n"); } ==== PWM capture code (PWMcapture.c, PWMcapture.h) ==== //the global variable enables us to find out from elsewhere if we are under ground control //enableing/diabling can be done using the enable disable functions // - Laurenceb /////////////////////////////////////////////////////////////////// #include "PWMcapture.h" #define periodupperlimit 6000 //24ms (prescale timer2 by 64) #define periodlowerlimit 3000 //12ms #define dutyupperlimit 800 //3.2ms #define dutylowerlimit 250 //1ms void enablegroundcontrol() { EICRB=0b01010000; //set int6 and int7 to trigger for any transition EIMSK=0b11000000; //enable int6 and int7 } void disablegroundcontrol() { EIMSK=0b00000000; //disable int6 and int7 if(groundcontrolon) { groundcontrolon=0; // go autonomous timer1PWMAOn(); timer1PWMBOn(); timerAttach(TIMER0OVERFLOW_INT,lowlevelguidance); // run low level guidance on timer0 PORTD=PORTD&~(1<<5); //LED off } } ISR(INT6_vect) // we will use int6 for pwm timing { static u08 numberofpulses,waitforanewpulse; u16 value; if (TCNT0<40 && !groundcontrolon) // we are just after a lowlevel guidance call, possily a clash { waitforanewpulse=1; // ignore the reset of this pulse as it will be screwed } else //not a clash { if(groundcontrolon) { PORTB=PORTB|((PINE>>1)&(1<<5)); // copy over to output1 } if(bit_is_set(PINE, 6)) //we are at the start of a pulse { value=gettimer(); if( ((value>periodupperlimit)||(valuedutyupperlimit)||(value10) { timer1PWMOff(); //switch to ground control mode EIMSK=0b11000000; //enable both interrupts groundcontrolon=1; timerDetach(TIMER0OVERFLOW_INT); // stop low level guidance on timer0 PORTD=PORTD|(1<<5); //LED on } else { if(groundcontrolon) { groundcontrolon=0; // go autonomous EIMSK=0b01000000; // disable int7 to save recources timer1PWMAOn(); timer1PWMBOn(); timerAttach(TIMER0OVERFLOW_INT,lowlevelguidance); // run low level guidance on timer0 PORTD=PORTD&~(1<<5); //LED off } } } } } ISR(INT7_vect) { if(groundcontrolon) { PORTB=PORTB|((PINE>>1)&(1<<6)); //copy over the input2 to the output2 } } u16 gettimer() { return(TCNT3L|(TCNT3H<<8)); // gives us the value of timer3 } ==== Low level guidance code (Lowlevel.c, Lowlevel.h) ==== // This is the fine guidance code, using the thermopiles, it is biased using a global variable // untested, but compiles ok with AVR studio 4.18 using AVR-GCC // obviously it will also need calibrating with the servos and airframe used //-Laurenceb #include "lowlevel.h" #include "attitude.h" volatile unsigned char reset; //used for resetting the PID control volatile float rollintegral,pitchintegral; //so we can acess externally void reinitlowlevel() { timerAttach(TIMER0OVERFLOW_INT,lowlevelguidance); //make sure its running reset=2; //reinitialise guidance rollintegral=0; //reset intagrals pitchintegral=0; } void lowlevelguidance() { attitude_type *theattitude; //pointer to attitude data static unsigned short servoa,servob; static float oldpitch,oldroll,pitchrunningaverage,rollrunningaverage,pwmpitch,pwmroll; static float deltapitchrunningaverage,deltarollrunningaverage; float pitchrunningaverageconstant_,deltapitchrunningaverageconstant_,rollrunningaverageconstant_,deltarollrunningaverageconstant_; float deltaroll,deltapitch; theattitude=getattitude(); if(theattitude->status=='n') { deltapitch=theattitude->pitch-oldpitch; oldpitch=theattitude->pitch; deltaroll=theattitude->roll-oldroll; oldroll=theattitude->roll; if(!reset) { pitchrunningaverageconstant_=pitchrunningaverageconstant; //we need to be able to reset these on demand deltapitchrunningaverageconstant_=deltapitchrunningaverageconstant; rollrunningaverageconstant_=rollrunningaverageconstant; deltarollrunningaverageconstant_=deltarollrunningaverageconstant; pitchrunningaverage+=(theattitude->pitch-pitchrunningaverage)*pitchrunningaverageconstant_; //SW low pass deltapitchrunningaverage+=(deltapitch-deltapitchrunningaverage)*deltapitchrunningaverageconstant_; rollrunningaverage+=(theattitude->roll-rollrunningaverage)*rollrunningaverageconstant_; deltarollrunningaverage+=(deltaroll-deltarollrunningaverage)*deltarollrunningaverageconstant_; } else { if(reset==2) { reset=1; pitchrunningaverageconstant_=1; //step one: set the pitch and roll correctly deltapitchrunningaverageconstant_=0; rollrunningaverageconstant_=1; deltarollrunningaverageconstant_=0; } else { reset=0; // the next iteration will be normal pitchrunningaverageconstant_=1; deltapitchrunningaverageconstant_=1; //step two: set the deltas correctly now we have two values rollrunningaverageconstant_=1; // the problem is that on reinitialisation our values are deltarollrunningaverageconstant_=1; //old and therefor not valid } pitchrunningaverage+=(theattitude->pitch-pitchrunningaverage)*pitchrunningaverageconstant_; //SW low pass deltapitchrunningaverage+=(deltapitch-deltapitchrunningaverage)*deltapitchrunningaverageconstant_; rollrunningaverage+=(theattitude->roll-rollrunningaverage)*rollrunningaverageconstant_; deltarollrunningaverage+=(deltaroll-deltarollrunningaverage)*deltarollrunningaverageconstant_; deltapitch=0; // when reset=2:wipe the old D terms, they are corrupted deltaroll=0; // when reset=1:the D term is sensitive to noise, leave it out for now } pitchintegral+=pitchrunningaverage; if (pitchintegral>pitchintegrallimit) { pitchintegral=pitchintegrallimit; } if (pitchintegral<-pitchintegrallimit) { pitchintegral=-pitchintegrallimit; } rollintegral+=rollrunningaverage; if (rollintegral>rollintegrallimit) //integral limits { rollintegral=rollintegrallimit; } if (rollintegral<-rollintegrallimit) { rollintegral=-rollintegrallimit; } pwmpitch=(pitchrunningaverage*PP)+(deltapitchrunningaverage*DP)+(pitchintegral*IP); pwmroll=(rollrunningaverage*PR)+(deltarollrunningaverage*DR)+(rollintegral*IR)+tweakfactor; servoa=trimAgain*(pwmpitch+pwmroll)+trimAbias; // V tail mixing in SW if (servoa>(trimAbias+limitA)) { servoa=trimAbias+limitA; } if (servoa<(trimAbias-limitA)) { servoa=trimAbias-limitA; } servob=trimBgain*(pwmpitch-pwmroll)+trimBbias; if (servob>(trimBbias+limitB)) //servo limits { servob=trimBbias+limitB; } if (servob<(trimBbias-limitB)) { servob=-trimBbias-limitB; } } else { reset=2; //we need to reset the PID as we have left the acceptable range if(theattitude->status=='d') { servob=trimBbias+limitB; servoa=trimAbias+limitA; } if(theattitude->status=='p') { servob=trimBbias-limitB; servoa=trimAbias-limitA; } if(theattitude->status=='l') { servob=trimBbias+limitB; servoa=trimAbias-limitA; } if(theattitude->status=='r') { servob=trimBbias-limitB; servoa=trimAbias+limitA; } } if(reset != 1) // so we are in normal flight or a recovery position { timer1PWMASet (servoa); timer1PWMBSet (servob); } timer1Init(); // reset timer1, lowlevel is called from the timer0 isr every 16ms } // so we reset timer1 to get a pulse out ==== ADC to attitude code (attitude.c, attitude.h)==== // This is untested and very rough, feel free to correct any errors // compiles ok using AVR studio 4.18 with AVR-GCC /////////////////////////////////////////////////////////////////// // the thermopiles are wired so that the sky appears +ive, and the output from the downward facing sensor is // subtratced from all the thermos // the inversion handling is very hard to work out :/ I like to picture a vector going through the wing //vertically, ie such that it will be vertical when roll=pitch=0. Then imagine a cube centered on //the origin, then when the vector hits the top of the cube we are in normal flight, when it hits the bottom //we are inverted, front=in a dive, back=in a stall and either side a strong turn. Now when you've thought about //that picture, imagine what the thermopiles will be reading in each case, and what the plane needs to do //(there a 6 cases). I think this code handles all 6 cases, but its hard to visualise, so feel free to comment. // - Laurenceb typedef struct //flight attitude pointer { char status; float roll; float pitch; } attitude_type; #define sensorup 0 //adc channels to use #define sensorleft 1 #define sensorright 2 #define sensorfront 3 attitude_type *getattitude(void); attitude_type *getattitude() { static attitude_type attitude; unsigned short up,left,right,front,upovertwo; up=a2dConvert10bit(sensorup); left=a2dConvert10bit(sensorleft); right=a2dConvert10bit(sensorright); front=a2dConvert10bit(sensorfront); if( up>left && up>right && up>front && up>512 && front >512) //normal flight { attitude.pitch=(front-((left+right)>>1))/up; attitude.roll=(left-right)/up; attitude.status='n'; return &attitude; } else // something badly screwed { upovertwo=up>>1; //halve up if((left-right)>upovertwo) //roll out of inversion { attitude.status='l'; return &attitude; } if((right-left)>upovertwo) //roll out t'other way { attitude.status='r'; return &attitude; } if(up>front) { attitude.status='p'; // otherwise we pitch up return &attitude; } if(up<=front) { if (up<512) { attitude.status='p'; // we are inverted, pitch up return &attitude; } else { attitude.status='d'; // otherwise we are in a stall, pitch down return &attitude; } } } } ===== Rogallo ===== [[projects:ukhas_glider_project:methods_of_control|Here]]