projects:ukhas_glider_project:code
This is an old revision of the document!
Table of Contents
Zagi
Initialisation code (Init.c, Init.h)
// this is under construction // - 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; } static int put_char1(char c, FILE *stream) { while(!bufferAddToEnd(Txbuff1,c)); //send the character return 0; //means ok? } static 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); //lcd is output sbi(DDRB,5); sbi(DDRB,6); //pwm outputs 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 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 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)||(value<periodlowerlimit)) && !waitforanewpulse) //check repetition rate only if last pulse ok { numberofpulses=0; } TCNT3L =0; //reset timer3 so we can start recording TCNT3H =0; waitforanewpulse=0; //we have our new pulse now } else if(!waitforanewpulse) //we are at the end of a pulse and its valid { value=gettimer(); if((value>dutyupperlimit)||(value<dutylowerlimit)) //check pulse lenght { numberofpulses=0; } else { if(numberofpulses<11) {numberofpulses++;} } if (numberofpulses>10) { 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)
#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)+(rollrunningaverage*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/code.1196785975.txt.gz · Last modified: 2008/07/19 23:32 (external edit)