UKHAS Wiki

UK High Altitude Society

User Tools

Site Tools

Trace: • kml_live_update
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/06 04:21] – proper use of semaphores 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 192: 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 199: 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 212: 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 236: 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 256: 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 267: Line 272:
 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;
Line 283: Line 288:
  {  {
  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 296: 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 303: Line 308:
  }  }
  our_state_local=predict_and_update(&our_state_local,&our_model,&control_vector,&measurement_vector);  our_state_local=predict_and_update(&our_state_local,&our_model,&control_vector,&measurement_vector);
- if(our_state_local.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_local.state.t-=360;  our_state_local.state.t-=360;
Line 320: 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 332: Line 337:
  {   {
  control_vector.b=P_C*a+I_C*integral+D_C*our_state_local.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 457: 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 482: 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);
Line 495: 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 509: 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)))
Line 529: Line 534:
  }  }
  wiggleservo();  wiggleservo();
- if (radio_cts)                     //CTS from radio+ if (radio_cts)                     //CTS from radio
  {  {
  dataprint(&Gps);  dataprint(&Gps);
Line 538: Line 543:
  }  }
  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
Line 551: Line 556:
  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 568: Line 573:
  led_right;     led_right;   
  }  }
-  if (radio_cts)                     //CTS from radio+  if (radio_cts)                     //CTS from radio
  {  {
  dataprint(&Gps);  dataprint(&Gps);
Line 612: 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.1210047666.txt.gz · Last modified: 2008/07/19 23:31 (external edit)
Donate Powered by PHP Valid HTML5 Valid CSS Driven by DokuWiki