UKHAS Wiki

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

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

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

#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

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