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:39] – Zagi code laurencebprojects:ukhas_glider_project:code [2008/07/19 23:33] (current) – external edit 127.0.0.1
Line 8: Line 8:
  
 <code c> <code c>
-//untested, first draft, compiles ok +//main code 
-//- Laurenceb+//-Laurenceb 
 + 
 //////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////
 #include <avr/io.h> // include I/O definitions (port names, pin names, etc) #include <avr/io.h> // include I/O definitions (port names, pin names, etc)
 #include <avr/interrupt.h> // include interrupt support #include <avr/interrupt.h> // include interrupt support
 #include <stdlib.h> #include <stdlib.h>
-#include <avr/iom64.h>+//#include <avr/iom64.h>
 #include <stdio.h> #include <stdio.h>
 +#include <string.h>
 +#include <math.h>
 #include <util/delay.h> #include <util/delay.h>
 +#include <avr/eeprom.h>
 #include "init.h" #include "init.h"
 +#include "attitude.h"
 #define altitudelimit 4000          //4km cutdown #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 wiggleservo(void); 
-u08 cutdownrulecheck(); +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 targetnorth;
 float targeteast; 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);
 +}
  
  
Line 44: Line 110:
  if(gpsGetInfo()->PosLLA.alt.f>altitudelimit)  if(gpsGetInfo()->PosLLA.alt.f>altitudelimit)
  {  {
 +  stdout=&mystdio0;
 +  printf("CUTDOWN, Altitude limit\r\n");
 +  stdout=&mystdio1;
 +  printf("CUTDOWN, Altitude limit\r\n");
   return 1;   return 1;
  }  }
-return 0;+ 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() int main()
 { {
 +uint16_t mem;
 +int Heightupto,L;
 +float K,Windx_total,Windy_total,X,Y,Target,North,East,Wind_y,Wind_x;
 init(); init();
-printf("Back in main/n"); +printf("Back in main\n"); 
-printf("enter target north/n"); +printf("enter target north\n"); 
-scanf("%f\r",&targetnorth);+scanf("%f\r\n",&targetnorth);
 printf("%f\n Ok, now target east\n",targetnorth); printf("%f\n Ok, now target east\n",targetnorth);
-scanf("%f\r",&targeteast);+scanf("%f\r\n",&targeteast);
 printf("%f\n",targeteast); printf("%f\n",targeteast);
 disablegroundcontrol(); disablegroundcontrol();
 timerDetach(TIMER0OVERFLOW_INT); // stop low level guidance on timer0  timerDetach(TIMER0OVERFLOW_INT); // stop low level guidance on timer0 
-printf("Killed lowlevel guidance and groundcontrol\n");+printf("Killed lowlevel guidance and groundcontrol\n now dumping eeprom");
 timer1PWMASet (trimAbias); timer1PWMASet (trimAbias);
 timer1PWMBSet (trimBbias); 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()) while(!cutdownrulecheck())
 { {
- ; //ascent code+ 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();  wiggleservos();
 + if(cutdownrulecheck())
 + {break;}
 } }
-//cutdown(1,12);+cutdown(1,12);
 reinitlowlevel(); reinitlowlevel();
 while(1) while(1)
 { {
- ; //descent code+ 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); return(1);
 } }
- 
- 
- 
- 
- 
  
  
Line 93: Line 282:
  
 <code c> <code c>
-// this is under construction+// initialisation code
 // - Laurenceb // - Laurenceb
 #include "init.h" #include "init.h"
Line 114: 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 121: 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 132: 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 149: 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 175: 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 408: 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))
projects/ukhas_glider_project/code.1196786360.txt.gz · Last modified: 2008/07/19 23:32 (external edit)
Donate Powered by PHP Valid HTML5 Valid CSS Driven by DokuWiki