===== 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]]