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 98: Line 103:
 int get_char( FILE * stream); int get_char( FILE * stream);
 </code> </code>
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
  
 ===== Main.c ===== ===== Main.c =====
Line 131: 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 138: Line 125:
 volatile u08 gyro_error=0x00; volatile u08 gyro_error=0x00;
    
-gps_type gps;+volatile gps_type Gps;
 //ground control globals //ground control globals
 #ifdef GROUND #ifdef GROUND
Line 183: Line 170:
    
  // set top count value to give correct period PWM  // set top count value to give correct period PWM
- ICR1 = (u16)F_CPU*delta_time/8.0;+ ICR1 = top_count;
    
  // clear output compare value A  // clear output compare value A
Line 210: 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 217: 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 230: 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 253: Line 240:
 float read_gyro() float read_gyro()
 { {
- static float turn_rate; 
  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 275: 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
 } }
- +  
-void set_PWM1A(u16 setting)+void set_servo(float setting)
 { {
- u08 sreg = SREG; + OCR1A=(u16)(pwm_count_time*(3.0+setting)); //pwm = 1.5 +-0.5us
-        cli(); +
-        OCR1A=setting; +
-        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 313: 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 319: 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 337: 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 348: 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 357: Line 345:
  control_vector.b=-1.0;  control_vector.b=-1.0;
  }  }
- set_PWM1A( (u16)(((float)F_CPU/(500.0*8.0))*(3.0+control_vector.b)) ); //PWM=1.5+-0.5 uS+ set_servo(control_vector.b);
  }  }
  else  else
Line 366: Line 354:
  if(pwm_counter==0x0A) //if under ground control, use the recorded pwm as a control input  if(pwm_counter==0x0A) //if under ground control, use the recorded pwm as a control input
  {  {
- control_vector.b=(float)(pwm_period-3*pwm_record_factor); //1500us -> 0, 2000 -> +2, 1000 -> -1+ control_vector.b=((float)pwm_period*(1.0/pwm_record_factor)-3.0);//1500us->0,2000->+1,1000->-1
  }  }
  #endif  #endif
 + if(!Kalman_flag) //our global kalman state is unlocked
 + {
 + our_state=our_state_local;
 + }
 } }
- +  
-u08 cutdownrulecheck(int time)+u08 cutdownrulecheck(int time, float * altitude)
 { {
- if(gps.altitude>altitudelimit)+ if(*altitude>altitudelimit)
  {  {
  printf("CUTDOWN, Altitude limit\r\n");  printf("CUTDOWN, Altitude limit\r\n");
Line 409: Line 401:
 void wiggleservo() void wiggleservo()
 { {
- OCR1A =(u16)(((float)F_CPU/500.0)*4.0);+ set_servo(1);
  _delay_ms(150);  _delay_ms(150);
- OCR1A =(u16)(((float)F_CPU/500.0)*2.0);+ set_servo(-1);
  _delay_ms(150);  _delay_ms(150);
- OCR1A =(u16)(((float)F_CPU/500.0)*3.0);+ set_servo(0);
 } }
    
Line 428: Line 420:
  return battery_factor*((float)(inb(ADCL) | (inb(ADCH)<<8)));    // read ADC (full 10 bits);  return battery_factor*((float)(inb(ADCL) | (inb(ADCH)<<8)));    // read ADC (full 10 bits);
 } }
- +  
-void dataprint()+void dataprint(gps_type * gps)
 { {
  char outputbuffer[80];  char outputbuffer[80];
  char longitude;  char longitude;
  char latitude;  char latitude;
- if (gps.longitude>0)+ if (gps->longitude>0)
  {  {
  longitude='E';  longitude='E';
Line 442: Line 434:
  longitude='W';  longitude='W';
  }  }
- if (gps.latitude>0)+ if (gps->latitude>0)
  {  {
  latitude='N';  latitude='N';
Line 450: Line 442:
  latitude='S';  latitude='S';
  }  }
- sprintf(outputbuffer,"%.6f,%c,%.6f,%c,%.1f,M,%.1f,%.1f,%.1f,%.1f,%.1f,%X",(double)gps.latitude,latitude, + Kalman_flag=TRUE; //lock the kalman data 
- (double)gps.longitude,longitude,(double)gps.altitude,(double)Heading,(double)Target,(double)our_state.state.t,+ 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, 
 + (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 467: 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 484: Line 479:
  int Heightupto=0;  int Heightupto=0;
  float K;  float K;
- float Windx_total=0; + float Wind_x=0; 
- float Windy_total=0; + float Wind_y=0;
- float Wind_x; +
- float Wind_y;+
  FILE mystdio = FDEV_SETUP_STREAM(put_char, get_char, _FDEV_SETUP_RW);  //so we can printf to the radio  FILE mystdio = FDEV_SETUP_STREAM(put_char, get_char, _FDEV_SETUP_RW);  //so we can printf to the radio
  setup();  setup();
Line 494: 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 507: 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 521: 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))+ while(!cutdownrulecheck(time,&(Gps.altitude)))
  {  {
   time++;   time++;
-  Windx_total += 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 
-  Windy_total += 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++;                           
- eeprom_write_byte(&windeast[Heightupto],(u08)(Windx_total/L + 127));         //we are storing as a signed byte + eeprom_write_byte(&windeast[Heightupto],(u08)(Wind_x/L + 127));         //we are storing as a signed byte 
- eeprom_write_byte(&windnorth[Heightupto],(u08)(Windy_total/L + 127));         //we find the average and shove it in the eeprom+ eeprom_write_byte(&windnorth[Heightupto],(u08)(Wind_y/L + 127));         //we find the average and shove it in the eeprom
  L = 0;                                                     L = 0;                                                   
  }  }
  wiggleservo();  wiggleservo();
- if (radio_cts)                     //CTS from radio+ if (radio_cts)                     //CTS from radio
  {  {
- dataprint();+ dataprint(&Gps);
  }  }
  wdt_reset();  wdt_reset();
- while(!gps.packetflag) //wait for some new gps + Gps.packetflag=FALSE; //"Unlock" global structure 
- gps.packetflag=FALSE;+ while(!Gps.packetflag); //wait for some new gps
  }  }
  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 580: Line 573:
  led_right;     led_right;   
  }  }
-  if (radio_cts)                     //CTS from radio+  if (radio_cts)                     //CTS from radio
  {  {
- dataprint();+ 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 591: 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.packetflag=FALSE;+ while(!Gps.packetflag); //wait for some new gps
  }  }
 } }
 + 
 /*  /* 
 char *datastring() //old code, might be useful char *datastring() //old code, might be useful
code/parafoil_main.1209600360.txt.gz · Last modified: 2008/07/19 23:31 (external edit)
Donate Powered by PHP Valid HTML5 Valid CSS Driven by DokuWiki