/****************************************************************************** // 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 // Persistor BIOS and I/O Definitions #include // Persistor PicoDOS Definitions #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #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; ibz[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_numti0 = 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;ibz[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++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) && (tnowpgood) 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 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 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 ( tnowdbar; // = 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 ( (pvalpgood ) { 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) && (es20) )//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 =0) control_head ( down, dh, roll_avg ); j = 0; // reset active control interval counter } //end if active control ---------------------------------------- //---------------------------------------------------------------- } //end while tnowi=%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 sbe_pump_hi(); // JUL2020 move to beginning of bottom turn //-- 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 pgood && ( (datp->ds.p +6)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. JUL2020 move to beginning //of bottom turn. #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 ; nmodemmodlist[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;hunkthis_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 (tnowtnext ) //------------------------------------------ { // 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; // 4exc_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 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 && tnowexc_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;idbar; //=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; idbar; // = 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; iz_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 *********************************************************************