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/06 13:48] – Zagi update 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
Line 22: Line 22:
 #include <avr/eeprom.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 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);
 void cutdown(char channel,char time); void cutdown(char channel,char time);
-u08 cutdownrulecheck();+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 windeast[1020];
 char EEMEM windnorth[1020]; char EEMEM windnorth[1020];
Line 34: Line 44:
 float targeteast; float targeteast;
 int time;                          //time 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() void wiggleservos()
Line 83: Line 142:
  }  }
 } }
 +
 +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; 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");
Line 109: Line 185:
 { {
  time++;  time++;
- ; //ascent code+ 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);
Line 116: Line 205:
 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 133: Line 282:
  
 <code c> <code c>
-// this is under construction+// initialisation code
 // - Laurenceb // - Laurenceb
 #include "init.h" #include "init.h"
Line 154: 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 161: 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 172: 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 189: 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 215: 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 448: 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.1196948901.txt.gz · Last modified: 2008/07/19 23:32 (external edit)
Donate Powered by PHP Valid HTML5 Valid CSS Driven by DokuWiki