UKHAS Wiki

UK High Altitude Society

User Tools

Site Tools

Trace:
projects:ukhas_glider_project:code

Differences

This shows you the differences between two versions of the page.

Link to this comparison view

Both sides previous revisionPrevious revision
Next revision		Previous revision
projects:ukhas_glider_project:code [2007/12/04 16:26] – Zagi code laurencebprojects:ukhas_glider_project:code [2008/07/19 23:33] (current) – external edit 127.0.0.1
Line 5: Line 5:
 {{projects:ukhas_glider_project:scheme3.png|}} {{projects:ukhas_glider_project:scheme3.png|}}
  
 +==== Main code (Zagi.c) ====
  
 +<code c>
 +//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);
 +}
 +
 +
 +</code>
  
 ==== Initialisation code (Init.c, Init.h) ==== ==== Initialisation code (Init.c, Init.h) ====
  
 <code c> <code c>
-// this is under construction+// initialisation code
 // - Laurenceb // - Laurenceb
 #include "init.h" #include "init.h"
Line 40: Line 303:
 } }
  
-static int put_char1(char c, FILE *stream)+ int put_char1(char c, FILE *stream)
 { {
    while(!bufferAddToEnd(Txbuff1,c));    //send the character    while(!bufferAddToEnd(Txbuff1,c));    //send the character
Line 47: Line 310:
    
  
-static int put_char0(char c, FILE *stream)+ int put_char0(char c, FILE *stream)
 { {
    while(!bufferAddToEnd(Txbuff0,c));    //send the character    while(!bufferAddToEnd(Txbuff0,c));    //send the character
Line 58: Line 321:
 { {
  char teststr[15];   char teststr[15]; 
-sbi(DDRD,5);                      //lcd is output+sbi(DDRD,5);                      //led is output
 sbi(DDRB,5); sbi(DDRB,5);
 sbi(DDRB,6);                      //pwm outputs  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 uartInit();                       //init uarts
 uartSetBaudRate(0,4800);          //GPS+radio uartSetBaudRate(0,4800);          //GPS+radio
Line 75: Line 343:
 //TXpointer1=Txbuff1->dataptr;    //not used as yet //TXpointer1=Txbuff1->dataptr;    //not used as yet
 //rprintfInit(uart1SendByte);        // configure rprintf to use UART1 for output //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 stdout = &mystdio1; //set our stdio out function
 stdin = &mystdio1; // set our stdio in funciton        stdin = &mystdio1; // set our stdio in funciton       
Line 101: Line 371:
 cbi(TIMSK, TOIE3);                      // its not supported by procyon, check this as its copied from timer1 cbi(TIMSK, TOIE3);                      // its not supported by procyon, check this as its copied from timer1
 timer1PWMInitICR(40000);                             // set pwm top count gives 20ms timer1PWMInitICR(40000);                             // set pwm top count gives 20ms
 +a2dInit();
 printf("turning ground control on\r\n"); printf("turning ground control on\r\n");
 enablegroundcontrol();                               //enable pwm capture enablegroundcontrol();                               //enable pwm capture
Line 241: Line 512:
  
 ==== Low level guidance code (Lowlevel.c, Lowlevel.h) ==== ==== Low level guidance code (Lowlevel.c, Lowlevel.h) ====
- 
- 
- 
- 
- 
  
  
 <code c> <code c>
-// This is the fine guidance code, using the thermopiles, it is biased using a global variable +// 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 +// untested, but compiles ok with AVR studio 4.18 using AVR-GCC   
-// obviously it will also need calibrating with the servos and airframe used+// obviously it will also need calibrating with the servos and airframe used  
 //-Laurenceb //-Laurenceb
  
-///////////////////////////////////////////////////////////////// +#include "lowlevel.h" 
-float tweakfactor;                                       //global, used for tweaking +#include "attitude.h"
-void highlevelguidance(void); +
-#define pitchrunningaverageconstant                      //software low pass filtering +
-#define deltapitchrunningaverageconstant +
-#define PP                                               //PID control constants +
-#define DP +
-#define IP +
-#define PR +
-#define DR +
-#define IR +
-#define pitchintegrallimit +
-#define rollintegrallimit +
-#define rollrunningaverageconstant  +
-#define deltarollrunningaverageconstant +
-#define trimAgain +
-#define trimBgain +
-#define trimAbias +
-#define trimBbias +
-#define limitA +
-#define limitB+
  
-typedef unsigned short u16;                              // to make timer.h work ok +volatile unsigned char reset;                              //used for resetting the PID control 
-typedef unsigned char u08;+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; 
 +}
  
  
Line 284: Line 539:
 attitude_type *theattitude;                              //pointer to attitude data attitude_type *theattitude;                              //pointer to attitude data
 static unsigned short servoa,servob; static unsigned short servoa,servob;
-static unsigned char reset;                              //used for resetting the PID control 
 static float oldpitch,oldroll,pitchrunningaverage,rollrunningaverage,pwmpitch,pwmroll; static float oldpitch,oldroll,pitchrunningaverage,rollrunningaverage,pwmpitch,pwmroll;
-static float deltapitchrunningaverage,deltarollrunningaverage,rollintegral,pitchintegral;+static float deltapitchrunningaverage,deltarollrunningaverage;
 float  pitchrunningaverageconstant_,deltapitchrunningaverageconstant_,rollrunningaverageconstant_,deltarollrunningaverageconstant_; float  pitchrunningaverageconstant_,deltapitchrunningaverageconstant_,rollrunningaverageconstant_,deltarollrunningaverageconstant_;
 float deltaroll,deltapitch; float deltaroll,deltapitch;
Line 351: Line 605:
   }   }
   pwmpitch=(pitchrunningaverage*PP)+(deltapitchrunningaverage*DP)+(pitchintegral*IP);   pwmpitch=(pitchrunningaverage*PP)+(deltapitchrunningaverage*DP)+(pitchintegral*IP);
-  pwmroll=(rollrunningaverage*PR)+(rollrunningaverage*DR)+(rollintegral*IR)+tweakfactor;+  pwmroll=(rollrunningaverage*PR)+(deltarollrunningaverage*DR)+(rollintegral*IR)+tweakfactor;
   servoa=trimAgain*(pwmpitch+pwmroll)+trimAbias;                                    // V tail mixing in SW   servoa=trimAgain*(pwmpitch+pwmroll)+trimAbias;                                    // V tail mixing in SW
   if (servoa>(trimAbias+limitA))   if (servoa>(trimAbias+limitA))
Line 404: Line 658:
  timer1Init();                         // reset timer1, lowlevel is called from the timer0 isr every 16ms  timer1Init();                         // reset timer1, lowlevel is called from the timer0 isr every 16ms
 }                                      // so we reset timer1 to get a pulse out  }                                      // so we reset timer1 to get a pulse out 
 +
  
  
projects/ukhas_glider_project/code.1196785568.txt.gz · Last modified: 2008/07/19 23:32 (external edit)
Donate Powered by PHP Valid HTML5 Valid CSS Driven by DokuWiki