UKHAS Wiki

UK High Altitude Society

User Tools

Site Tools

Trace:
code:parafoil_main

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
code:parafoil_main [2008/05/01 00:06] – laurencebcode:parafoil_main [2008/08/16 21:11] (current) – laurenceb
Line 4: Line 4:
  
 {{code:dependencies.png?400|}} {{code:dependencies.png?400|}}
 +
 +===== TSIP version for lassen iq =====
 +
 +[[code:parafoil_tsip|TSIP based version here]]
 + (nmea version has been "mothballed")
  
 ===== Main.h ===== ===== Main.h =====
Line 112: Line 117:
 volatile kalman_state our_state; volatile kalman_state our_state;
 volatile kalman_model our_model; volatile kalman_model our_model;
 +volatile u08 Kalman_flag;
    
 volatile float Heading; volatile float Heading;
Line 191: Line 197:
  //IO setup  //IO setup
  gyro_off; //SS to the gyro - pullups  gyro_off; //SS to the gyro - pullups
- PORTD|=( (1<<3) | (1<<4) ); //pullups to radio cts and dead man switch + PORTD|=( (1<<3) | (1<<4) ); //pullups to radio cts and dead man switch
  DDRB=0b00101111; //talk to servo and gyro  DDRB=0b00101111; //talk to servo and gyro
  DDRC=0b00011000; //indicator leds  DDRC=0b00011000; //indicator leds
Line 198: Line 204:
  //Kalman setup  //Kalman setup
  our_model.F.tl=1.0; //kalman model setup  our_model.F.tl=1.0; //kalman model setup
- our_model.F.tr=delta_time;+ our_model.F.tr=delta_time; //propogator
  our_model.F.bl=0.0;  our_model.F.bl=0.0;
  our_model.F.br=1.0-(damping_constant*delta_time);  our_model.F.br=1.0-(damping_constant*delta_time);
    
- our_model.B.tl=0.0;+ our_model.B.tl=0.0; //control input matrix
  our_model.B.tr=(delta_time*delta_time*control_gain)/2.0;  our_model.B.tr=(delta_time*delta_time*control_gain)/2.0;
  our_model.B.bl=0.0;  our_model.B.bl=0.0;
Line 211: Line 217:
  our_model.H.bl=0.0;  our_model.H.bl=0.0;
  our_model.H.br=1.0; //this is set for a gyro update  our_model.H.br=1.0; //this is set for a gyro update
-  +  //model noise covariance matrix 
- our_model.Q.tl=0.1/sqrt_delta_time; //estimated - corresponds to heading change+ our_model.Q.tl=5.0*delta_time; //estimated - corresponds to heading change
  our_model.Q.tr=0.0;  our_model.Q.tr=0.0;
  our_model.Q.bl=0.0;  our_model.Q.bl=0.0;
- our_model.Q.br=1.0/sqrt_delta_time; //estimated - corresponds to rate change+ our_model.Q.br=10.0*delta_time; //estimated - corresponds to rate change
    
- our_model.R.tl=20.0; //or around 20 in the case of a GPD update+ our_model.R.tl=100.0; //Data covariance
  our_model.R.tr=0.0;  our_model.R.tr=0.0;
  our_model.R.bl=0.0;  our_model.R.bl=0.0;
- our_model.R.br=1.0; //approximate for an MLX90609+ our_model.R.br=2.0; //approximate for an MLX90609
    
  our_state.state.t=0.0;  our_state.state.t=0.0;
  our_state.state.b=0.0;  our_state.state.b=0.0;
    
- our_state.P.tl=150.0; //we might be pointing in any direction+ our_state.P.tl=8000.0; //we might be pointing in any direction (~90^2)
  our_state.P.tr=0.0;  our_state.P.tr=0.0;
  our_state.P.bl=0.0;  our_state.P.bl=0.0;
- our_state.P.br=20.0; //rough estimate for turn rate error at release+ our_state.P.br=400.0; //rough estimate for turn rate error^2 at release
    
 } }
Line 235: Line 241:
 { {
  u16 a;  u16 a;
- gyro_on; //SS = 0+ gyro_on; //SS = 0
  SPDR=read_rate;  SPDR=read_rate;
  while(!(inb(SPSR) & (1<<SPIF)));  while(!(inb(SPSR) & (1<<SPIF)));
Line 255: Line 261:
  a|=(u16)SPDR;  a|=(u16)SPDR;
  gyro_off;  gyro_off;
- a=(a>>1)&mask_word; //all based on the rogallo code+ a=(a>>1)&mask_word; //all based on the rogallo code
  return (float)(125.0/1024.0)*(float)(a-gyro_null); //+-125 degree/sec gyro, 11 bit resolution  return (float)(125.0/1024.0)*(float)(a-gyro_null); //+-125 degree/sec gyro, 11 bit resolution
 } }
Line 261: Line 267:
 void set_servo(float setting) void set_servo(float setting)
 { {
- u08 sreg = SREG; + OCR1A=(u16)(pwm_count_time*(3.0+setting)); //pwm = 1.5 +-0.5us
-    cli(); +
-    OCR1A=(u16)(pwm_count_time*(3.0+setting)); //pwm = 1.5 +-0.5us +
- SREG = sreg;+
 } }
    
 ISR(TIMER1_COMPA_vect) //fires at the end of the PWM pulse ISR(TIMER1_COMPA_vect) //fires at the end of the PWM pulse
 { {
- sei(); //we need to be able to nest UART interrupts in here+ sei(); //we need to be able to nest UART interrupts in here
  vector measurement_vector;  vector measurement_vector;
  static u08 timer;  static u08 timer;
  static vector control_vector;  static vector control_vector;
 + static kalman_state our_state_local;
  static float integral;  static float integral;
  static float target;  static float target;
  float a;  float a;
  control_vector.t=0.0; //we control one servo, bottom  control_vector.t=0.0; //we control one servo, bottom
 + if(!timer)
 + {
 + our_state_local=our_state; //sets up the correct local state
 + }
  if(Heading_flag)  if(Heading_flag)
  {  {
  our_model.H.tl=1.0;  our_model.H.tl=1.0;
- measurement_vector.t=Heading; //from the main loop nav code+ measurement_vector.t=Heading; //from the main loop nav code
  target=Target; //copy over the global variable  target=Target; //copy over the global variable
  Heading_flag=FALSE;  Heading_flag=FALSE;
Line 293: Line 301:
  {  {
  our_model.H.br=0.0;  our_model.H.br=0.0;
- measurement_vector.b=0.0; //faulty gyro, limp home mode+ measurement_vector.b=0.0; //faulty gyro, limp home mode
  }  }
  else  else
Line 299: Line 307:
  our_model.H.br=1.0;  our_model.H.br=1.0;
  }  }
- our_state=predict_and_update(&our_state,&our_model,&control_vector,&measurement_vector); + our_state_local=predict_and_update(&our_state_local,&our_model,&control_vector,&measurement_vector); 
- if(our_state.state.t>180) //keeps us in the =-180 degree range+ if(our_state_local.state.t>180) //keeps us in the =-180 degree range
  {  {
- our_state.state.t-=360;+ our_state_local.state.t-=360;
  }  }
- if(our_state.state.t<-180)+ if(our_state_local.state.t<-180)
  {  {
- our_state.state.t+=360;+ our_state_local.state.t+=360;
  }  }
- a=our_state.state.t-target;+ a=our_state_local.state.t-target;
  if(a>180) //heading offset  if(a>180) //heading offset
  {  {
Line 317: Line 325:
  a+=360;  a+=360;
  }  }
- integral+=a; //integral term limits+ integral+=a; //integral term limits
  if(integral>integral_limit)  if(integral>integral_limit)
  {  {
Line 328: Line 336:
  if(timer>4)  if(timer>4)
  {   {
- control_vector.b=P_C*a+I_C*integral+D_C*our_state.state.b; //PID control + control_vector.b=P_C*a+I_C*integral+D_C*our_state_local.state.b; //PID control 
- if(control_vector.b>1) //servo limits+ if(control_vector.b>1) //servo limits
  {  {
  control_vector.b=1.0;  control_vector.b=1.0;
Line 349: Line 357:
  }  }
  #endif  #endif
 + if(!Kalman_flag) //our global kalman state is unlocked
 + {
 + our_state=our_state_local;
 + }
 } }
    
Line 430: Line 442:
  latitude='S';  latitude='S';
  }  }
 + Kalman_flag=TRUE; //lock the kalman data
  while((TCNT1>>8)>(safe_time_start) && (TCNT1>>8)<(safe_time_end)) //wait until we arent going to be interrupted  while((TCNT1>>8)>(safe_time_start) && (TCNT1>>8)<(safe_time_end)) //wait until we arent going to be interrupted
  sprintf(outputbuffer,"%.6f,%c,%.6f,%c,%.1f,M,%.1f,%.1f,%.1f,%.1f,%.1f,%X",(double)gps->latitude,latitude,  sprintf(outputbuffer,"%.6f,%c,%.6f,%c,%.1f,M,%.1f,%.1f,%.1f,%.1f,%.1f,%X",(double)gps->latitude,latitude,
  (double)gps->longitude,longitude,(double)gps->altitude,(double)Heading,(double)Target,(double)our_state.state.t,  (double)gps->longitude,longitude,(double)gps->altitude,(double)Heading,(double)Target,(double)our_state.state.t,
  (double)our_state.state.b,(double)getbatteryvoltage(),pwm_counter); //we generate a string-position and filter info  (double)our_state.state.b,(double)getbatteryvoltage(),pwm_counter); //we generate a string-position and filter info
 + Kalman_flag=FALSE; //unlock the data
  if(eeprom_read_byte(&flight_status)) //reuse the latitude variable  if(eeprom_read_byte(&flight_status)) //reuse the latitude variable
  {  {
Line 448: Line 462:
 int put_char(char c, FILE * stream) int put_char(char c, FILE * stream)
 { {
- loop_until_bit_is_set(UCSR0A, UDRE0);         //unbuffered tx comms+ loop_until_bit_is_set(UCSR0A, UDRE0); //unbuffered tx comms
  UDR0 = c;  UDR0 = c;
   return 0;   return 0;
Line 465: Line 479:
  int Heightupto=0;  int Heightupto=0;
  float K;  float K;
- gps_type gps; //local gps 
  float Wind_x=0;  float Wind_x=0;
  float Wind_y=0;  float Wind_y=0;
Line 474: Line 487:
  if(undead_man) //dead man switch to gnd D.4  if(undead_man) //dead man switch to gnd D.4
  {  {
- while(!gps.status) //wait for a gps lock+ while(!Gps.status) //wait for a gps lock
  {  {
- printf("Waiting for GPS\r\n%.5f\r\n%.5f",(double)gps.latitude,(double)gps.longitude);+ printf("Waiting for GPS\r\n%.5f\r\n%.5f",(double)Gps.latitude,(double)Gps.longitude);
  wdt_reset();  wdt_reset();
- while(!gps.packetflag) //wait for some new gps + Gps.packetflag=FALSE; 
- gps.packetflag=FALSE;+ while(!Gps.packetflag); //wait for some new gps
  }  }
  for (L=0;L<255;L++)  for (L=0;L<255;L++)
Line 487: Line 500:
  wdt_reset();  wdt_reset();
  }  }
- if(eeprom_read_byte(&flight_status)) //set eeprom status to ascending+ if(eeprom_read_byte(&flight_status)) //set eeprom status to ascending
  {  {
  eeprom_write_byte(&flight_status,0x00);  eeprom_write_byte(&flight_status,0x00);
Line 501: Line 514:
  disable_ground_control();  disable_ground_control();
  #endif  #endif
- L=0; //need it =0 for next stage+ L=0; //need it =0 for next stage
  printf("Ok, ready for launch\r\n");  printf("Ok, ready for launch\r\n");
  }  }
- if(!eeprom_read_byte(&flight_status)) //0x00=ascent+ if(!eeprom_read_byte(&flight_status)) //0x00=ascent
  {  {
- while(!cutdownrulecheck(time,&(gps.altitude)))+ while(!cutdownrulecheck(time,&(Gps.altitude)))
  {  {
   time++;   time++;
-  Wind_x += gps.speed*sin(Deg2rad*gps.heading);            //add it to our total wind +  Wind_x += Gps.speed*sin(Deg2rad*Gps.heading);            //add it to our total wind 
-  Wind_y += gps.speed*cos(Deg2rad*gps.heading);+  Wind_y += Gps.speed*cos(Deg2rad*Gps.heading);
   L++;                              //incrament the denominator for our averaging   L++;                              //incrament the denominator for our averaging
                 //find the altitude in 50m incraments                 //find the altitude in 50m incraments
- if ((int)(gps.altitude/50.0) > Heightupto)    //do we now fall into another (higher) 50m incrament ?+ if ((int)(Gps.altitude/50.0) > Heightupto)    //do we now fall into another (higher) 50m incrament ?
   {   {
  Heightupto++;                             Heightupto++;                           
Line 521: Line 534:
  }  }
  wiggleservo();  wiggleservo();
- if (radio_cts)                     //CTS from radio+ if (radio_cts)                     //CTS from radio
  {  {
- dataprint(&gps);+ dataprint(&Gps);
  }  }
  wdt_reset();  wdt_reset();
- while(!Gps.packetflag) //wait for some new gps + Gps.packetflag=FALSE; //"Unlock" global structure 
- gps=Gps; //copy over global variable + while(!Gps.packetflag); //wait for some new gps
- Gps.packetflag=FALSE;+
  }  }
  cutdown(12);  cutdown(12);
- eeprom_write_byte(&flight_status,0x01); //we have cutdown=decent+ eeprom_write_byte(&flight_status,0x01); //we have cutdown=decent
  }  }
  sbi(TIMSK1, OCIE1A); //enable output compare interrupt - turns on guidance code  sbi(TIMSK1, OCIE1A); //enable output compare interrupt - turns on guidance code
  for(;;)  for(;;)
  {  {
- L=(int)gps.altitude/50.0;+ L=(int)Gps.altitude/50.0;
  Wind_x=(float)(eeprom_read_byte(&windeast[L])-127);  Wind_x=(float)(eeprom_read_byte(&windeast[L])-127);
  Wind_y=(float)(eeprom_read_byte(&windnorth[L])-127); //load valid wind data  Wind_y=(float)(eeprom_read_byte(&windnorth[L])-127); //load valid wind data
  //direction to target (uses equatorial degree units)  //direction to target (uses equatorial degree units)
- Target = Rad2deg*atan2((targeteast - gps.longitude)*cos(Deg2rad*gps.latitude),targetnorth - gps.latitude);+ Target = Rad2deg*atan2((targeteast - Gps.longitude)*cos(Deg2rad*Gps.latitude),targetnorth - Gps.latitude);
          //wind compensation  in ENU frame          //wind compensation  in ENU frame
- Heading =Rad2deg*atan2(gps.speed*sin(gps.heading)-Wind_x,gps.speed*cos(gps.heading)-Wind_y);+ Heading =Rad2deg*atan2(Gps.speed*sin(Gps.heading)-Wind_x,Gps.speed*cos(Gps.heading)-Wind_y);
  Heading_flag=TRUE;  Heading_flag=TRUE;
- K=Heading-Target; //k is now our heading offset (from now on is just for leds)+ K=Heading-Target; //k is now our heading offset (from now on is just for leds)
  if (K >180)  if (K >180)
  {  {
  K -= 360;  K -= 360;
- }                                                       //all to keep us in +-180 degree range+ } //all to keep us in +-180 degree range
  if(K < -180)  if(K < -180)
  {  {
Line 561: Line 573:
  led_right;     led_right;   
  }  }
-  if (radio_cts)                     //CTS from radio+  if (radio_cts)                     //CTS from radio
  {  {
- dataprint(&gps);+ dataprint(&Gps);
  }  }
  #ifdef GROUND  #ifdef GROUND
- if(gps.altitude<200.0) //near the ground, ground control+ if(Gps.altitude<200.0) //near the ground, ground control
  {  {
  enable_ground_control();  enable_ground_control();
Line 572: Line 584:
  #endif  #endif
  wdt_reset();  wdt_reset();
- while(!Gps.packetflag) //wait for some new gps +  Gps.packetflag=FALSE; //"Unlock" the global variable, so GPS parsing ISR can update it 
- gps=Gps; //copy over the global variable + while(!Gps.packetflag); //wait for some new gps
- Gps.packetflag=FALSE;+
  }  }
 } }
Line 606: Line 617:
  sprintf(outputbuffer,"%s%X",outputbuffer,checksum(outputbuffer)); //we generate a string   sprintf(outputbuffer,"%s%X",outputbuffer,checksum(outputbuffer)); //we generate a string 
  return outputbuffer; //with checksum, containing position and filter info  return outputbuffer; //with checksum, containing position and filter info
-}*/ +}*/</code>
-</code>+
code/parafoil_main.1209600405.txt.gz · Last modified: 2008/07/19 23:31 (external edit)
Donate Powered by PHP Valid HTML5 Valid CSS Driven by DokuWiki