C+ Program Listing (AUTO1P5)

The following is a listing of one of the versions of the autopilot program.  The programming language is Z-World's C+.  Note that this version of the program does not include the altitude shift feature.  That featured required only a simple program change to allow the desired altitude variable (alt_target) to be continuously changed at a selected rate; the autopilot would simply "chase" this moving target.  This feature allowed altitude changes being done without disengaging and reengaging the autopilot.

The bug that is mentioned in the prefatory notes was the result of the BL1100 computer remembering the values of all variables from one powering on of the computer to the next.  One counter variable was never reset to zero by the program and, after hundreds of hours of use, this field "overflowed", causing the program to malfunction.  That programming bug was easily fixed.

// auto1p5.c (SOURCE CODE MODULE is AUTO1P5.DOC)

// THIS BLOCK OF COMMENTS WAS ADDED ON 02-28-07.

// THIS IS APPARENTLY THE ONLY REMAINING COPY OF ANY OF MY AUTOPILOT PROGRAMS.

// THE CURRENT AND FINAL VERSION WAS AUTO1P8.DOC.  AFTER AUTO1P5.DOC, FURTHER

// MODIFICATIONS WERE MADE TO FIX THE FOSSETT STRATOBOWL BUG THAT RENDERED ONE

// OF STEVE’S AUTOPILOTS INOPERABLE DUE TO A COUNT FIELD NEVER BEING RESET, AND

// SUBSEQUENTLY OVERFLOWING.  ALSO, AN ADDITIONAL CHANGE WAS MADE TO SUPPORT

// THE ALTITUDE SHIFT FEATURE.

// END OF 02-28-07 COMMENTS.  --bpc 

Includes the following mods:

// auto1p2.c, 08-30-95 (after first flight test with no engage)

//   1. rfi checking disabled (circuit is unstable)

//   2. minimum burn time reduced to 0.05 seconds (minimum time too large

//        eliminates too much of rhythm heating for small duty cycles;

//        minimum burn time must be > 0 to avoid counter interrupt relay

//        timing errors)

// auto1p3.c, 09-02-95 (after second flight test with slow drift up or down)

//   1. vv & acc coefficients corrected in weighted sum (coefficients

//        were for ft/sec & ft/sec/sec, but vv & acc units in this

//        program are ft/MIN and ft/MIN/MIN!)

//   2. acceleration calculated from vertical velocity difference over

//        five seconds (to damp out noise)

//   3. effect of acceleration in weighted sum reduced by half (based on

//        further simulation)

// auto1p4.c, 09-23-95 (three weeks after first good test flight)

//   1. added code to update d_c every five minutes to the moving average

//        of the d_c's for the last five five-minute periods (this should

//        cope better with the balloon's changing heat requirements caused

//        by sunset/sunrise or by the loss of weight)

// auto1p5.c, 12-04-95 (after Aspen to Altoona R-77 flight test)

//   1. added code to zero last_total_burn_cntr on switch from manual mode to

//        autopilot mode (lack of this code was causing mysterious failure to

//        heat at all with all else looking ok)

//   2. altered burn-requesting code to prevent interrupt routine from

//        shutting relay (n) off before program turned relay (n+1) on (this

//        problem sometimes resulted in a relay staying on for six heating

//        cycles (locking on))

//   3. added code to bitsleds routine to turn off all relays if ap_rqstd_sw

//        is off (precaution to be sure relay does not stay on when autopilot

//        is switched back to manual)  

//   4. added code to allow selecting cycle time (if blast switch is held down

//        while switching from manual to autopilot then cycle is 15 seconds,

//        otherwise cycle is 5 seconds)

                                   

   shared double cntr;             // counts from ap_rqstd_sw=1

   shared double total_burn_cntr;  // total burn time since cntr=0

   shared unsigned int heat_on_sw; // indicates burner is on or off

   shared double burn_end_cntr;    // time at which to end burn

   shared int relay;               // number of next relay to use (0-5)

   shared unsigned int blast_sw;   // blast sw closed  (bit in||pgm; LED5 on)

   unsigned int sol_enable_sw; // rotary sw pos 3||4 (bit in;      LED2 on)

   unsigned int ap_rqstd_sw;   // rotary sw pos 4    (bit in;             )

   unsigned int ap_in_com_sw;  // a/p has accepted control   (pgm; LED6 on) 

   unsigned int malf_sw;       // malfunction detected       (pgm; LED8 on)

   unsigned int rfi_sw;        // rfi detected               (pgm; LED7 on)

   unsigned int spare_sw;      // (meaning undefined)(             LED3 on)

   unsigned int old_ap_rqstd_sw; // used to recognize change to ap_rqstd_sw=1

   float d_c;                  // average duty cycle of burner

   float rfi_count;            // rfi count

   double vv;                  // VERTICAL VELOCITY (feet/minute)     

   double acc;                 // ACCELERATION (feet/min/min)

   double dalt;                // (alt - alt_target) (feet)              

   float cycle;                // length of decision cycle (seconds)

  

main()

{

   long int i,j;              // only to control iteration           

   float a;                   // accumulated altitude counts         

   float A;                   // average of altitude counts           

   float ah,al;               // altitude high, low so far            

   float sa1,sa2,sa3,sa4;     // last four values of A               

   float alt_ave;             // average of last four values of A    

   float temp;                // temporary variable used for calc    

   float alt;                 // ALTITUDE (feet amsl)                

   unsigned int first_loop;   // indicates first pass though main pgm loop

   float v;                   // accumulated vv counts               

   float V;                   // average of vertical velocity count  

   float vl,vh;               // vertical velocity high, low so far  

   float sv1,sv2,sv3,sv4;     // last four values of V               

   float var_ave;             // average of last four values of V    

   float vv_alt;              // altitude calculated from vv         

   float vv_zero;             // factor added in vv calc to adjust   

                              //   for zero error detected by program  

   double alt_cntr;           // value of cntr for current alt value read 

   double old_alt_cntr;       // value of cntr for old alt value     

   double vv_cntr;            // value of cntr for current vv value read

   double old_vv_cntr;        // value of cntr for old vv value

   double var_cntr;           // value of cntr at beginning of       

                              //   period of -100<vv<+100              

   double vv_zero_cntr;       // value of cntr at last vv_zero calc  

   double AC;                 // calculated acceleration (unused?)

   float sac1,sac2,sac3,sac4,sac5,sac6,sac7,sac8,sac9,sac10;

                              // last ten values of AC  (unused?)               

   float old_vv;              // prior value of vv

   float alt_target;          // altitude selected for level flight

   void bitsleds();           // routine to gets bits and set LEDs

   void audit();              // check for error conditions

   void hitwd();              // routine to reset watchdog timer

   int wderror();             // non-zero if reset was watchdog

   double old_dalt;           // saved value of dalt; for d_c reset routine

   double old_cntr;           // time AT start of last pass thru main loop

   double this_burn;          // length (in seconds) of latest burn

   double adjust_burn;        // burn length adjustment from dalt, vv, and acc

   double factor;             // relative weight of dynamic variables vs d_c

   double delta_time;         // time since last burn evaluation

   double d_c_cntr;           // cntr value for next d_c update

   void error_rtn();          // error routine; loops WITHOUT hitwd()

   float last_total_burn_cntr;    // value five minutes ago

   float dc5, dc4, dc3, dc2, dc1; // last five values of d_c

 

#if ROM

   ERROR_EXIT=error_rtn;      // only appears in EPROM version

#else     

   runwatch();                // doesn't appear in EPROM version

#endif

 

   hitwd();                   // reset watchdog timer

 

   if (wderror()) {           // initialize ap_in_com_sw

      if (ap_in_com_sw) {;}   //   and d_c if appropriate

         else {d_c=0;}        //

      for (i=0; i<5; i++) {           // flash malf LED and sound alarm

                   iset ( PIODB,2 );  //   five times to indicate that

                   Stall(50);         //   a watchdog-timer-initiated

                   ires ( PIODB,2 );  //   restart has occurred

                   Stall(500);

                          }

                  }

      else {ap_in_com_sw=0;   // zero ap_in_com_sw and d_c

            old_ap_rqstd_sw=0; // initialize old_ap_rqstd_sw

            d_c=0;}           //   on normal startup

/***************** INITIALIZE VARIABLES AT STARTUP *********************/

   for (relay=0; relay<6; relay++) {Set_PBus_Relay(0x007,relay,0);}

                              // QUICKLY make sure relay contacts are open

/************************** SET UP BIT IO  *****************************/

   outport ( PIOCB,0xCF );    // select Port B in Mode 3

   outport ( PIOCB,0x13 );    // select bits 0001 0011 as input

/************************** TURN OFF ALL LEDS **************************/

   IRES ( PIODB,2 );          // malfunction LED (PB2) (GND is OFF)

   ISET ( PIODB,3 );          // solenoid valve disabled LED (PB3)

   ISET ( PIODB,5 );          // autopilot in command LED (PB5)

   ISET ( PIODB,6 );          // RFI LED (PB6)

   ISET ( PIODB,7 );          // blast valve selected LED (PB7)

// Note that +5V is OFF for all except malfunction LED

/***************** SET UP AND START COUNTER (cntr) *********************/

   outport ( DCNTL, 0x30 );   //                                     

   setdaisy ( 3 );            // setup KIO daisy chain               

   setctc ( 2, 1, 180, 1 );   // initialize ctc, enable interrupts   

/********************** STATUS-MONITOR SHELL LOOP **********************/

 while(1)

 {

   hitwd();                   // RESET WATCHDOG TIMER

/************************ INITIALIZE VARIABLES *************************/

   cntr=0;           var_cntr=0;       vv_zero_cntr=0;   vv_zero=0;

   first_loop=1;

   al=9999;          vl=9999;

   i=0;              j=0;              ah=0;             vh=-9999;

   total_burn_cntr=0;

   heat_on_sw=0;     rfi_sw=0;

   acc=0;            vv=0;             dalt=0;   // feet, minutes, seconds;

   factor=2.0;

   relay=0;

   d_c_cntr=60000;  // do first d_c update at 5 minutes from a/p in com

 

   while (!ap_in_com_sw) { // NOT a watchdog restart out of ap in com loop

      hitwd();             // reset watchdog timer

      bitsleds();

      if (ap_rqstd_sw) {cntr=total_burn_cntr=0;}                

      while ((ap_rqstd_sw) && (!ap_in_com_sw)) {

      if ((cntr>12000) && (total_burn_cntr>1200) &&

            ((total_burn_cntr/cntr)>.01) && ((total_burn_cntr/cntr)<.5))

         {

          d_c=total_burn_cntr/cntr;

          last_total_burn_cntr=total_burn_cntr;

          d_c_cntr=cntr+60000;

          dc5=d_c; dc4=d_c; dc3=d_c; dc2=d_c; dc1=d_c;

          ap_in_com_sw=1;

          for (i=0; i<2; i++) {  // flash malfunction LED and sound alarm

                              iset ( PIODB,2 );

                              Stall(50);

                              ires ( PIODB,2 );

                              Stall(500);

                              }

         }                

      hitwd();             // reset watchdog timer

      bitsleds();      

                                               }

                         }

/******************** AUTOPILOT-IN-COMMAND LOOP ***********************/

   while (ap_in_com_sw) {

      hitwd();                 // reset watchdog timer

/**************** INITIALIZE VARIABLES FOR MAIN LOOP ******************/

      a=0; v=0;                // zero alt and vv accumulators

/********* READ PRESSURE TRANSDUCER AND CALCULATE alt IN FEET *********/

      for (i=0; i<200; i++) {

         bitsleds();       // invoke function to read bits and set LEDs

         Stall(10);

         a = a + ad_rd12(14);} // read pressure transducer 200 times

      alt_cntr=cntr;           // save time of altitude read

      A = a/i;                 //   and average pressure counts

      if (first_loop==1) {sa4=A; sa3=A; sa2=A; sa1=A;}

          else

      {sa4=sa3; sa3=sa2; sa2=sa1; sa1=A;}

      alt_ave=(sa4+sa3+sa2+sa1)/4;  // average last four reading sets

      temp=pow(((alt_ave+430.446)*0.000238404),-0.19026);

      alt=-((288.15/temp)-288.15)/0.00198;  // calculate alt (in feet)

      if (first_loop==1) {vv_alt=alt;

                         alt_target=alt;   

                         old_alt_cntr=cntr;

                        }

      dalt=alt-alt_target;     // immediately calculate dalt!

/******************* SAVE HIGH AND LOW ALT SO FAR *********************/

      j=j+1;

      if (j>3) {

        if (alt<al) {al=alt;}

        if (alt>ah) {ah=alt;}

               }

 

      hitwd();                // reset watchdog timer

/********* READ VARIOMETER AND CALCULATE vv IN FEET/MINUTE ************/

      for (i=0; i<200; i++) {

         bitsleds();       // invoke function to read bits and set LEDs

         Stall(10);

         v = v + ad_rd12(4);}  // read rate of climb

      vv_cntr=cntr;            // save time of vv read

      var_ave = v/i;

      vv=(var_ave/1.365)+vv_zero;    // calculate vv (in feet/min)

      if (vv<vl) {vl=vv;}

      if (vv>vh) {vh=vv;}

/**************** CALCULATE acc FROM vv AND old_vv ********************/

//      if (first_loop==1) {AC=0;}

//          else {AC=((vv-old_vv)*12000.0)/(vv_cntr-old_vv_cntr);

//               }             

//      if (first_loop==1) {sac10=AC; sac9=AC; sac8=AC; sac7=AC;

//            sac6=AC; sac5=AC; sac4=AC; sac3=AC; sac2=AC; sac1=AC;}

//          else

//      {sac10=sac9; sac9=sac8; sac8=sac7; sac7=sac6; sac6=sac5;

//        sac5=sac4; sac4=sac3; sac3=sac2; sac2=sac1; sac1=AC;}

//      acc=(sac10+sac9+sac8+sac7+sac6+sac5+sac4+sac3+sac2+sac1)/10;

                                    // average last ten reading sets

                                    // acc (in feet/min/min)

//      old_vv=vv;

//      old_vv_cntr=vv_cntr;        

/*********************** PRINT DATA FOR TESTING ***********************/

#if !ROM

  printf("%.1f %.1f %.1f  %.1f %.1f %.1f  %.1f  %.0f %.2f  %.2f  %.3f %.3f\n",

    alt,al,ah,vv,vl,vh,vv_alt,vv_zero_cntr,vv_zero,acc,

    total_burn_cntr/cntr,d_c);

#endif

/*************** CALCULATE CURRENT ALTITUDE FROM VV *******************/

      vv_alt=vv_alt+(vv*(alt_cntr-old_alt_cntr)/12000);

      old_alt_cntr=alt_cntr;

/******************** CHECK FOR ERROR CONDITIONS **********************/

      // latest possible acc, vv, dalt, d_c must be available here!

      audit();                 // call error checking routine

      hitwd();                 // reset watchdog timer

/************************ MAKE BURN DECISION **************************/

   if (first_loop) {old_cntr=cntr; delta_time=0;}

      else {delta_time=(cntr-old_cntr)/200.0;}

 

   if ((!heat_on_sw) && (delta_time>cycle))  // assess need to heat only if

                     {                   // not already heating AND cycle

      old_cntr=cntr;                     // seconds have passed since

                                         // start of last assessment

     

      if (first_loop) {acc=0;}              // calculate acceleration from

        else

      {acc=(vv-old_vv)/(delta_time/60.0);}  //   last five seconds' change

      old_vv=vv;                            //   in vertical velocity

                                        

      adjust_burn=-delta_time*d_c*\

         ((dalt/33.3)+(vv/16.7)+(acc/66.6)); // units error corrected and

                                             //   acceleration divisor

                                             //   changed from 33.3 to 66.6

                                             //   on 09-02-95 (auto1p3.c)

      if (adjust_burn>(factor*delta_time*d_c))\

         adjust_burn=(factor*delta_time*d_c);

         else

      if (adjust_burn<-(delta_time*d_c))\

         adjust_burn=-(delta_time*d_c);

      this_burn=(delta_time*d_c)+adjust_burn;

      if (this_burn<.05) {this_burn=0;} // prevents burn too short to make,

                                        //   but also will cause balloon to

                                        //   tend to fly below target alt

                                        //   ("dynamic heating" takes effect)

         else {                                 // this block does burn

               relay=relay+1;                   // increment

               if (relay>5) {relay=0;}          //   relay number

               burn_end_cntr=cntr+(this_burn*200.0); // set time to stop burn

               Set_PBus_Relay(0x007,relay,1);   // CLOSE relay contacts

               heat_on_sw=1;                    // indicates burn under way

               ires(PIODB,7);                   // turn ON blast LED

              }

                     }

 

     if (d_c_cntr<cntr) {dc5=dc4; dc4=dc3; dc3=dc2; dc2=dc1;

                         dc1=(total_burn_cntr-last_total_burn_cntr)/60000.0;

                         d_c_cntr=cntr+60000;

                         last_total_burn_cntr=total_burn_cntr;

                         d_c=(dc5+dc4+dc3+dc2+dc1)/5.0;

                        }

/******** CALCULATE NEW VARIO ZERO ADJUSTMENT WHEN APPROPRIATE ********/

//  reset vario_zero only after |vv| has been <100 ft/min for 10 seconds

//  AND alt is within 200 feet of alt_target AND 5 minutes have elapsed

//  since last vario_zero reset

      if (vv>100 || vv<-100) var_cntr=alt_cntr;

          else

      if (((alt_cntr-var_cntr)>2000) && (dalt<200 && dalt>-200) &&

           ((alt_cntr-vv_zero_cntr)>60000)) {

          vv_zero=vv_zero+

            (((alt-vv_alt)*12000)/(alt_cntr-vv_zero_cntr));

          vv_zero_cntr=alt_cntr;

          vv_alt=alt;                      

                                            }

      first_loop=0;            // turn off first loop flag

   }

/******* end of while(ap_in_com_sw) [AUTOPILOT-IN-COMMAND LOOP] *******/

 

 }

/************ end of while(1) [STATUS-MONITOR SHELL LOOP] *************/

}

 

/************************* end of main() ******************************/

 

/******* FUNCTION TO READ BITS AND SET LEDS ***************************/

void

bitsleds()

{

/*********************** READ BITS IN *********************************/

   blast_sw      = (!IBIT ( PIODB,0 )); // GND (!IBIT) is ON

   sol_enable_sw = (!IBIT ( PIODB,1 )); // GND (!IBIT) is ON

   ap_rqstd_sw   = (!IBIT ( PIODB,4 )); // GND (!IBIT) is ON

/************************* SET LEDS ***********************************/

   if (sol_enable_sw) {ires ( PIODB,3 );

                       if (blast_sw||heat_on_sw) {ires ( PIODB,7 );}

                          else {iset ( PIODB,7 );}

                      }

      else {iset ( PIODB,3 );  // turn OFF sol-enabled LED

            iset ( PIODB,7 );} // turn OFF blast LED

   if (ap_rqstd_sw)

      if (ap_in_com_sw) {ires ( PIODB,5 );}  // turn ON  ap in com LED

          else {iset ( PIODB,5 );}           // turn OFF ap in com LED

      else {ap_in_com_sw=0;                  // turn OFF ap_in_com_sw        

            old_ap_rqstd_sw=0;               // turn OFF old_ap_rqstd_sw

            for (relay=0; relay<6; relay++) {Set_PBus_Relay(0x007,relay,0);}

            iset ( PIODB,5 );}               // turn OFF ap in com LED

   if (!old_ap_rqstd_sw)       // if old_ap_rqstd_sw not on

      if (ap_rqstd_sw) {old_ap_rqstd_sw=1;

                        if (blast_sw) {cycle=15;} else {cycle=6;}

                       } 

   return;

}

/**********************************************************************/

 

/************** FUNCTION TO CHECK FOR ERROR CONDITIONS ****************/

void

audit()

{

/************** CHECK FOR ERROR CONDITIONS AND SET BITS ***************/

   if ((acc>1000)||(acc<(-1000))) malf_sw=1; // |acc| > 1000 feet/min/min

      else                                             

   if ((vv>300)||(vv<(-300))) malf_sw=1;   // |vv| > 300 feet/min

      else                                            

   if ((dalt>500)||(dalt<-500)) malf_sw=1; // |dalt| > 500 feet

      else                                             

   if (d_c>0.5) malf_sw=1;                 // duty cycle > 0.5

      else malf_sw=0;                                 

//   rfi_count=ad_rd12(0);  // get rfi count

//   if ((rfi_count>170)||(rfi_count<(150))) {malf_sw=1; rfi_sw=1;}

//      else rfi_sw=0;                      

/************************* SET LEDS ***********************************/

   if (malf_sw)       {iset ( PIODB,2 );} else {ires ( PIODB,2 );}

   if (rfi_sw)        {ires ( PIODB,6 );} else {iset ( PIODB,6 );}

     

   return;    // return to continue running!

}

/**********************************************************************/

 

/*************************** ERROR ROUTINE ****************************/

void

error_rtn()

{

   while(1) {;}   // loop to force watchdog timeout

}

/**********************************************************************/

 

/********** INTERRUPT ROUTINE TO COUNT IN 200ths OF A SECOND **********/

#INT_VEC CTC2_VEC ccc          // set interrupt vector for interrupt rtn

 

interrupt reti int ccc ()      // increment counter 200 times per second    

{

   cntr++;

   if (heat_on_sw || blast_sw) {total_burn_cntr++;} // sum heating count

                                                    //   since pgm start      

   if (heat_on_sw)

     {if (burn_end_cntr>cntr)

           {;}

              else

           {heat_on_sw=0;                    // turn OFF heat switch

            iset(PIODB,7);                   // turn OFF blast LED

            Set_PBus_Relay(0x007,relay,0);}  // OPEN relay contacts

     }

}

/**********************************************************************/

 

/************* INTERRUPT ROUTINE TO HANDLE POWER FAILURE **************/

#JUMP_VEC NMI_VEC lowvolt

 

interrupt retn int lowvolt ()

{  int i;

   void DI();

   void EI();

   void hitwd();

     

   DI();

   while(1)

   {

      if (!powerlo()) {EI(); return;}  // not low voltage

      // fatal error: shut off solenoid valve and flash malf LED & alarm

      for(i=0; i<6; ++i) {Set_PBus_Relay(0x007,i,0);} // OPEN all

                                                      //   relay contacts

      iset ( PIODB,7 );        // turn OFF blast LED

      while (1)

      {  iset ( PIODB,2 );     // turn ON malfunction LED (top LED)

         Stall(2000);

         ires ( PIODB,2 );     // turn OFF malfunction LED

         Stall(1000);

         hitwd();              // reset watchdog timer

      }

   }

}

/**********************************************************************/