UKHAS Wiki

At present the plan is for a system as so..

//main code
//-Laurenceb
 
////////////////////////////////////////////////////////////////
#include <avr/io.h>		// include I/O definitions (port names, pin names, etc)
#include <avr/interrupt.h>	// include interrupt support
#include <stdlib.h>
//#include <avr/iom64.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <util/delay.h>
#include <avr/eeprom.h>
#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 ; i<size ; i++,c_buf++)
		{
                sum += *c_buf;
        }
        return(sum);
}
 
 
void wiggleservos()
{
timer1PWMASet (trimAbias+limitA);
timer1PWMBSet (trimBbias+limitB);
_delay_ms(150);
timer1PWMASet (trimAbias-limitA);
timer1PWMBSet (trimBbias-limitB);
_delay_ms(150);
timer1PWMASet (trimAbias);
timer1PWMBSet (trimBbias);
}
 
u08 cutdownrulecheck()
{
 if(gpsGetInfo()->PosLLA.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
// - 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");
}

//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
}

// 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

// 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;
                  } 
              }
        }  
}

Here