/** \file * * Contains the CBIT class implementation. * * Copyright (c) 2007,2008,2009 MBARI * MBARI Proprietary Information. All Rights Reserved */ #include "CBIT.h" #include "CBITIF.h" #include #include #include #include "supervisor/CommandExec.h" #include "data/ConfigReader.h" #include "data/Slate.h" #include "data/UniversalDataReader.h" #include "data/UniversalDataWriter.h" #include "scienceModule/ESPComponentIF.h" #include "data/UniversalURI.h" #include "supervisor/Supervisor.h" #include "units/Units.h" #include "sensorModule/OnboardIF.h" #include "controlModule/SpeedControlIF.h" #include "sensorModule/Power24vConverterIF.h" #define ATMOSPHERE_PSI (14.696 ) #define WATER_DETECTION_THRESHOLD ( 3 ) // Water detection alarms are allowed to increment this high #define MAX_GF_CURRENT ( 1.5 ) // Current limit on the ground fault detection circuit limited to 1 mA. Giving 50% extra to avoid false positives from offsets and the precision of the current limiting diode. #define MAX_GF_BOUND_ERRORS ( 3 ) // Rescan immediately up to this many times if a bounding error is encountered while doing a GF scan bool CBIT::GFScanForce_( false ); CBIT::CBIT( const Module* module ) : SyncTestComponent( CBITIF::NAME, module ), wdtInitialized_( false ), readConfigCriticalReported_( false ), abortDepthTimeout_( 5 ), envFailTimeout_( 10 ), battThresholdFailTimeout_( 12 ), // 3 cycles of battery data battFailReportTime_( Timestamp::NOT_SET_TIME ), battFailReportInterval_( 60 ), // Report failed main battery hourly by default gfChanSetupTimeout_( 1 ), locationCheckTimeout_( 120 ), nanLocationActive_( false ), nanLocationReported_( false ), coulombCountTimeout_( 600 ), abortDepthActive_( false ), previouslyBelowAbort_( false ), stopDepthActive_( false ), previouslyBelowStop_( false ), waterDetectedCriticalSet_( false ), waterDetectedBurnwireActivated_( false ), allowAuxLeak_( false ), waterDetectionCount_( 0 ), backplaneFailed_( false ), envFailed_( false ), envExcursionFault_( false ), battThresholdFailed_( false ), avgHumidity_( 0 ), samples_( 1 ), navErrorThreshold_( 100 ), navErrorCriticalReporting_( true ), cfMissing_( false ), gfScanInProgress_( false ), Pwr24On_( 0 ), gfScanBoundErrors_( 0 ), gfScan_( OPEN_SCAN ), gfChan_( LPC3Reg::CHAN_A0 ), adGFCurrent_( CBITMainGroundfaultIF::AD, CBITMainGroundfaultIF::AD_VREF, CBITMainGroundfaultIF::AD_RES, CBITMainGroundfaultIF::AD_TIMEOUT, !simulateHardware(), logger_ ), adWaterAlarmBow_( CBITWaterAlarmBowIF::AD, CBITWaterAlarmBowIF::AD_VREF, CBITWaterAlarmBowIF::AD_RES, !simulateHardware(), logger_ ), adWaterAlarmStern_( CBITWaterAlarmSternIF::AD, CBITWaterAlarmSternIF::AD_VREF, CBITWaterAlarmSternIF::AD_RES, !simulateHardware(), logger_ ), adWaterAlarmAux_( CBITWaterAlarmAuxIF::AD, CBITWaterAlarmAuxIF::AD_VREF, CBITWaterAlarmAuxIF::AD_RES, !simulateHardware(), logger_ ), debugEFCTimeout_( 600 ), runFaultClassifier_( false ), runElevOffsetCalc_( false ), empiricalFaultCounter_( 0 ), empiricalFaultBinSum_( 0 ), empiricalFaultEleTmp_( 0 ), empiricalFaultPitchTmp_( 0 ), empiricalFaultDepth_( 0 ), empiricalFaultPositiveFlagCount_( 0 ), elevOffsetCounter_( 0 ), clearCounterEOM_( false ), elevOffsetEleTmp_( 0 ), debug_( false ) { logger_.syslog( "Construct Continuous Built In Test." ); // input readers clearFaultCmdReader_ = newDataReader( CBITIF::CLEAR_FAULT_CMD ); clearLeakFaultCmdReader_ = newDataReader( CBITIF::CLEAR_LEAK_FAULT_CMD ); empiricalFaultElevOffsetReader_ = newDataReader( CBITIF::EFC_ELEV_OFFSET ); // input readers from onboard sensors and other components pressureReader_ = newDataReader( OnboardIF::PRESSURE_READING ); humidityReader_ = newDataReader( OnboardIF::HUMIDITY_READING ); temperatureReader_ = newDataReader( OnboardIF::TEMPERATURE_READING ); speedCmdReader_ = newDataReader( SpeedControlIF::SPEED_CMD ); verticalModeReader_ = newDataReader( VerticalControlIF::VERTICAL_MODE ); power24vConverterDataReader_ = newDataReader( Power24vConverterIF::POWER_24V_CONVERTER ); for( int i = 0; i < ( BPC1AIF::NUMBATTS + BPC1BIF::NUMBATTS ); i++ ) { battPacks_[i].battTempReader_ = Slate::NewReader( BPC1IF::NAME, BPC1IF::BATT_TEMP_BASENAME + i, this, Units::CELSIUS() ); } // Universal inputs depthReader_ = newUniversalReader( UniversalURI::DEPTH ); depthRateReader_ = newUniversalReader( UniversalURI::DEPTH_RATE ); elevatorAngleReader_ = newUniversalReader( UniversalURI::PLATFORM_ELEVATOR_ANGLE ); latitudeReader_ = newUniversalReader( UniversalURI::LATITUDE ); longitudeReader_ = newUniversalReader( UniversalURI::LONGITUDE ); pitchReader_ = newUniversalReader( UniversalURI::PLATFORM_PITCH_ANGLE ); batteryVoltageReader_ = newUniversalReader( UniversalURI::PLATFORM_BATTERY_VOLTAGE ); fixResidualPercentDistanceTraveledReader_ = newUniversalReader( UniversalURI::FIX_RESIDUAL_PERCENT_DISTANCE_TRAVELED ); // Slate outputs shorepowerWriter_ = newDataWriter( CBITIF::SHOREPOWER_ON ); aHUsedWriter_ = newDataWriter( CBITIF::AH_USED ); faultWriter_ = newUniversalWriter( UniversalURI::PLATFORM_FAULT, Units::ENUM, 0 ); leakFaultWriter_ = newUniversalWriter( UniversalURI::PLATFORM_FAULT_LEAK, Units::ENUM, 0 ); gfScanActiveWriter_ = newDataWriter( CBITIF::GF_ACTIVE_STATE ); // Ground fault current gfChanA0CurrentWriter_ = newDataWriter( CBITIF::GFCHANA0_READING ); gfChanA1CurrentWriter_ = newDataWriter( CBITIF::GFCHANA1_READING ); gfChanA2CurrentWriter_ = newDataWriter( CBITIF::GFCHANA2_READING ); gfChanA3CurrentWriter_ = newDataWriter( CBITIF::GFCHANA3_READING ); gfChanB0CurrentWriter_ = newDataWriter( CBITIF::GFCHANB0_READING ); gfChanB1CurrentWriter_ = newDataWriter( CBITIF::GFCHANB1_READING ); gfChanB2CurrentWriter_ = newDataWriter( CBITIF::GFCHANB2_READING ); gfChanB3CurrentWriter_ = newDataWriter( CBITIF::GFCHANB3_READING ); gfChanOpenCurrentWriter_ = newDataWriter( CBITIF::GFCHANOPEN_READING ); // Emperical Fault Classifier writers empiricalFaultElevOffsetWriter_ = newDataWriter( CBITIF::EFC_ELEV_OFFSET ); empericalFaultDetectedWriter_ = newDataWriter( CBITIF::EMPERICAL_FAULT_READING ); binnedDepthRateWriter_ = newDataWriter( CBITIF::BINNED_DEPTH_RATE_READING ); /// Configuration Readers abortDepthCfgReader_ = newConfigReader( CBITIF::ABORT_DEPTH_CFG ); // Depth at which we drop the weight. Should be greater than all depth envelopes stopDepthCfgReader_ = newConfigReader( CBITIF::STOP_DEPTH_CFG ); // Depth at which we stop the mission. Should be greater than all depth envelopes and less than abort depth humidityThresholdCfgReader_ = newConfigReader( CBITIF::HUMIDITY_THRESHOLD_CFG ); // relative humidity pressureThresholdCfgReader_ = newConfigReader( CBITIF::PRESSURE_THRESHOLD_CFG ); // Onboard pressure must measure greater than this offset from 1 ATM tempThresholdCfgReader_ = newConfigReader( CBITIF::TEMP_THRESHOLD_CFG ); // Only need to use kelvin for temperature deltas vehicleOpenCfgReader_ = newConfigReader( CBITIF::VEHICLE_OPEN_CFG ); abortDepthTimeoutCfgReader_ = newConfigReader( CBITIF::ABORT_DEPTH_TIMEOUT_CFG ); // If this time has elapsed and we're still below abort depth, drop the weight, end the mission. battFailReportCfgReader_ = newConfigReader( CBITIF::BATT_FAIL_REPORT_CFG ); // Report main battery failover at this multiple envTimeoutCfgReader_ = newConfigReader( CBITIF::ENV_TIMEOUT_CFG ); // If this time has elapsed and pressure or humidity are still above threshold, end the mission. faultClassifierCfgReader_ = newConfigReader( CBITIF::RUN_FAULT_CLASSIFIER ); // Runs the empirical fault classifier as part of CBIT if true elevOffsetCalcCfgReader_ = newConfigReader( CBITIF::RUN_ELEV_OFFSET_CALCULATOR ) ;// Runs the empirical fault classifier's elevator offset calculator as part of CBIT if true navErrorThresholdCfgReader_ = newConfigReader( CBITIF::NAV_ERROR_THRESHOLD_CFG ); navErrorCriticalCfgReader_ = newConfigReader( CBITIF::RUN_NAV_ERROR_CRITICAL ); // Will throw a critical is navigation error is above threshold allowAuxLeakCfgReader_ = newConfigReader( CBITIF::ALLOW_AUX_LEAK ); // Won't throw a critical if water detected in aux battTempThresholdCfgReader_ = newConfigReader( CBITIF::BATT_TEMP_THRESHOLD_CFG ); // The allowable threshold for battery temperature gfChanA0ThresholdCfgReader_ = newConfigReader( CBITIF::GFCHANA0_THRESHOLD_CFG ); // Current above baseline that triggers a ground fault gfChanA1ThresholdCfgReader_ = newConfigReader( CBITIF::GFCHANA1_THRESHOLD_CFG ); // Current above baseline that triggers a ground fault gfChanA2ThresholdCfgReader_ = newConfigReader( CBITIF::GFCHANA2_THRESHOLD_CFG ); // Current above baseline that triggers a ground fault gfChanA3ThresholdCfgReader_ = newConfigReader( CBITIF::GFCHANA3_THRESHOLD_CFG ); // Current above baseline that triggers a ground fault gfChanB0ThresholdCfgReader_ = newConfigReader( CBITIF::GFCHANB0_THRESHOLD_CFG ); // Current above baseline that triggers a ground fault gfChanB1ThresholdCfgReader_ = newConfigReader( CBITIF::GFCHANB1_THRESHOLD_CFG ); // Current above baseline that triggers a ground fault gfChanB2ThresholdCfgReader_ = newConfigReader( CBITIF::GFCHANB2_THRESHOLD_CFG ); // Current above baseline that triggers a ground fault gfChanB3ThresholdCfgReader_ = newConfigReader( CBITIF::GFCHANB3_THRESHOLD_CFG ); // Current above baseline that triggers a ground fault gfScanTimeoutCfgReader_ = newConfigReader( CBITIF::GFSCAN_TIMEOUT_CFG ) ; // How often to scan for ground faults // Offsets for leakage current gfBattOffsetCfgReader_ = newConfigReader( CBITIF::GFBATTOFFSET_CFG ); gf24OffsetCfgReader_ = newConfigReader( CBITIF::GF24OFFSET_CFG ); gf12OffsetCfgReader_ = newConfigReader( CBITIF::GF12OFFSET_CFG ); gf5OffsetCfgReader_ = newConfigReader( CBITIF::GF5OFFSET_CFG ); gf3_3OffsetCfgReader_ = newConfigReader( CBITIF::GF3_3OFFSET_CFG ); gf3_15OffsetCfgReader_ = newConfigReader( CBITIF::GF3_15OFFSET_CFG ); gfCommOffsetCfgReader_ = newConfigReader( CBITIF::GFCOMMOFFSET_CFG ); } CBIT::~CBIT() {} // Initialize function void CBIT::initialize( void ) { logger_.syslog( "Initialize CBIT Component." ); this->setAllowableFailures( 1 ); this->setRetryTimeout( 10 ); abortStartTime_ = Timestamp::Now(); // Restart the clocks stopDepthStartTime_ = Timestamp::Now(); envFailStartTime_ = Timestamp::Now(); gfStartTime_ = Timestamp::Now(); battThresholdFailStartTime_ = Timestamp::Now(); debugEFCTimestamp_ = Timestamp::Now(); readConfigCriticalReported_ = false; coulombCountCheckTime_ = Timestamp::Now(); samples_ = 1; // Set up dio pins if( LPC3Reg::InitializeMuxPins() ) { logger_.syslog( "Initialized mux pins." ); } else { logger_.syslog( "Failed to initialize mux pins.", Syslog::FAULT ); } // Get the size of the list of components unsigned int listsize = ComponentRegistry::GetEntryCount(); if( listsize <= 1 ) { faultWriter_->write( Units::ENUM, true ); logger_.syslog( Str( "CBIT failed to initialize." ), Syslog::ERROR ); } if( Supervisor::WasRunning() ) { logger_.syslog( "LAST RESTART WAS UNINTENTIONAL.", Syslog::FAULT ); } logger_.syslog( "Initializing the watchdog timer." ); // Initialize the watchdog timer // Set up to be driven by PERIPH_CLK @ 13 Mhz. // Full 32 bit register cycle will be 4294967295 * (1/13,000,000) ~ 5.506 minutes if( LPC3Reg::InitializeWDT() ) { wdtInitialized_ = true; // Check for previous WDT reset if( !simulateHardware() && ( LPC3Reg::WDTimRes_ & 0x1 ) ) { logger_.syslog( Str( "LAST REBOOT DUE TO WATCHDOG TIMER RESET." ), Syslog::CRITICAL ); } else { logger_.syslog( Str( "Last reboot was NOT due to watchdog timer." ), Syslog::INFO ); } } else { logger_.syslog( Str( "Watchdog Timer failed to initialize." ), Syslog::CRITICAL ); this->setFailure( FailureMode::HARDWARE ); } // Report backtrace if present checkBacktrace(); if( simulateHardware() ) { return; } logger_.syslog( "Initializing heartbeat." ); // Kick off the heartbeat LPC3Reg::InitializeHBC(); logger_.syslog( "Deactivating GF circuits." ); // Open all GF detection circuits LPC3Reg::DeactivateGF(); // And power off LPC3Reg::DeactivateGFPwr(); logger_.syslog( "Deactivating emergency mode." ); // "Safe" the drop weight, backup power, and radio signals LPC3Reg::DeactivateEmergMode(); if( LPC3Reg::QueryBackplanePower() || LPC3Reg::PowerOnLoads() ) { logger_.syslog( "Backplane powered." ); } } // Load parameters bool CBIT::readConfig( void ) { // Check if all the parameters are read correctly bool ok = true; ok &= abortDepthCfgReader_->read( Units::METER, abortDepth_ ); ok &= stopDepthCfgReader_->read( Units::METER, stopDepth_ ); if( stopDepth_ > abortDepth_ ) { logger_.syslog( Str( "Stop depth set deeper than abort depth!" ), Syslog::FAULT ); } ok &= humidityThresholdCfgReader_->read( Units::PERCENT, bitHumidityThreshold_ ); ok &= pressureThresholdCfgReader_->read( Units::POUND_PER_SQUARE_INCH, bitPressureThreshold_ ); ok &= tempThresholdCfgReader_->read( Units::CELSIUS, bitTempThreshold_ ); ok &= battTempThresholdCfgReader_->read( Units::CELSIUS, battTempThreshold_ ); int boolTemp; ok &= vehicleOpenCfgReader_->read( Units::BOOL, boolTemp ); cbitVehicleOpen_ = boolTemp; ok &= faultClassifierCfgReader_->read( Units::BOOL, boolTemp ); runFaultClassifier_ = boolTemp; ok &= elevOffsetCalcCfgReader_->read( Units::BOOL, boolTemp ); runElevOffsetCalc_ = boolTemp; double timeoutTemp; ok &= abortDepthTimeoutCfgReader_->read( Units::SECOND, timeoutTemp ); abortDepthTimeout_ = timeoutTemp; int battFailReport; ok &= battFailReportCfgReader_->read( Units::MINUTE, battFailReport ); battFailReportInterval_ = battFailReport; double timeoutEnv; ok &= envTimeoutCfgReader_->read( Units::SECOND, timeoutEnv ); envFailTimeout_ = timeoutEnv; int checkNav; ok &= navErrorCriticalCfgReader_->read( Units::BOOL, checkNav ); navErrorCriticalReporting_ = checkNav; ok &= navErrorThresholdCfgReader_->read( Units::PERCENT, navErrorThreshold_ ); double timeoutScan; ok &= gfScanTimeoutCfgReader_->read( Units::SECOND, timeoutScan ); gfScanTimeout_ = timeoutScan; int allowLeak; ok &= allowAuxLeakCfgReader_->read( Units::BOOL, allowLeak ); allowAuxLeak_ = allowLeak; // gfChanX_ThresholdCfgReader's read only during scanGFChan return ok; } /// The actual "payload" of the component void CBIT::run() { // Make sure we've got the latest from the top... // Note that we ignore the return value here because a critical will trigger with message "No configuration setting for..." // in the event that a config variable doesn't exist. readConfig(); // Kick the WDT and heartbeat once per cycle assertVitals(); // If Logger has received a Critical error, stop the mission checkCriticals(); // Look for stop or abort depths checkDepth(); // Logs the shore power status monitorShorePower(); // Read and report the running amount of energy used reportCoulombCount(); // Check for failed main battery checkMainBattery(); // Check for backplane power checkBackplanePower(); // Check for overcurrent condition checkBackplaneOvercurrent(); // Read water sensors checkLeak(); // See if card is still in place monitorCFCard(); // Make sure location is a real value //checkLocation(); // Look for a really bad fix residual value checkNavigation(); monitorEnvironmentals(); monitorBatteryTemps(); handleComponentFaults(); // Check GF if it's time scanGFChan(); if( runFaultClassifier_ ) { empiricalFaultClassification(); } } // Uninit function void CBIT::uninitialize( void ) { logger_.syslog( "Uninitialize CBIT Component." ); if( simulateHardware() ) { return; } // Power off loads logger_.syslog( "Powering off loads." ); LPC3Reg::PowerOffLoads(); //Disable WDT. logger_.syslog( "Disabling WDT." ); LPC3Reg::UninitializeWDT(); // Open all GF detection circuits logger_.syslog( "Opening all GF detection circuits." ); LPC3Reg::DeactivateGF(); } /// Should return [myNamespace]::SIMULATE_HARDWARE, or [myNamespace]::POWER, etc ConfigURI CBIT::getConfigURI( ConfigOption configOption ) const { return configOption == CONFIG_SIMULATE_HARDWARE ? CBITIF::SIMULATE_HARDWARE : ConfigURI::NO_CONFIG_URI; } void CBIT::assertVitals( void ) { // Kick the WDT once per cycle if( !simulateHardware() ) { if( wdtInitialized_ ) { LPC3Reg::StrobeWDT(); } } // Send heartbeat if( !simulateHardware() ) { LPC3Reg::StrobeHBC(); } } void CBIT::checkBackplanePower( void ) { // Check for backplane power if( simulateHardware() || LPC3Reg::QueryBackplanePower() ) { // Backplane powered backplaneFailed_ = false; } else { if( backplaneFailed_ == false ) { logger_.syslog( Str( "Backplane power off. Attempting to restore." ), Syslog::FAULT ); if( LPC3Reg::PowerOnLoads() ) { logger_.syslog( Str( "Backplane power restored." ), Syslog::IMPORTANT ); } else { logger_.syslog( Str( "Backplane power failed to restore. Resetting CPU." ), Syslog::CRITICAL ); backplaneFailed_ = true; } } else // The backplane has failed and the failure has been logged. { backplaneFailed_ = false; // Just in case... LPC3Reg::SWReset(); // Attempt to turn on the radio(s) via emergency card if this keeps up. } } } void CBIT::checkBackplaneOvercurrent( void ) { if( !simulateHardware() && LPC3Reg::QueryBackplaneOvercurrent() ) { logger_.syslog( Str( "Backplane overcurrent detected." ), Syslog::CRITICAL ); } } void CBIT::checkBacktrace( void ) { Str backtraceFile = "Data/backtrace"; FILE* fid = fopen( backtraceFile.cStr(), "r" ); if( NULL == fid ) { // No backtrace file return; } // Report contents of backtrace file char line [ 256 ]; int lineCount( 0 ); Str syslogEntry = "Found backtrace file:\n"; while( ( lineCount < 20 ) && ( fgets( line, sizeof( line ), fid ) != NULL ) ) { syslogEntry += line; lineCount++; } logger_.syslog( syslogEntry, Syslog::FAULT ); fclose( fid ); // Rename backtrace file to Data/backtraceXX struct stat st; int sfx( 0 ); Str newBacktraceFile( backtraceFile + sfx ); while( stat( newBacktraceFile.cStr(), &st ) == 0 ) { ++sfx; newBacktraceFile = backtraceFile + sfx; } logger_.syslog( "Renaming backtrace file to " + newBacktraceFile, Syslog::FAULT ); rename( backtraceFile.cStr(), newBacktraceFile.cStr() ); } void CBIT::checkCriticals( void ) { if( Logger::LastCriticalErrorReceived() != Timestamp::NOT_SET_TIME && Logger::LastCriticalErrorReceived() != Timestamp::EPOCH_START_TIME ) { if( Supervisor::Instance_ != NULL ) { CommandExec::DoSchedulePause( Syslog::FAULT, Logger::LastCriticalErrorReceived() ); logger_.syslog( "Critical error at " + Logger::LastCriticalErrorReceived().toSmallString(), Syslog::INFO ); Supervisor::Instance_->stopMission( "CBIT::checkCriticals" ); Logger::ClearCrtiticalErrorReceived(); } } } void CBIT::checkDepth( void ) { double depth( 0 ); if( depthReader_->isActive() && depthReader_->wasTouchedSinceLastRun( this ) && depthReader_->read( Units::METER, depth ) ) { if( depth >= abortDepth_ ) // Make sure we're not below abort depth { if( ( previouslyBelowAbort_ ) && ( abortStartTime_.elapsed() > abortDepthTimeout_ ) ) // Two cycles in a row AND threshold time elapsed will result in drop weight activation { if( abortDepthActive_ == false ) { abortDepthActive_ = true; logger_.syslog( "ABORT DEPTH REACHED. ACTIVATING BURNWIRE.", Syslog::CRITICAL ); LPC3Reg::ActivateBurnwire(); if( Supervisor::Instance_ != NULL ) Supervisor::Instance_->stopMission( "CBIT::checkDepth" ); // stop the current mission } } else { previouslyBelowAbort_ = true; } } else { abortStartTime_ = Timestamp::Now(); // Restart the clock abortDepthActive_ = false; previouslyBelowAbort_ = false; } if( depth >= stopDepth_ ) // Check for stop depth { if( ( previouslyBelowStop_ ) && ( stopDepthStartTime_.elapsed() > abortDepthTimeout_ ) ) // Two cycles in a row AND threshold time elapsed will result in mission termination { if( stopDepthActive_ == false ) { stopDepthActive_ = true; logger_.syslog( "STOP DEPTH REACHED. Terminating Mission.", Syslog::CRITICAL ); if( Supervisor::Instance_ != NULL ) Supervisor::Instance_->stopMission( "CBIT::checkDepth" ); // stop the current mission } } else { previouslyBelowStop_ = true; } } else { stopDepthStartTime_ = Timestamp::Now(); // Restart the clock stopDepthActive_ = false; previouslyBelowStop_ = false; } } // if you can read valid depth // TODO: Implement a new feature in CBIT that warns if depth has not been written recently. } void CBIT::checkLeak( void ) { // Check for water in the hull if( !simulateHardware() ) { short waterDetectedBitmask = waterDetected(); if( waterDetectedBitmask && ( !waterDetectedBurnwireActivated_ ) ) // We have water. Let's filter slightly to make sure it's not a false alarm. { if( ( waterDetectionCount_ += 1 ) > WATER_DETECTION_THRESHOLD ) // Threshold exceeded. { Str critMsg = "WATER DETECTED IN PRESSURE HULL:"; if( waterDetectedBitmask & ( 1 << WATER_ALARM_BOW ) ) critMsg += " BOW"; if( waterDetectedBitmask & ( 1 << WATER_ALARM_STERN ) ) critMsg += " STERN"; if( waterDetectedBitmask & ( 1 << WATER_ALARM_AUX ) ) critMsg += " AUX"; // Activate the burnwire unless it's an aux-only leak that we've decided to allow if( !( waterDetectedBitmask == ( 1 << WATER_ALARM_AUX ) && allowAuxLeak_ ) ) { LPC3Reg::ActivateBurnwire(); critMsg += ". BURNWIRE ACTIVATED" ; waterDetectedBurnwireActivated_ = true; // If we critical'd before (but didn't activate the burnwire) on an allowed aux-only // leak that is now present elsewhere, stop the mission and critical again so we know // about the change if( waterDetectedCriticalSet_ ) { logger_.syslog( critMsg, Syslog::CRITICAL ); // Stop the current mission if( Supervisor::Instance_ != NULL ) { Supervisor::Instance_->stopMission( "CBIT::checkLeak" ); } } } // Take action only one time. If we've already set the critical, stopped the mission, // and powered down an ESP there's nothing left to do. if( !waterDetectedCriticalSet_ ) { waterDetectedCriticalSet_ = true; logger_.syslog( critMsg, Syslog::CRITICAL ); // Stop the current mission if( Supervisor::Instance_ != NULL ) { Supervisor::Instance_->stopMission( "CBIT::checkLeak" ); } // Check for ESP on board and power down if present int ESPLoaded = false; Slate::ReadOnce( ESPComponentIF::LOAD_AT_STARTUP, Units::BOOL, ESPLoaded, logger_ ); if( ESPLoaded ) { StrValue ESPLoadCtrl; Slate::ReadOnce( ESPComponentIF::LOAD_CONTROL, ESPLoadCtrl, logger_ ); // power down the ESP Str lcb = "! echo 0 > " + ESPLoadCtrl.asString(); CommandExec::DoCommand( lcb.cStr(), false ); logger_.syslog( Str( "Powered off ESP with command: " ) + Str( lcb ), Syslog::IMPORTANT ); } else { logger_.syslog( Str( "No ESP to power off." ), Syslog::IMPORTANT ); } } } else // Might be interesting to know we had a detection event even though it's not above the threshold yet { logger_.syslog( Str( "WATER DETECTED IN PRESSURE HULL. Alarm Count: " + Str( waterDetectionCount_ ) ), Syslog::ERROR ); } } else // No water. Decrement alarm counter until 0 { if( waterDetectionCount_ > 0 ) { waterDetectionCount_ -= 1; } } } } void CBIT::checkLocation( void ) { double latitude = nanf( "" ); double longitude = nanf( "" ); latitudeReader_->read( Units::RADIAN, latitude ); longitudeReader_->read( Units::RADIAN, longitude ); if( isnan( latitude ) || isnan( longitude ) ) { // Start the clock if we haven't yet if( !nanLocationActive_ ) { locationCheckStartTime_ = Timestamp::Now(); nanLocationActive_ = true; // We have encountered a nan location logger_.syslog( "NAN location detected.", Syslog::IMPORTANT ); // TODO: Is there a good way to determine which component wrote it? } if( locationCheckStartTime_.elapsed() > locationCheckTimeout_ && !nanLocationReported_ ) { logger_.syslog( Str( "Location has been nan for set timeout." ), Syslog::CRITICAL ); nanLocationReported_ = true; } } else { nanLocationActive_ = false; nanLocationReported_ = false; } } void CBIT::checkMainBattery( void ) { //Check for failed main battery if( !simulateHardware() && !LPC3Reg::QueryBattPower() ) { if( battFailReportTime_.elapsed() > battFailReportInterval_ ) { logger_.syslog( "Main Battery Failure", Syslog::FAULT ); battFailReportTime_ = Timestamp::Now(); } } } void CBIT::empiricalFaultClassification( void ) { const double pi = 3.1415926; double depth( 0 ); float pitch( nanf( "" ) ); float depthRate( nanf( "" ) ); float elevAngle( nanf( "" ) ); float speedCmd( nanf( "" ) ); float empiricalFaultElevOffset( 0 ); int verticalMode = -1; int EFCbinSize = 8; int EOMsampleSize = 7200; // Check incoming data quality bool dataCheckEFC[7] = { false }; if( speedCmdReader_->isActive() && speedCmdReader_->read( Units::METER_PER_SECOND, speedCmd ) ) { dataCheckEFC [0] = true; } if( depthReader_->isActive() && depthReader_->read( Units::METER, depth ) ) { dataCheckEFC [1] = true; } if( depthRateReader_->isActive() && depthRateReader_->read( Units::METER_PER_SECOND, depthRate ) ) { dataCheckEFC [2] = true; } if( elevatorAngleReader_->isActive() && elevatorAngleReader_->read( Units::RADIAN, elevAngle ) ) { dataCheckEFC [3] = true; } if( pitchReader_->isActive() && pitchReader_->read( Units::RADIAN, pitch ) ) { dataCheckEFC [4] = true; } if( verticalModeReader_->isActive() && verticalModeReader_->read( Units::ENUM, verticalMode ) ) { dataCheckEFC [5] = true; } if( depth > 1 && speedCmd == 1 && verticalMode == 5 && !isnan( depthRate ) && !isnan( elevAngle ) && !isnan( pitch ) ) { dataCheckEFC [6] = true; // Make sure operational profile is compatible for analysis } if( !runElevOffsetCalc_ ) { clearCounterEOM_ = true; // Queue clear vars if EOM is switched off } // Incoming data check-sum int dataChecksumEFC = 0; for( int k = 0; k < 7; k++ ) { dataChecksumEFC += dataCheckEFC[k]; } // Get to work if data checks out if( dataChecksumEFC == 7 ) { // Elevator Offset Monitor (EOM) if( runElevOffsetCalc_ ) { if( clearCounterEOM_ ) { elevOffsetCounter_ = 0; // Clear counter elevOffsetEleTmp_ = 0; // Clear sum-up elev angle for bin avg. empiricalFaultElevOffset = 0; // Clear mean elevator angle (offset) clearCounterEOM_ = false; // Announce to syslog logger_.syslog( "EFC elev offset counter cleared!", Syslog::DEBUG ); } elevOffsetCounter_ += 1; // Pulse counter elevOffsetEleTmp_ += elevAngle; // Sum-up elev angle for bin avg. if( elevOffsetCounter_ == EOMsampleSize ) { empiricalFaultElevOffset = elevOffsetEleTmp_ / EOMsampleSize; // Calc mean elevator angle (offset) // Write offset value to syslog logger_.syslog( "EFC elev offset (n=" + Str( EOMsampleSize ) + "): elevOffset = " + Str( empiricalFaultElevOffset * ( 180 / pi ) ) + " degree", Syslog::IMPORTANT ); // Write to slate empiricalFaultElevOffsetWriter_->write( Units::RADIAN, empiricalFaultElevOffset ); clearCounterEOM_ = true; } } // Empirical Fault Classifier (EFC) if( empiricalFaultElevOffsetReader_->isActive() && empiricalFaultElevOffsetReader_->read( Units::RADIAN, empiricalFaultElevOffset ) ) { empiricalFaultCounter_ += 1; // Pulse counter empiricalFaultBinSum_ += depthRate; // Sum-up depthRate for bin avg. // Grab timestamp and depth when bin is half full if( empiricalFaultCounter_ == round( EFCbinSize / 2 ) ) { empiricalFaultTimestamp_ = Timestamp::Now(); empiricalFaultDepth_ = depth; } // Pitch angle: Get extream pitch angle value for bin if( fabs( empiricalFaultPitchTmp_ ) < fabs( pitch ) ) { empiricalFaultPitchTmp_ = pitch; } // Elevator angle: Correct elevator angle offset if( !isnan( empiricalFaultElevOffset ) ) { elevAngle = elevAngle - 0.85 * empiricalFaultElevOffset; // Correct elevator angle offset } // Elevator angle: Get extream elevator angle value for bin if( fabs( empiricalFaultEleTmp_ ) < fabs( elevAngle ) ) { empiricalFaultEleTmp_ = elevAngle; } // Compute bin avg and evaluate filtering criteria if bin size is reached if( empiricalFaultCounter_ == EFCbinSize ) { float binAvgDepthRate = ( empiricalFaultBinSum_ / EFCbinSize ); // Empirical fault classification filtering criteria (return non-zero if anomaly detected) int qual = 0; if( empiricalFaultEleTmp_ < ( -10 * ( pi / 180 ) ) && ( binAvgDepthRate < 0.1 ) ) // Commanding extream Ele up in non-transition state { qual = 1; } if( empiricalFaultEleTmp_ > ( 10 * ( pi / 180 ) ) && ( binAvgDepthRate > -0.1 ) ) // Commanding extream Ele down in non-transition state { qual = 2; } if( empiricalFaultEleTmp_ < ( 0 ) && ( binAvgDepthRate > 0.4 ) ) // Going down fast when Ele commanding up { qual = 3; } if( empiricalFaultEleTmp_ < ( 0 ) && ( empiricalFaultPitchTmp_ < ( -30 * ( pi / 180 ) ) ) ) // Pitched down hard when Ele commanding up { qual = 4; } if( empiricalFaultPitchTmp_ < ( -35 * ( pi / 180 ) ) ) // Pitched down very hard (case control-loop fails). { qual = 5; } if( ( qual != 0 ) & ( empiricalFaultDepth_ > 2 ) ) { // Count consecituve fault classifications empiricalFaultPositiveFlagCount_ += 1; if( empiricalFaultPositiveFlagCount_ > 2 ) // arm. safe. fire. (Fault persists) { // Write to syslog logger_.syslog( "EFC: Fault classified (type " + Str( qual ) + ") - Depth = " + Str( empiricalFaultDepth_ ) + ", binAvgDepthRate = " + Str( binAvgDepthRate ) + ", pitchAngle = " + Str( empiricalFaultPitchTmp_ * ( 180 / pi ) ) + ", elevAngle = " + Str( empiricalFaultEleTmp_ * ( 180 / pi ) ), Syslog::INFO ); } } else { empiricalFaultPositiveFlagCount_ = 0; // Reset positive flag Count sequence case no fault } // Write data empericalFaultDetectedWriter_->write( Units::ENUM, qual, empiricalFaultTimestamp_ ); binnedDepthRateWriter_->write( Units::METER_PER_SECOND, binAvgDepthRate, empiricalFaultTimestamp_ ); // Reset EFC vars empiricalFaultCounter_ = 0; empiricalFaultBinSum_ = 0; empiricalFaultEleTmp_ = 0; empiricalFaultPitchTmp_ = 0; empiricalFaultTimestamp_ = 0; empiricalFaultDepth_ = 0; if( debugEFCTimestamp_.elapsed() > debugEFCTimeout_ ) { logger_.syslog( "EFC evaluating classification filtering criteria", Syslog::DEBUG ); debugEFCTimestamp_ = Timestamp::Now(); } } } } // Data check-sum false else { // Reset EFC vars empiricalFaultCounter_ = 0; empiricalFaultEleTmp_ = 0; empiricalFaultBinSum_ = 0; empiricalFaultPitchTmp_ = 0; empiricalFaultTimestamp_ = 0; empiricalFaultDepth_ = 0; if( debugEFCTimestamp_.elapsed() > debugEFCTimeout_ ) { logger_.syslog( "EFC running - data check-sum false", Syslog::DEBUG ); debugEFCTimestamp_ = Timestamp::Now(); } } } void CBIT::handleComponentFaults( void ) { // Check for reported faults if( clearFaultCmdReader_->isActive() && clearFaultCmdReader_->wasTouchedSinceLastRun( this ) && clearFaultCmdReader_->asInt( Units::ENUM ) != 0 ) { faultWriter_->write( Units::ENUM, false ); } if( clearLeakFaultCmdReader_->isActive() && clearLeakFaultCmdReader_->wasTouchedSinceLastRun( this ) && clearLeakFaultCmdReader_->asInt( Units::ENUM ) != 0 ) { faultWriter_->write( Units::ENUM, false ); } // Now the code that sets the faults to positive follows: unsigned int listsize = ComponentRegistry::GetEntryCount(); // Get a list of all the components for( unsigned int i = 0; i < listsize; i++ ) { Component* component = ( Component* )ComponentRegistry::GetIndexed( i ); // First check for null if( component != NULL ) { // Then see if there is a failure if( ( component->isFailed() ) ) { // Report as an error for the syslog if( !( component->getFailureReported() ) ) { logger_.syslog( FailureMode::ToString( component->getFailureType() ) + " in component: " + component->getName(), Syslog::ERROR ); component->setFailureReported( true ); } // Check if there have been multiple failures if( component->getFailCount() >= component->getAllowableFailures() ) { faultWriter_->write( Units::ENUM, true ); // Set this component as reported so we don't keep sending info if( !( component->getCriticalFailureReported() ) ) { logger_.syslog( FailureMode::ToString( component->getFailureType() ) + " in component: " + component->getName(), component->isFailureMissionCritical() ? Syslog::CRITICAL : Syslog::FAULT ); component->setCriticalFailureReported( true ); } // Check to see if it's time to retry this component if( component->getTimeOfFailure().elapsed() > component->getRetryTimeout() && component->isFailureUninitialized() ) { logger_.syslog( "Clearing failed state for component " + component->getName(), Syslog::INFO ); component->setFailure( FailureMode::NONE ); // RecoveringFromCritical and CriticalFailureReported are both intentionally set after the call to resetFailCount to enable // the distinction between this, a routine attempt to bring up a failed component, and the component itself calling // resetFailCount following successful completion of its tasks after a critical failure. component->resetFailCount(); component->setRecoveringFromCritical( true ); component->setCriticalFailureReported( false ); } } // Check to see if it's time to retry this component else if( component->isFailureUninitialized() ) { logger_.syslog( "Clearing failed state for component " + component->getName(), Syslog::INFO ); component->setFailure( FailureMode::NONE ); } } // Clear our failure reporting bool else if( !( component->isFailed() ) && ( component->getFailureReported() ) ) { component->setFailureReported( false ); component->setCriticalFailureReported( false ); } } } } void CBIT::monitorBatteryTemps( void ) { // Check battery thresholds not checked in battery component for( int i = 0; i < ( BPC1AIF::NUMBATTS + BPC1BIF::NUMBATTS ); i++ ) { if( battPacks_[i].battTempReader_->wasTouchedSinceLastRun( this ) ) { bool noTempFault = true; float temp = nanf( "" ); battPacks_[i].battTempReader_->read( Units::CELSIUS, temp ); // Per spec, typical cell temperatures will be in the range of -20°C to +75°C. if( ( temp > -20.0 && temp < 75.0 ) && temp > battTempThreshold_ ) { noTempFault = false; if( ( battThresholdFailed_ == false ) && ( battThresholdFailStartTime_.elapsed() > battThresholdFailTimeout_ ) ) { logger_.syslog( Str( "Battery temperature above threshold." ), Syslog::CRITICAL ); battThresholdFailed_ = true; for( int j = 0; j < ( BPC1AIF::NUMBATTS + BPC1BIF::NUMBATTS ); j++ ) { float reportTemp; battPacks_[j].battTempReader_->read( Units::CELSIUS, reportTemp ); logger_.syslog( Str( "Batt #" + Str( j ) + " " + Str( reportTemp, 1 ) + " deg C." ), Syslog::IMPORTANT ); } } } if( noTempFault ) // Nothing out of range this time around { battThresholdFailStartTime_ = Timestamp::Now(); battThresholdFailed_ = false; } } } } void CBIT::monitorCFCard( void ) { // Monitor CF card if( !simulateHardware() && LPC3Reg::CFMissing() ) { if( !cfMissing_ ) { logger_.syslog( Str( "MMC card no longer detected\n" ), Syslog::CRITICAL ); cfMissing_ = true; } } else { cfMissing_ = false; } } void CBIT::checkNavigation( void ) { // This thing doesn't get written very often so let's check if( fixResidualPercentDistanceTraveledReader_->wasTouchedSinceLastRun( this ) ) { float fixResidualPercentDistanceTraveled; if( fixResidualPercentDistanceTraveledReader_->read( Units::PERCENT, fixResidualPercentDistanceTraveled ) ) { // Our nav is worse than the allowable threshold if( fixResidualPercentDistanceTraveled > navErrorThreshold_ ) { if( navErrorCriticalReporting_ ) { //Throw a critical logger_.syslog( "Navigation error has exceeded threshold. navErrorCriticalReporting is enabled.", Syslog::CRITICAL ); } else // A not so critical { logger_.syslog( "Navigation error has exceeded threshold.", Syslog::IMPORTANT ); } } } } } void CBIT::monitorEnvironmentals( void ) { // Check environmental thresholds if( !cbitVehicleOpen_ ) { double pressure; int humidity, temperature; if( !pressureReader_ > isActive() || !pressureReader_->read( Units::POUND_PER_SQUARE_INCH, pressure ) ) { pressure = 0; } if( !humidityReader_->isActive() || !humidityReader_->read( Units::PERCENT, humidity ) ) { humidity = 0; } if( !temperatureReader_->isActive() || !temperatureReader_->read( Units::CELSIUS, temperature ) ) { temperature = 0; } if( ( fabs( pressure - ATMOSPHERE_PSI ) < bitPressureThreshold_ ) || ( humidity > bitHumidityThreshold_ ) || ( temperature > bitTempThreshold_ ) ) { if( ( envFailed_ == false ) && ( envFailStartTime_.elapsed() > envFailTimeout_ ) ) { logger_.syslog( Str( "Environmental Failure. Press:" + Str( pressure ) + " PSI. " + "Humidity:" + Str( humidity ) + "%. Temp:" + Str( temperature ) + " C. ABORTING MISSION" ), Syslog::CRITICAL ); envFailed_ = true; // Stop the current mission if( Supervisor::Instance_ != NULL ) { Supervisor::Instance_->stopMission( "CBIT::monitorEnvironmentals" ); } } } else { envFailStartTime_ = Timestamp::Now(); envFailed_ = false; } if( samples_ < 100 && humidity > 0 ) { avgHumidity_ += humidity; if( ++samples_ == 100 ) { avgHumidity_ = avgHumidity_ / samples_; } } // Check to see if environmental data has jumped outside of 50% of baseline // 10% is the low end of the instrument measurement. Don't bother checking the delta if it's less than 10%RH else if( ( humidity >= 10 ) && ( humidity > avgHumidity_ * 1.5 ) ) { if( envExcursionFault_ == false ) { logger_.syslog( Str( "Humidity exceeds 150% of running average: Humidity:" + Str( humidity ) + " %. Average:" + Str( avgHumidity_ ) ), Syslog::FAULT ); envExcursionFault_ = true; } } } } void CBIT::monitorShorePower( void ) { // Monitor shore power if( simulateHardware() || LPC3Reg::QueryShorepower() ) { shorepowerWriter_->write( Units::BOOL, true ); } else { shorepowerWriter_->write( Units::BOOL, false ); } } void CBIT::reportCoulombCount( void ) { if( ( !simulateHardware() ) && ( coulombCountCheckTime_.elapsed() > coulombCountTimeout_ ) ) { coulombCountCheckTime_ = Timestamp::Now(); // reset the timer FILE* fid = fopen( "/mnt/mmc/fuelgauge-daemon/log/coulomb_count.txt", "r" ); double ah_used = 0; if( NULL != fid ) { char line [ 80 ]; if( fgets( line, sizeof line, fid ) != NULL ) { Str lineStr( line ); // convert the line to an int and convert to aH. microamp hours to amp hours ah_used = Str::ToDouble( line ); ah_used = ah_used / 1000000; aHUsedWriter_->write( Units::AMPERE_HOUR, ah_used ); } else { logger_.syslog( "Could not parse coulomb counter log file", Syslog::FAULT ); } } else { logger_.syslog( "Could not open coulomb counter log file", Syslog::FAULT ); } } } void CBIT::scanGFChan( void ) { if( simulateHardware() ) { return; } // If a scan request comes in via isDataRequested, consider it a "forced" or initiated scan that will run multiple times if a bounding error occurs. if( gfScanActiveWriter_->isDataRequested() ) { GFScanForce_ = true; } if( ( gfStartTime_.elapsed() < gfScanTimeout_ ) && !GFScanForce_ && !gfScanInProgress_ ) { return; } bool ok( true ); // Every channel has a threshold.... ok &= gfChanA0ThresholdCfgReader_->read( Units::MILLIAMPERE, gfChanA0Threshold_ ); ok &= gfChanA1ThresholdCfgReader_->read( Units::MILLIAMPERE, gfChanA1Threshold_ ); ok &= gfChanA2ThresholdCfgReader_->read( Units::MILLIAMPERE, gfChanA2Threshold_ ); ok &= gfChanA3ThresholdCfgReader_->read( Units::MILLIAMPERE, gfChanA3Threshold_ ); ok &= gfChanB0ThresholdCfgReader_->read( Units::MILLIAMPERE, gfChanB0Threshold_ ); ok &= gfChanB1ThresholdCfgReader_->read( Units::MILLIAMPERE, gfChanB1Threshold_ ); ok &= gfChanB2ThresholdCfgReader_->read( Units::MILLIAMPERE, gfChanB2Threshold_ ); ok &= gfChanB3ThresholdCfgReader_->read( Units::MILLIAMPERE, gfChanB3Threshold_ ); // And every channel has an offset ok &= gfBattOffsetCfgReader_->read( Units::MILLIAMPERE, gfBattOffset_ ); ok &= gf24OffsetCfgReader_->read( Units::MILLIAMPERE, gf24Offset_ ); ok &= gf12OffsetCfgReader_->read( Units::MILLIAMPERE, gf12Offset_ ); ok &= gf5OffsetCfgReader_->read( Units::MILLIAMPERE, gf5Offset_ ); ok &= gf3_3OffsetCfgReader_->read( Units::MILLIAMPERE, gf3_3Offset_ ); ok &= gf3_15OffsetCfgReader_->read( Units::MILLIAMPERE, gf3_15Offset_ ); ok &= gfCommOffsetCfgReader_->read( Units::MILLIAMPERE, gfCommOffset_ ); if( !gfScanInProgress_ ) { LPC3Reg::ActivateGFPwr(); // Turn on power to the monitoring circuit logger_.syslog( "Beginning ground fault scan", Syslog::IMPORTANT ); // Those negatively affected by a scan are being warned gfScanActiveWriter_->write( Units::BOOL, true ); gfScanInProgress_ = true; gfScan_ = OPEN_SCAN; return; // let a cycle go by } switch( gfScan_ ) { case OPEN_SCAN: // Read open circuit value for baseline LPC3Reg::DeactivateGF(); gfCurrentOpen_ = ( ( adGFCurrent_.readVolts() - 2.5 ) / 1.574 ); // 2.5V offset. 1.574V/mA gfScan_ = CHAN_A0_SCAN; gfChan_ = LPC3Reg::CHAN_A0; LPC3Reg::ActivateGF( gfChan_ ); gfStartTime_ = Timestamp::Now(); break; case CHAN_A0_SCAN: // VBAT if( gfStartTime_.elapsed() >= gfChanSetupTimeout_ ) { gfCurrentA0_ = ( ( adGFCurrent_.readVolts() - 2.5 ) / 1.574 ); // 2.5V offset. 1.574V/mA // Applying the leakage offset for the battery is a special case since the // battery voltage is subject to change. Let's grab the voltage and apply a // more accurate offset if possible. If not, we'll use the nominal we already have. float voltage( nanf( "" ) ); if( batteryVoltageReader_->read( Units::VOLT, voltage ) && !isnan( voltage ) ) { // Here lerror in Amps is Vbus - Vcm nominal of the AD8207 (3.5V) / Rcm nominal of the AD8207 (126k). gfBattOffset_ = ( ( voltage - 3.5 ) / 126000 ) * 1000; // grab the actual offset and convert to mA. } gfCurrentA0_ -= gfBattOffset_; // Apply offset LPC3Reg::DeactivateGF(); gfScan_ = CHAN_A1_SCAN; gfChan_ = LPC3Reg::CHAN_A1; LPC3Reg::ActivateGF( gfChan_ ); gfStartTime_ = Timestamp::Now(); } break; case CHAN_A1_SCAN: // +24V_LC if( gfStartTime_.elapsed() >= gfChanSetupTimeout_ ) { // Check to see if the 24V bus is powered. if( !power24vConverterDataReader_->read( Units::BOOL, Pwr24On_ ) ) { logger_.syslog( "Error reading 24V bus power status. Assuming off for scan purposes.", Syslog::ERROR ); Pwr24On_ = false; } if( Pwr24On_ ) { gfCurrentA1_ = ( ( adGFCurrent_.readVolts() - 2.5 ) / 1.574 ); // 2.5V offset. 1.574V/mA gfCurrentA1_ -= gf24Offset_; // Apply offset } else { gfCurrentA1_ = 0.0; } LPC3Reg::DeactivateGF(); gfScan_ = CHAN_A2_SCAN; gfChan_ = LPC3Reg::CHAN_A2; LPC3Reg::ActivateGF( gfChan_ ); gfStartTime_ = Timestamp::Now(); } break; case CHAN_A2_SCAN: // +12.0V_LC if( gfStartTime_.elapsed() >= gfChanSetupTimeout_ ) { gfCurrentA2_ = ( ( adGFCurrent_.readVolts() - 2.5 ) / 1.574 ); // 2.5V offset. 1.574V/mA gfCurrentA2_ -= gf12Offset_; // Apply offset LPC3Reg::DeactivateGF(); gfScan_ = CHAN_A3_SCAN; gfChan_ = LPC3Reg::CHAN_A3; LPC3Reg::ActivateGF( gfChan_ ); gfStartTime_ = Timestamp::Now(); } break; case CHAN_A3_SCAN: // +5.0V_LC if( gfStartTime_.elapsed() >= gfChanSetupTimeout_ ) { gfCurrentA3_ = ( ( adGFCurrent_.readVolts() - 2.5 ) / 1.574 ); // 2.5V offset. 1.574V/mA gfCurrentA3_ -= gf5Offset_; // Apply offset LPC3Reg::DeactivateGF(); gfScan_ = BANK_DELAY; gfChan_ = LPC3Reg::CHAN_B0; } break; case BANK_DELAY: // Hold to switch between mux banks gfScan_ = CHAN_B0_SCAN; LPC3Reg::ActivateGF( gfChan_ ); gfStartTime_ = Timestamp::Now(); break; case CHAN_B0_SCAN: // +3.3V_LC if( gfStartTime_.elapsed() >= gfChanSetupTimeout_ ) { gfCurrentB0_ = ( ( adGFCurrent_.readVolts() - 2.5 ) / 1.574 ); // 2.5V offset. 1.574V/mA gfCurrentB0_ -= gf3_3Offset_; // Apply offset LPC3Reg::DeactivateGF(); gfScan_ = CHAN_B1_SCAN; gfChan_ = LPC3Reg::CHAN_B1; LPC3Reg::ActivateGF( gfChan_ ); gfStartTime_ = Timestamp::Now(); } break; case CHAN_B1_SCAN: // CPU-A_3.15V if( gfStartTime_.elapsed() >= gfChanSetupTimeout_ ) { gfCurrentB1_ = ( ( adGFCurrent_.readVolts() - 2.5 ) / 1.574 ); // 2.5V offset. 1.574V/mA gfCurrentB1_ -= gf3_15Offset_; // Apply offset LPC3Reg::DeactivateGF(); gfScan_ = CHAN_B2_SCAN; gfChan_ = LPC3Reg::CHAN_B2; LPC3Reg::ActivateGF( gfChan_ ); gfStartTime_ = Timestamp::Now(); } break; case CHAN_B2_SCAN: // CPU-B_3.15V if( gfStartTime_.elapsed() >= gfChanSetupTimeout_ ) { gfCurrentB2_ = ( ( adGFCurrent_.readVolts() - 2.5 ) / 1.574 ); // 2.5V offset. 1.574V/mA gfCurrentB2_ -= gf3_15Offset_; // Apply offset LPC3Reg::DeactivateGF(); gfScan_ = CHAN_B3_SCAN; gfChan_ = LPC3Reg::CHAN_B3; LPC3Reg::ActivateGF( gfChan_ ); gfStartTime_ = Timestamp::Now(); } break; case CHAN_B3_SCAN: // GND if( gfStartTime_.elapsed() >= gfChanSetupTimeout_ ) { gfCurrentB3_ = ( ( adGFCurrent_.readVolts() - 2.5 ) / 1.574 ); // 2.5V offset. 1.574V/mA gfCurrentB3_ -= gfCommOffset_; // Apply offset LPC3Reg::DeactivateGF(); gfScan_ = CALCULATE; } break; case CALCULATE: if( checkGFBounds( MAX_GF_CURRENT ) ) { // Write all the values gfChanA0CurrentWriter_->write( Units::MILLIAMPERE, gfCurrentA0_ ); gfChanA1CurrentWriter_->write( Units::MILLIAMPERE, gfCurrentA1_ ); gfChanA2CurrentWriter_->write( Units::MILLIAMPERE, gfCurrentA2_ ); gfChanA3CurrentWriter_->write( Units::MILLIAMPERE, gfCurrentA3_ ); gfChanB0CurrentWriter_->write( Units::MILLIAMPERE, gfCurrentB0_ ); gfChanB1CurrentWriter_->write( Units::MILLIAMPERE, gfCurrentB1_ ); gfChanB2CurrentWriter_->write( Units::MILLIAMPERE, gfCurrentB2_ ); gfChanB3CurrentWriter_->write( Units::MILLIAMPERE, gfCurrentB3_ ); gfChanOpenCurrentWriter_->write( Units::MILLIAMPERE, gfCurrentOpen_ ); int totalFaults = 0; // Chan A0 if( fabs( gfCurrentA0_ ) > gfChanA0Threshold_ ) { totalFaults += 1; } // Chan A1 if( fabs( gfCurrentA1_ ) > gfChanA1Threshold_ ) { totalFaults += 1; } // Chan A2 if( fabs( gfCurrentA2_ ) > gfChanA2Threshold_ ) { totalFaults += 1; } // Chan A3 if( fabs( gfCurrentA3_ ) > gfChanA3Threshold_ ) { totalFaults += 1; } // Chan B0 if( fabs( gfCurrentB0_ ) > gfChanB0Threshold_ ) { totalFaults += 1; } // Chan B1 if( fabs( gfCurrentB1_ ) > gfChanB1Threshold_ ) { totalFaults += 1; } // Chan B2 if( fabs( gfCurrentB2_ ) > gfChanB2Threshold_ ) { totalFaults += 1; } // Chan B3 if( fabs( gfCurrentB3_ ) > gfChanB3Threshold_ ) { totalFaults += 1; } if( totalFaults == 8 ) { reportGFScan( "Low side ground fault detected" ); } else if( totalFaults == 0 ) { reportGFScan( "No ground fault detected" ); } else { reportGFScan( "Ground fault detected" ); } gfStartTime_ = Timestamp::Now(); // Bounding check was good so reset the clock here. gfScanBoundErrors_ = 0; // Reset the error count GFScanForce_ = false; } else // had a bounding error { logger_.syslog( "Ground fault scan returned out of bounds value. Re-running scan.", Syslog::ERROR ); gfScanBoundErrors_ += 1; // Let's increment so we don't end up in an infinite loop of GF scanning if( gfScanBoundErrors_ >= MAX_GF_BOUND_ERRORS ) { logger_.syslog( "Ground fault scan returned out of bounds value in succession " + Str( MAX_GF_BOUND_ERRORS ) + " times. Will re-run scan on normal schedule.", Syslog::IMPORTANT ); gfStartTime_ = Timestamp::Now(); // reset the clock and scan again next time. GFScanForce_ = false; gfScanBoundErrors_ = 0; // Reset the error count as well to allow for multiple scans if out of bounds } } // And here we clean up no matter what happened back there so we're set to scan again next time. gfScan_ = OPEN_SCAN; gfChan_ = LPC3Reg::CHAN_A0; gfScanInProgress_ = false; gfScanActiveWriter_->write( Units::BOOL, false ); LPC3Reg::DeactivateGFPwr(); // Turn off power to the monitoring circuit break; } } void CBIT::reportGFScan( Str scanTxt ) { if( Pwr24On_ ) { logger_.syslog( scanTxt + "\nmA:\nCHAN A0 (Batt): " + gfCurrentA0_ + "\nCHAN A1 (24V): " + gfCurrentA1_ + "\nCHAN A2 (12V): " + gfCurrentA2_ + "\nCHAN A3 (5V): " + gfCurrentA3_ + "\nCHAN B0 (3.3V): " + gfCurrentB0_ + "\nCHAN B1 (3.15aV): " + gfCurrentB1_ + "\nCHAN B2 (3.15bV): " + gfCurrentB2_ + "\nCHAN B3 (GND): " + gfCurrentB3_ + "\nOPEN: " + gfCurrentOpen_ + "\nFull Scale: +/- 1 mA", Syslog::IMPORTANT ); } else { logger_.syslog( scanTxt + "\nmA:\nCHAN A0 (Batt): " + gfCurrentA0_ + "\nCHAN A1 (24V) N/A reporting off" + "\nCHAN A2 (12V): " + gfCurrentA2_ + "\nCHAN A3 (5V): " + gfCurrentA3_ + "\nCHAN B0 (3.3V): " + gfCurrentB0_ + "\nCHAN B1 (3.15aV): " + gfCurrentB1_ + "\nCHAN B2 (3.15bV): " + gfCurrentB2_ + "\nCHAN B3 (GND): " + gfCurrentB3_ + "\nOPEN: " + gfCurrentOpen_ + "\nFull Scale: +/- 1 mA", Syslog::IMPORTANT ); } } // Checks to make sure value read is within bounds of circuit. Returns true if within range bool CBIT::checkGFBounds( float maxCurrent ) { // offsets intentionally left in for max current comparision. if( fabs( gfCurrentA0_ ) > maxCurrent || fabs( gfCurrentA1_ ) > maxCurrent || fabs( gfCurrentA2_ ) > maxCurrent || fabs( gfCurrentA3_ ) > maxCurrent || fabs( gfCurrentB0_ ) > maxCurrent || fabs( gfCurrentB1_ ) > maxCurrent || fabs( gfCurrentB2_ ) > maxCurrent || fabs( gfCurrentB3_ ) > maxCurrent ) { return false; } return true; } void CBIT::SetGFScanForce( bool forceVal ) { GFScanForce_ = forceVal; } short CBIT::waterDetected( void ) { float volts; short retVal = 0; volts = adWaterAlarmBow_.readVolts(); if( debug_ ) logger_.syslog( "Bow Water Detect:" + Str( volts ) + " V", Syslog::INFO ); if( volts > 0.5 ) { if( !waterDetectedCriticalSet_ ) { logger_.syslog( "WATER ALARM BOW.", Syslog::FAULT ); } retVal |= ( 1 << WATER_ALARM_BOW ); } volts = adWaterAlarmStern_.readVolts(); if( debug_ ) logger_.syslog( "Stern Water Detect:" + Str( volts ) + " V", Syslog::INFO ); if( volts > 0.5 ) { if( !waterDetectedCriticalSet_ ) { logger_.syslog( "WATER ALARM STERN.", Syslog::FAULT ); } retVal |= ( 1 << WATER_ALARM_STERN ); } volts = adWaterAlarmAux_.readVolts(); if( debug_ ) logger_.syslog( "Aux Water Detect:" + Str( volts ) + " V", Syslog::INFO ); if( volts > 0.5 ) { if( !waterDetectedCriticalSet_ ) { logger_.syslog( "WATER ALARM AUX.", Syslog::FAULT ); } retVal |= ( 1 << WATER_ALARM_AUX ); } return retVal; } // Initialize battery pack values CBIT::BattPack::BattPack() : voltage_( 0.0 ), battVoltageReader_( NULL ), current_( 0.0 ), battCurrentReader_( NULL ), temp_( 0.0 ), battTempReader_( NULL ) {}