/******************************************************************************
//	miss_fnx.c			mission functions
//	
//*****************************************************************************
//	
//	Jan2009, created
//
//  0902 Jan2010 updated for profile number, dive-set number, cycle # algorithm
//       profile # now reported back as main index
//
//
//==== Notes =================================================================
//		update: May2010
//		fixed order of adp_post_process for top_turn, so adp header is written first
//      update: 12Oct2010
//      fixed exception-set bug from 0xf000 to 0x8000 for going slow on descent.
//
// 0902.5 feb11, heading control changes, based on ee_ro_use settings
//	1. time-out in menu, to quit back to base menu
//	2. verify no time conflicts with asynchronous hyd/pitch/roll functions.
//  3. verify start-up procedure mimics the old code.
//  4. adding some calls to dump hyd/pitch/roll info to flash occassionaly
//	
// 0903.4 aug11 include Benthos calls
// 0903.6 dec11, longer wait for surface pump
//  1. in do_acm add lifo storage for diag info if bails out on bad block request.
//     This ensures we get diagnostic info at the shore station
//
// aug15: added zooglider day-test code
// aug15: if SBD broken, it will now turn off manual, current-crossing.
// aug16: if SBD broken, also turn off the drift-mode.
******************************************************************************/
#include	<cfxbios.h>		// Persistor BIOS and I/O Definitions
#include	<cfxpico.h>		// Persistor PicoDOS Definitions

#include	<assert.h>
#include	<ctype.h>
#include	<errno.h>
#include	<float.h>
#include	<limits.h>
#include	<locale.h>
#include	<math.h>
#include	<setjmp.h>
#include	<signal.h>
#include	<stdarg.h>
#include	<stddef.h>
#include	<stdio.h>
#include	<stdlib.h>
#include	<string.h>
#include	<time.h>


#include    "cf2_qsm332.h"
#include    "spi_cmd.h"
#include    "gpio_mux.h"
#include    "pump.h"
#include    "pitch_roll.h"
#include    "nav.h"
#include    "sbd.h"
#include    "adp.h"
#include    "sbe.h"
#include    "tcm2.h"
#include    "gps.h"
#include    "lifo.h"
#include    "pplus.h"
#include    "dat_fnx.h"
#include    "miss_fnx.h"
#include    "setup.h"
#include    "ntk.h"
#include    "zooCam.h"

#if BENTHOS==1  || NPS==1
  #include "BenthCF2.h" //Benthos modem is being used
  short acm_xmits;             //number of commands sent to the remote
  extern short TSerInUse;  //GLOBAL defined in start.c for Benthos
                  // =1 if TSerial port of TPU is in use, else 0
                  //Used to prevent code from changing System clock or turning off
                  //the oscillator or TPU in low power states.To do so messes up 
                  //TSerial interrupt servicing and baud rates.
  short  do_acm( void );
#endif

#if PH==1
  #include "ph.h" //ph is being used
  extern short TSerInUse;  //GLOBAL defined in start.c for Benthos
                  // =1 if TSerial port of TPU is in use, else 0
                  //Used to prevent code from changing System clock or turning off
                  //the oscillator or TPU in low power states.To do so messes up 
                  //TSerial interrupt servicing and baud rates.
#endif

// globals
extern char   Verbose;  // switch for displaying more info
extern char   IRQ_Flag; // interrupt flag: says what caused the interrupt
extern short  iparam [ MAX_PARAM ]; // global parameter settings
extern struct all_param all;



//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
void   mission  ( void ) // ***************************************************
{    // main mission code

  short xmit_time, i,j, ncyc, gps_good=0;
  short iprof, op_mode, done;  //0902 switch from idive to iprof
  short ignore_gps=0; //dec11, flag to ignore gps during the mission start-up
  long  next_xmit_tm, last_xmit;
  short gps_bad = true; // default that gps is bad

  short  stat2;
  ushort psurf;

  short nsent;  // =#of messages sent through satellite
  // struct gps_param gps;
  //struct mis_param *mis;  // 0901 OBSOLETE
  //mis = all.mis; // point to the mission parameters
  
  #if DEBUG==1
    cprintf("in DEBUG mode\n");
  #else
    cprintf("in MISSION mode\n");
  #endif
   
  spi_set_wdog( 1, 255 ); // multiplier=1, all action bits active
    
    //=== ISUS ==================================================
    #if ISUS==1  // then has an ISUS sensor, initialize
      cprintf("initializing the ISUS sensor\n");
      isus_on(); // turn on the power
      cprintf("Isus power on: waiting 15s for turn-on\n");
      DelayMilliSecs(15000);
      cprintf("now initializing\n");
      i = isus_init(3); // try initializing
      cprintf("Init status = ");
      if (i) 
      { cprintf(" OK\n");
        all.isus->err = 0;
      }
      else 
      {  cprintf(" FAILED\n");
         all.isus->err = ISUS_ERR_INIT;
      }
      
      i = isus_off_rqst( 3 ); // try to put into power-off mode
      isus_off();
      all.isus->mode = ISUS_OFF;
      all.isus->err |= i; // save error code
    #endif //====================================================
    


  init_accum_err( ); // clear the accumulative error arrays
  all.roll_stat=0;
  
  all.eng->navg  = iparam [ IA_num_avg ] ; // initialize (for SBD packet)
  for (i=0; i<N_BZ; i++) 
       all.exc->bz[i] = 2000; // init.
  
  all.exc->gps_abort = FALSE; // set that GPS=OK
  all.exc->sbd_bad   = FALSE; // set that SBD=OK
  all.exc->abort     = FALSE; // set to NOT abort
  all.exc->pgood     = TRUE;  // assume that pressure is good
  all.exc->rfx       = 1;     // in abort-mode, argos time multiplier
  all.exc->nose_down = 0;     // in abort-mode, #counts to nose down
  all.exc->gps_cold  = FALSE; // dec11, allows ability to remotely do cold-reset
  iparam [ IA_argos_mult]= 0; // mission-mode multiplier when SBD is broken
  
  
  //dec11, make sure it is the value in eeprom
  op_mode    = (short) VEEFetchLong( "OPMODE", NO_VEE_VAL); // Spray mode of op
  ignore_gps = ( op_mode == -2 ); // setting op_mode remotely=-2 will ignore gps
  cprintf("op_mode = %d, ignore_gps = %d\n",op_mode, ignore_gps );
  // =0 = normal, <0 = given remote reset command, >0=spurious reset
  if ( op_mode<1 ) //+++++++++++++++++++++++++++++++++++++++++++++
  {  // normal or remote reset: do initial gps/surface comms
     // if =0, then normal start, we're on the boat
     // if <0, remote rst cmd rcv'd, must be on surface as well.
     set_op_mode(1); // set flag that we're starting anew
     // this will cause the next dive to be the 'test' shallow dive

	// 24jun09 reset the hydraulic parameters
	// this mainly resets total pump time=0.
	// this guards against entry where the oil bladders are empty,
	// =pressure-detect start
	spi_pwr_rst ( CS_HYD ); //resets pump accumulator
    all.hyd->nbad = 0; // Set #bad amps during pump to 0
     
     cprintf("Will get average of Pressure\n");
     #if SBE==1
        psurf = sbe_avg_p(4);  // get surface SBE p avg
     #else
        print( "compile error, not supported!");
     #endif
     
     if ( psurf>0 ) all.flt->pmin = psurf; // valid #: reset
     // else just use the last recorded value of pmin
     cprintf(" Pressure counts offset = %6u\n",all.flt->pmin);

     if ( op_mode == 0 )  //=====================================
     { // normal entry, write to files 
        write_config( 0   );  // write the .cf0 file
        waypt_display(TRUE);  // write waypoint file
        display_route(TRUE);  // write route file
        all.sbd->send_waypts = TRUE; //flag to send back waypts
        all.sbd->send_param  = TRUE; //flag to send back params
        // and do delay/water-detect start
        check_surf_start( );  // if fails, goes to abort code
     } //========================================================
     
    // if op_mode <0, remote reset, already on the surface
    service_wdog(); 
    
    //=== ZooGlider =============================================
    #if ZOOG==1
      all.zcam->zcamState   = ZCAM_OFF; // ZCAM is OFF: no pending action
      all.zcam->zcam_stat   = 0;    // init to 0.
      //=== aug16 repeat for zonar
      all.zonar->zonarState = ZCAM_OFF; 
      all.zonar->zonar_stat = 0;    // init to 0.
    /* sep15, removing this day-test code: now done during bottom_turn
      //>>>Day Test Only !!!!
      // will leave it on until we get to the abort code
      i = start_zoog_dive( ZCAM_OFF );
      // i=0 = all OK, no error status
      // else i=err status, zooCam is off
      // i is already stored to all.zcam->zcam_stat
    */
    #endif //====================================================
      
    //get GPS & send back
    // idive = iparam [ EA_surf_num ];
    all.time->ti0 = RTCGetTime (0, 0 ); //0902 timestamp for gps try
    all.time->ti_cyc = all.time->ti0; // UX time for this cycle
    all.ncyc = 0; // 0902 start off with the dive-set cycle=0
    
    // aug15, if ZOOG, following adds the zooglider startup status.
    // sep15, removed this since we're past the day-test version.
#if DEBUG == 0
    gps_good = try_gps_sbd( 0 );  // 0=gps_ver=start-mission
#else
    cprintf("In DEBUG MODE - skipping gps/sbd\n");
    //Verbose = 1;
#endif

    if ( !ignore_gps ) //dec11
    { //only abort if don't want to ignore the gps
      if ( gps_good == 0) all.exc->abort = TRUE; 
    } 

    all.time->ti_last_sbd  = RTCGetTime( 0, 0); // initialize
    all.time->ti_last_gps  = RTCGetTime( 0, 0); // initialize
    
    #if DEBUG==1 // then fake out so we can proceed
      gps_good = TRUE;
      all.exc->abort = FALSE;
    #endif
    
    
    // initialize parameters
    all.roll_stat     =0; // start mission with no roll errors
    all.exc->exc_stat =0; // exception status=0
    all.eng->surf_tm  =0; // set to zero: is set in descend
    all.eng->vent     =0; // reset the vent status=0 @ start
  
    get_theta(  );    // compute heading to next waypt
    all.nav->drx = 0; // init the DR values
    all.nav->dry = 0;
        
  } // end op_mode<1 +++++++++++++++++++++++++++++++++++++++++++++++
  else
  { // op_mode>=1 = spurious reset +++++++++++++++++++++++++++++++++
    // try to recover as gracefully as possible.
    all.exc->exc_stat |= EXC_SPURIOUS_RST;   
    // 0x2000=flag that it is a spurious reset
    check_surf( 48 ); //48x = up to 4 hrs to get to surface
    nsent = surface( ); // send back data
  } // end op_mode >=1 +++++++++++++++++++++++++++++++++++++++++++++
  
  // we should be on the surface, ready to start
  ncyc = 0;  // ncyc = cycle counter for #profiles per 'dive-set'
  all.ncyc = 0; // store in global so available to other fnx's
  // a dive starts and ends between two surface communications
  // (therefore a dive's start and end positions should be known)
  
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  while (     (iparam[ EA_prof_num ] < iparam [ IA_nprofs ] ) 
         &&  !(all.exc->abort)   ) 
  { // do while still dives and no abort errors
    // NOTE the first step is to increment the dive counter, so the above
    // comparison is for prof_num<nprofs, not <=
    // 0902, use profiles; not surfacings

     if ( ncyc == 0 ) //--------------------------------------------------
     { // first profile for this surfacing, set up the needed info -------
        spi_pwr_rst( CS_PITCH );  // zero out accum. of pitch motor
        spi_pwr_rst( CS_ROLL  );  // zero out accum. of roll motor
        setup_pump(); //re-init the pump settings, in case changed via shore
        all.time->ti0 = RTCGetTime ( 0, 0 );  // UX time for this dive
        incr_surf_num(); //increments the surfacing# both in eeprom & iparam
        incr_prof_num();  // 0902 increment the profile# 
        flash_open('D');  // open a new file (also adds a dive and time marker)
        //----------------------------------------------------------------
        //-- have returned from surface() with a new GPS fix. Store it
        flash_gps( CF_GPS1 ); //GPS1=start-dive: writes it out w/new dive#
        // start a new iridium packet for this next profile:
        iprof = iparam[ EA_prof_num ]; // profile #
        all.klifo = assign_lifo(all.lifo, iprof, 0); 
        // 0902 starts new packet using the profile #
        lifo_dive_mark(); // 0902 SBD time/dive mark
        lifo_gps( 1, 1); // append 1=start-of-dive, 1=attempted  
        cprintf("New lifo %d for New profile %d \n",all.klifo, iprof);  
        all.roll_stat = 0; // reset roll status=0, ready for the next dive
        all.hyd->nbad = 0; // Set #bad amps during pump to 0
        all.dat->es.ad_alt = 0;  // zero the altimeter reading
     } //-----------------------------------------------------------------
     //--- ready to go dive ! --------------------------------------------
    
    go_profile( TRUE ); // TRUE=backup off the surface=OK
    
    // See if time to do a surface transmission (GPS/SBD)
    next_xmit_tm = iparam [ IA_min_xmit_tm ]; // in minutes
    next_xmit_tm = next_xmit_tm*60; // now in seconds
    last_xmit = RTCGetTime(0,0) - all.time->ti_last_sbd; //=ti since last
    xmit_time = ( last_xmit > next_xmit_tm );  // true if time to xmit
    
    op_mode = iparam [ EA_op_mode ]; // operation mode
    done = iparam [ EA_prof_num ] >= iparam[ IA_nprofs ]; //0902 prof#>=max
    
    //-----------------------------------------------------------------
    //-----------------------------------------------------------------
    if ( (xmit_time)  || (done) || 
         (op_mode==1) || (all.exc->abort)   ) //  op_mode==1 = 1st dive
    // do on first and last dive for sure, or if aborted
    { // If TRUE then it is time to do surface gps & satellite xmit
      // surface gets gps fixes, does satellite xmit,
      // and updates waypoint and timer values
      
      // Make sure that we are on the surface
	   if ( all.exc->pgood ) //we believe P,
		  stat2 = check_surf(  12 );  //  pump up to 12x to get to surf
	   cprintf("chk_surf stat=%d\n",stat2);
	   
	   // set IA_argos_mult based just on end-mission/abort status
	   if ( all.exc->abort || done ) 
	        iparam [ IA_argos_mult] = 1;
	   // set =1 if we're aborting or at the end of mission
	   // it can also be set via shore command
       
       nsent = surface( );   // do gps/ sbd/ gps & update timers
       ncyc = 0;  // reset the profile counter to 0
       all.ncyc = 0;
       
       check_surf_press( );  // get the surface pressure 
       // sets abort =TRUE if psurf > 20 dBar : not on surface!
       // this will bump us out before the next dive is tried.

       //dec11, only set abort if don't want to ignore gps
       if  ( op_mode==1 ) // 1st dive exception ,,,,,,,,,,,,,,,,,
       { // nsent=0 means no Iridium, gps->valid=0 means no fix
         //  abort if gps or Satellite bad
         gps_bad = ( all.gps->valid ==0 ); // TRUE if bad gps
         if ( ignore_gps ) gps_bad = false; // force if want to ignore
         if ( ( gps_bad || nsent==0) && (!DEBUG) ) 
             all.exc->abort = TRUE;  
       } //,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
       
    } // NOTE that surface and go_profile set the abort flag if necessary
    //-----------------------------------------------------------------
    //-----------------------------------------------------------------
    else // NOT time to do surface stuff ------------------------------
    { // we should be ascending at ~10 m depth,
      top_turn( );  // just do the top turn ascent portion
      // this returns at first sign of starting a new descent.
      // top_turn also does the data packing, but NOT if SBE_1Hz==1
      // it also stores ADP to the CF1...
      // will return with abort_miss set if too deep in top turn.
      
      #if SBE==1 // then we need to see if we're doing 1 Hz data
        #if SBE_1HZ ==1 // then taking 1 Hz data
            print ( " COMPILE ERROR: 1HZ option removed Oct08 ")
        #endif // make sure the SBE pump is off & we're not profiling
        sbe_stop_profile( ); // quit taking data & leave sbe in lowpower
      #endif

      // start a new iridium packet for the next dive coming up:
      ncyc++; // increment the cycle counter for within dive-set
      all.ncyc = ncyc;
      incr_prof_num(); // 0902, increment the total profile counter
      flash_dive_mark(ncyc); // store the cycle, dive, profile counter
      all.klifo = assign_lifo(all.lifo, iparam [ EA_prof_num ],ncyc); 
      lifo_dive_mark(); //0902 Jan2010 write dive marker to SBD

    } //  end else ----------------------------------------------------
    //-----------------------------------------------------------------
    
      if (op_mode<2) set_op_mode (2); 
      // set operation mode=2 = past first dive
    
      // Check bottom turn depths--------------------------------------
      j=0;
      for (i=0;i<N_BZ;i++) // keep track of number of shallow profiles
      {  if ( all.exc->bz[i] < 50 ) j++;
         cprintf("bz[%2d]= %4d\n", i,  all.exc->bz[i] );
      }
      // if 2 or more shallow profiles and gps is not working, abort
      // feb12, change to SBD NOT WORKING
      if ( (j>1) && (all.exc->sbd_bad) && (!DEBUG) ) 
    	all.exc->abort = TRUE;
      //---------------------------------------------------------------
      
  } // end while dives to do & mission is ok ++++++++++++++++++++++++++++++
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  
  cprintf("now going to end-mission code\n");
  abort_mission( all.exc->abort ); // aug15 pass error code
 
  return;
} //***************************************************************************


short  check_surf_press ( void ) // *******************************************
{ // check the surface pressure value 
  // this should be called when we think that we're on the surface,
  // i.e., at the start of a dive cycle.
  
  // returns 1 if we need to abort !
  float pval;
  ushort psurf;
  
  #if SBE==1
     psurf = sbe_avg_p(4);  // get surface SBE p avg
  #else
     print("compile error, not supported"); 
  #endif
  if (psurf==0) //-----------------------------------------------------
  { // jan13: surface pressure is bad : try resetting the uart
    all.exc->exc_stat |= EXC_BAD_PRESS; // set flag
	setup_one_tpu_uart( TU_SBE ); //jan13, re-init the uart
    psurf = sbe_avg_p(4);  // try again
  } //-----------------------------------------------------------------

  pval = dBar( psurf, all.flt);  // now in dBar
  printf("Surf press cnts = %6u = %6.2f dBar\n",psurf, pval);
  
  if (psurf==0) //-----------------------------------------------------
  { // surface pressure is bad : continue based on just time 
    all.exc->exc_stat |= EXC_BAD_PRESS; 
    //0x0400= turn on bit  = flag that press is bad
    iparam[ IA_route_now ] = 0;  // go to HOME waypoint
    all.exc->pgood = FALSE;  // set as flag not to believe press
    
  } // end if p is NOT believable
  else  //-------------------------------------------------------------
  { // surface press seems valid :
    all.exc->pgood = TRUE;  // set as flag to believe press
    
    if (pval>20) //pppppppppppppppppppppppppppppppppppppppppppp
    { // p is good but we're not at the surface ???
      // this is very likely a hydraulics problem, and we want
      // to abort even if rcvry_ok = FALSE: w/o an engine we don't go!
      all.exc->exc_stat |= EXC_BELOW_SURF; 
      // 0x0004 = flag that we're too deep
      all.exc->abort = TRUE; // something is not good!
      return(1); // this gets us to the abort_miss code
    } // endif pval  pppppppppppppppppppppppppppppppppppppppppp
    
  } // end if..else ---------------------------------------------------
  
  // we're here if press is believable
  if (psurf>0) all.flt->pmin = psurf;  // if valid set to new surf value

return(0);
} //***************************************************************************


short  check_surf (  short ntry )  //******************************************
{ // check pressure & try pumping more to get to surface
  // normally enter here ~12m from the surface,
  // and shouldn't require any extra pumps.
  //--- returns # of extra pumps
  
  short i,j, dt, pi_use, pit;
  float pval, xp;
  ulong tnow, tstop;   // present time
  struct dat_param *dat;  // data struct w/data values

  tnow = RTCGetTime(0,0);  // get the present time
  dat  = all.dat;      // point to data structure
  dt = SAMP_DT_UP;    // 0903 feb11: time between samples for ascent

  cprintf("surface check\n");
  pi_use  = ( iparam [ IA_pi_stat ] >0 );  // TRUE to use pitch angle 
  pit     =   iparam [ IA_glide_angle ];
  
  #if PPLUS==1 //mar11, make sure no loads applied
  // this will get us some samples with no load on cells.
    zero_loads();
  #endif //---------------------------------------

  Close_Valve;  // make sure the valve is closed
  
  pval = dat->dbar; // =last depth value, in dBar
    
  j = 0;  // counter of # times we've tried to get to the surface
  //=======================================================================
  while ( (pval> 20 ) && ( j++<ntry ) ) 
  {  // while we're too deep, keep pumping
     go_pump ( 50, OIL_AFTER_T ); // pump, but pay attn to the oil sensor
     // the pump now runs independently, so we can do other things...
     tstop = tnow + 300; // total wait cycle = 300s
     
    
    while ( tnow<tstop  &&   pval>7  ) //------------------------
    { // wait up to 250 s or until we're above 7 m=normal exit from ascent
      hibernate( &tnow, dt, PIT_IRQ2 );  // updates tnow
      take_data( );  // take data
      pval = dat->dbar; 
      printf("p>20 in chk_surf = %6.2f \n",pval);
    } //---------------------------------------------------------
    
    if ( pval>20 ) //--------------------------------------------
    { // recovering from bad pumping, correct the pitch 
       xp = (float)  dat->es.pitch/10.0; // present pitch value
       if (pi_use) // then control the pitch 
          control_pitch( xp, pit, 1.0 ); 
    } //---------------------------------------------------------
  } //=====================================================================
  

  i=0; // now wait to get from ~20 dBar to the surface ~200 s
  while ( pval>2 && i++<30 )  //---------------------------------
  { // while not at the surface, wait a little longer
      hibernate( &tnow, dt, PIT_IRQ2 );  // updates tnow
      take_data( );  // take data
      pval = dat->dbar; 
      printf("p<20 in chk_surf = %6.2f \n",pval);
  } //-----------------------------------------------------------

  // no matter what, turn things off & exit
  #if SBE==1 // ---------------------------------------------
    #if SBE_1HZ == 1 // then stop profile and xfr data to RAM
      print ( " COMPILE ERROR: 1HZ option removed Oct08 ")
    #endif
    // else doing 8 s data, already in RAM..set flag & stop pump
    sbe_stop_profile( ); // leave sbe in lowpower
  #endif //---------------------------------------------------

  #if ADP==1 // then quit taking adp data---------------------
    if (dat->take_adp)
    { // then the adp might be on and ready to sample
          if (all.adp->on_adp) adp_exit_sample();  // exit sample mode
          all.adp->on_adp = 0; // set flag that we're turning off
          adp_off(); // turn off the sonar
          dat->take_adp   = FALSE; // set flag=FALSE = quit taking doppler
          // the data is transferred to the CF1 and packed in 'surface'
    } // end if take_adp=TRUE --------------------------------
  #endif
  
  #if NTK==1  // JAN14 quit taking adp data ------------------
    if (dat->take_adp) //=============================
    { // sampling data: stop, collect data & store
      ntk_stop_pro();
      all.adp->on_adp = 0; // set flag that we're turning off
      dat->take_adp   = FALSE; // quit taking doppler
    } //================================================
  #endif //-----------------------------------------------------
  
    #if ZOOG==1  // sep15, we want to turn off the zooCam
      if (all.zcam->zcamState != ZCAM_OFF )
      { // then it is NOT off, turn it off gracefully
        all.zcam->zcamState = ZCAM_STOP_RQST ; // STOP REQUEST
        zooCam_state(); // will now try to stop.
        // state = ZCAM_OFF
      } //-------------------------------------------
      
      // aug16, do the same for zonar
      if ( all.zonar->zonarState != ZCAM_OFF )
      { // then it is NOT off, turn it off gracefully
        all.zonar->zonarState = ZCAM_STOP_RQST ; // STOP REQUEST
        zonarState(); // will now try to stop.
      } //-------------------------------------------
      
    #endif //====================================================
  #if PH==1
   ph_off();
  #endif

  if (j>0) all.exc->exc_stat |= EXC_EXTRA_PUMP; //jan13
  // 0x0040 = flag that extra pumps req'd
  cprintf("Exit check_surf\n");

  return(j);
} //***************************************************************************


void  go_profile( short backup_ok ) //*****************************************
{ // Does a single profile operation = descend, then profile on ascent
  
  short stat1; // = status of descent, bottom turn, and ascent
  
  all.dat->i = 0;   // reset profile ram data buffer to 1st record
  all.dat->ie= 0;   // reset eng. ram data buffer to 1st record
  all.dat->isamp=0; // counter of #data samples for this dive

#if PPLUS==1  // mar11 polyplus battery cell, set up params
  pp_start_dive(); // initialize values
#endif  //take_data automatically does the rest

  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  {   // do one profile cycle
	
	//reset the hydraulic parameters
	spi_pwr_rst ( CS_HYD ); //resets pump times, avg arrays to zero
	
	// descend for certain time or depth
	stat1 = descend( backup_ok );  // do normal descent off of surface
	  
	// do the bottom turn : transition from descent to ascent
	stat1 = bottom_turn( ); // returns >0 if pump failure
	cprintf("bot_turn stat=%d\n",stat1); //& exc->abort=TRUE if stat1>0

	// Always try ascend code...this will give extra pump tries
	// and a longer wait to get to the surface

	stat1 = ascend( );
	cprintf("ascend stat=%d\n",stat1);  //stat1>0 if sinking in ascend

  }  
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  cprintf("End go_profile\n");
	
  return;
} // **************************************************************************



short top_turn(  void )  //****************************************************
{ // we're ascending past the shallow depth limit & want to head back down
  // mimic flight control of the bottom turn to minimize further ascent
  // and then call the glide(down) code
  
  short ierr=0, dt, val, pi_use;
  short done  =FALSE; // is set TRUE when we're done turning
  short going_down = FALSE; // flag that we've started going down

  float pval, pmin, xp;
  ulong tnow, max_t;   
  struct dat_param *datp;  // data struct w/data values

  cprintf("Top turn\n");
  pi_use  = ( iparam [ IA_pi_stat ] >0);  // TRUE to use pitch angle 
  tnow = RTCGetTime(0,0);    // get the present time
  max_t = tnow + 600; // max time to wait to turn around
  
  datp = all.dat;    // point to global structure
  //dt = SAMP_DT;  // time between samples
  dt = SAMP_DT_UP;    // 0903 feb11: time between samples for ascent

  val = +5; // go to a shallow glide angle to slow ascent
  pval  = pmin =  datp->dbar; 
  Roll_Zero;         //roll to 0 degrees
  
    // Check depth before opening the valve
    if (pval>20)  //------------------------------------------------
    { // we're too deep; likely hydraulic problem??
      all.exc->exc_stat |= EXC_BELOW_SURF; // turn on error bit
      all.exc->abort = TRUE; // something is not good!
      return(1); // this gets us to the abort_miss code
    } // endif pval ------------------------------------------------

  Open_Valve;        // open the hydraulic valve
  DelayMilliSecs(1000); Open_Valve; // try again
    
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  while ( !done  && tnow < max_t ) //++++++++++++++++++++++++++++++++++++++
  { // watch ascent rate & control pitch & heading
    // control pitch and roll
    xp = (float) datp->es.pitch/10.0; // present pitch value
    if (pi_use) control_pitch( xp, val, 1.0 ); 
    
    hibernate( &tnow, dt, PIT_IRQ2 );
    take_data( );
    pval = datp->dbar;   // dBar now
    
    // keep track of the minimum pressure observed
    if (pval < pmin) pmin = pval; // we're still ascending
    // see if we've started to go down yet : 0.1 m threshold
    if ( pval > (pmin+0.1) ) going_down=TRUE; // then going  down
    if ( pval < 2 ) // we're within 2 m of the surface 
    	going_down = TRUE; // may as well as act like going down.
    
    // 0901 code differs from earlier, as it automatically exits the while
    // once going down, and packs the data for transmission.
    // this forces each profile to be packed in its own message.
    done = going_down;
    printf("top turn p=%6.1f\n",pval);
  } // end while ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    
    
    #if SBE==1 // -------------------------------------------------
      #if SBE_1HZ ==0 //  NOT taking 1 Hz data, stop taking data
         sbe_stop_profile( ); // quit taking data 
         data_pack( ); // pack the most recent ascent profile
         // if multi-cycles cause an overflow, data_pack's call to
         // append_lifo will start a new output packet
         datp->i     = 0; //reset ram data buffer to 1st record
         datp->ie    = 0; //reset eng. data buffer
         datp->isamp = 0; // reset #take_data samples 
         // if SBE_1HZ==1 then want to keep profiling; do nothing
      #endif
    #else // else SBE==0, not SBE, do data pack now
      print ("compile error: nonSBE not supported\n" )
    #endif // end if SBE==1 ---------------------------------------
        
    #if ISUS==1 //-------------------------------------------------
      isus_profile_end(  );
    #endif

    // if ADP, write out buffer data now as well
    #if ADP==1  //-------------------------------------------------
      if (datp->take_adp)
      { // True if we wanted to take data
        if (all.adp->on_adp) adp_exit_sample();  
        all.adp->on_adp = 0;  // set flag that it is off
        adp_off(); // turn off ADP
        datp->take_adp = FALSE; // quit taking doppler
        adp_post_process( ); // process the data
        // may10, swapped the order, as post_process writes the ADP header
        adp_flash_dump(  CF_ADP );  // write the ADP data to the CF1
      } // end if take_adp
    #endif // end if ADP==1 ---------------------------------------
    
    #if NTK==1 //JAN14 shut off NTK ADP--------------------------
      if (datp->take_adp) //=============================
       { // sampling data: stop, collect data & store
         ntk_stop_pro();
         datp->take_adp   = FALSE; // quit taking doppler
         adp_post_process(); // process the data
         adp_flash_dump(CF_ADP);  // write the ADP data to the CF1      
       } //================================================
     #endif//-------------------------------------------------------
    
    #if PPLUS==1 //-------------------
      pp_SBD_pack(); // pack PP data
    #endif //-------------------------

    //=== ZooGlider =============================================
    #if ZOOG==1  // sep15, we want to turn off the zooCam
      if (all.zcam->zcamState != ZCAM_OFF )
      { // then it is NOT off, 
        all.zcam->zcamState = ZCAM_STOP_RQST ; // STOP REQUEST
        zooCam_state(); // will now try to stop.
        // state = ZCAM_OFF
        all.zcam->zcam_stat = 0; // nov15 init to 0: no errors yet
      } //-------------------------------------------
      
      // aug16, do the same for zonar
      if ( all.zonar->zonarState != ZCAM_OFF )
      { // then it is NOT off, turn it off gracefully
        all.zonar->zonarState = ZCAM_STOP_RQST ; // STOP REQUEST
        zonarState(); // will now try to stop.
      } //-------------------------------------------
    #endif //====================================================
  
    //===========pH sep18========================================
    #if PH == 1
       ph_data_pack(); //pack data
       //reset RAM index
       all.ph->i   = 0;
       ph_off();
    #endif //====================================================
    cprintf("exit top turn: data packed\n");
  return(ierr);
} //***************************************************************************

short  descend( short backup_ok )  //******************************************
{ // descend until certain depth or max time
  // if backup_ok ==1, then allow backing off the surface
  
  short ierr, down, dt, theta, cf_stat;
  float pval, plimit, c, adh, dh;
  ulong tnow, t0;   // present time
  ulong surf_to;
  struct dat_param *datp;  // data struct w/data values


  tnow = t0 = RTCGetTime(0,0);    // get the present time
  datp = all.dat;    // point to global structure
  dt = SAMP_DT_DN;  // 0903 feb11: time between samples
  theta = all.nav->theta; // course to steer, magnetic

  Open_Valve;        // open the hydraulic valve
  DelayMilliSecs(1000); Open_Valve; // try again

  roll_wing ( ZERO_WING, 1); // roll wing flat, 1=seek zero if pot is bad
  //Roll_Zero;         //roll to 0 degrees

  down = iparam[ IA_pitch_0]  + iparam[ IA_pi_dn ]; // nose down
  run_p_r_wait ( CS_PITCH, 1, down ) ;  
    
  pval = 0; // initialize to valid value
  surf_to = (long) SURF_WAIT;  // time out to leave surface
  surf_to += t0; // actual time to leave the surface
  
  //surf_to = 60; //>>>R DEBUG


  //--- make sure SBE and ADP are in low-power states ----------
  sbe_stop_profile( ); // quit taking data & put sbe in lowpower
  #if SBE==1  // then using SBE --------------------------------
    #if SBE_1HZ == 1 // then we want to sample both up & down
      print ( " COMPILE ERROR: 1HZ option removed Oct08 ")
    #endif
  #endif //-----------------------------------------------------
  
  #if ADP==1  // make sure adp will not be sampled ------------
    all.adp->on_adp = 0; // flag that it is off
    adp_off(); // make sure adp is off
    all.dat->take_adp = FALSE; // and we won't try to take adp data
  #endif //-----------------------------------------------------

  #if NTK==1  // JAN14 make sure NTK will not be sampled -------
    all.adp->on_adp   = 0; //flag that it is off
    ntk_off(); // make sure adp is off
    all.dat->take_adp = FALSE; // don't take adp data
  #endif //-----------------------------------------------------
  
  #if ZOOG==1  // nov15 zooCam ---------------------------------
    all.zcam->zcam_stat   = 0;    // init to 0: no errors yet
    all.zonar->zonar_stat = 0;    // init zonar status=0
  #endif //-----------------------------------------------------
  
  //=== see if we need to back off the surface ==================
  adh = dh = 0; // initialize
  ierr = get_tcm2( 1 ); // get present compass heading = c
  if (ierr==0) //------------------------------------------
  { // then good compass heading
    // compute heading error
    c = all.comp->head; // compass heading
    dh = c - theta; // course correction = actual - desired
    dh = limit180(dh); // limit to +/- 180 degrees
    adh = flt_abs(dh); // absolute value
  } //-----------------------------------------------------

  // take action depending upon how far off-course we are
  if ( adh>90 && backup_ok )
  { // try backing up
    back_up(  dh ); //>>>R DEBUG
  }
  else //========================================================
  { // we're close enough, just go forward
  
	  while ( (pval<2) && (tnow<surf_to ) ) //---------------
	  { //sample while we're still at surface or up to SURF_WAIT
	    //it takes ~600 s on bench to evacuate FULL bladder
	    //plus 60 sec to go 10 m
	    hibernate( &tnow, dt, PIT_IRQ2 );  
	    take_data( );  // take data
	    if (all.exc->pgood) 
	       pval = datp->dbar;  // update p
	    cf_stat = (short) ( tnow - t0);  //time waiting so far
	    printf("press @ pval<2 = %6.2f @%6d\n",pval,cf_stat);
	    
	      // sbe_pro_on==1 only if SBE_1HZ==1 && SBE==1
	      //if ( (pval>0.5) &&  (all.dat->sbe_pro_on) ) 
	      //	sbe_pump_on(); // we're deep enough to run the pump
	  } //---------------------------------------------------
	  // now we should be below the top 2 m : go do gliding stuff
  } // end if..else adh =========================================
  
  
  // setup for descent glide, with active pitch and control
  tnow = RTCGetTime(0,0) - t0;  //=time to sink 2m  
  pval = datp->dbar;    
  all.eng->surf_tm = (short) ( tnow /10 ); //=time to sink to 2m
  if ( pval<2 )   
    all.exc->exc_stat |= EXC_HI_SURF_TM; //jan13
    //  0x0080 = taking too long to leave surface

  down = TRUE;  // set true that we're going down
  all.flt->esum = 0; // set integral error sum to 0
  all.flt->isum = 0; // 0902.5 feb11: set #points accum in esum=0
  plimit = iparam [ IA_zmax ];  // max limit to go to
  if ( iparam [ EA_op_mode ]== 1) // then we're in the first dive
     plimit = iparam [ IA_first_zmax ];  //  do shallow dive first time
     
  ierr = glide( down, plimit );  //>0 if goes way too deep
  

  //if adp altimeter, turn off
  #if ADP==1 // then ADP ----------------------
    all.adp->on_adp = adp_off();
    // leaving in the hook to close an altimeter file.
    // If recording, it should go to data file with right tags,
    // so shouldn't need a separate file..but just in case:
    cf_stat = iparam[ IA_use_alt ] &0xf; // first nibble
    if ( cf_stat==2 || cf_stat==3 )
    { // if '2' or '3' then we should have been recording
       // since now writing everything with tags to flash
       // shouldn't need any further action.
    }
  #endif // end if ADP -------------------------

  #if NTK==1  // JAN14 make sure NTK will not be sampled -------
    if (all.adp->on_adp) ntk_pwrdwn(); // turn off gracefully
    all.adp->on_adp   = ntk_off(); // make sure adp is off
    // ad2cp records all data to its flash, so no reason to 
    // store other info here
    // leaving in the hook to close an altimeter file.
    // If recording, it should go to data file with right tags,
    // so shouldn't need a separate file..but just in case:
    cf_stat = iparam[ IA_use_alt ] &0xf; // first nibble
    if ( cf_stat==2 || cf_stat==3 )
    { // if '2' or '3' then we should have been recording
       // since now writing everything with tags to flash
       // shouldn't need any further action.
    }
  #endif //-----------------------------------------------------
  
  // update the file buffer.
  cf_stat = flash_update_FAT();  
  
  return(ierr);
} //***************************************************************************


void  back_up( float dh ) //***************************************************
{ // point tail down and back off the surface rolled.
  // this SHOULD make for 4 times faster turning (~2 deg/s rate).
  // dh = heading error = actual - target
  ulong tnow;   
  float pval=0, adh, xp, pit, rol;
  short i, pi_use, dt;

  dt = SAMP_DT_DN;    // 0903 feb11: time between samples for descent
  pi_use  = (iparam [ IA_pi_stat ] >0);  // TRUE to use pitch angle 
  tnow = RTCGetTime(0,0); // get present time
  all.exc->exc_stat |= EXC_BACK_OFF; //jan13
  // 0x0020 = flag that we're backing off the surface
  
  i = iparam[ IA_pitch_0] - 3*iparam[ IA_pi_dn ]/2; // guess for tail 20 deg down
  send_p_r_run ( CS_PITCH, 1, i ) ;  
  // puts tail down ~20 degrees when the bladders are empty

  pit = 20;  // we want the nose 20 degrees up
  rol = 30; // we want roll = 30 degrees : positive=stbd wing down
  if (dh>0) rol = -rol; // if dh>0, we need to turn ccw = s
  //  continue to back up until either p>10 dBar, or |dhtbd wing up
  roll_wing ( ZERO_WING, 1); // roll wing flat, 1=seek zero if pot is bad
  //Roll_Zero;      //roll to 0 degrees
  
  // continue to back up until either p>10 dbar or |dh| < 60 degrees
  i=0; // counter
  do   //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  {
    hibernate( &tnow, dt, PIT_IRQ2 );
    take_data( );  // take data
    if ( all.exc->pgood ) 
      pval=all.dat->dbar; // new depth
    dh = all.dat->es.comp/10.0 - all.nav->theta; // heading error
    dh = limit180(dh); // limit to +/- 180 degrees
    adh = flt_abs(dh); // absolute value
    printf("back_up p = %6.2f dh= %6.1f\n", pval, dh);
    
    if (pval>0.5 && pi_use ) //-------------------------------------
    { // then we're off the surface, control pitch & roll
      xp = (float)  all.dat->es.pitch/10.0; // present pitch value
      control_pitch(xp, pit, 1.0); // pitch up 20 degrees
      xp = (float)  all.dat->es.roll/10.0; // present roll value
      control_roll(xp,rol ); // roll 30 degrees
      // above waits for completion
      // following would be for SBE_1HZ data...option removed
	  //if ( all.dat->sbe_pro_on) // only true if we're doing 1Hz sampling.
	  //	sbe_pump_on(); // if profiling on way down, turn on the pump
    } //------------------------------------------------------------

  } while ( (pval<10) && ( adh > 60 ) && (i++<100) ); //+++++++++++++++++
  
  // return to correct roll and pitch 
  Roll_Zero;  // zero wing angle
  i = iparam[ IA_pitch_0]  + iparam[ IA_pi_dn ]; // nose down
  send_p_r_run ( CS_PITCH, 1, i );   
  
  // take 2 more scans ; pitch & roll should be done by then
  for (i=0; i<2; i++) //-----------------------------------
  {
    hibernate( &tnow, dt, PIT_IRQ2 );
    take_data( );  // take data
  } //-----------------------------------------------------
    
  return;
} //***************************************************************************


short  ascend( void )  //******************************************************
{ //ascend until at certain depth or max time
  //returns >0 if we actually sink in glide()
  
  short ierr, down;
  float plimit, z0=0, p_adp=0;
  struct dat_param *datp;
  datp = all.dat; // point to the data structure

  down = FALSE;  // we're now going up
  // We can potentially do a shallow-turn around in top turn,
  // so use a deeper plimit setting.  If we don't do a top turn,
  // then check_surf will get us the rest of the way there
  plimit = 12;  // exit when we're within plimit of the surface
  
  #if PH==1 //sep18 make sure pH is on and muxed properly ------------
      start_ph();
  #endif
  
  // if an ADP, initialize to take data
  #if ADP==1  // true if this is an ADP glider //---------------------
      all.adp->on_adp =  0; // flag that it is off
      adp_off();  // make sure adp is off
      all.adp->iscan = 0; // no data taken: reset to 0 (Apr07)
	  // set the start pressure value to take ADP data  
      p_adp = datp->dbar +0.1;  // =last pressure sample +0.1
      // 0902, skip if zmax ==0 ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
      if (   iparam[ IA_adp_zmax ] > 0 )
      { // then take ADP data
        z0 = (float) iparam[ IA_adp_zmax ]+4; // depth wanted to turn on ADP 
        if ( p_adp > z0 ) p_adp = z0; // if deeper than z0, set to z0
        datp->p_adp = p_adp;  // 0702 depth where adp is turned on
        datp->take_adp = TRUE; // set flag=TRUE: we want to take doppler data
      } else
      { // we DON'T want to take ADP data
        datp->take_adp = FALSE;
        datp->p_adp = -999;
      } //,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,

  #endif //------------------------------------------------------------
  
  #if NTK==1  // true if this is a Nortek glider //-------------- JAN14
      all.adp->on_adp =  ntk_off();  // make sure ntk is off
      all.adp->iscan  = 0; // no data taken: reset to 0 (Apr07)
	  //--- set the start pressure value to take NTK data  
      p_adp = datp->dbar +0.1;  // =last pressure sample +0.1
      if (   iparam[ IA_ntk_zmax ] > 0 )
       { // then take ADP data
        z0 = (float) iparam[ IA_ntk_zmax ]+4; // depth wanted to turn on ADP
        if ( p_adp > z0 ) p_adp = z0; // if deeper than z0, set to z0
        datp->p_adp = p_adp;  // 0702 depth where adp is turned on
        datp->take_adp = TRUE; // set flag=TRUE: we want to take doppler data
       } else
       { // we DON'T want to take ADP data
        datp->take_adp = FALSE;
        datp->p_adp = -999;
       } //,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,

  #endif //------------------------------------------------------------ 
  
  #if ZOOG==1  // true if this has a Zonar //-------------------- Aug16
      all.zonar->zonarState = ZONAR_START_AT_Z;  //have it init start-depth.  
      zonarState(); // initializes depths.
  #endif //------------------------------------------------------------ 
  
  
  	
  #if BENTHOS==1
      //Spray with Benthos modem--see if want to do Ac Comms at end of ascend
      if( iparam[IA_acm_tm]>0 && iparam[IA_acm_mode]==2 &&
          iparam[EA_op_mode]!=1 )   //LAR 2009-12-15 No comms on 1st test dive
      {  //we want to do AC comms while rising in ascend
         uchar dbg[255];
         ierr = glide(down,(float)iparam[IA_acm_dpth]); // ascend until depth <IA_acm_dpth

         if( all.hyd->status == PUMP_WAIT )  //Is the pump running? 4Nov2009
         {  //Hydraulic pump is running--
            sprintf(dbg,"ASCEND   %6ld Pump ON  hyd->total=%d\n",
                        RTCGetTime(0,0),all.hyd->total);
            buf_to_cf1V( strlen(dbg),dbg);  
            printf("%s",dbg);
            hyd_stop_pump(); //kill pump to be quiet. 
            sprintf(dbg,"ASCEND   %6ld Pump is OFF hyd->total=%d\n",
                        RTCGetTime(0,0),all.hyd->total);
            buf_to_cf1V( strlen(dbg),dbg);  
            printf("%s",dbg);
         } 
         else
         {  sprintf(dbg,"ASCEND   %6ld Pump is OFF hyd->total=%d\n",
                        RTCGetTime(0,0),all.hyd->total);
            buf_to_cf1V( strlen(dbg),dbg);  
            printf("%s",dbg);
         } 
         sbe_stop_profile(); // quit taking data & leave sbe in lowpower
         do_acm(); //do ac comms 
      } 
      else
      {
         ierr = glide(down,plimit); // ascend until depth <plimit
      }
  #else
      //Spray with no Benthos modem
      ierr = glide(down,plimit); // ascend until depth <plimit
  #endif
    
  // DelayMilliSecs(1000);
  
  return(ierr);
} //***************************************************************************


#define ECLIP 30 // Clip heading error to +/-ECLIP for error accum.

short  glide ( short down, float plimit ) //***********************************
{ // glide with active control on pitch and heading
  // down  = true if gliding down 
  // plimit = depth (dBar) to not go beyond (either max or surface value)

  // returns 8= panic at too far deep
  // returns 7= high current while running pump at 50m prime
  // returns 0=normal
  
  // dec14, adding 50m variable pump time

  short j_act, jstart, jsamples, pumped, j,dt, hdt, use_oil;
  short  pit, esum_max, ip;
  short theta;  //=desired heading
  short use_alt=0, alt, alt_z=500;  //=altimeter reading
  short nw=0; // counter of #times checked vertical velocity
  short idpmp;  // pitch counts to move per incremental buoyancy pump on ascent
  short ipfwd;  // pitch counts to move fwd for ~3 degrees more pitch
  float wdmin;  // minimum velocity on descent
  float etau, es;
  short pi_stat, pi_use;
  short alt_stat = 0; // status of altimeter
  float pitch_avg, roll_avg, pval, dh, dp, p0, phi, pa, w;
  short pmp_50m =8; //dec14, default to 8s pump

  ulong tnow, Btime, tmax, chk_valve;  
  short z_panic; 
    
  struct dat_param *datp;  // data struct w/data values
  struct nav_param *navp;  // navigation struct
  struct hyd_param *hp;    // hydraulics structure
  
  etau  =  (float) (iparam[ IA_ro_etau ])*0.1;  // 0711 scale for int. head error
  if (etau == 0) etau = 1; //jan16 allow negative values
  etau = etau * all.flt->etau;  // new weight
  all.flt->ediv = etau; // feb11 keep value: is used to calc new up/down rolls

  datp   = all.dat;     
  navp   = all.nav;
  hp     = all.hyd;
  esum_max = (short) ( 30 * etau); // MAX etau for 30 degree roll
  
  tnow = RTCGetTime(0,0);  // get the time
  Btime = tnow;  // time since last buoyancy pump adjustment on ascent
  
  dt = SAMP_DT_UP;    // 0903 feb11: time between samples for ascent
  tmax = iparam [ IA_max_dive_tm ];  // max time to glide, minutes
  tmax = tmax*60; // convert from minutes to seconds
  if (!all.exc->pgood) tmax = tmax/2;  // don't believe p, go half as deep
  tmax = tmax + tnow;  // when clock gets here, stop gliding
  theta =  navp->theta;  //=desired heading, magnetic, =-1 if do circle
  
  z_panic = 1300; //  1400 = crush depth - 15% = potential hull failure
  
  if (down) //------------------------------------------------------
  {  // then descending, initialize altimeter variables
     alt = datp->es.ad_alt = 0;  // initialize value
     all.adp->alt_db   =  0; // initialize for ADP altimeter
     all.eng->alt      = 99; // flag that hasn't been sampled yet.
     all.eng->z_at_alt =  0; // "" 
     dt = SAMP_DT_DN;    // 0903 feb11: time btwn samples for descent
      //--- dec14 get time to pump on descent @ 50m
     pmp_50m = iparam [ IA_pmp_50m ];;
     if (pmp_50m <  2 ) pmp_50m =   2; // set to minimum
     if (pmp_50m >300 ) pmp_50m = 300; // keep to maximum
  } //--------------------------------------------------------------
  
  chk_valve = VALVE_TIME + tnow; //#secs to check valve is closed
  pit = iparam [ IA_glide_angle ]; // pitch to use
  if (down) pit = -pit; // nose down going down
  p0 = datp->dbar; // =dBar depth going into glide
  
  cprintf("glide theta=%d, pitch=%d, tmax=%d\n",theta,pit,tmax);

  wdmin   = (float) iparam [ IA_pi_wdn ];  // min velocity on descent

  pi_stat = iparam [ IA_pi_stat ]; // status of the pitch control
  pi_use  = ( pi_stat>0);  // TRUE to use pitch angle sensor feedback
  
  ipfwd = (short) iparam[ IA_pitch_10d ]*.3; 
  // ipfwd = guess on pitch counts to move fwd for 3 deg nose down.
  // idpmp = guess to move pitch back per every 10 s of pumping
  idpmp = ( short) ( iparam[IA_delta_pump] * iparam[IA_pi_pmp]*0.1 );
  
  // pay attn to using the pitch angle sensor or not ------------------
  if (pi_use) // then do immediate pitch correction -------------------
  {  pitch_avg = (float)  datp->es.pitch/10.0; // present pitch value
     control_pitch(pitch_avg, pit, 1.0 );
  } //-----------------------------------------------------------------
  else // pi_stat=0 = broken pitch angle sensor
  { // just want to use manual values
     if (down) 
       ip = iparam[ IA_pitch_0]  + iparam[ IA_pi_dn ]; // nose down
     else      
       ip = iparam[ IA_pitch_0]  + iparam[ IA_pi_up ]; // nose up
        
     send_p_r_run ( CS_PITCH, 1, ip ); 
  } //-----------------------------------------------------------------
  
  
  if (theta>=0) //-----------------------------------------------------
  { // also do roll correction
    roll_avg = datp->es.roll*0.1; // present roll
    // printf("avg_roll = %6.1f\n",roll_avg); 
    dh = datp->es.comp*0.1 - theta;  // compass error
    dh = limit180(dh); // limit to +/- 180 degrees
    control_head(down, dh, roll_avg  );
  } //-----------------------------------------------------------------

  j_act =  T_ACT  / dt; //=#samples btwn active attitude control
  jstart = TSTART / dt; // #samples delay before start averaging
  jsamples = j_act - jstart + 1;  //total # samples averaged
  //every j_act*SAMP_DT interval, pitch & roll will be actively controlled
  //after pitch/roll change the first jstart samples are ignored
  //and then the remaining samples are used to compute avg pitch,roll,dh/SAMP_DT
  
  if (theta == -1) // then roll port wing down 20 deg & do circles
  {  j = 2* iparam[ IA_roll_10d ] + iparam[ IA_roll_0 ];  //abs. counts
     send_p_r_run ( CS_ROLL, 1, j ); // absolute roll to j
  }
  
  j=0; //  zero out counter of #samples between active control calls
  pumped = FALSE;  //we have not pumped any oil yet
  
  while ( tnow<tmax ) //+++++++++++++++++++++++++++++++++++++++++++++++++++
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  { // glide over allocated time ++++++++++++++++++++++++++++++++++++++++++
    
    hibernate( &tnow, dt, PIT_IRQ2 );
    take_data( );  // take data
    pval = datp->dbar; // = latest depth, dbar
    
    if ( (pval>z_panic) && all.exc->pgood ) // too deep!..abort -----
    { 
      for (j=0; j<5; j++ ) // try a bunch of times
      { burn_3(); take_data( ); }
      all.exc->abort = TRUE;  //set to abort
      all.exc->exc_stat |= EXC_TOO_DEEP; //jan13
      // 0x0008 = turn on bit  = flag that we're too deep
      return ( 8 );  // return value = got too deep
    } //--------------------------------------------------------------

      
    //**check to see if we've exceeded our pressure limit -----------
    if (down)  // going down, don't go past plimit
    {  printf("Going down, pval=%6.2f  pmax=%6.2f \n",pval,plimit);
       if ( (pval>plimit) && all.exc->pgood )
       { cprintf("past plimit\n");
         return(0);  // we've gone past, return
       }
    }
    else  // we're going up, look for a minimum value
    {  printf("Going up, pval=%6.2f  pmin=%6.2f \n",pval,plimit);
       if ( (pval<plimit) && all.exc->pgood )
       { cprintf("past plimit\n"); // we're too shallow, return
         return(0);  
       } // end if too shallow
    } //-------------------------------------------------------------
      
    //-----------------------------------------------------------------
    if (!pumped && down) // haven't pumped yet and going down
    {     // see if we should do a safety pump
        if ( ( (pval>50)&& all.exc->pgood) || (tnow> chk_valve ) )
        {  // we're beyond 50 dBar or timed-out; time to pump
           check_vent( );  // Check the vent status Feb08
           // run the pump for 16 s
           go_pump ( 16, OIL_NO ); // prime pump (valve is open )
           watch_pump ( 18, 1 ); //1=take data every 8 s
           //printf("safety pump p = %6.2f \n",pval );
           
           spi_pwr_rst( CS_HYD );  // zero out accum. of pump time
           //zero out pump accumulator
           cprintf("Closing valve\n");
           DelayMilliSecs(500);
           Close_Valve;  // close the hydraulic valve
           DelayMilliSecs(500);
           cprintf("safety pump for %d s\n", pmp_50m);
           go_pump ( pmp_50m, OIL_NO ); // pump for pmp_50m; dec14
           watch_pump ( pmp_50m+2, 1 ); // 1 = take data every 8s
           pumped = TRUE;  //set flag that we've pumped
           
           if ( all.hyd->bad_amp  &&  iparam[ IA_amp_action ]>0 )
           {   all.exc->exc_stat |= EXC_BAD_AMP; //jan13
               // 0x0200 = flag that high amps @ 50 m
               return(7); // if bad amps, exit now
           } // above catches high amps, and exits early.
          
        } //end if pval>50 and time to pump
    }  //end if not pumped yet //--------------------------------------

    j++;  //increment active counter
    if (j==jstart)  //we've delayed right time after active cntl
    { //---------------------------------------------------------------
      pitch_avg = datp->es.pitch;  //initialize accumulator to present value
      roll_avg  = datp->es.roll;   //same here
      // 0901, store last pitch, roll operation status
      if (  last_ops ( CS_PITCH )  ) // if there is a new op to store...
         {  flash_motor (); // write to flash
            clear_ops (CS_PITCH ); // clear the pitch buffer
         }
      if (  last_ops ( CS_ROLL  )  ) // if there is a new op to store...
         {  flash_motor (); // write to flash
            clear_ops ( CS_ROLL ); // clear the roll buffer
         }
      
      use_alt = iparam[ IA_use_alt ] &0xf; // if >0, use it
      #if ZOOG==1 // use value to turn on the zooCam
        if ( (use_alt) && (down) && (pval> iparam[IA_alt_z_1st] ) )
        { // then we want to turn on the zooCam
          if ( (all.zcam->zcamState == ZCAM_OFF) && (all.zcam->zcam_stat== 0) )
          { // then it is off and no errors (first time)
            all.zcam->zcamState = ZCAM_START_RQST ; // request start-up
            zooCam_state(); // & execute: turns on the zooCam
          }
        }
        use_alt = 0;  //sep15 kluge: hijack to control zooCam,
        //this way, the next IF will be avoided: sep15 zooglider has no altimeter
      #endif
      
      if ( (use_alt) && (down) && (pval> iparam[IA_alt_z_1st] ) ) 
      {  // time to use the altimeter aaaaaaaaaaaaaaaaaaaaaaaaaaaaa
         alt_stat = check_altim(  use_alt, alt_stat, w );
         if (alt_stat>4) //ssssssssssssssssssssssssssssssssss
         { // then bottom-trigger, exit in alt_stat s
              if (alt_stat<30) return(0); // if <30s, return now!
              tnow =  RTCGetTime(0,0);  //get time now
              tmax = tnow + alt_stat-20; //=time to exit loop
         } //sssssssssssssssssssssssssssssssssssssssssssssssss
         
      } //aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa
       
    } // end if j==jstart ---------------------------------------------
    else if (j>jstart) // keep accumulating ---------------------------
    {  pitch_avg += datp->es.pitch; //add in new value
       roll_avg  += datp->es.roll;
    }  // end if j==jstart --------------------------------------------
    
    //-----------------------------------------------------------------
    //-----------------------------------------------------------------
    if (j==j_act) //then it's time to do the active control  ----------
    { pitch_avg = pitch_avg /(jsamples*10); //=avg pitch value
      roll_avg  = roll_avg  /(jsamples*10);
      
      if (all.exc->pgood)  //  ppppppppppppppppppppppppppppppppppppp
      { //  believe P, estimate w, compute dr, in meters ppppppppppp
        pa = pitch_avg;
        dp = p0 - pval;  // >0 on ascent (P decreasing w/time)
        phi = datp->es.comp/10.;  // magnetic heading
        accum_dr ( pa, phi, dp ); // accumulate into dr
        p0 = pval;  // save for next estimate
                
        // check vertical speed every 300 s .....................
        if ( (tnow - Btime) >300 ) // 300 s allows settling time
        { // compute w : for both descent & ascent
          nw++;  // counter of #vertical velocity checks.
          w = 100*dp /(j_act*dt); // vertical speed, cm/s
          // if (w<0) w = -w; // take absolute value
          // for 64 s if dp noise = 20 cm, w noise < .3 cm/s
          printf("w=%6.1f\n",w);
          Btime = tnow;  // = reset timer to compute w
          
          if ( nw==2 && pi_stat==1 ) //nnnnnnnnnnnnnnnnnnnnnnnn
          {  // Believe both angle and pot,
             // save pot setting in case pitch angle breaks.
             dh = flt_abs(pitch_avg - pit); // = abs error in pitch
             if (dh<2)
             { // within 2 degrees of desired pitch, store value
			     ip = all.dat->es.ad_pitch; // pot setting
			     if (  ip >iparam[ MA_pitch_min ] )
			     { // then legal cnts iiiiiiiiiiiiiiiiiiii
			        if (down ) 
			        { // save the latest pitch down setting
			          ip = ip - iparam[ IA_pitch_0];
			          iparam[ IA_pi_dn ] = ip; 
			        }
			        else 
			        { // save the latest pitch up setting
			          ip = ip - iparam[ IA_pitch_0]; 
			          iparam[ IA_pi_up ] = ip; 
			        }
			     } // iiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiii

             } //end dh<2
          } //nnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnnn

          if  (down && p0>10 && wdmin <=0 ) // dddddddddddddddd
          { // we are going down & we want to pay attn to wdmin
             if ( w>-1  ) 
             {  // then not going down
                 tmax = tnow;
                 // below was 0xf000: wrong value!! should only be 1 bit
                 all.exc->exc_stat |= EXC_NOT_SINKING; //jan13
                 // 12oct2010 changed to 0x8000
             } // set so we exit loop 
             
             else if ( w>wdmin  && pit>-35 ) // then going too slow
             {  // pitch fwd to speed up
                send_p_r_run ( CS_PITCH, 0, ipfwd ); 
                pit = pit -3; // increase nose down by 3 deg.
                printf("w=%3.1f, New pitch down = %d\n",w, pit);
             }
             
          } //ddddddddddddddddddddddddddddddddddddddddddddddddddd
          
          //uuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuu
          if ( (!down) && ( w< iparam[ IA_wmin_up ] )  )  
          { // then going up & going too slow : pump some more
            hdt = iparam[IA_delta_pump];    // do incremental pump
            // if normal speed,use auto-detect 
            if (w>5) use_oil = OIL_AFTER_T; // auto-detect option
                else use_oil = OIL_NO;      // slow, pump no matter what
            // Move manually back to give ~right pitch
            send_p_r_run ( CS_PITCH, 0,  -idpmp );
            cprintf("Pump %5d, pitch back %5d\n",hdt,idpmp);
            // turn on the pump
            go_pump ( hdt, use_oil ); // turn on pump for hdt sec
            
          } // end if going up & slow uuuuuuuuuuuuuuuuuuuuuuuuuuu
          
        } // end if >300 s since last w check ...................
          
      }  // end if pressure is good pppppppppppppppppppppppppppppppp

      // now continue with active control
      dh = datp->es.comp/10. - theta;  // compass error, magnetic
      dp = dh = limit180(dh); // limit to +/- 180 degrees
      // limit accum error range to avoid big esum swings
      // this is to avoid exceptions when large steering problems
      if (dp> ECLIP) dp= ECLIP; //  limit error accum.
      if (dp<-ECLIP) dp=-ECLIP; //  limit error accum.
      
      //--feb11, mods to how esum gets accum. -------------------
      //--below simplifies to just not accum errors if z<20
      es = all.flt->esum + dp; // present accum.error
      if ( (es> -esum_max) && (es<esum_max) && (pval>20) )//ee
      { // then haven't reached the limit, & away from surface
        all.flt->esum = (short) es; // keep new value
        all.flt->isum++; // increment counter
      } //eeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeee
      //else esum stays the same: will always be <esum_max
      //---------------------------------------------------------
      
      if (pi_use) // Only do pitch control if wanted
         control_pitch ( pitch_avg, pit, 1.0 );
      
      if (theta>=0)
         control_head ( down, dh, roll_avg   ); 
         
      j = 0;  // reset active control interval counter
     } //end if active control ----------------------------------------
     //----------------------------------------------------------------
     
  } //end while tnow<tmax +++++++++++++++++++++++++++++++++++++++++++++++++
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  
  cprintf("end glide,dat->i=%d\n",all.dat->i);
  
  return(0);
} // end glide ****************************************************************
//*****************************************************************************


short  bottom_turn ( void  ) //************************************************
{ // pump while trying to maintain a steady pitch down,
  // when pressure rises, switch to a steady pitch up
  // Returns TRUE if pressure is still increasing or low V or high I

  long x, t_up;  
  long amp_avg=0, bat_avg=0, navg=0;
  short i, pi_val, theta, max_amp=0, ip, dt, ipmp0 ;
  short init_up;  // flag that gets set true if we've started going up
  ulong tnow, endt, tchange, tstart, ti_amp=0;
  short ierr, ti_pmp, pi_use, rising, tpmp0, pumped0;
  short ro_use, r0d, ro_bottom; //feb11 additions
  short try2 = FALSE;  //2nd pump try
  ushort pmax, izmax;
  struct dat_param *datp;  // data struct w/data values
  float xp, dh, zmax, dp, p0; // etau;
  #if BENTHOS==1 || NPS==1
    uchar dbg[255];
  #endif

  datp = all.dat;
    
  theta = all.nav->theta; // =desired heading, magnetic
  pi_use  = ( iparam [ IA_pi_stat ] >0);  // TRUE to use pitch angle 
  ro_use  =   iparam [ IA_ro_use  ];      //feb11, has roll control settings
  ro_bottom = ro_use & RO_BOTTOM; // feb11, >0 IF want to do active heading
  //--following 2 lines removed w/new feb11 code; now uses fp->ediv
  //etau  =  (float) ( iparam[ IA_ro_etau ] )*0.1;  
  //etau = etau* all.flt->etau;  // new weight for I error
  // esum/etau = roll degrees needed to go straight

  p0 = datp->dbar;  // initial pressure upon entry

  //-- feb11, update the zero roll setting ----------------------------
    i = (2*all.flt->esum)/all.flt->ediv; // *2 = lsb=0.5deg: same as 0711 code
    all.eng->r_err = (char) i; // keep for eng. msg
    r0d = iparam [ IA_ro_dn ]; // last value
    
    if (p0> 90 && ((ro_use & RO_SET_0) == 0) ) //====================
    { //-- most likely stabilized in course & want to update
      if (all.flt->em == 0) //--------------------------------
      { // then no integrated correction was done
        i = all.flt->isum; // #of dh points accum'd
        if (i<1) i=1; // make sure is legal
        i = all.flt->esum / i;  //avg head error 
        i = i*all.flt->rm; // converts from dh to dr
        all.eng->r_err = (char) 2*i; // keep for eng. msg
      } //-----------------------------------------------------
      else //--------------------------------------------------
      { // then doing integrated correction
        i = all.flt->esum *all.flt->em; // avg error
      } //-----------------------------------------------------
      i = r0d - i; // i=new guessed roll to go straight going DOWN
      if (i> 30) i= 30; // limit
      if (i<-30) i=-30; // limit
      if ( (ro_use & RO_WEIGHT) ==0 )
      { xp = 0.25; } // use 1/4 weighting
      else 
      { xp = (ro_use & RO_WEIGHT) /16; //now range 1-15
        xp = xp/16; // range (1-15)/16;
      }
      xp = r0d*(1-xp) + i*xp; // new value
      r0d = (short) xp;
      if (DEBUG) cprintf("New r0d = %d\n", r0d);
      iparam [ IA_ro_dn ] = r0d;
      
    } //=============================================================
    
    if (ro_bottom ==0 ) //==============================
    { // feb11 NOT doing active steering control, go to this new value
      xp = (float)  datp->es.roll/10.0; // present roll value
      control_roll( xp, r0d );
    } //================================================
  //-------------------------------------------------------------------

  #if PPLUS==1 //feb12, make sure no loads applied
    pp_zero_loads_bottom();
  #endif //---------------------------------------
  
  dt = SAMP_DT_UP;    // 0903 feb11: time between samples for ascent
  ti_pmp = iparam [ IA_max_pump ]; // max time to try to pump to rise
  // ti_pmp time is long in case pump is slow
  // once we start heading up, pump time is adjusted to get right buoyancy
  //0903 feb11 pump changes for initial pump period.
  tpmp0  = iparam [ IA_drift_pmp ]; // time we WANT to pump in drift mode
  ipmp0  = 0; //  actual drift pump time, =0 if did not drift
  pumped0 = FALSE; // flag that haven't pumped for tpmp0 yet

  pmax = datp->ds.p;  // present value of press counts
  //pi_val = -5;  // go to a shallow dive angle of -5 degrees
  pi_val = -10;   // 0903 feb11: -10 = still flying well
  init_up = FALSE; // flag that we've not transitioned to going UP

  Close_Valve;  // make sure the valve is closed
  if (Verbose) cprintf("bottom turn: closing valve\n");
  DelayMilliSecs(500);
  
  // 0903 feb11, do drift mode after pumping for a spec. time
  if ( iparam[ IA_drift_tm ]>0 ) //dddddddddddddddddddddddddd
  {  // we want to drift for awhile
     go_pump ( tpmp0, OIL_NO );      // turn on the pump
     //dec11, mod from i+2 to tpmp0+2...bug!
     ipmp0   = watch_pump ( tpmp0+2, TRUE );   // wait for pump to finish
     pumped0 = TRUE; // have done the initial pump
     Pitch_Level;    // level pitch
     go_drift( );                // drift

     //nov11 moved BENTHOS call to here: this requires doing the
     //drift mode first before trying the ACM
     #if BENTHOS==1   //added 22Sep09 ,mod 16FEB11 (removed go_drift call)
       //version for Spray with Benthos modem--added 22Sep09 to give option
       //of Ac Comms either here in bottom_turn or at surface after ascend
       if ( iparam[IA_acm_mode]==1 ) //bbbbbbbbbbbbbbbbbb
        { //--then want to use the ACM now.
          //removed old code verifying that the pump is off
          //write header string that sampling now
            sprintf(dbg,"BOTTOM TURN   %6ld Pump is OFF hyd->total=%d\n",
                        RTCGetTime(0,0),all.hyd->total);
            buf_to_cf1V( strlen(dbg),dbg);  
            printf("%s",dbg);
           
          do_acm(); //do ac comms 
        } //bbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbb

     #endif 

  } //ddddddddddddddddddddddddddddddddddddddddddddddddddddddd
  // no matter if we've drifted or not, do the normal bottom turn algorithm
    
  tstart = tnow = RTCGetTime(0,0);  // get the time now
  endt = tnow + ti_pmp; // run pump until this time
  tchange = tnow + ti_pmp/2;  // change attitude halfway thru
  go_pump ( ti_pmp, OIL_NO ); // turn on the pump

  ierr = 0;  // start off with no errors ++++++++++++++++++++++++++++++++++
  while ( tnow < endt )  //++++++++++++++++++++++++++++++++++++++++++++++++
  { // do until time to quit ++++++++++++++++++++++++++++++++++++++++++++++

    // 0901 code, switch from closely monitoring pump to having the pump
    // module doing it,
    hibernate(&tnow, dt, PIT_IRQ2 );    
    take_data( );  //will update the hydraulic struct with present values
      if (all.hyd->bad_amp) //-----------------------
      {  // we need to exit now if bad amps 
         // if bad_amp flag is set, pump is already stopped by too high amps 
         endt= tnow-1; // set time so we'll exit
      }  //------------------------------------------
    //bat_avg += datp->es.ad_bat_14; //old
    //amp_avg += datp->es.ad_current;//old
    // jan15, do not avg a bad voltage value
    if (all.hyd->volt > 100) //,,,,,,,,,,,,,,,,
    { // then >1 V; measured something
      bat_avg += all.hyd->volt; // already scaled to eng.
      amp_avg += all.hyd->amp; // " "
      navg++;
    } //,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
    if (datp->ds.p > pmax) pmax = datp->ds.p; // keep track of max depth
    
    //rrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrr
    if ( (tnow-tstart) >= tpmp0 ) pumped0 = TRUE; //0903
    if ( all.exc->pgood && ( datp->ds.p <pmax ) ) pumped0 = TRUE; //0903
    //pumped0 = TRUE if either drifted or have pumped for tpmp0 s or not sinking.
    if (pumped0) pi_val = iparam [ IA_glide_angle ];
    //if either true, go to pitch up.
    
    rising =  ( all.exc->pgood && ( (datp->ds.p +6)<pmax ) );
    #if DEBUG==1
       cprintf("rising=%d, pmax=%d, p=%d, secs=%ld, tc=%ld\n",
               rising, pmax, datp->ds.p, tnow, tchange);
    #endif
    if ( !init_up && (  rising || tnow>tchange ) )
    {  // first indication that we're going up : 6 cnts ~ .3 m
       // OR we've timed up...no matter what,
       // we should continue assuming that we are ascending
       init_up = TRUE; // set flag that we've turned around
       ti_pmp = (short)(tnow - tstart); //time pumped to neutral
       pi_val = iparam [ IA_glide_angle ]; // set to positive value
         // save some engineering values
         zmax  = dBar( pmax, all.flt);
         izmax = (ushort) zmax;  //recast as unsigned short
         all.eng->zmax    = izmax;
         all.eng->ti_pump = ti_pmp + ipmp0; // 0903 add in drift pmp time
         all.eng->vac     = vacuum(); //  units to use?
       
       //---nov11 removed benthos code and put after drift time:
       //---MUST drift some before trying the ACM.
              
       // set up for starting to rise ------------
       ti_pmp = iparam[ IA_buoy_pump]; // =desired time to pump
       hyd_more_pump(ti_pmp); // manually adust total time
       tnow = RTCGetTime(0,0); 
       tchange = tnow; //=time that we start to rise
       cprintf("pumping for another %d s\n", ti_pmp );
       endt = tnow + ti_pmp+2; // set new end time
       sbe_pump_hi(); // 0711 turn on the SBE pump @high speed
       #if ISUS==1 // isus sensor, turn on
         isus_on(); // this will allow 16s for it to initialize
         all.isus->mode = ISUS_ON; // before calling isus_profile_start below
       #endif  

       if (!pi_use) // 0711, then pitch angle is broken
       {  
          ip = iparam[ IA_pitch_0]  + iparam[ IA_pi_up ]; // nose up
          send_p_r_run ( CS_PITCH, 1, ip );   
       }
       //--feb11, set heading control stuff ------------
       if ( (ro_use & RO_ESUM) == 0 )
       { //feb11, reset esum to 0
         all.flt->esum = 0;
         all.flt->isum = 0;
         if (DEBUG) cprintf("esum zeroed\n");
       } //------------------------
       //--do best guess to go straight on ascent
       xp = (float)  datp->es.roll/10.0; // present roll value
       control_roll( xp, iparam [ IA_ro_up ] );
       //-----------------------------------------------
              
    }  //rrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrr
    
    //ppppppppppppppppppppppppppppppppppppppppppppppp
    if ( rising && !try2 &&   (endt-tnow)<20 )
    { //within 20s from being done pumping
      try2 = TRUE; //tested for 2nd pump try
      if ( (datp->ds.p +50) >pmax ) //----------
      { // then we've risen<2m...on bottom?
         // try pumping 60s more
         cprintf("will pump extra 60s\n");
         hyd_more_pump(60); // manually adust total time
         endt = tnow + 60; // set new end time
      } //-------------------------------------
    } //ppppppppppppppppppppppppppppppppppppppppppppp
    
    cprintf("init_up=%d, p=%u, pmax=%u\n",init_up, datp->ds.p, pmax);
    xp = (float)  datp->es.pitch/10.0; // present pitch value
    dh = datp->es.comp/10.;  // compass
    dp = datp->dbar - p0; // change in pressure
    accum_dr( xp, dh, dp); // accumulate the dr
    p0 = datp->dbar; // update for next time around
    
    // as we pump, tail becomes more buoyant, and nose gets too far down
    if  (xp< (float)pi_val && pi_use ) // true if nose is too far down
      { control_pitch( xp, pi_val, 0.8);  } // bring nose up
      
    // also check heading
      xp = datp->es.roll/10.; // present roll
      dh = dh - theta;  // compass error, magnetic
      dh = limit180(dh); // limit to +/- 180 degrees
      //feb11, add ro_bottom conditional 
      if (theta>=0 && pi_use && ro_bottom)  // then control
         control_head(!init_up, dh, xp  ); 
    
    // if >16s since the SBE pump is on, set up to take SBE stuff
    t_up = RTCGetTime(0,0) - tchange;
    if ( t_up > 24  && !all.dat->sbe_pro_on ) //ttttttttttttttttt
       // turn on pump, cal sbe a/d & set flag to take sbe data
       // this automatically drops the SBE pump from HI to normal speed.
       {  sbe_start_profile(  ); 
          //feb11, assume we have started to fly again: do head-control
          ro_bottom=1; // YES, time to do heading stuff
          #if ISUS==1 
            isus_profile_start(   );
          #endif
       } //tttttttttttttttttttttttttttttttttttttttttttttttttttttt
      
  } // end while ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++


  //-----------------------------------------------------------------------
  //now clean up things at the end of bottom_turn -------------------------
  hyd_stop_pump(  ); // make sure that the pump is off
  check_amp_action( ); // decide what to do if amps are bad
  // do the above since hyd->bad_amp is cleared in following pump tries
    // save averaged battery+current
    if (navg>0)
    {  // then something to avg
       i = (short) (bat_avg/navg); // already scaled in display_pump
       all.eng->bat = i;     //c2eng(i, W_14V);  //V*100
       i = (short) (amp_avg/navg); // already scaled in display_pump
       all.eng->current = i; 
       if (all.eng->bat < 1000  ) ierr = 2;  // low voltage for Lithium!
    }
  
  
    // store other motor info
       ti_amp = all.hyd->ti_max; // 0902; was NOT correctly set in 0901
       //>>> ti_max = 0.1s resolution; need to divide by 100,
       ti_pmp = (short) (ti_amp/10) << 8; // keep to 10s accuracy, shift to msb
       max_amp = all.hyd->max_amp/2;  // .02 resolution
       if (max_amp>255) max_amp = 255; //=5.1 amp max
       ti_pmp = ti_pmp + max_amp;  // msb=time, lsb=current
       all.eng->max_amp = ti_pmp; // store in engineering structure
       cprintf("V=%d  I=%d Imax=%d ti=%lu  %d\n", all.eng->bat, 
       all.eng->current, max_amp,ti_amp, all.eng->max_amp); 

  hpr_pwr_get( CS_HYD ); // gets the total pump time from hyd module....
  flash_hyd();   // store hyd info to flash
  
  if (!all.dat->sbe_pro_on)
  { sbe_start_profile( );  }// then put sbe  in profile mode
  
  if (all.exc->pgood) //-------------------------------------------
  { //if pressure is good, see if we actually started to rise
     x = (long) pmax - (long) datp->ds.p;  // difference in counts
     printf("end bottom turn,dp = %ld\n",x);

     if ( (zmax>10) && (x<20)   ) //xxxxxxxxxxx
     {  // then ascended <20 counts ~< 1 dBar
        pump_recovery( ); // try to recover
        ierr = 1; // set flag that recovery was necessary
     } // end if didn't rise xxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
  } //--------------------------------------------------------------
  
    if (all.hyd->bad_amp) //---------------------------------------
    { // then bad amps, try to recover
      // this may be the 2nd recovery attempt if the above failed
      check_amp_action( );
      pump_recovery( );
      ierr=3;  // high amps
    } // end if bad amps -------------------------------------------

  // store latest max depth
  for (i=1; i< N_BZ; i++) // shift all numbers up 1: bz[i]=bz[i-1]
      all.exc->bz[N_BZ -i] = all.exc->bz[N_BZ - i -1];
  all.exc->bz[0] = izmax;  // store the latest

// Depending on error and chances of recovery, do different things:
  // ierr==1 means pump failed to cause ascent
  // ierr==2 is low battery voltage
  // ierr=3 is high current : exception is covered by check_amp_action
  // If recovery is NOT OK, try to limp home, else abort

  if ( ( ierr ==1)  || (ierr==2)  ) 
    check_pump_recovery( );  

  #if PPLUS==1 //feb12, re-apply the old loads
    pp_reset_loads_bottom ();
  #endif //---------------------------------------
    
  return( ierr );  //returns TRUE if there is a problem
} //***************************************************************************

#if BENTHOS==1 || NPS==1
short do_acm( void) //***************************************
{  // do_acm is derived from go_drift for Spray with Benthos modem  26Mar10
   // IA_acm_tm = minutes to spend in acoustic modem activities
   // Returns immediately if IA_acm_tm==0
   // if ee_acm_mod=0   then no acoustic modem activities
   //              =1   acoustic comms are done at bottom of dive after turn
   //              =2   acoustic comms are done at end of ascend near the surface
   // returns early if goes too deep..i.e. needs pump recovery
   // returns 2 if ran out of drifting time before modem list was exhausted
   // returns 1 if drifted 100m deeper from starting depth
   //        -1 if there is no modem to look for
   //        -2 if transfer wasn't started
   //    else 0 if data transferred with or without CRC errors
   ulong ds, Tend, Tstart;
   short navg=0;
   short err=0,stat,lstat,MaxBtry;
   short Nhunks,hunk,subindex,nmodem;
   short CRCblk; //0 if good CRC on all sub-block xfers
   float pavg=0;
   uchar dbg[255];
   short orig_pitch,new_pitch;  //original pitch counts at start of drift
   short orig_nsub = 0;


   struct drift_param *DFT;
   struct acm_param *ACM;
   struct dat_param *DP;
   
   if (iparam[IA_acm_mode]!=1 && iparam[IA_acm_mode]!=2 ) return(0); //illegal mode
   if (iparam[IA_acm_tm] <= 0 ) return(0); //no time for communications
   if (iparam[EA_op_mode] == 1) // then we're in the first test dive LAR2009-12-15 
      return( 0 );  // never try ac comms in the test dive   LAR2009-12-1

   //30jul09 add capability of servicing more than one
   //modem per drift. Use database modlist[i]  The list is terminated by a
   //modlist[last].id=-1;  for each i copy modlist[i] into ACM->modem at start of processing
  
   ACM=all.acm;  //points to ACM struct for params.
   DP = all.dat; //points to ACM struct for data
   DFT = all.drift;  // point to the drift structure
   DFT->p0 = (short)DP->dbar;  // start drift depth [m]
   
   //all.klifo = assign_lifo(all.lifo, iparam [ EA_prof_num ],0); //18FEB11, start off with a new lifo
   
   ds = ( (long)iparam[IA_acm_tm] )*60L;   // #seconds to do ac comms
   printf("Ac Comms for %ld seconds from depth=%d\n", ds,DFT->p0);
   Tstart = RTCGetTime(0,0); // present time
   Tend  = Tstart + ds;  // time to quit drifting  
 
   for( nmodem=0 ; nmodem<MAX_MODEMS ; nmodem++ )
   { //The modem database has up to MAX_MODEMS, indices 0 thru MAX_MODEMS-1
     //Try each member in the list until the REMid=-1

     acm_xmits=0; //initialize

     if( err ==1 )
     { //Spray descended since begin of drift during previous modem
       break;  //and quit drifting right now
     }     
     //check if there is a modem to check
     if( (ACM->modlist[nmodem].id > 99) || (ACM->modlist[nmodem].id < 0) )
     { err=-1; //quit drifting right away when hit end of modem list
       break;
     } 
     
     if( RTCGetTime(0,0) > Tend )  //but don't go past Tend
     { //time to quit drifting
       sprintf(dbg,"do_acm   %6ld Drift terminated by time\n",RTCGetTime(0,0));
       buf_to_cf1V( strlen(dbg),dbg);  
       printf("%s",dbg);
       err=2;
       break;//quit drifting right away when run out of time
     }
     
     //Copy info into struct ACM->modem for next modem in database
     GetModemInfo(nmodem);  //28Mar10 The value of ACM->modem.dft_pit is
                                //copied from EEPROM ee_acm_ptch for all modems.
   
     sprintf(dbg,"Dive %3d %6ld Start do_acm REMid=%d\n",
                 iparam[EA_surf_num],RTCGetTime(0,0),ACM->modem.id);
     buf_to_cf1V( strlen(dbg),dbg);  //header line with source, time, # bytes
     printf("\n%s",dbg);
    
     flash_update_FAT();
     
     if ( nmodem==0 )
     {  //adjust Spray pitch but only once per acoustic session
        orig_pitch= get_p_r_pot(CS_PITCH);  //save pitch counts at start of drift
        new_pitch=iparam[IA_acm_ptch];  //this is a relative change in pitch counts
        if ( new_pitch != 0 )  
        { //change Spray pitch before trying to establish the link
          send_p_r_run(CS_PITCH,0,new_pitch);
        }
        sprintf(dbg,"ACPCH    %6ld pch,orig,new=%6d %6d %6d\n",RTCGetTime(0,0),new_pitch,
                     orig_pitch,get_p_r_pot(CS_PITCH));
        buf_to_cf1V( strlen(dbg),dbg);  
        printf("%s",dbg);
     }
     
     take_data(); // samples P, eng sensors, stores to flash. Tickles wdog
     //Write the results of the take_data to the CF1 "V" file
     sprintf(dbg,"do_acm   %6u %6.1f dB h,p,r= %6d %6d %6d\n",
                  DP->ds.secs,DP->dbar,DP->es.comp,DP->es.pitch,DP->es.roll);
     buf_to_cf1V( strlen(dbg),dbg);  //header line with source, time, # bytes
     printf("\n%s",dbg);

     stat=OpenAcLnk3();  //use (newer) OpenAcLnk3 (24Jul09)
     sprintf(dbg,"do_acm   %6ld OpenAcLnk err=%d\n",RTCGetTime(0,0),stat);
     buf_to_cf1V( strlen(dbg),dbg);  
     printf("%s",dbg);
     if( stat !=0 )
     { //OpenAcLink was unable to establish a link with the REMOTE
       //This could be because the LOCAL couldn't be
       //accessed or REMOTE link wasn't established or ??
       lifo_acdiag(); //queue lifo_acdiag mess. with failure & LOC status
       err = -2;
     }
   
     //OpenAcLink was successful at setting things up
      
     while( RTCGetTime(0,0) < Tend && err==0 ) //+++++++++++++++++++++++++++++++++
     {  // drift until time to return or run out of data to get
   
        take_data(); // samples P, eng sensors, stores to flash
        //Write the results of the take_data to the CF1 "V" file
        sprintf(dbg,"do_acm    %6u %6.1f dB h,p,r= %6d %6d %6d\n",
                     DP->ds.secs,DP->dbar,DP->es.comp,DP->es.pitch,DP->es.roll);
        buf_to_cf1V( strlen(dbg),dbg);  //header line with source, time, # bytes
        printf("\n%s",dbg);
      
        pavg += DP->dbar; // accumulate average pressure
        navg++; // increment counter
           
        //modem version doesn't do the small pumps to maintain drift depth.
        //Only does the big bail out if go 100 m deeper.
        if ( (DP->dbar - (float)DFT->p0) > 100. ) // catch a bad depth! zzzzzzzzzzzzzzzzzz
        {  err=1;  break; }// exit now 
           
        //set ACM->this_blk to the 4K REM block to get from REMOTE
        if( WhatBlock( ACM ) != 0)
        {  err=0;
           lifo_acdiag(); //queue lifo_acdiag message with failure info, JAN12  
           break; 
        }//the list has no more blocks to obtain 
        
        //protect against ee_acm_try <= 0 in case operator goofs on command 
        MaxBtry = iparam[IA_acm_try];
        MaxBtry = MaxBtry <=0 ? 1:MaxBtry;
        printf("MaxBtry=%d\n",MaxBtry);//debug
        
        //try up to ee_acm_try times to read ACM->this_blk
        //Normally if the REMOTE has retry enabled, one would set ee_acm_try= 1)
        for( ACM->RdB_tries=1 ; ACM->RdB_tries<=MaxBtry ; ACM->RdB_tries++)
        { //modem.nsub determines how to do the transfer of this_blk 4K bloc
          if ( (ACM->modem.nsub==1) || (ACM->modem.nsub==2) ||
               (ACM->modem.nsub==4) || (ACM->modem.nsub==8)    )
          { //break the transfer of 4K this_blk into nsub sub-blocks of 256-bytes

            Nhunks=16/ACM->modem.nsub;  //number of (nsub*256) byte hunks in a 4K block
            //The data from all Nhunks will be concatenated in ACM->buf by XferSubBlk
            //01FEB11 move buffer init to here:
            ACM->by_read=0;
            ACM->buf[0] = '\0';
            CRCblk=0;  //CRC AND of all sub hunks.  0 iff all hunks are valid CRC
  
            //Set Sreg57 on REM to desired # of sub-blocks per transfer
            stat=SetSreg(ACM->remID,57,ACM->modem.nsub,3 );  //3 attempts
            sprintf(dbg,"do_acm   %6ld remID=%d Set S57=%d err=%d\n",
                         RTCGetTime(0,0),ACM->remID,ACM->modem.nsub,stat);
            printf("%s",dbg);//print diagnostic
            buf_to_cf1V( strlen(dbg),dbg);  //and write to CF1          
                                
            for(hunk=0;hunk<Nhunks;hunk++)
            { //try to get each hunk of data
              subindex=16*ACM->this_blk + hunk*ACM->modem.nsub;//sub-block index of start of
              stat=XferSubBlk(subindex,ACM);
         
              sprintf(dbg,"do_acm   %6ld ID:%d  Subindex:%d Try:%d XfSbBlk err=%d\n",
                         RTCGetTime(0,0),ACM->remID,subindex,ACM->RdB_tries,stat);
              printf("%s",dbg);//print diagnostic
              buf_to_cf1V( strlen(dbg),dbg);  //and write to CF1
              if( stat==-3 )
                { //no response from the REMOTE in this hunk
                  //the data stream was annotated in XferSubBlk
                }
              else if( stat==-6)
                { //bad CRC in this hunk of sub-blocks
                  CRCblk=-6; //one bad hunk means whole block had bad CRC
                }
              else if( stat==-5 )
                { //the hunk is empty so there won't be any more in this_blk 
                  if( hunk==0 )
                   { //the entire block is empty
                    break; //breaks with stat=-5
                   }
                  else
                   { //there is some data in the block
                    stat=lstat; //return status of previous hunk
                    break;
                   }
                }//end else if stat==-5
                
              lstat=stat; //save status of previous hunk(which wasn't empty)
            } //end for hunk=0:Nhunks
            if (orig_nsub) //if this is greater than zero, it means we didnt do a full
                           //block xfer, so set it back to its orig value for the next
                           //full block xfer.
             {
                 ACM->modem.nsub = orig_nsub;
                 orig_nsub = 0; //set to zero so dont do this again
             } //end if orig_nsub
            if( CRCblk==0 )
            { //all hunks had good CRC
              stat=CRCblk;//CRC status is AND of all hunks
              ACM->CRC=0; //set the flag that will come back in Iridium message
            }
            else if ( CRCblk==-6 )
            { //at least one hunk had bad CRC
              stat=CRCblk;//CRC status is AND of all hunks
              ACM->CRC=1; //set the flag that will come back in Iridium message
            }
          }
          else 
          { //do the transfer as a single 4K block
            stat=XferBlk(ACM);  
            sprintf(dbg,"do_acm   %6ld ID:%d  Blk:%d Try:%d XferBlk err=%d\n",
                       RTCGetTime(0,0),ACM->remID,ACM->this_blk,ACM->RdB_tries,stat);
            printf("%s",dbg);//print diagnostic
            buf_to_cf1V( strlen(dbg),dbg);  //and write to CF1
          }
        
          if( stat == 0|| stat == -6 || stat == -4 || stat == -7)
          { //data xferred either with or without CRC error (0,-6)
            //bad format or bad header (-4,-7)
            //send back what we got
            lifo_acdiag(); //queue lifo_acdiag message with failure info
            lifo_acraw();   //05Mar09  send bad messages, warts and all
                                 //lifo_acdat sends ACM->by_read bytes starting at ACM->buf
            //05Mar09 write entire RAW TS buffer to CF1 flash
            //This includes headers + message + trailer
            sprintf(dbg,"ACRAW    %6ld %4d Blk:%d Try:%d\n",
                        RTCGetTime(0,0),ACM->by_read,ACM->this_blk,ACM->RdB_tries);
            printf("%s",dbg);
            buf_to_cf1V( strlen(dbg),dbg);     //header line with source, time, # bytes
            buf_to_cf1V( ACM->by_read, ACM->buf);  //write the entire RAW message
            if(stat==0 )
            {  err=0; //success
               break;  //no more retries
            }
            else if (stat==-6)
            {  err=0; //even a CRC error is viewed as a sort-of success
               continue;  //but make another try if ee_acm_try allows
            }
            else
            {  err=-2;  //bad format or bad header are errors that
               continue;  //deserve another try if ee_acm_try allows
            }
          }
          else if( stat == -5 )
          { //this_blk is empty
            if( ACM->xfer_mode==NEW_BLK )
            {  //this_blk is a NEW block so all successive new blocks are empty too
               ACM->new_blk=ACM->lnew_blk+1;  //so don't check for new data any more
            }
            else if( ACM->xfer_mode==OLD_BLK )
            {  //this_blk is an OLD block so all successive old blocks are empty too
               ACM->old_blk=ACM->lold_blk+1;  //so don't check for old
            }
            err=0; //not having data isn't an error so err==0
            break;  //advance to next block on the list
          }
          else if( stat != 0 )  
          { //XferBlk had some other error which prevented data transfer
            //   -2 = no response from LOC modem or invalid command
            //   -3 = REMOTE no response
            //   -8 = can't start the REMOTE because it is transmitting
            err=-2;  //unable to start the transfer 
          } 
         
          //make another try at this block       
        }
      
        //no more retries to get this block
        if( stat!=0 && stat!=-6  && stat!=-4 && stat!=-7)
        {  //have to display acdiag for type -2.-3.-5,-8 errors
           lifo_acdiag(); //queue lifo_acdiag message with failure info
        }

        //go on to next block if any
        if( stat==-2 || stat==-8 )
        { //-2 = no response from LOC modem.  FATAL so don't try next block
          //-8 = REM is always transmitting when LOC ries to start it
          break; //wait until next dive for another try
        }
        else if( stat==-3 || stat==-5)
        { //-3 = REMOTE no response--maybe next block we can hear?
          //-5 = this block is empty--maybe next block has some?
          continue;
        }
        
        //stay with this modem until it finishes or run past Tend
     } //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

     //We're done with comms to this modem
     //Update modem database based on success of transfers
     //30May2010  -- Adjust4NxtDive does nothing at present
     //              It needs work before being implemented.See notes in
     //              source code for Adjust4NxtDive in Benthos.c
     Adjust4NxtDive(ACM); //adjust block list in modem struct for next dive
                          //so Spray doesn't ask for a block that it
                          //successfully got on this dive. 
     //Save Adjustments for next dive in database for this modem
     //(otherwise this info will get lost when we advance to next modem in the list)
     ACM->modlist[nmodem].ib_new  = ACM->modem.ib_new ;  
     //ACM->modlist[nmodem].ib_rpt  = ACM->modem.ib_rpt;//30May10--OLD_BLK modem
                                                        //data params only changed
                                                        //by Ground Station  
     lifo_acenergy(ACM->modlist[nmodem].id,acm_xmits); //queue lifo_acenergy message
     printf("END modem id=%d, acm_xmits=%d\n",ACM->modlist[nmodem].id,acm_xmits);  
     //go find next modem in the database
   }

   take_data(); // samples P, eng sensors, stores to flash
   sprintf(dbg,"EnddoACM %6ld %6.1f to %6.1f\n",RTCGetTime(0,0),(float)DFT->p0,DP->dbar);
   buf_to_cf1V( strlen(dbg),dbg);  //header line with source, time, # bytes
   printf("\n%s",dbg);
             
   flash_update_FAT();
     
     //Kill the LOCAL modem 
   TSerInUse=0;  //allow CPU lowest power and change Sysclock
   DelayMilliSecs(1000);  //settle before
   acm_off();//turn off LOCAL modem

   send_p_r_run(CS_PITCH,1,orig_pitch);//restore the orig battery position in case it was changed
   
   if (navg>0) pavg /= navg; //avg press
   DFT->pavg = (short) pavg; // average drift pressure
   DFT->pe = (short) all.dat->dbar;
   DFT->pmp_tm = 0;
   DFT->tm = (short)(( RTCGetTime(0,0) - Tstart)/60); // actual drift minutes
   DFT->nsamp = navg; // actual #points averaged

   return(err);
 } //end of do_acm for Spray with Benthos modem=====

#endif  //end of version for Benthos Spray


//*****************************************************************************
short  go_drift ( void  ) //***************************************************
{  // after becoming 'neutral' in the bottom turn, can now drift at depth
   // returns early if goes too deep..i.e. needs pump recovery
   // returns 1 if 100m too deep, else 0.

   struct drift_param *d;
   ulong ds, tnow, tmax, tsleep=60, dt, tstart, tnext;
   short navg=0, pmp_tm=0, p0, dpmp=0;
   short err=0;
   float pavg=0, pval;
   
   // initialize
   d        = all.drift;  // point to the drift structure
   d->p0=p0 = (short)  all.dat->dbar;  // start drift depth [m]
   d->tm    = iparam [ IA_drift_tm ];  // drift time, minutes
   d->nsamp = iparam [ IA_drift_ns ];  // # of samples to take
   if (d->nsamp<1) d->nsamp=1; // keep legal

   ds = (long) (d->tm)  *60;       // #s to drift
   dt =  ds / (long)d->nsamp;  // time between samples
   cprintf("Drifting for %ld s, sampling every %ld s\n", ds, dt);
 
   tnow  = tstart = RTCGetTime(0,0); // present time
   tmax  = tnow + ds -1;  // time to quit
   tnext = tnow + dt -10;  // time of next sample
   
   while (tnow<tmax) //++++++++++++++++++++++++++++++++++++++++++++++++++++
   {  // drift until time to return
      sleep_check( tsleep, PIT_ONLY, TRUE );
      take_data( ); // samples P, eng sensors, stores to flash
      tnow = RTCGetTime(0,0);
      
      if ( tnow>tnext ) //------------------------------------------
      {  // time to take a sample
         tnext +=dt; // increment time for the next sample
         
         pval = all.dat->dbar;  // depth
         pavg += pval; // accumulate
         navg++; // increment counter
         pval = pval - p0; // if >0, we've been sinking
         
         if (pval>100) // catch a bad depth! zzzzzzzzzzzzzzzzzz
           { tnow=tmax+1; err=TRUE; } // exit now
         else // if too deep, pump some more
         {
           dpmp = pval*0.1; // pump 10s ~10cc for ea 100m too deep
           if (dpmp>2) //pppppppppppppppppppppppppppppppppp
           {  // then it is worth pumping
              go_pump( dpmp, OIL_NO );
              pmp_tm += watch_pump( dpmp, 0); // returns actual time it pumped
           } //pppppppppppppppppppppppppppppppppppppppppppp
         } //zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz
         
      } //----------------------------------------------------------
      
   } //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   
   if (navg>0) pavg /= navg; //avg press
   d->pavg = (short) pavg; // average drift pressure
   d->pe = (short) all.dat->dbar;
   d->pmp_tm = pmp_tm;
   tnow = ( RTCGetTime(0,0) - tstart)/60;  // minutes drifting
   d->tm = (short) tnow; // actual drift minutes
   d->nsamp = navg; // actual #points averaged
   
   return(err);
 } //**************************************************************************

short  surface ( void )  //****************************************************
{ // do surface stuff : get gps fix, send satellite data, + another gps
  // returns # messages that were sent via satellite
  // if the mission is not done, then it increments the dive counter
  //    and opens a new data file, with the GPS fix inserted
  // JUL14 add lifo_surface call
  
  short good, good1, good2, cf_stat;
  short isurf, pot_good, dlifo=0;
  short r0u, ro_use, pmax, i;  
  short nsent=0;
  float xp;

  short pmptime, rf_tm;
  ulong t0, tnow, abort_time, dt;
  
  isurf = iparam [ EA_surf_num ];
  cprintf("Surface number = %d\n",isurf);

  t0 = RTCGetTime (0, 0);  // UX time entering fnx

  roll_wing ( ZERO_WING, 1); // roll wing flat, 1=seek zero if pot is bad
  zero_pitch_angle(); // go to zero-pitch @ surface: uses IA_pi_stat value for method

    //--feb11 update guess of best roll for ascent ==================
    ro_use = iparam [ IA_ro_use ]; //flags for what to update
    r0u    = iparam [ IA_ro_up ]; // last value
    pmax   = all.eng->zmax; // max depth of dive
    if (pmax>100 && ( (ro_use & RO_SET_0) == 0) ) //=================
    { //-- most likely stabilized in heading & want to update
      if (all.flt->em == 0) //--------------------------------
      { // then no integrated correction was done
        i = all.flt->isum; // #of dh points accum'd
        if (i<1) i=1; // make sure is legal
        i = all.flt->esum / i;  //avg head error 
        i = i*all.flt->rm; // converts from dh to dr
      } //-----------------------------------------------------
      else //--------------------------------------------------
      { // then doing integrated correction
        i = all.flt->esum *all.flt->em; // avg error
      } //-----------------------------------------------------
      i = r0u + i; // i=new guessed roll to go straight going UP
      if (i> 30) i= 30; // limit
      if (i<-30) i=-30; // limit
      if ( (ro_use & RO_WEIGHT) ==0 )
      { xp = 0.25; } // use 1/4 weighting
      else 
      { xp = (ro_use & RO_WEIGHT)/16; //now range 1-15
        xp = xp/16; // range (1-15)/16;
      }
      xp = r0u*(1-xp) + i*xp; // new value 
      r0u = (short) xp;
      if (DEBUG) cprintf("New r0u = %d\n",r0u);
      iparam [ IA_ro_up ] = r0u;
    } //=============================================================
    //== reset heading error accum.
    all.flt->esum = 0;
    all.flt->isum = 0;
    //===============================================================

  if ( iparam [ EA_op_mode ] ==1 || iparam [ IA_argos_mult] ) 
      Argos_On; // if 1st dive or desired, turn on Argos
  
  hpr_pwr_get( CS_HYD ); // gets the total pump time from hyd module....
  flash_hyd();   // store hyd info to flash
  pmptime = iparam[ IA_max_pump ] - all.hyd->total;  //potential remaining
  if (pmptime>10) // then pump ++++++++++++++++++++++++++++++++++++++++++
  {  // pump remaining time; mar11, switch from OIL_YES, to OIL_AFTER_T
     go_pump ( pmptime, OIL_AFTER_T );  // this starts up the pump
  } //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  // while pumping, we can go do other things

  #if ISUS==1 //--------------------
    isus_profile_end(   );
  #endif //-------------------------

  // Take shallow adp data, +++++++++++++++++++++++++++++++++++++++++++++
  #if ADP==1 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
       dlifo = all.klifo; // this is the present SBD data pointer
       adp_post_process(  );  // process data & put in new SBD
       adp_shallow( ); // take shallow data & add to SBD
       adp_flash_dump( CF_ADP); // write ADP data to the CF1
       all.klifo = dlifo; // restore the lifo to the data SBD pointer
  #endif //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
   #if NTK==1 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++ JAN14
       dlifo = all.klifo; // this is the present SBD data pointer
       adp_post_process( );  // process data & put in new SBD
       adp_flash_dump(CF_ADP); // write ADP data to the CF1
       
       #if ISUS==0 //IIIIIIIIIIIIIIIIIIIIIIIII
         //adp_shallow(all); // take shallow data & add to SBD.  Commented out
                              //until we get a shallow NTK algorithm
       #endif //IIIIIIIIIIIIIIIIIIIIIIIIIIIIII
       all.klifo = dlifo; // restore the lifo to the data SBD pointer
  #endif //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  
  #if PPLUS==1 //-------------------
    pp_SBD_pack(); // pack PP data
  #endif //-------------------------

  #if ZOOG == 1 // zooglider
       dlifo = all.klifo; // this is the present SBD data pointer
       zonarPost   (  );  // process data & put in new SBD
       lifo_zooCam (  );  // add to lifo packet the last zooCam status
       zooFlashDump( CF_ZONAR ); // write zonar data to the flash
       all.klifo = dlifo; // restore the lifo to the data SBD pointer
  #endif
  

  // If op_mode==1 = first dive in the mission ------------------
  if ( iparam [ EA_op_mode ] ==1 || iparam [ IA_argos_mult] >0 )
  { // allow more RF time for tracking on the surface
    rf_tm = RF_TM_1; //  time to wait doing argos
    if ( iparam [ IA_argos_mult ] >1 ) 
       rf_tm = rf_tm * iparam [ IA_argos_mult];
    
    sleep_check( rf_tm, PIT_ONLY, TRUE );
  } //-----------------------------------------------------------

  #if SBE==1 //---------------------------
    #if SBE_1HZ == 1  
      print ( " COMPILE ERROR: 1HZ option removed Oct08 ")
    #endif
  #endif //-------------------------------
  
  Argos_Off;  // make sure Argos is off
  
  //--- write out surface drift from last dive cycle
  if ( calc_surf_drift() ) // true if good
     lifo_surf_drift(1); // then store to lifo

  // NOW wait for the pump to be done, //dec11, added 10 s
  if (pmptime>10) watch_pump ( pmptime+	10, 0 );  //nov11 only wait if pump was turned on
  DelayMilliSecs(600); // wait before getting last info
  last_hyd(); // gets the last values no matter what
  DelayMilliSecs(600); // wait before getting avg pwr info
  hpr_pwr_get( CS_HYD ); // =final power values
  flash_hyd();   // store hyd info to flash

    
  //Get the GPS fix =end-of-dive position +++++++++++++++++++++++++++++++
  #if ANT_XFR_SW  // go to desired wing and set up the switch --------
     pot_good = RF_Wing_Setup(  GPS_UNIT );
  #else // there is no antenna switch, use the stbd wing
     pot_good = roll_wing(STBD_WING,1);
  #endif  //----------------------------------------------------------

  hpr_pwr_get( CS_PITCH ); // =final values
  hpr_pwr_get( CS_ROLL  ); // =final values
  // flash_pwr();   // store power info to flash: now done in lifo_pwr
  lifo_pwr();    // and write to output packet

  tnow = RTCGetTime(0,0) - t0; // time in fnx so far
  tnow = tnow/10; // in tens of seconds
  //put in MSB the time before gps on surface
  all.eng->surf_tm += (short) ( tnow << 8 );
   
  all.ubx->index = 0; // start-of-surface
  good1 = get_gps(  pot_good );   // = good if first gps is valid

  // eng_nav_store is in data_pack
  data_pack ( ); // adds on the profile data, plus the eng. packet
  // Doing gps AFTER data_pack +eng ensures that the eng. dive#
  // is associated with the gps below
  //===========pH sep18========================================
  #if PH == 1
       ph_data_pack(); //pack data
       //reset RAM index
       all.ph->i   = 0;
       ph_off();
  #endif //====================================================
  lifo_gps  ( 2, 1 ); // add GPS, 2=end-of-dive, 1=gps attempted
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

  
  
  // Send the SBD data packets ++++++++++++++++++++++++++++++++++++++++++
  #if ANT_XFR_SW  // go to desired wing and set up the switch---------
     pot_good = RF_Wing_Setup(  SBD_UNIT );
  #else // there is no antenna switch, use the starboard wing
     pot_good = roll_wing(PORT_WING,1);
  #endif  //----------------------------------------------------------
  
  //wait for Iridium to transmit the contents of the LIFO
  #if SBD==0 // use GS command -----------------------------
    printf("Compilation error")
  #else // use SBD command set
	nsent = sbd_surface( pot_good );
	flash_sbd(); // write to flash
  #endif //-------------------------------------------------
  cprintf("#messages sent = %d \n",nsent);
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   
  
  
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  // we have officially ended the 'dive' by getting an end-fix and 
  // sending back the data.  It is time to close the flash file for
  // this dive, after writing any last info.
  good1 = all.gps->valid;  // = true if 1st surface fix is good
  flash_gps( CF_GPS2 ); //=end-dive GPS
  // engineering packet was sent to flash using cf1_eng in  data_pack()
  cf_stat=flash_close( ); // closes the file: adds the end-marker as well.
  if (cf_stat != 0) 
  {  cprintf("bad close @ end of dive %d\n", cf_stat );
     all.exc->exc_stat |= EXC_BAD_FLASH; // jan13
     // 0x0100 = flag =file close error
  }
  

  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  // It is now time to start the new dive IF it is not time to abort
   if ( iparam [ EA_prof_num ] >= iparam [ IA_nprofs ] || all.exc->abort )
      return( nsent );  // we are done! Time to exit early
   
   // else we continue with the next dive 
   // First order of business is to get a new GPS fix +++++++++++++++++++
   #if ANT_XFR_SW  // go to desired wing and set up the switch
     pot_good = RF_Wing_Setup(  GPS_UNIT );
   #else // there is no antenna switch, use the starboard wing
	 pot_good = roll_wing( STBD_WING, 1);
   #endif 

   all.ubx->index = 1; // end-of-surface drift
   good2 = get_gps(  pot_good );  // = 1 if the fix is good
   // this will update the gps struct with the latest fix.
   
   // if either gps is ok, use the good one to estimate new direction.
   good = ( good1 || good2 );  // is good if either one is good
   all.gps->valid = good; // update flag to believe/not believe fix
   get_theta( ); // computes new direction to proceed
   
   tnow = RTCGetTime( 0, 0 ); // time now
   if (good) // then had a valid fix ----------------------------
   {   // reset all pertinent GPS parameters --------------------
       all.time->ti_last_gps  = tnow;    // reset to now
       all.nav->drx = 0;
       all.nav->dry = 0;
       all.exc->gps_abort = FALSE; // GPS is working
       all.nav->bad_gps = iparam [ IA_no_gps ]; 
       // If no gps, will take this action
       if (all.nav->bad_gps == -2) //if bad, steer by setting
           all.nav->bad_gps = all.nav->true_theta;
   } else  //----------------------------------------------------
   { // no good fix, see if it has caused an exception
      dt = tnow - all.time->ti_last_gps;  // time elapsed since last
      abort_time =  (long) iparam [ IA_abort_max_gps ];  // hrs
      abort_time = abort_time * 3600L; // seconds
      if ( dt  > abort_time ) // xxxxxxxxxxxxxxxxxxxxx
      { // too much time !!!!
         all.nav->bad_gps = all.nav->home;  // =Last heading home
         all.exc->gps_abort = TRUE; // set flag that gps is broken
         // Will now head off in BAD_GPS direction until too shallow, then abort
      } //xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx
   } //----------------------------------------------------------
   //++++ done with the GPS for start-of-next dive ++++++++++++++++++

   // update the time since the last good SBD comms -----------------
   if (nsent>0) //---------------------------------------------------
   {  // then at least one message got through
      all.time->ti_last_sbd  = tnow;
      all.exc->sbd_bad = FALSE; // feb12, SBD is working
   } else   //--------------------------------------------------
   {  // no messages, see if it has caused an exception
      dt = tnow - all.time->ti_last_sbd;  // time elapsed since last
      abort_time =  (long) iparam [ IA_abort_max_sat ];  // hrs
      abort_time = abort_time * 3600L; // seconds
      if ( dt  > abort_time ) // too much time!!!
        check_sbd_rcvry( ); // see what we should do
   } //--------------------------------------------------------------

   // time, dive #, and handling flash are done on the return from surface
     
    
  #if DEBUG==1  // in debug mode, pass up on aborting
    all.exc->abort = FALSE;  
  #endif
  
  return ( nsent );
} //***************************************************************************


#define Z60 6.0 // ~meters traveled in 1 minute  ******************************
//*****************************************************************************
short check_altim( short use_alt, short alt_stat, float w )
{ // sample the altimeter. 
  // if use_alt==2 or 3, then record to flash
  // w = vertical velocity
  // alt_stat = present altimeter status
  //    0 = first call: need to initialize the ADP
  //    1 = no bottom detected
  //    2 = bottom detected, but not time yet to turn around
  //    3 = 1st time we detected in range, but not 2nd time.
  //    >4 = time (s) that we want to turn around
  // returns alt_stat: if >4, then need to exit glide.

  // ADP==1 if Sontek ADP is installed,
  // ADP==0 means it is a Tritech altimeter    
  
  short alt, alt_z, ti;
  float pa, dt, pval;
  
  alt_z = iparam [ IA_alt_z ];  // height-off-bottom to turn around
  alt = use_alt; // dummy assignment to keep compiler from complaining
  
    //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    #if ADP==1 // Then use the ADP altimeter code
    
       if (alt_stat==0) //------------------------------------------
       {  // then we need to initialize things before sampling
          all.adp->on_adp = 1;  // set flag that we're turning on ADP
          adp_on(); // send the cmd
          DelayMilliSecs(1000);
          adp_exit_sample(); // make sure that we're in the ADP main menu
          adp_set_alt(); // set up the altimeter settings
          alt_stat=1; // done initializing
          
             if ( use_alt==2 || use_alt==3 ) //uuuuuuuuuuuuuuuu
             {  // old code to open up a new file
                // this code just writes to the open file
             } //uuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuuu
       } // end if alt_stat==0 -------------------------------------
        
       alt = pa = adp_altimeter( );  // get a reading
       
     #elif NTK==1 //JAN14, nortek ad2cp
       if (alt_stat==0) //------------------------------------------
       {  // then we need to initialize things before sampling
          // turn on & set up the altimeter settings
          alt_stat = ntk_alt_start(); 
       } // end if alt_stat==0 -------------------------------------
       if (alt_stat) // ready to acquire
       { 
         alt = pa = ntk_altimeter();  // get a reading
       } else // BAD start-up
       { 
         alt = pa = 86;
         all.adp->on_adp = ntk_off(); // couldn't configure, turn off 
       }
       
    #else // then it's the Tritech Altimeter ++++++++++++++++++++++++++++
       alt = (short) altimeter(2); 
       pa = alt*AC2Z; // now in meters
    #endif //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    
    // pa = depth to bottom, in m
    if ( pa < 80 ) alt_stat=2; // set flag that bottom was detected
    
    //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    if ( ( pa< (alt_z+Z60))  && (pa>0.5) ) //++++++++++++++++++++++++++++ 
    { // check 1 more time: pa>0.5 precludes broken altimeter
        alt_stat=3; // set flag=bottom within range on first reading
        
        #if ADP==1 // then call the ADP altimeter code -------------
           DelayMilliSecs(4000); // wait 4 s..if fish,hopefully gone
           alt = pa = adp_altimeter( );
        #elif NTK==1 // then nortek -----------------------------
           DelayMilliSecs(4000); // wait 4 s..if fish,hopefully gone
           alt = pa = ntk_altimeter();
        #else // then it's the Tritech Altimeter -------------------
           alt = (short) altimeter(2); 
           pa = alt*0.02; // now in meters
        #endif //---------------------------------------------------
        
           // check 2nd value for bottom detect
           if ( pa< (alt_z + 2 +Z60) ) //---------------------------
           {  // ok...it is probably real
             
              // store info to dat & get latest P
              all.dat->es.ad_alt =  alt;   // save value to dat struct
              take_data( );  // take latest data & store: updates P

              dt = ( pa - alt_z); // distance to trigger depth [m]
              w = flt_abs(w); // absolute value
              if (w<5) w=5; // make sure it is a reasonable #
              if (w>20) w=20;
              dt = (100*dt)/ w; // time, in s, to alt_z
              ti = (short) dt; // recast as integer
              if (ti>100) ti=100; // keep limited
              if (ti<5) ti=5; // keep >4
              
              alt_stat = ti; // 4<ti<100 = time(s) to turn around
              cprintf("altimeter dt to exit descent=%ds\n",ti);
              all.exc->exc_stat |= EXC_BOT_DETECT;  //jan13
              // 0x0010 =turn-around on bottom-detect

           } // end if 2x in row -----------------------------------
           
    } // end if first time ++++++++++++++++++++++++++++++++++++++++++++++

     // store info to dat and engineering structures
     all.dat->es.ad_alt =  alt;   // update the value to dat struct
     pval = all.dat->dbar; // = latest depth, dbar
     all.eng->alt = (short) pa;  // distance to the bottom(m)
     // Note that the eng->alt = [m] to bottom. 
     #if ADP==1 || NTK==1 // then also pack max dB reading into MSB
        all.eng->alt += all.adp->alt_db*256;
     #endif
     all.eng->z_at_alt = (short) pval; // Spray depth
     // bottom depth = alt(lsb) + pval.
    
  return(alt_stat);
} //***************************************************************************



//*****************************************************************************
//**** start-of-mission functions *********************************************

void   check_surf_start ( void ) //********************************************
{   // dependent upon the  settings, wait for start of deployment
    // JUN14 mods for remote start, renaming tm_roll to tm_ship
    // 

   short  tm_ship, tm_press, mdelay;
   ulong  tnow, tstop;
   short  water_detect; // flag = TRUE if detected we're in the water
      
    mdelay = iparam [ IA_delay_start ]; // delay minutes @ the beginning 
    cprintf("Will delay for %4d minutes ", mdelay );

    tm_ship = iparam [ IA_try_start ]; // = hours for remote start command
    //JUN14 renaming tm_roll for remote start in HOURS
    tm_press= iparam [ IA_try_press ]; // = hrs to try press-detect

    tnow = RTCGetTime(0,0); // present UX time
        
    if ( tm_press >0)  //==============================================
    { // we start by pressure-detect
        cprintf(" before trying water-detect by pressure for %d hrs\n",
             tm_press );
    } else if ( tm_ship > 0 ) //=======================================
    { // then will do remote start
        cprintf(" CLOSING VALVE before waiting for ship-deploy command %d hours\n",
              tm_ship );
        Close_Valve;         // JUN14 close valve as well
        all.hyd->total = 0;  // JUN14 zero out accumulator of pump time
    } //===============================================================
    cprintf("Will do %d profiles \n", iparam [ IA_nprofs ] );
    
    if ( mdelay > 0 ) //===============================================
    { // then we want to wait before trying pressure or roll 
      
      //JUN14, with ship-start, leave Argos off
      //if ( tm_press ==0 ) // turn on Argos as a signal
      //   Argos_On; 
      // this also allows the boat to do some DF work if necessary.
      
      tstop = tnow + mdelay*60; // end-time
      do //------------------------------------------------------
      {  // wait before starting 
         hibernate(&tnow,  10, PIT_ONLY ); // wait 10s
         service_wdog(); // update the watch-dog
      } while (tnow < tstop ); //--------------------------------
      
      Argos_Off; 
    } //===============================================================
    
    
    //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // Dependent upon if we want to do a pressure or roll water-start,
    // call the appropriate function
    water_detect = TRUE; // set true, in case we're doing neither
    if ( tm_press > 0 ) //------------------------------------------
    { // then do the pressure-detect start
      water_detect = dBar_detect(  tm_press );
      if (water_detect) //wwwwwwwwwwwwwwwwwwwwwwwwwwwwwwwww
      { // Turn on Argos for 4 minutes @ surface
        #if DEBUG == 0
        	Argos_On; // turn on argos
        	sleep_check( 240, PIT_ONLY, TRUE );
       		Argos_Off; // turn off argos
       	#endif
      } // end if water dectect wwwwwwwwwwwwwwwwwwwwwwwwwww
    }
    else //---------------------------------------------------------
    { // then not a pressure-start, wait for ship-start cmd
      if ( tm_ship > 0 )
      {  // then check for remote start command JUN14
         water_detect = try_ship_start( tm_ship );
      }
    } // end if..else press or roll water-detect--------------------
    //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

    
    if (!water_detect) //-------------------------------------------
    { // then we failed on the water-detect
      abort_mission( 1 );  // need to add some other abort signal??
      // this allows the user to reset by shore command if needed.
      Reset_CF2(); // cause software-reset, in case return was spurious
    } //------------------------------------------------------------
    
    return;
} //***************************************************************************

#define TRY_ROLL (60)  // # of degrees to try to roll 
#define RWAG 20  // if we actually roll this much, then we're in the water
#define TRY_SECS (300)  // # of seconds to wait between tries

short  try_roll(  short tm_try) //*********************************************
{ // at a set interval, try to wag the wings.
  // if we can wag, then we must be in the water
  // tm_try = # of minutes to try
  
  // returns TRUE when we can wag.
  // returns FALSE if we failed
  
  short wag = FALSE;  // flag that we've successfully wagged.
  short r0, rtry, rwag, pmptime;
  ushort psurf;
  float r1, r2, r3, dr12, dr32, pval;
  ulong tnow, tend;
  
  r0   =      iparam [ IA_roll_0 ];  //  pot counts for zero roll
  rtry = r0 + iparam [ IA_roll_10d ]* TRY_ROLL *0.1;
  // rtry> r0 will roll stbd wing up, and TCM2 roll will be negative
  rwag = RWAG;  // if we wag a fraction of the desired degrees we're happy
  // lifting a wing out of the water at the surface causes it to roll less.
  
  Roll_Zero; //roll wing flat
  Pitch_Xmit; //go to flat pitch
  
  tnow = RTCGetTime(0,0); // present UX time
  tend = tnow + (long)tm_try *60; // max time to try
  
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  do //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  {  // try multiple times to try to wag
    service_wdog(); // update the watchdog
    // get r1 = 1st roll...should be zero
    r1 = avg_tcm2(3, ANG_ROLL);
    send_p_r_run ( CS_ROLL, 1, rtry ); // move roll to rtry counts = stbd wing up
    r2 =  avg_tcm2(3, ANG_ROLL); // r2 should be <0
    
    Roll_Zero; // roll back flat
    r3 = avg_tcm2(3, ANG_ROLL);
    // now test for a wag
    dr12 = (r1 - r2); // should be >0
    dr32 = (r3 - r2); // should be >0
    
    if ( (dr12>rwag) && ( dr32>rwag) )
    { wag = TRUE;  } // we wagged
    else
    {  // hibernate before trying again
       sleep_check( TRY_SECS, PIT_ONLY, TRUE );
    }

    
    // check pressure, in case it was put over with bladders empty ========
    #if SBE==1
      psurf = sbe_avg_p( 3 );  // get  SBE p avg
    #else
      print ("compile error: not supported");
    #endif
    pval = dBar( psurf, all.flt );  // now in dBar
    
    if ( pval > 3 ) //----------------------------------------------
    { // we're off the surface, pump!
       Close_Valve;  // make sure the valve is closed
       pmptime = iparam[ IA_max_pump ];  // time to pump for extra buoy
       go_pump ( pmptime, OIL_YES ); // pump, but pay attn to the oil sensor
       pmptime = watch_pump ( pmptime, 0 ); //returns actual time it pumped

       if (all.hyd->bad_amp)
       { all.exc->abort = TRUE;
         return(FALSE); //!!!!!!!!!!!!!!!!!!!!
       } // bad motor, abort immediately
       
       tend += pmptime + 20; // allow more time to detect surface
    } //------------------------------------------------------------

    tnow = RTCGetTime(0,0); // present UX time
  } while (wag==FALSE  && tnow<tend );
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  // exits with the wings flat
    
  return ( wag );
} //***************************************************************************

short  dBar_detect  (  short tm_hrs ) //***************************************
{ // At a set time interval, read pressure.
  // If it is > 4 dBar, then we must be in the water.
  // Pump oil out and see if the pressure has decreased.
  // Wait until P < 3 dBar.
  // We will try for up to tm_hrs.
  
  // returns TRUE if we succeeded.
  // returns FALSE if we failed.
  
  short  ok=FALSE;  // flag that we succeeded.
  short  pmptime, itry;
  short  tm_wait = 120; // wait 120 s between pressure readings
  ushort psurf;
  float  pval, p2;
  ulong  tnow, tend, tm_try;
  
  Pitch_Xmit; //go to flat pitch
  tnow = RTCGetTime(0,0);
  tm_try = (long) tm_hrs;  // hours to try, recast as long
  tm_try = tm_try*3600; // = seconds
  tend = tnow + tm_try; // max time to try
  
  do  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  {  // check pressure multiple times +++++++++++++++++++++++++++++++++++
  
    #if SBE==1
      psurf = sbe_avg_p(3);  // get  SBE p avg
    #else
      print ("compile error: not supported");
    #endif
    pval = dBar( psurf, all.flt );  // now in dBar
    printf("Detect dBar = %6.2f\n",pval);
    
    if ( pval > 4 ) //----------------------------------------------
    {  // we're off the surface, so it is time to pump
       Close_Valve;  // make sure the valve is closed
       pmptime = iparam[ IA_max_pump ];  // time to pump for extra buoy
       go_pump ( pmptime, OIL_AFTER_T ); // pump, but pay attn to the oil sensor
       pmptime = watch_pump ( pmptime, 0 ); //returns actual time it pumped

       if (all.hyd->bad_amp)
       { all.exc->abort = TRUE;
         return(FALSE); //!!!!!!!!!!!!!!!!!!!!
       } // bad motor, abort immediately

       // we are likely back on the surface at this point
       // but wait some more if necessary
       itry=1;
       do  //iiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiii
       { // wait until we rise past 3m depth
         #if SBE==1
            psurf = sbe_avg_p(3);
         #else
            print ("compile error: not supported");
         #endif
         p2 = dBar( psurf, all.flt );
         if ( p2>3 ) // wait some more 
           sleep_check( tm_wait, PIT_ONLY, TRUE );
       } while ( p2>3 && (itry++ < 5) ); //iiiiiiiiiiiiiiii
       
       if ( (pval-p2)>2  || p2<3 ) ok=TRUE;
       // we've pumped and we're rising, or near the surface

    } //------------------------------------------------------------
    //--------------------------------------------------------------
    else // we're still close to the surface, wait some more time
    { 
      sleep_check( tm_wait, PIT_ONLY, TRUE ); 
    } // end else //------------------------------------------------
    
    tnow = RTCGetTime(0,0);
  } while (ok==FALSE  && tnow<tend );  //++++++++++++++++++++++++++++++++
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    
  return(ok);
} //***************************************************************************




//*****************************************************************************
//**** Exception Routines *****************************************************
//*****************************************************************************

void  burn_3 ( void ) // ******************************************************
{ // turn on burn wire for 3 minutes 
  // this is to get around a potential corrosion issue when burn is on

    Burn_On;  
    sleep_check( 180, PIT_ONLY, TRUE );

    Burn_Off; // leave burn wire off
    all.exc->exc_stat |= EXC_BURN_ON; //jan13
    //0x0002 = flag that burn wire is activated
    
    return;
} //***************************************************************************


short  pump_recovery ( void )  //**********************************************
{ // If pumping fails at the bottom turn-around, try pumping more
  // until we get going again, which equals ascent rate = 5 cm/s
  // returns TRUE if ascent speed got >5 cm/s : we recovered
  // The time between pumps is dependent upon w
  // if not in danger of sinking too rapidly, take longer between pumps,
  // thus giving the motor more time to cool off.
  // NOTE 10 cm/s descent = 6 meters per 60 s
  
  short i,j,k, k0, dt, pit, t0, pi_use, z_panic;
  float p0, pval, xp, w;
  long tnow;   // present time
  struct dat_param *dat;  // data struct w/data values

  z_panic = 1300; //  1400 = crush depth - 15% = potential hull failure
  cprintf("pump_recovery\n");
  pi_use  = ( iparam [ IA_pi_stat ]  >0);  // TRUE to use pitch angle 
  tnow = RTCGetTime(0,0);
  dat = all.dat;   // point to the data structure
  
  dt = SAMP_DT_UP;    // 0903 feb11: time between samples for ascent
  pit = 35;  // desired pitch = 35 degrees up to try & get air out
  t0 = 64; // #s to pump, and then wait before pumping again
  k = k0 = t0/dt;  //will wait for k samples between pumping
  //  k=time to cool off if bad amps on entry
  
  if (all.hyd->bad_amp) //=======================================
  { //  then high amps, give some time to cool off
    for (i=0;i<k;i++)
    {  hibernate(&tnow, dt, PIT_IRQ2);
       take_data( ); 
    }  // end for i
  }  //==========================================================

  j = 0;  // counter of # times we've tried to pump
  do //==========================================================
  { // while we're not ascending, keep pumping
    p0 = dat->dbar;  //=initial pressure
    // control pitch -----------------------
    xp = (float)  dat->es.pitch/10.0; // present pitch value
    if (pi_use) control_pitch(xp, pit, 1.0 ); 


    Close_Valve;  // make sure the valve is closed
    go_pump ( t0, OIL_NO ); // pump regardless of oil sensor

    for (i=0; i<k0; i++) //------------------
    { // wait 64 s & take data as well
      hibernate(&tnow, dt, PIT_IRQ2 ); 
      take_data( );  // take data
    } //------------------------------------


    pval = dat->dbar; // = latest value
    w = 100*(p0 - pval) / (t0); // =ascent rate, cm/s
    printf("pump_recovery %d w=%5.2f\n",j,w);
    
    // confine the off time somewhere between t0 and 4*t0
    k = (w+10)/2; // integer value, w<0 if still descending
    if (k>4) k=4; // limit to 25% duty cycle, true if w>-2; not sinking fast
    if (k<1) k=1; // likewise, no faster than 50% duty cycle true if w<-8.
    k = k*k0; // # of samples to take

    for (i=0; i<k; i++) //------------------
    { // wait 64 s & take data as well
      hibernate(&tnow, dt, PIT_IRQ2 ); 
      take_data( );  // take data
    } //------------------------------------
    
    if ( (pval>z_panic) && all.exc->pgood ) // too deep!..abort -----
    { 
      for (j=0; j<5; j++ ) // try a bunch of times
      { burn_3(); take_data( ); }
      all.exc->abort = TRUE;  //set to abort
      all.exc->exc_stat |= EXC_TOO_DEEP; //jan13
      // turn on flag that we're too deep
    } //--------------------------------------------------------------

  } while (  (j++<10) && ( w < 5) ); 
  //=============================================================
  
  return(w>5);
} //***************************************************************************


void  check_pump_recovery ( void ) //******************************************
{ // glider pump behavior is questionable, decide what to do
  short rcvry_ok;
  
     rcvry_ok = iparam [ IA_pmp_rcvry ];
     printf("Rcvry_ok= %d\n",rcvry_ok);
  
     all.exc->exc_stat |= EXC_PUMP_RCVRY; // jan13 added the define
     // turn on bit 1 = flag that we checked recovery
     switch ( rcvry_ok )  //---------------------------------------------
     { // decide action based on value of rcvry_ok
       case 3 : all.exc->abort = TRUE;  // use for day-test, easy to pick up
            break;
       case 2 : //  Head for Home
            iparam [ IA_route_now ] = 0;
            // NO break: also do the settings for case 1 = shallow dives
       case 1 : // go to shallow dives, but otherwise keep going
            iparam [ IA_zmax        ] = 100; // go to 100m dives
            iparam [ IA_max_dive_tm ] = 25; // and limit the glide time
            break;
       case 0 : break; // don't do anything
     } // end switch ----------------------------------------------------
  
  return;
} //***************************************************************************

void  check_sbd_rcvry ( void ) //**********************************************
{ // glider comms is questionable, decide what to do
  short rcvry_ok;
  struct route_param *route;

  route = all.nav->route;
 
     rcvry_ok = iparam [ IA_sbd_rcvry ];
     printf("sbd_rcvry= %d\n",rcvry_ok);
  
     all.exc->exc_stat  |= EXC_SBD_RCVRY; // jan13
     // turn on bit = flag that we checked recovery
     iparam [ IA_argos_mult] = 2; 
     // value says to use argos @ surface for 2*5 = 10 minutes
     all.exc->sbd_bad = TRUE; // feb12, SBD is bad
     //=== aug15, make sure other steering modes are off.
     iparam [ IA_cross     ] =   0; // turn off current-crossing
     iparam [ IA_set_nprof ] =   0; // turn off current-crossing
     iparam [ IA_man_nprof ] =   0; // turn off manual
     // set up for current-bucking
     iparam [ IA_use_set   ] =   1; // use set correction
     iparam [ IA_set_min   ] = -30; // set correction limit
     iparam [ IA_set_max   ] =  30; // set correction limit
     //=== aug16, make sure we are out of drift mode!
     iparam [ IA_drift_tm  ] =   0; // force drift mode off
     
     
     switch ( rcvry_ok ) //----------------------------------------------
     { // decide action based on value of rcvry_ok
       case 3 : all.exc->abort = TRUE;  // use when easy to pick up
            break;
       case 2 : // go to shallow dives, but otherwise keep going
            iparam [ IA_zmax        ] = 100; // only dive down to 100 m
            iparam [ IA_max_dive_tm ] = 25; // limit glide time
            // NO break: also do the settings for case 1 = shallow dives
       case 1 : //  Head for Home
            iparam [ IA_route_now ] = 0;
            route->rlist[0]  = 0; // 1st position always points to HOME=0
            route->blist[0]  = 0; // and always use auto-bearing
            route->detect[0] = 0; // use range-only
            route->circle[0] = 8; // 8km is pretty close for a broken one..
            break;
       case 0 : break; // don't do anything
       } // end switch --------------------------------------------------
  
  return;
} //***************************************************************************

void   check_amp_action( void ) //*********************************************
{ // If bad amps, decide the fate of the glider.
  // The higher the value, the more severe the action
  
     if (all.hyd->bad_amp)  //=========================================
     { // bad current, decide action to take
       switch ( iparam [ IA_amp_action ] )
       { //++++++++++++++++++++++++++++++++++++++++++
         case 0 : break; // don't do anything
         case 1 : // do shallow dives
            iparam [ IA_zmax        ] = 100; // dive to 100 m
            iparam [ IA_max_dive_tm ] = 25; // limit glide time
            break;
         case 2 : // abort the mission!
            all.exc->abort = TRUE;
            break;
       } //++++++++++++++++++++++++++++++++++++++++++
       
     } //==============================================================

  return;
} //***************************************************************************

//*** JUN14 wait on ship for correct start-up commands ****************
#define TM_CHK_P 600 // how often [s] to check P below
//---this value should be set to MAX_TDOG so we correctly service
//---the watchdog circuit.

short  try_ship_start ( short tm_hrs  ) //******
{ // assumes waiting for deployment onboard ship, or stored on land
  // the bladders are empty initially
  // the gps/sbd is tried every tm_argos, plus pressure reading.
  // normal shore commands are used to set timing.
  // when ready to deploy, it is put  outside,
  // and the shore command to pump oil out is sent.
  // This is the queue that it is getting ready to be deployed.

  // Once in the water, sending the ABORT command will 
  // actually start the mission.
  //----RETURNS -----
  // TRUE if the ABORT command was received, plus one pump-out command.
  // FALSE if:
  //    p > 5m  (thrown over w/o pumping out).
  //    exceeded max time to wait.
  
  short secs, good;
  short use_argos = 0; //aug13, flag whether to turn on argos
  ushort p;
  short pump_start, got_abort, done;
  ulong tnow, rf_stop, dt, tend, tm_try, tstart;
  ulong pt;
  float z;
  short adg_ran = false;
  
  set_surf_num ( 0 );
  set_prof_num ( 0 ); //use dive #=0 for all pre-deployment tries
  //reset_rtc(all->gps); //oct08, maintains unix time across resets
  //SetupLP(); // re-initialize (NEEDED???)
  service_wdog(); //zero the watchdog
  
  Roll_Zero; // roll flat
  Pitch_Xmit; //go to flat pitch
  tnow   = RTCGetTime(0,0);// present UX time
  tstart = tnow;           // time we started
  tm_try = (long) tm_hrs;  // hours to try, recast as long
  tm_try = tm_try*3600;    // = seconds
  tend   = tstart+ tm_try; // max time to try

  service_wdog();
  
  clear_lifo_stat(all.lifo); // set status of any old messages to DO NOT SEND
  Close_Valve;               // make sure the valve is closed

  iparam [ IA_abort_pump ] = FALSE;  // haven't tried to pump yet
  pump_start = FALSE;      // haven't pumped out yet
  got_abort  = FALSE;      // haven't gotten the ABORT yet
  done       = FALSE;      // not done waiting yet
  //--- jun15, set up for adg (auto-degassing) as well.
  iparam [ IA_adg_ncycle  ] = 0;  // do NO adg cycles: this is set by shore cmd.
  
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  while (!done) // do loop until time to move on  +++++++++++++++++++++
  { 

    // see if received shore command from last comms ==========
    // aug13, do this immediately to set use_argos
    if (  iparam [ IA_abort_pump ] ) // set by 'L' shore cmd ===
    {   pump_start = TRUE; //yes, we want to pump
        iparam [ IA_abort_pump ] = FALSE; // reset the flag
        use_argos = TRUE; //turn on argos now while waiting
    } //=======================================================

#if USE_ADG == 1
    // jun15, check for auto-degas call
    if (  iparam [ IA_adg_ncycle ] > 0 ) // set by '51'  cmd ===
    {   // do the auto-degassing feature
        remote_adg_vent(); // do the adg
        // this will exit early if it detects z>10m.
        iparam [ IA_adg_ncycle ] = 0; // reset the value
        adg_ran = true; // flag that we've run at least once
    } //=======================================================
#endif

    Roll_Zero; // roll back flat
    //=== wait desired time before checking shore commands ========
    tnow    =  RTCGetTime(0,0); // get present time
    dt      =  iparam [ IA_argos_tm ]; // minutes for Argos
    rf_stop = tnow + dt*60; // time to turn OFF argos
    if (use_argos) Argos_On; //aug13 turn on Argos only if pumped out
    
    do //--------------------------------------------------
    { // periodically check P
      tnow =RTCGetTime(0,0); // get present time
      dt =  rf_stop - tnow; // time remaining
      if ( dt > TM_CHK_P ) dt = TM_CHK_P; // keep to min.
      hibernate( &tnow, dt,PIT_IRQ2_RXD ); // go to lowpower; updates tnow
      service_wdog();
      //,,, check P ,,,,,,,,,,,,,,,,,,,,,,,,
      p = sbe_avg_p(4);  // get  SBE p avg
      z = dBar( p, all.flt );
      if  ( z > 10 )
      { tnow = rf_stop +1; // TOO DEEP! set to exit
        done = TRUE; // set flag that need to exit fnx
      } //,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,

    } while (tnow < rf_stop ); //---------------------------
    
    Argos_Off; // turn off Argos
    //=============================================================    
    
    if ( (pump_start) || (z>10) ) //===========================
    {  // shore command to pump, or too deep!
       Close_Valve;  // make sure the valve is closed
       check_vent();  // 0711, check the vent status
       pt = iparam [ IA_max_pump ]; // try max time to pump
       go_pump( pt, OIL_YES ); // pay attn to the oil sensor
       secs = watch_pump ( pt + 4, 0 ); // wait until  pump is done
       hpr_pwr_get( CS_HYD ); // gets the total pump time from hyd module....
       flash_hyd();   // store hyd info to flash
       pump_start = FALSE; // turn off
    } //=======================================================
    
    if (!done) //==============================================
    { // then want to send stuff & act if got a shore command, 
      if (!adg_ran) clear_lifo_stat(all.lifo); 
      // jun15, only clear out if we don't have adg messages queued
      // clear out old so we don't backlog lots of inside-fixes.
      //all.gps->Max_T = UeeGetWord(ee_gps_tm);  // max gps time
      // update above in case changed by shore cmd.
      // this should happen in get_gps() called by try_gps_sbd()
      good = try_gps_sbd ( 3 );  // 3=abort gps version
    } //=======================================================
    
    if (all.exc->abort) // got the ABORT shore command========
    {   // which means we want to ABORT this fnx,
        // and start the mission.
        got_abort = TRUE; // YES, done waiting
        done      = TRUE; // done with the loop
        all.exc->abort = FALSE; // turn off the flag
            // so mission doesn't abort early
    } //=======================================================
    
    tnow =RTCGetTime(0,0);
    
    //--aug13, update tend if eeprom time-out was modified from shore
    tm_try = (long)iparam [ IA_try_start ]; // = hours for remote start command
    // above allows value to be modified from shore.
    tm_try = tm_try*3600;    // = seconds
    tend   = tstart+ tm_try; // max time to try
    
    if ( tnow>tend ) done=TRUE; // exceeded max time
  } // end while not done +++++++++++++++++++++++++++++++++++++++++++++
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

  return (got_abort);
} // **************************************************************************

void  abort_mission ( short err ) //*******************************************
{ 
  // aug15 adding input = err:
  // 0 = normal entry from end-of-mission = no errors
  // 1 = exception error: could be a problem: do all pumping
  // 9 = entry from shore command; all is normal
  
  short secs, i, isurf, awake, good, pi_zero;
  ushort p,t, nbad = 0, nose_down=0;
  short dt, Pmp_Try = PMP_TRY;
  short pump_again, bad_sbd=0, bad_gps=0, nsat, wing_now;
  ulong tnow, rf_time;
  float z;
  short pitch_max, pitch_xmit;

  pi_zero  = ( iparam [ IA_pi_stat ] >1);  // TRUE to use pitch angle for zero pitch
  pitch_xmit = iparam [ IA_pitch_xmt ] ;
  pitch_max = iparam [ MA_pitch_max ]  - pitch_xmit; //  max nose down: dec15 fix bug - thanks LAR!
  // pitch_max = max pitch forward from pitch_xmit position
  
  spi_set_wdog( 1, 255 ); // multiplier=1, all action bits active
  
  //  open a new file
  cprintf("Abort Mission, opening new file\n");
  all.time->ti0 = RTCGetTime ( 0, 0 );  // UX time for this dive
  all.time->ti_cyc = all.time->ti0; // UX time for this cycle
  isurf = incr_surf_num(); //increments the surface# both in eeprom & iparam
  incr_prof_num(); //0902 increment the profile# 
  flash_open('D'); // open another data file (adds a dive mark automatically)
  // if buffer is NOT empty, will write to an 'E' file for emergency recovery
       
       
  roll_wing ( ZERO_WING, 1); // roll wing flat
  zero_pitch_angle(); // go to zero-pitch @ surface: uses IA_pi_stat value for method

  Argos_On; // turn on the transmitter
  
  clear_lifo_stat(all.lifo); // set status of any old messages to DO NOT SEND
  
    //=== ZooGlider =============================================
    /* uncomment for the very first day test
    #if ZOOG==1  
      //>>>Day Test Only !!!!
      // test is now done: turn off the zoocam
      zcam_snoop(); 
      // image # + timestamp saved to all.zcam in above fnx.
      flash_zoog(); // and save to flash.
      i = all.zcam->zcam_stat;  // start-error status: 0=none
      if (i == 0) { stop_zoog_dive(); }
      else        { zCam_off( );      } // make sure power is off
    #endif //====================================================
    */

  Close_Valve;  // make sure the valve is closed
  DelayMilliSecs(200);

  spi_pwr_rst  ( CS_HYD ); //clears pump accum.
  go_pump( Pmp_Try/2, OIL_YES ); // pay attn to the oil sensor
  secs = watch_pump ( Pmp_Try/2 + 4, 0 ); // hang around until the pump is done
  hpr_pwr_get( CS_HYD ); // gets the total pump time from hyd module....
  flash_hyd();   // store hyd info to flash

    
  i=0;
  do //==========================================================
  { // try 3 more times to pump, once every Pmp_Try seconds
    service_wdog();
    go_pump( Pmp_Try, OIL_YES ); // pay attn to the oil sensor
    secs = watch_pump ( Pmp_Try + 4, 0 ); // wait until pump is done
    hpr_pwr_get( CS_HYD ); // gets the total pump time from hyd module....
    flash_hyd();   // store hyd info to flash
    
    secs = Pmp_Try - secs; // =extra amount of time to wait
    if ( secs<10 ) secs=10;
    if ( err==1 )
    { // aug15, then exception, wait set time
      awake = sleep_check( secs, PIT_IRQ2_RXD, TRUE ); 
    } // this makes a constant-abort time total
    // returns AWAKE_RXD if awoken by the serial line
  } while ( i++<3 && ( awake != AWAKE_RXD ) ); //===============
  flush_pp( 0 ); // dec14 make sure ping-pong buffer gets written now.
  
  // rotate thru Argos/gps/Satellite
  awake      = FALSE; // assume no one is home yet
  pump_again = FALSE;
  iparam [ IA_abort_pump ] = FALSE;  // do not pump unless requested from shore
  wing_now = PORT_WING;  // start off with the port wing
  
  
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  while (!awake) // do infinite loop until awakened +++++++++++++++++++++++++
  { // argos/GPS/SBD until awakened by '.' on RS232
        spi_pwr_rst( CS_PITCH );  // zero out accum. of pitch motor
        spi_pwr_rst( CS_ROLL  );  // zero out accum. of roll motor
    all.roll_stat=0;   // , start each cycle w/no roll errors
    roll_wing ( ZERO_WING, 1); // roll the wing flat; multiple tries
    
    nose_down = all.exc->nose_down; // This allows reset from 'L' shore cmd
    if ( iparam [ IA_pi_stat ] <2 ) //-------------
    {  // then potentiometer is healthy, 
       // rely more on argos by tail further up if gps/sbd fail
       send_p_r_run ( CS_PITCH, 1, pitch_xmit +nose_down );
       // this will put nose further down
    } //-------------------------------------------
    else // ---- we don't trust the pot
    { zero_pitch_angle(); } // will go to zero pitch based on pitch angle

    #if SBE==1
      p = sbe_avg_p(4);  // get surface SBE p avg
    #else
      print("compile error: not defined"); // get avg p using 2003 code
    #endif
    
    t = 0; // set as placeholder in cf1 file
    z = dBar( p, all.flt );
    Argos_On; // make sure RF beacon is on
    
    if (iparam [ IA_abort_pump ]) // set by 'L' shore cmd--------
    {   pump_again=TRUE; //
        iparam [ IA_abort_pump ] = FALSE; // reset flag
    } //---------------------------------------------------------
    
    // pump_again is also set if nbad>2, see below
    if ( (pump_again) || (z>10) ) //-----------------------------
    {  // either no GPS & no Iridium or we still think we're deep
       check_vent( );  //  check the vent status
       spi_pwr_rst  ( CS_HYD ); //clears pump accum.
       dt = iparam [ IA_max_pump ]; // try max time to pump
       go_pump( dt, OIL_YES ); // pay attn to the oil sensor
       secs = watch_pump ( dt + 4, 0 ); // wait until  pump is done
       // above now services the watchdog every 60s.
       hpr_pwr_get( CS_HYD ); // gets the total pump time from hyd module....
       flash_hyd();   // store hyd info to flash
       pump_again = FALSE; // reset flag
    } //---------------------------------------------------------
    
    // do argos xmit for desired time ---------------------------
      rf_time = iparam [ IA_argos_tm ]*60; // time, in sec to do Argos
      if ( all.exc->rfx >1 )   // then multiply argos time
        rf_time = rf_time*all.exc->rfx; // rfx set by 'L' or SBD/GPS health.
       
      if ( rf_time > 28800 ) rf_time = 28800; // no more than 8 hrs
      awake = sleep_check( rf_time, PIT_IRQ2_RXD, TRUE ); // this services wdog
      Argos_Off;
    //-----------------------------------------------------------
    
    if (!awake) //=========================================================
    { // Try GPS and SBD 

      zero_pitch_angle(); // go to zero-pitch @ surface: 
      // uses IA_pi_stat value for desired method.

      good = try_gps_sbd ( 3 );  // 3=abort gps version
      flash_sbd(); // write stats to flash
      //update following in case changed by shore command
      pi_zero  = ( iparam [ IA_pi_stat ] > 1 );  

      if (good) //-------------------------------------------
      { // reset to initial values if good
        all.exc->rfx =1; 
        nbad = 0; 
        all.exc->nose_down=0; 
        pump_again  = FALSE; 
      } 
      else // nothing got through---------------------
      { nbad++; // increment counter of #bad in a row
      } //---------------------------------------------------
      
      if (nbad>2) //-----------------------------------------
      { // 3x w/o any message, rely more on Argos
         if (all.exc->rfx > 1000) all.exc->rfx = 1000;
         // dec14, above keeps 2*rfx <2000 = NO overflow for short.
         // note if rf_time = 60s, then 2000*60 = 120000 >> 28800 max above.
         // rfx>480 has no effect on max argos tm.
         all.exc->rfx = all.exc->rfx*2;  // double time in argos mode
         pump_again = TRUE; // try pumping again
         nbad=0; // reset
         // Accumulate nose_down over errors, to get tail further up
         nose_down += 500; // try to get tail further up for Argos
         if (nose_down >pitch_max ) nose_down=pitch_max;
         all.exc->nose_down = nose_down; // This allows change by shore command
      } //---------------------------------------------------
      
      // Keep track of SBD and GPS performance --------------
      if ( all.sbd->nsent ==0 ) // none sent
        bad_sbd++;  // increment counter
      else
        bad_sbd=0; // got through, reset
      // do the same for gps
      nsat =  all.gps->nsat & 0x0f;  // #of sats is the ls nibble
      if ( nsat <3)
        bad_gps++;  // increment counter
      else
        bad_gps=0; // ok, reset to 0

      cprintf("nsent %d nsat %d\n",all.sbd->nsent, nsat);
      cprintf("Bad sbd %d  bad gps %d\n",bad_sbd, bad_gps);
      //-----------------------------------------------------
      
      // make a decision whether we need to drop the weight
      if ( (bad_gps>2) && (bad_sbd>2) ) //-------------------
      { // true if 3x haven't seen any satellites whatsoever
         cprintf("Dropping weight\n");
         burn_3(); burn_3();  // drop the weight
      } //---------------------------------------------------
      
      
      tnow = RTCGetTime(0,0);
      cprintf("ti = %ld, p=%u  sbd =%d\n", tnow, p, good);
      flash_gps( CF_GPS3 );//GPS3= abort-mode
      flash_store( 2, (uchar*) &p, CF_ABORT);  // store pressure
      hpr_pwr_get( CS_PITCH ); // =final values
      hpr_pwr_get( CS_ROLL  ); // =final values
      flash_pwr();   // store power info to flash
      flash_close(); // close the flash file
      
      all.time->ti0 = RTCGetTime ( 0, 0 );  // UX time for this dive
      all.time->ti_cyc = all.time->ti0; // UX time for this cycle
      isurf = incr_surf_num(); //incr. the surfacing# both in eeprom & iparam
      incr_prof_num(); //0902 increment the profile# 
      flash_open('D'); // open a new  file (adds a dive mark )
      
    } // end if not awake==================================================
    
  } // end while not awake ++++++++++++++++++++++++++++++++++++++++++++++++++
  //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

  // will get here if awakened from lowpower mode
      tnow = RTCGetTime(0,0);
      cprintf("ti = %ld, p=%u  sbd =%d\n", tnow, p, good);
      flash_gps( CF_GPS3 );//GPS3= abort-mode
      flash_store( 2, (uchar*) &p, CF_ABORT);  // store pressure
      flash_close(); // close the flash file
  cprintf("EXIT END_MISSION.  RESET OP_MODE to officially exit the mission\n");

  return;
} // **************************************************************************


//*** END *********************************************************************
