/** \file * * Contains the VerticalControl class implementation. * * Copyright (c) 2007,2008,2009 MBARI * MBARI Proprietary Information. All Rights Reserved */ #include "VerticalControl.h" #include "VerticalControlIF.h" #include #include #include "controlModule/LoopControlIF.h" #include "controlModule/SpeedControlIF.h" #include "data/ConfigReader.h" #include "data/Location.h" #include "data/Slate.h" // Temporary include to get Slate::ReadOnce #include "data/StrValue.h" #include "data/UniversalDataReader.h" #include "data/UniversalURI.h" #include "io/LPC3Reg.h" #include "servoModule/ElevatorServoIF.h" #include "supervisor/Supervisor.h" #include "units/Units.h" #include "utils/AuvMath.h" VerticalControl::VerticalControl( const Module* module ) : SyncControlComponent( VerticalControlIF::NAME, module ), elevatorPitchIntegral_( nanf( "" ) ), massPitchIntegral_( nanf( "" ) ), massFilter_( NULL ), massFilterPos_( 0 ), massFilterBins_( 0 ), lastPitchCmd_( nanf( "" ) ), lastPitchChange_( Timestamp::NOT_SET_TIME ), depthIntegral_( nanf( "" ) ), depth2buoyInt_( nanf( "" ) ), lastDepthRateCmd_( nanf( "" ) ), depthTrajectory_( false ), buoyancyPosition_( nanf( "" ) ), lastBuoyancyPosition_( nanf( "" ) ), lastPitch_( nanf( "" ) ), lastDepthCmd_( nanf( "" ) ), periodCmd_( 0.0f ), speedCmd_( 0.0f ), previouslyBelowExcursion_( false ), depthRateFilter_( NULL ), depthRateFilterIndex_( 0 ), failToGoDownStart_( Timestamp::NOT_SET_TIME ), failToGoUpStart_( Timestamp::NOT_SET_TIME ), failToGoDownDepth_( nanf( "" ) ), failToGoUpDepth_( nanf( "" ) ), missionEnded_( false ), burnwireActivated_( false ), depthRateSamples_( 0 ) { logger_.syslog( "Construct VerticalControl." ); lastBuoyancyCmd_ = buoyancyDefault_; // Input settings from the slate verticalModeReader_ = newDataReader( VerticalControlIF::VERTICAL_MODE ); //, this, Units::ENUM( VerticalControlIF::SURFACE ) ); depthCmdReader_ = newDataReader( VerticalControlIF::DEPTH_CMD ); //, this, Units::METER() ); depthRateCmdReader_ = newDataReader( VerticalControlIF::DEPTH_RATE_CMD ); //, this, Units::METER_PER_SECOND() ); pitchCmdReader_ = newDataReader( VerticalControlIF::PITCH_CMD ); //, this, Units::RADIAN( 0.0 ) ); pitchRateCmdReader_ = newDataReader( VerticalControlIF::PITCH_RATE_CMD ); //, this, Units::RADIAN_PER_SECOND( 0.2 ) ); buoyancyCmdReader_ = newDataReader( VerticalControlIF::BUOYANCY_CMD ); //, this, Units::CUBIC_CENTIMETER( nan( "" ) ) ); massPositionCmdReader_ = newDataReader( VerticalControlIF::MASS_POSITION_CMD ); //, this, Units::METER() ); elevatorAngleCmdReader_ = newDataReader( VerticalControlIF::ELEVATOR_ANGLE_CMD ); //, this, Units::RADIAN() ); /// Other controllers' Command readers periodCmdReader_ = newDataReader( LoopControlIF::PERIOD_CMD ); //, this, Units::SECOND() ); speedCmdReader_ = newDataReader( SpeedControlIF::SPEED_CMD ); //, this, Units::METER_PER_SECOND() ); /// Config readers buoyancyDefaultCfgReader_ = newConfigReader( VerticalControlIF::BUOYANCY_DEFAULT_CFG ); buoyancyLimitHiCCCfgReader_ = newConfigReader( VerticalControlIF::BUOYANCY_LIMIT_HI_CC_CFG ); buoyancyLimitLoCCCfgReader_ = newConfigReader( VerticalControlIF::BUOYANCY_LIMIT_LO_CC_CFG ); buoyancyNeutralCfgReader_ = newConfigReader( VerticalControlIF::BUOYANCY_NEUTRAL_CFG ); //, this, Units::CUBIC_CENTIMETER() ); buoyancyPumpDepthCfgReader_ = newConfigReader( VerticalControlIF::BUOYANCY_PUMP_DEPTH_CFG ); depthDeadbandCfgReader_ = newConfigReader( VerticalControlIF::DEPTH_DEADBAND_CFG ); //, this, Units::METER() ); depthRateDeadbandCfgReader_ = newConfigReader( VerticalControlIF::DEPTH_RATE_DEADBAND_CFG ); depthRateSamplesCfgReader_ = newConfigReader( VerticalControlIF::DEPTH_RATE_SAMPLES_CFG ); dropWtDepthExcursionCfgReader_ = newConfigReader( VerticalControlIF::DROP_WEIGHT_DEPTH_EXCURSION_CFG ); dropWtOverrideDelayCfgReader_ = newConfigReader( VerticalControlIF::DROP_WEIGHT_OVERRIDE_DELAY_CFG ); elevatorDeadbandCfgReader_ = newConfigReader( VerticalControlIF::ELEVATOR_DEADBAND_CFG ); //, this, Units::RADIAN() ); elevatorLimitCfgReader_ = newConfigReader( VerticalControlIF::ELEVATOR_LIMIT_CFG ); //, this, Units::RADIAN() ); elevatorTurnTimeCfgReader_ = newConfigReader( VerticalControlIF::ELEVATOR_TURN_TIME_CFG ); excursionDepthTimeoutCfgReader_ = newConfigReader( VerticalControlIF::EXCURSION_DEPTH_TIMEOUT_CFG ); kdDepthBuoyCfgReader_ = newConfigReader( VerticalControlIF::KD_DEPTH_BUOY_CFG ); //, this, Units::SECOND() ); kdDepthCfgReader_ = newConfigReader( VerticalControlIF::KD_DEPTH_CFG ); //, this, Units::RADIAN_SECOND_PER_METER() ); kdDepthRateBuoyCfgReader_ = newConfigReader( VerticalControlIF::KD_DEPTH_RATE_BUOY_CFG ); //, this, Units::SECOND() ); kdPitchElevatorCfgReader_ = newConfigReader( VerticalControlIF::KD_PITCH_ELEVATOR_CFG ); //, this, Units::SECOND() ); kdPitchMassCfgReader_ = newConfigReader( VerticalControlIF::KD_PITCH_MASS_CFG ); //, this, Units::SECOND() ); kiDepthBuoyCfgReader_ = newConfigReader( VerticalControlIF::KI_DEPTH_BUOY_CFG ); //, this, Units::RECIPROCAL_SECOND() ); kiDepthCfgReader_ = newConfigReader( VerticalControlIF::KI_DEPTH_CFG ); //, this, Units::RADIAN_PER_SECOND_PER_METER() ); kiDepthOffCfgReader_ = newConfigReader( VerticalControlIF::KI_DEPTH_OFF_CFG ); //, this, Units::METER_PER_SECOND() ); kiDepthRateBuoyCfgReader_ = newConfigReader( VerticalControlIF::KI_DEPTH_RATE_BUOY_CFG ); //, this, Units::RECIPROCAL_SECOND() ); kiPitchElevatorCfgReader_ = newConfigReader( VerticalControlIF::KI_PITCH_ELEVATOR_CFG ); //, this, Units::RECIPROCAL_SECOND() ); kiPitchMassCfgReader_ = newConfigReader( VerticalControlIF::KI_PITCH_MASS_CFG ); //, this, Units::RECIPROCAL_SECOND() ); kpDepthBuoyCfgReader_ = newConfigReader( VerticalControlIF::KP_DEPTH_BUOY_CFG ); //, this, Units::RATIO() ); kpDepthCfgReader_ = newConfigReader( VerticalControlIF::KP_DEPTH_CFG ); //, this, Units::RADIAN_PER_METER() ); kpDepthRateBuoyCfgReader_ = newConfigReader( VerticalControlIF::KP_DEPTH_RATE_BUOY_CFG ); //, this, Units::RATIO() ); kpPitchElevatorCfgReader_ = newConfigReader( VerticalControlIF::KP_PITCH_ELEVATOR_CFG ); //, this, Units::RATIO() ); kpPitchMassCfgReader_ = newConfigReader( VerticalControlIF::KP_PITCH_MASS_CFG ); //, this, Units::RATIO() ); limitDepthTrajectoryCfgReader_ = newConfigReader( VerticalControlIF::LIMIT_DEPTH_TRAJECTORY_CFG ); massDeadbandCfgReader_ = newConfigReader( VerticalControlIF::MASS_DEADBAND_CFG ); //, this, Units::METER() ); massDefaultCfgReader_ = newConfigReader( VerticalControlIF::MASS_DEFAULT_CFG ); //, this, Units::METER() ); massFilterLimitCfgReader_ = newConfigReader( VerticalControlIF::MASS_FILTER_LIMIT_CFG ); //, this, Units::RADIAN() ); massFilterWidthCfgReader_ = newConfigReader( VerticalControlIF::MASS_FILTER_WIDTH_CFG ); //, this, Units::SECOND() ); massPositionLimitFwdCfgReader_ = newConfigReader( VerticalControlIF::MASS_POSITION_LIMIT_FWD_CFG ); //, this, Units::METER() ); massPositionLimitAftCfgReader_ = newConfigReader( VerticalControlIF::MASS_POSITION_LIMIT_AFT_CFG ); //, this, Units::METER() ); massTurnTimeCfgReader_ = newConfigReader( VerticalControlIF::MASS_TURN_TIME_CFG ); maxBuoyDiveAccelCfgReader_ = newConfigReader( VerticalControlIF::MAX_BUOY_DIVE_ACCEL_CFG ); //, this, Units::METER_PER_SECOND_SQUARED() ); maxBuoyDiveRateCfgReader_ = newConfigReader( VerticalControlIF::MAX_BUOY_DIVE_RATE_CFG ); maxBuoyIntCfgReader_ = newConfigReader( VerticalControlIF::MAX_BUOY_INT_CFG ); //, this, Units::CUBIC_CENTIMETER() ); maxDepthIntCfgReader_ = newConfigReader( VerticalControlIF::MAX_DEPTH_INT_CFG ); //, this, Units::RADIAN() ); maxDiveAccelCfgReader_ = newConfigReader( VerticalControlIF::MAX_DIVE_ACCEL_CFG ); //, this, Units::METER_PER_SECOND_SQUARED() ); maxDiveRateCfgReader_ = newConfigReader( VerticalControlIF::MAX_DIVE_RATE_CFG ); //, this, Units::METER_PER_SECOND() ); maxPitchElevatorIntCfgReader_ = newConfigReader( VerticalControlIF::MAX_PITCH_ELEVATOR_INT_CFG ); //, this, Units::RADIAN() ); maxPitchMassIntCfgReader_ = newConfigReader( VerticalControlIF::MAX_PITCH_MASS_INT_CFG ); //, this, Units::METER() ); maxPitchRateCfgReader_ = newConfigReader( VerticalControlIF::MAX_PITCH_RATE_CFG ); minAscendPitchCfgReader_ = newConfigReader( VerticalControlIF::MIN_ASCEND_PITCH_CFG ); minDepthExcursionCfgReader_ = newConfigReader( VerticalControlIF::MIN_DEPTH_EXCURSION_CFG ); nominalDtCfgReader_ = newConfigReader( LoopControlIF::NOMINAL_DT_CFG ); pitchLimitCfgReader_ = newConfigReader( VerticalControlIF::PITCH_LIMIT_CFG ); //, this, Units::RADIAN() ); stopDepthExcursionCfgReader_ = newConfigReader( VerticalControlIF::STOP_DEPTH_EXCURSION_CFG ); stopOverrideDelayCfgReader_ = newConfigReader( VerticalControlIF::STOP_OVERRIDE_DELAY_CFG ); stopOverrideDelayBuoyCfgReader_ = newConfigReader( VerticalControlIF::STOP_OVERRIDE_DELAY_BUOY_CFG ); surfaceThresholdCfgReader_ = newConfigReader( VerticalControlIF::SURFACE_THRESHOLD_CFG ); useElevIntInDepthModeCfgReader_ = newConfigReader( VerticalControlIF::USE_ELEV_INT_IN_DEPTH_MODE ); // Servo Config elevDeviationCfgReader_ = newConfigReader( ElevatorServoIF::DEVIATION_ANGLE ); // Measurements from the slate depthReader_ = newUniversalReader( UniversalURI::DEPTH ); //, this, Units::METER( 0.0 ) ); pitchReader_ = newUniversalReader( UniversalURI::PLATFORM_PITCH_ANGLE ); //, this, Units::RADIAN( 0.0 ) ); pitchRateReader_ = newUniversalReader( UniversalURI::PLATFORM_PITCH_RATE ); //, this, Units::RADIAN_PER_SECOND( 0.0 ) ); depthRateReader_ = newUniversalReader( UniversalURI::DEPTH_RATE ); //, this, Units::METER_PER_SECOND( 0.0 ) ); elevatorAngleReader_ = newUniversalReader( UniversalURI::PLATFORM_ELEVATOR_ANGLE ); //, this, Units::RADIAN( 0.0 ) ); massPositionReader_ = newUniversalReader( UniversalURI::PLATFORM_MASS_POSITION ); //, this, Units::METER( 0.0 ) ); buoyancyPositionReader_ = newUniversalReader( UniversalURI::PLATFORM_BUOYANCY_POSITION ); //, this, Units::CUBIC_CENTIMETER( 0.0 ) ); // "Internal" Outputs from here to the slate depth2buoyIntInternalWriter_ = newDataWriter( VerticalControlIF::DEPTH2BUOYINT_INTERNAL ); //, this, Units::CUBIC_CENTIMETER( 0.0 ) ); depthErrorInternalWriter_ = newDataWriter( VerticalControlIF::DEPTH_ERROR_INTERNAL ); //, this, Units::METER( 0.0 ) ); depthIntegralInternalWriter_ = newDataWriter( VerticalControlIF::DEPTH_INTEGRAL_INTERNAL ); //, this, Units::RADIAN( 0.0 ) ); dtWriter_ = newDataWriter( VerticalControlIF::DT_INTERNAL ); //, this, Units::SECOND() ); elevatorIntegralWriter_ = newDataWriter( VerticalControlIF::ELEVATOR_INTEGRAL_INTERNAL ); //, this, Units::RADIAN( 0.0 ) ); massIntegralWriter_ = newDataWriter( VerticalControlIF::MASS_INTEGRAL_INTERNAL ); //, this, Units::METER( 0.0 ) ); massPitchErrorWriter_ = newDataWriter( VerticalControlIF::MASS_PITCH_ERROR_INTERNAL ); //, this, Units::RADIAN( 0.0 ) ); pitchInternalWriter_ = newDataWriter( VerticalControlIF::PITCH_INTERNAL ); //, this, Units::RADIAN( 0.0 ) ); smoothDepthInternalWriter_ = newDataWriter( VerticalControlIF::SMOOTH_DEPTH_INTERNAL ); //, this, Units::METER( 0.0 ) ); // Special output from here to the slate to SpeedControl speedCmdWriter_ = newDataWriter( SpeedControlIF::SPEED_CMD ); //, this ); // Outputs from here to the slate to Servos elevatorAngleActionWriter_ = newDataWriter( VerticalControlIF::ELEVATOR_ANGLE_ACTION ); //, this, Units::RADIAN( 0.0 ) ); massPositionActionWriter_ = newDataWriter( VerticalControlIF::MASS_POSITION_ACTION ); //, this, Units::METER( 0.0 ) ); buoyancyActionWriter_ = newDataWriter( VerticalControlIF::BUOYANCY_ACTION ); //, this, Units::CUBIC_CENTIMETER( buoyancyDefault_ ) ); // For reading last positions, even if from Maintain command elevatorAngleActionReader_ = newDataReader( VerticalControlIF::ELEVATOR_ANGLE_ACTION ); //, this, Units::RADIAN( 0.0 ) ); massPositionActionReader_ = newDataReader( VerticalControlIF::MASS_POSITION_ACTION ); //, this, Units::METER( 0.0 ) ); verticalMode_ = VerticalControlIF::FLOAT_ON_SURFACE; } VerticalControl::~VerticalControl() { if( depthRateFilter_ != NULL ) { delete[] depthRateFilter_; } if( NULL != massFilter_ ) { delete[] massFilter_; } } // Initialize function void VerticalControl::initialize( void ) { logger_.syslog( "Initialize VerticalControlComponent." ); excursionStartTime_ = Timestamp::Now(); buoyancyDefault_ = nanf( "" ); ok_ = readConfig(); if( isnan( buoyancyDefault_ ) ) { buoyancyDefault_ = 950.0; } if( !ok_ ) { logger_.syslog( Str( "Error: Error loading parameters in initialization routine. Returning.\n" ), Syslog::CRITICAL ); } } /// The actual "payload" of the component void VerticalControl::run() { if( !ok_ ) { logger_.syslog( Str( "Reinitializing VerticalControl.\n" ), Syslog::ERROR ); initialize(); } if( !ok_ || !verticalModeReader_->isActive() ) { verticalMode_ = VerticalControlIF::FLOAT_ON_SURFACE; } if( ok_ ) { controlVertical(); } /// Apply buoyancy or drop weight overrides if necessary. overrideVertical(); depthIntegralInternalWriter_->write( Units::RADIAN, depthIntegral_ ); depth2buoyIntInternalWriter_->write( Units::CUBIC_CENTIMETER, depth2buoyInt_ ); massIntegralWriter_ ->write( Units::METER, massPitchIntegral_ ); elevatorIntegralWriter_->write( Units::RADIAN, elevatorPitchIntegral_ ); } /// Uninit function void VerticalControl::uninitialize( void ) { logger_.syslog( "Uninitialize VerticalControlComponent." ); } ////Private Methods /// Load VerticalControl parameters bool VerticalControl::readConfig( void ) { // Check if all the parameters are read correctly bool ok = true; float dropWtOverrideDelay( nanf( "" ) ); float excursionDepthTimeout( nanf( "" ) ); float stopOverrideDelay( nanf( "" ) ); float stopOverrideDelayBuoy( nanf( "" ) ); int depthRateSamples( depthRateSamples_ ); ok &= buoyancyDefaultCfgReader_->read( Units::CUBIC_CENTIMETER, buoyancyDefault_ ); ok &= buoyancyLimitHiCCCfgReader_->read( Units::CUBIC_CENTIMETER, buoyancyLimitHiCC_ ); ok &= buoyancyLimitLoCCCfgReader_->read( Units::CUBIC_CENTIMETER, buoyancyLimitLoCC_ ); ok &= buoyancyNeutralCfgReader_->read( Units::CUBIC_CENTIMETER, buoyancyNeutral_ ); ok &= buoyancyPumpDepthCfgReader_->read( Units::METER, buoyancyPumpDepth_ ); ok &= depthDeadbandCfgReader_->read( Units::METER, depthDeadband_ ); ok &= depthRateDeadbandCfgReader_->read( Units::METER_PER_SECOND, depthRateDeadband_ ); ok &= depthRateSamplesCfgReader_->read( Units::COUNT, depthRateSamples_ ); ok &= dropWtDepthExcursionCfgReader_->read( Units::METER, dropWtDepthExcursion_ ); ok &= dropWtOverrideDelayCfgReader_->read( Units::METER, dropWtOverrideDelay ); ok &= elevatorDeadbandCfgReader_->read( Units::RADIAN, elevDeadband_ ); ok &= elevatorLimitCfgReader_->read( Units::RADIAN, elevatorLimit_ ); ok &= elevatorTurnTimeCfgReader_->read( Units::RADIAN, elevatorTurnTime_ ); ok &= excursionDepthTimeoutCfgReader_->read( Units::SECOND, excursionDepthTimeout ); ok &= kdDepthCfgReader_->read( Units::RADIAN_SECOND_PER_METER, kdDepth_ ); ok &= kdDepthBuoyCfgReader_->read( Units::SECOND, kdDepthBuoy_ ); ok &= kdDepthRateBuoyCfgReader_->read( Units::SECOND, kdDepthRateBuoy_ ); ok &= kdPitchElevatorCfgReader_->read( Units::SECOND, kdPitchElevator_ ); ok &= kdPitchMassCfgReader_->read( Units::SECOND, kdPitchMass_ ); ok &= kiDepthCfgReader_->read( Units::RADIAN_PER_SECOND_PER_METER, kiDepth_ ); ok &= kiDepthBuoyCfgReader_->read( Units::RECIPROCAL_SECOND, kiDepthBuoy_ ); ok &= kiDepthOffCfgReader_->read( Units::METER_PER_SECOND, kiDepthOff_ ); ok &= kiDepthRateBuoyCfgReader_->read( Units::RECIPROCAL_SECOND, kiDepthRateBuoy_ ); ok &= kiPitchElevatorCfgReader_->read( Units::RECIPROCAL_SECOND, kiPitchElevator_ ); ok &= kiPitchMassCfgReader_->read( Units::RECIPROCAL_SECOND, kiPitchMass_ ); ok &= kpDepthCfgReader_->read( Units::RADIAN_PER_METER, kpDepth_ ); ok &= kpDepthBuoyCfgReader_->read( Units::RATIO, kpDepthBuoy_ ); ok &= kpDepthRateBuoyCfgReader_->read( Units::RATIO, kpDepthRateBuoy_ ); ok &= kpPitchElevatorCfgReader_->read( Units::RATIO, kpPitchElevator_ ); ok &= kpPitchMassCfgReader_->read( Units::RATIO, kpPitchMass_ ); ok &= limitDepthTrajectoryCfgReader_->read( Units::BOOL, limitDepthTrajectory_ ); ok &= massDeadbandCfgReader_->read( Units::METER, massDeadband_ ); ok &= massDefaultCfgReader_->read( Units::METER, massDefault_ ); ok &= massFilterLimitCfgReader_->read( Units::RADIAN, massFilterLimit_ ); ok &= massFilterWidthCfgReader_->read( Units::SECOND, massFilterWidth_ ); ok &= massPositionLimitFwdCfgReader_->read( Units::METER, massPositionLimitFwd_ ); ok &= massPositionLimitAftCfgReader_->read( Units::METER, massPositionLimitAft_ ); ok &= massTurnTimeCfgReader_->read( Units::SECOND, massTurnTime_ ); ok &= maxBuoyDiveAccelCfgReader_->read( Units::METER_PER_SECOND_SQUARED, maxBuoyDiveAccel_ ); ok &= maxBuoyDiveRateCfgReader_->read( Units::METER_PER_SECOND, maxBuoyDiveRate_ ); ok &= maxBuoyIntCfgReader_->read( Units::CUBIC_CENTIMETER, maxBuoyInt_ ); ok &= maxDepthIntCfgReader_->read( Units::RADIAN, maxDepthInt_ ); ok &= maxDiveAccelCfgReader_->read( Units::METER_PER_SECOND_SQUARED, maxDiveAccel_ ); ok &= maxDiveRateCfgReader_->read( Units::METER_PER_SECOND, maxDiveRate_ ); ok &= maxPitchElevatorIntCfgReader_->read( Units::RADIAN, maxPitchElevatorInt_ ); ok &= maxPitchMassIntCfgReader_->read( Units::METER, maxPitchMassInt_ ); ok &= maxPitchRateCfgReader_->read( Units::RADIAN_PER_SECOND, maxPitchRate_ ); ok &= minAscendPitchCfgReader_->read( Units::RADIAN, minAscendPitchCfg_ ); ok &= minDepthExcursionCfgReader_->read( Units::METER, minDepthExcursion_ ); ok &= nominalDtCfgReader_->read( Units::SECOND, nominalDt_ ); ok &= pitchLimitCfgReader_->read( Units::RADIAN, pitchLimit_ ); ok &= stopDepthExcursionCfgReader_->read( Units::METER, stopDepthExcursion_ ); ok &= stopOverrideDelayCfgReader_->read( Units::SECOND, stopOverrideDelay ); ok &= stopOverrideDelayBuoyCfgReader_->read( Units::SECOND, stopOverrideDelayBuoy ); ok &= surfaceThresholdCfgReader_->read( Units::METER, surfaceThreshold_ ); ok &= useElevIntInDepthModeCfgReader_->read( Units::BOOL, useElevIntInDepthMode_ ); // Servo ok &= elevDeviationCfgReader_->read( Units::RADIAN, elevDeviation_ ); dropWtOverrideDelay_ = Timespan::Seconds( dropWtOverrideDelay ); excursionDepthTimeout_ = Timespan::Seconds( excursionDepthTimeout ); stopOverrideDelay_ = Timespan::Seconds( stopOverrideDelay ); stopOverrideDelayBuoy_ = Timespan::Seconds( stopOverrideDelayBuoy ); if( depthRateSamples_ != depthRateSamples ) { if( depthRateFilter_ != NULL ) { delete[] depthRateFilter_; } depthRateFilter_ = new float[ depthRateSamples_ ]; for( int i = 0; i < depthRateSamples_; ++i ) { depthRateFilter_[i] = 0; } depthRateFilterIndex_ = 0; } return ok; } void VerticalControl::controlVertical( void ) { if( verticalModeReader_-> isActive() && verticalModeReader_->wasTouchedSinceLastRun( this ) ) { int verticalMode; verticalModeReader_->read( Units::ENUM, verticalMode ); verticalMode_ = ( VerticalControlIF::VerticalMode )verticalMode; } else { if( Supervisor::IsMissionManagerFailed() ) { verticalMode_ = VerticalControlIF::FLOAT_ON_SURFACE; } else { verticalMode_ = VerticalControlIF::NONE; } } // // If buoyancyCmd_ is not being actively set by a behavior, set it to // nanf(""), in which case setBuoyancy() will reset it to // buoyancyDefault_ before writing it to buoyancyAction. // // setBuoyancy(buoyancyCmd_) is just a pass-through of a constant (or // step) specified by the behavior (Cmd) to the EZServeo (Action), except // in DEPTH mode where controlDepth() has the opportunity to dynamically // modify buoyancyCmd_. // // In DEPTH mode, if buoyancyCmd_ is NaN, controlDepth() asssumes the // mission author desires active buoyancy control and activates the // control loop. In this case setBuoyancy() does not write // buoyancyDefault_. // buoyancyCmd_ = nanf( "" ); if( buoyancyCmdReader_-> isActive() && buoyancyCmdReader_->wasTouchedSinceLastRun( this ) ) { buoyancyCmdReader_->read( Units::CUBIC_CENTIMETER, buoyancyCmd_ ); } lastBuoyancyPosition_ = buoyancyPosition_; if( buoyancyPositionReader_-> isActive() ) { buoyancyPositionReader_->read( Units::CUBIC_CENTIMETER, buoyancyPosition_ ); } float oldPeriod = periodCmd_; if( !periodCmdReader_->wasTouchedSinceLastRun( this ) || !periodCmdReader_->read( Units::METER_PER_SECOND, periodCmd_ ) || isnan( periodCmd_ ) ) { periodCmd_ = nominalDt_; } if( speedCmdReader_->wasTouchedSinceLastRun( this ) ) { speedCmdReader_->read( Units::METER_PER_SECOND, speedCmd_ ); } else { speedCmd_ = 0.0f; } float oldWidth = massFilterWidth_; readConfig(); if( oldWidth != massFilterWidth_ || oldPeriod != periodCmd_ ) { if( NULL != massFilter_ ) { delete[] massFilter_; } massFilterBins_ = ( int )ceilf( massFilterWidth_ / periodCmd_ ); massFilter_ = new float[massFilterBins_]; for( int i = 0; i < massFilterBins_; ++i ) { massFilter_[i] = nanf( "" ); } massFilterPos_ = 0; } float depthCmd( nanf( "" ) ), depthRateCmd( nanf( "" ) ), pitchCmd( nanf( "" ) ), pitchRateCmd( nanf( "" ) ); switch( verticalMode_ ) { case VerticalControlIF::NONE: depthTrajectory_.setXDot( 0.0f ); depthIntegral_ = nanf( "" ); depth2buoyInt_ = nanf( "" ); elevatorPitchIntegral_ = nanf( "" ); massPitchIntegral_ = nanf( "" ); setMassAndElevator( massDefault_, nanf( "" ) ); setBuoyancy( buoyancyCmd_ ); break; case VerticalControlIF::SURFACE: setBuoyancy( buoyancyLimitHiCC_ ); controlDepth( 0.0 ); break; case VerticalControlIF::DEPTH: if( depthCmdReader_->read( Units::METER, depthCmd ) && !isnan( depthCmd ) ) { controlDepth( depthCmd ); } else { controlDepth( 0.0f ); } setBuoyancy( buoyancyCmd_ ); break; case VerticalControlIF::DEPTH_RATE: depthTrajectory_.setXDot( 0.0f ); if( pitchCmdReader_->wasTouchedSinceLastRun( this ) ) { pitchCmdReader_->read( Units::RADIAN, pitchCmd ); } if( depthRateCmdReader_->read( Units::METER_PER_SECOND, depthRateCmd ) && !isnan( depthRateCmd ) ) { controlDepthRate( depthRateCmd, pitchCmd ); } else { controlDepthRate( 0.0f, pitchCmd ); } setBuoyancy( buoyancyCmd_ ); break; case VerticalControlIF::PITCH_ZERO: depthTrajectory_.setXDot( 0.0f ); setBuoyancy( buoyancyCmd_ ); depthIntegral_ = nanf( "" ); depth2buoyInt_ = nanf( "" ); setPitch( 0.0 ); break; case VerticalControlIF::PITCH: depthTrajectory_.setXDot( 0.0f ); setBuoyancy( buoyancyCmd_ ); depthIntegral_ = nanf( "" ); depth2buoyInt_ = nanf( "" ); if( pitchCmdReader_->wasTouchedSinceLastRun( this ) && pitchCmdReader_->read( Units::RADIAN, pitchCmd ) && !isnan( pitchCmd ) ) { setPitch( pitchCmd ); } else { setPitch( 0.0 ); } break; case VerticalControlIF::PITCH_RATE: depthTrajectory_.setXDot( 0.0f ); setBuoyancy( buoyancyCmd_ ); depthIntegral_ = nanf( "" ); depth2buoyInt_ = nanf( "" ); if( pitchRateCmdReader_->wasTouchedSinceLastRun( this ) && pitchRateCmdReader_->read( Units::RADIAN_PER_SECOND, pitchRateCmd ) && !isnan( pitchRateCmd ) ) { setPitchRate( pitchRateCmd ); } else { setPitch( 0.0 ); } break; case VerticalControlIF::MASS_AND_ELEVATOR: depthTrajectory_.setXDot( 0.0f ); depthIntegral_ = nanf( "" ); depth2buoyInt_ = nanf( "" ); elevatorPitchIntegral_ = nanf( "" ); massPitchIntegral_ = nanf( "" ); setBuoyancy( buoyancyCmd_ ); { float massPositionCmd( nanf( "" ) ); if( massPositionCmdReader_->isActive() && massPositionCmdReader_->wasTouchedSinceLastRun( this ) ) { massPositionCmdReader_->read( Units::METER, massPositionCmd ); } float elevatorAngleCmd( nanf( "" ) ); if( elevatorAngleCmdReader_->isActive() && elevatorAngleCmdReader_->wasTouchedSinceLastRun( this ) ) { elevatorAngleCmdReader_->read( Units::RADIAN, elevatorAngleCmd ); } setMassAndElevator( massPositionCmd, elevatorAngleCmd ); } break; case VerticalControlIF::FLOAT_ON_SURFACE: floatAtSurface(); break; default: logger_.syslog( "Error: Invalid depth mode specified (" + Str( verticalMode_ ) + ") - Floating at surface.", Syslog::ERROR ); floatAtSurface(); break; } } void VerticalControl::floatAtSurface() { float depth( nanf( "" ) ); if( ! depthReader_->read( Units::METER, depth ) ) { depth = nanf( "" ); } if( isnan( depth ) || depth <= buoyancyPumpDepth_ ) { setBuoyancy( buoyancyLimitHiCC_ ); if( isnan( depth ) ) { setMassAndElevator( massPositionLimitAft_, -elevatorLimit_ ); } else if( depth <= surfaceThreshold_ ) { depthIntegral_ = nanf( "" ); depth2buoyInt_ = nanf( "" ); elevatorPitchIntegral_ = nanf( "" ); massPitchIntegral_ = nanf( "" ); setMassAndElevator( massDefault_, 0.0 ); } else { controlDepth( 0.0 ); } } else { controlDepth( 0.0 ); } } void VerticalControl::controlDepth( float depthCmd ) { float dt( AuvMath::NanZero( dt_.asFloat() ) ); float depth( 0.0 ); float depthRate( 0 ); float smoothDepth; float depthError; float buoyDepthError; depthCmd = AuvMath::NanZero( depthCmd ); // // Zero the pitch-to-elevator integrator, but leave the pitch-to-mass on. if( !useElevIntInDepthMode_ ) { elevatorPitchIntegral_ = nanf( "" ); } depthReader_->read( Units::METER, depth ); if( isnan( depth ) || ( depth < 0 && depthCmd <= 0.0 ) ) { floatAtSurface(); return; } depthRateReader_->read( Units::METER_PER_SECOND, depthRate ); bool activePitchCmd( false ); float pitchCmd( nanf( "" ) ); if( !pitchCmdReader_->wasTouchedSinceLastRun( this ) || !pitchCmdReader_->read( Units::RADIAN, pitchCmd ) || isnan( pitchCmd ) ) { pitchCmd = pitchLimit_; } else { pitchCmd = AuvMath::Limit( fabsf( pitchCmd ), 0.0f, pitchLimit_ ); activePitchCmd = true; } float depthRateCmd; if( !depthRateCmdReader_->wasTouchedSinceLastRun( this ) || !depthRateCmdReader_->read( Units::METER_PER_SECOND, depthRateCmd ) || isnan( depthRateCmd ) ) { depthRateCmd = maxDiveRate_ * speedCmd_ / SpeedControlIF::SPEED_FAST * sin( pitchCmd ) / sin( pitchLimit_ ) + ( speedCmd_ == 0 ? maxBuoyDiveRate_ : 0.0f ); } float maxDiveAccel( speedCmd_ == 0 ? maxBuoyDiveAccel_ : maxDiveAccel_ ); if( limitDepthTrajectory_ && ( isnan( lastDepthCmd_ ) || fabs( lastDepthCmd_ - depthCmd ) > 1.0f ) ) { depthTrajectory_.setX( depth ); depthTrajectory_.setXDot( 0.0 ); } smoothDepth = depthTrajectory_.project( depth, dt, depthCmd, depthRateCmd, maxDiveAccel ); depthError = depth - smoothDepth; // This makse sure that we can get to the surface in depth mode! float depthDeadband( AuvMath::Min( depthCmd, depthDeadband_ ) ); if( fabs( smoothDepth - depthCmd ) < depthDeadband ) { if( fabs( depthError ) > depthDeadband ) { if( depthError >= depthDeadband ) { depthError -= depthDeadband; } else { depthError += depthDeadband; } } else if( depth > surfaceThreshold_ && depth > depthDeadband ) { depthError = 0.0f; } } if( limitDepthTrajectory_ && depth < surfaceThreshold_ && depthCmd > surfaceThreshold_ && smoothDepth > surfaceThreshold_ ) { // Until we've finally started diving, let's slow down our depth error. depthTrajectory_.setX( depth + surfaceThreshold_ * 2 ); } if( !isnan( buoyancyCmd_ ) ) { // Buoyancy is already specified. Leave buoyancyCmd_ alone. depth2buoyInt_ = 0.; if( depthError != 0.0f ) { if( !isnan( depthIntegral_ ) && fabs( depthTrajectory_.getXDot() ) < kiDepthOff_ ) { depthIntegral_ += kiDepth_ * dt * depthError; depthIntegral_ = AuvMath::Limit( depthIntegral_, maxDepthInt_, -maxDepthInt_ ); } else { depthIntegral_ = 0.; } } setPitch( AuvMath::Limit( kpDepth_ * ( float )sqrt( nominalDt_ / dt ) * depthError + AuvMath::NanZero( depthIntegral_ ) + kdDepth_ * ( float )sqrt( nominalDt_ / dt ) * depthRate, -pitchLimit_, pitchLimit_ ) ); } else { // Buoyancy is not specified. Close the loop. depthIntegral_ = 0.; buoyDepthError = depth - depthCmd; // Here we use a separate depth error for just buoyancy control. Simply the actual difference between commanded and actual with no smoothing. float minBuoyInt, maxBuoyInt; minBuoyInt = buoyancyLimitLoCC_ - buoyancyNeutral_; if( minBuoyInt < -maxBuoyInt_ ) minBuoyInt = -maxBuoyInt_; maxBuoyInt = buoyancyLimitHiCC_ - buoyancyNeutral_; if( maxBuoyInt > maxBuoyInt_ ) maxBuoyInt = maxBuoyInt_; if( buoyDepthError != 0.0f ) { if( !isnan( depth2buoyInt_ ) && fabs( depthTrajectory_.getXDot() ) < kiDepthOff_ ) { depth2buoyInt_ += buoyDepthError * sqrt( nominalDt_ / dt ) * kiDepthBuoy_ * dt; depth2buoyInt_ = AuvMath::Limit( depth2buoyInt_, minBuoyInt, maxBuoyInt ); } else { depth2buoyInt_ = 0.; } buoyancyCmd_ = kpDepthBuoy_ * sqrt( nominalDt_ / dt ) * buoyDepthError + depth2buoyInt_ + kdDepthBuoy_ * sqrt( nominalDt_ / dt ) * depthRate + buoyancyNeutral_; //Because it's set to NaN above. lastBuoyancyCmd_ = buoyancyCmd_; } else { buoyancyCmd_ = lastBuoyancyCmd_; } if( speedCmd_ > 0.0f ) { setPitch( AuvMath::Limit( kpDepth_ * ( float )sqrt( nominalDt_ / dt ) * buoyDepthError + AuvMath::NanZero( depthIntegral_ ) + kdDepth_ * ( float )sqrt( nominalDt_ / dt ) * depthRate, -pitchLimit_, pitchLimit_ ) ); } else { float pitch( 0.0f ); pitchReader_->read( Units::RADIAN, pitch ); float pitchRate( 0.0f ); pitchRateReader_->read( Units::RADIAN_PER_SECOND, pitchRate ); pitchCmd = activePitchCmd ? pitchCmd : 0.0f; float massPositionAction = massControl( pitch, pitchCmd, pitchRate, true ); setMassAndElevator( massPositionAction, 0.0f ); } } // logger_.syslog( "depthIntegral_ = ", depthIntegral_ ); // logger_.syslog( "depth2buoyInt_ = ", depth2buoyInt_ ); // logger_.syslog( "maxBuoyInt_ = ", maxBuoyInt_ ); // logger_.syslog( "depthDeadband_ = ", depthDeadband_ ); // logger_.syslog( "kpDepthBuoy_ = ", kpDepthBuoy_ ); // logger_.syslog( "kiDepthBuoy_ = ", kiDepthBuoy_ ); // logger_.syslog( "kdDepthBuoy_ = ", kdDepthBuoy_ ); // logger_.syslog( "buoyancyNeutral_ = ", buoyancyNeutral_ ); smoothDepthInternalWriter_->write( Units::METER, smoothDepth ); depthErrorInternalWriter_->write( Units::METER, depthError ); lastDepthCmd_ = depthCmd; } void VerticalControl::controlDepthRate( float depthRateCmd, float pitchLimit ) { float dt( AuvMath::NanZero( dt_.asFloat() ) ); if( isnan( pitchLimit ) ) { pitchLimit = pitchLimit_; } float depth( nanf( "" ) ); if( !depthReader_->read( Units::METER, depth ) || isnan( depth ) ) { floatAtSurface(); return; } if( depth <= 0 && depthRateCmd < 0 ) { return; } if( isnan( buoyancyCmd_ ) ) { float minBuoyInt, maxBuoyInt; minBuoyInt = buoyancyLimitLoCC_ - buoyancyNeutral_; maxBuoyInt = buoyancyLimitHiCC_ - buoyancyNeutral_; if( minBuoyInt < -maxBuoyInt_ ) minBuoyInt = -maxBuoyInt_; if( maxBuoyInt > maxBuoyInt_ ) maxBuoyInt = maxBuoyInt_; float depthRate( nanf( "" ) ); if( !depthRateReader_->read( Units::METER_PER_SECOND, depthRate ) || isnan( depthRate ) ) { floatAtSurface(); return; } float depthRateError( depthRate - depthRateCmd ); if( !isnan( lastDepthRateCmd_ ) && lastDepthRateCmd_ == depthRateCmd && !isnan( depth2buoyInt_ ) && fabs( depthTrajectory_.getXDot() ) < kiDepthOff_ ) { depth2buoyInt_ += depthRateError * sqrt( nominalDt_ / dt ) * kiDepthRateBuoy_ * dt; depth2buoyInt_ = AuvMath::Limit( depth2buoyInt_, minBuoyInt, maxBuoyInt ); } else { depth2buoyInt_ = 0.; } buoyancyCmd_ = kpDepthRateBuoy_ * sqrt( nominalDt_ / dt ) * depthRateError + depth2buoyInt_ + kdDepthRateBuoy_ * sqrt( nominalDt_ / dt ) * depthRate + buoyancyNeutral_; //Because it's set to NaN above. lastBuoyancyCmd_ = buoyancyCmd_; } float pitch( 0.0f ); pitchReader_->read( Units::RADIAN, pitch ); float pitchRate( 0.0f ); pitchRateReader_->read( Units::RADIAN_PER_SECOND, pitchRate ); float massPositionAction( 0.0f ); float elevatorAngleAction( 0.0f ); float pitchCmd( 0.0f ); if( speedCmd_ > 0.0f ) { pitchCmd = AuvMath::Limit( ( float ) - asin( depthRateCmd / speedCmd_ ), -pitchLimit, pitchLimit ); massAndElevatorControl( pitch, pitchCmd, pitchRate, massPositionAction, elevatorAngleAction ); } else { massPositionAction = massControl( pitch, 0.0, pitchRate, true ); } setMassAndElevator( massPositionAction, elevatorAngleAction ); lastDepthRateCmd_ = depthRateCmd; } void VerticalControl::setPitch( float pitchCmd ) { float pitch( nanf( "" ) ); if( !pitchReader_->read( Units::RADIAN, pitch ) || isnan( pitch ) ) { // Assume the worst for the actual pitch of the vehicle pitch = -pitchLimit_; } float pitchRate( nanf( "" ) ); if( !pitchRateReader_->read( Units::RADIAN, pitchRate ) || isnan( pitchRate ) ) { // Assume the worst for the actual pitch rate of the vehicle pitchRate = -maxPitchRate_; } pitchInternalWriter_->write( Units::RADIAN, pitchCmd ); float massPositionAction( nanf( "" ) ); if( massPositionCmdReader_->isActive() && massPositionCmdReader_->wasTouchedSinceLastRun( this ) ) { float massPositionCmd( nan( "" ) ); if( massPositionCmdReader_->read( Units::METER, massPositionCmd ) && !isnan( massPositionCmd ) ) { massPositionAction = massPositionCmd; } } float elevatorAngleAction( nanf( "" ) ); if( elevatorAngleCmdReader_->isActive() && elevatorAngleCmdReader_->wasTouchedSinceLastRun( this ) ) { float elevatorAngleCmd( nan( "" ) ); if( elevatorAngleCmdReader_->read( Units::RADIAN, elevatorAngleCmd ) && !isnan( elevatorAngleCmd ) ) { elevatorAngleAction = elevatorAngleCmd; } } // Deal with failed (or missing?) elevator if( !elevatorAngleActionWriter_->isImplemented() ) { // Allow the mass to move, even if specified otherwise massPositionAction = nanf( "" ); // Stop the prop while we are pointing the wrong way if( ( pitchCmd > 0.0f && pitch < 0.0f ) || ( pitchCmd < 0.0f && pitch > 0.0f ) ) { speedCmdWriter_->write( Units::METER_PER_SECOND, 0.0f ); } } if( !isnan( pitchCmd ) && !isnan( pitch ) && !isnan( pitchRate ) ) { pitchCmd = AuvMath::Limit( pitchCmd, pitchLimit_, -pitchLimit_ ); if( isnan( massPositionAction ) && isnan( elevatorAngleAction ) ) { massAndElevatorControl( pitch, pitchCmd, pitchRate, massPositionAction, elevatorAngleAction ); } else if( isnan( massPositionAction ) ) { massPositionAction = massControl( pitch, pitchCmd, pitchRate ); } else if( isnan( elevatorAngleAction ) ) { elevatorAngleAction = elevatorControl( pitch, pitchCmd, pitchRate ); } } setMassAndElevator( massPositionAction, elevatorAngleAction ); } void VerticalControl::setPitchRate( float pitchRateCmd ) { float pitch( nanf( "" ) ); if( !pitchReader_->read( Units::RADIAN, pitch ) || isnan( pitch ) ) { // If we don't now the pitch, head up! pitch = pitchLimit_; } float dt( AuvMath::NanZero( dt_.asFloat() ) ); pitchRateCmd = AuvMath::Limit( pitchRateCmd, -maxPitchRate_, maxPitchRate_ ); setPitch( AuvMath::Limit( pitch + dt * pitchRateCmd, -pitchLimit_, pitchLimit_ ) ); } /// attempts to achieve the desired mass and elevator settings void VerticalControl::setMassAndElevator( float massPositionAction, float elevatorAngleAction ) { if( massPositionCmdReader_->isActive() && massPositionCmdReader_->wasTouchedSinceLastRun( this ) ) { float massPositionCmd( nan( "" ) ); if( massPositionCmdReader_->read( Units::METER, massPositionCmd ) ) { if( !isnan( massPositionCmd ) ) { massPositionAction = massPositionCmd; } } } if( elevatorAngleCmdReader_->isActive() && elevatorAngleCmdReader_->wasTouchedSinceLastRun( this ) ) { float elevatorAngleCmd( nan( "" ) ); if( elevatorAngleCmdReader_->read( Units::RADIAN, elevatorAngleCmd ) ) { if( !isnan( elevatorAngleCmd ) ) { elevatorAngleAction = elevatorAngleCmd; } } } if( !isnan( massPositionAction ) ) { massPositionAction = AuvMath::Limit( massPositionAction, massPositionLimitFwd_, massPositionLimitAft_ ); float lastMassPositionAction; if( !massPositionActionReader_->read( Units::METER, lastMassPositionAction ) || isnan( lastMassPositionAction ) || fabs( massPositionAction - lastMassPositionAction ) > massDeadband_ ) { massPositionActionWriter_->write( Units::METER, massPositionAction ); } } if( !isnan( elevatorAngleAction ) ) { float lastElevatorAngleAction; if( elevatorAngleActionReader_->read( Units::RADIAN, lastElevatorAngleAction ) && !isnan( lastElevatorAngleAction ) && fabs( elevatorAngleAction ) < elevatorLimit_ && fabs( elevatorAngleAction - lastElevatorAngleAction ) < elevDeadband_ ) { if( AuvMath::Sign( elevatorAngleAction ) != AuvMath::Sign( lastElevatorAngleAction ) ) { elevatorAngleAction = 0.0f; } else { elevatorAngleAction = lastElevatorAngleAction; } } elevatorAngleAction = AuvMath::Limit( elevatorAngleAction, -elevatorLimit_, elevatorLimit_ ); elevatorAngleActionWriter_->write( Units::RADIAN, elevatorAngleAction ); } } void VerticalControl::setBuoyancy( float buoyancyCmd ) { if( isnan( buoyancyCmd ) ) { buoyancyCmd = buoyancyDefault_; } float depth( nanf( "" ) ); if( depthReader_->read( Units::METER, depth ) && depth <= buoyancyPumpDepth_ ) { buoyancyCmd = AuvMath::Limit( buoyancyCmd, buoyancyLimitLoCC_, buoyancyLimitHiCC_ ); buoyancyActionWriter_->write( Units::CUBIC_CENTIMETER, buoyancyCmd ); } } // Controls vehicle pitch via the elevator float VerticalControl::elevatorControl( float pitch, float pitchCmd, float pitchRate ) { float pitchError( pitch - pitchCmd ); float dt( AuvMath::NanZero( dt_.asFloat() ) ); float pitchProportional( kpPitchElevator_ * pitchError ); float pitchDifferential( ( kdPitchElevator_ * pitchRate ) ); //Kd * omega_B_N_B_y massPitchIntegral_ = nanf( "" ); if( verticalMode_ == VerticalControlIF::PITCH_ZERO || verticalMode_ == VerticalControlIF::PITCH || useElevIntInDepthMode_ ) { if( isnan( elevatorPitchIntegral_ ) ) { elevatorPitchIntegral_ = 0.; } elevatorPitchIntegral_ += kiPitchElevator_ * dt * pitchError; // limit the integrator to prevent windup elevatorPitchIntegral_ = AuvMath::Limit( elevatorPitchIntegral_, maxPitchElevatorInt_, -maxPitchElevatorInt_ ); } float elevatorAngleAction( pitchProportional + AuvMath::NanZero( elevatorPitchIntegral_ ) + pitchDifferential ); elevatorAngleAction = AuvMath::Limit( elevatorAngleAction, -elevatorLimit_, elevatorLimit_ ); return elevatorAngleAction; } // Controls vehicle pitch via the mass shifter float VerticalControl::massControl( float pitch, float pitchCmd, float pitchRate, bool forceIntegrator ) { float dt( AuvMath::NanZero( dt_.asFloat() ) ); float massPitchError( getMassPitchFilterError( pitch, pitchCmd, dt ) ); massPitchErrorWriter_->write( Units::RADIAN, massPitchError ); float pitchProportional( kpPitchMass_ * massPitchError ); float pitchDifferential( kdPitchMass_ * pitchRate ); //Kd * omega_B_N_B_y if( verticalMode_ == VerticalControlIF::PITCH_ZERO || verticalMode_ == VerticalControlIF::PITCH || forceIntegrator ) { if( isnan( massPitchIntegral_ ) ) { massPitchIntegral_ = 0.; } massPitchIntegral_ += kiPitchMass_ * dt * massPitchError; // limit the integrator to prevent windup massPitchIntegral_ = AuvMath::Limit( massPitchIntegral_, maxPitchMassInt_, -maxPitchMassInt_ ); } else { massPitchIntegral_ = nanf( "" ); } float massPositionAction( massDefault_ + pitchProportional + AuvMath::NanZero( massPitchIntegral_ ) + pitchDifferential ); massPositionAction = AuvMath::Limit( massPositionAction, massPositionLimitAft_, massPositionLimitFwd_ ); lastPitch_ = pitch; return massPositionAction; } void VerticalControl::massAndElevatorControl( float pitch, float pitchCmd, float pitchRate, float &massPositionAction, float &elevatorAngleAction ) { float pitchError( pitch - pitchCmd ); float dt( AuvMath::NanZero( dt_.asFloat() ) ); float pitchProportional( kpPitchElevator_ * pitchError ); float pitchDifferential( ( kdPitchElevator_ * pitchRate ) ); //Kd * omega_B_N_B_y float elevatorAngle( nanf( "" ) ); float massPosition( nanf( "" ) ); dtWriter_->write( Units::SECOND, dt ); if( massPositionReader_->read( Units::METER, massPosition ) && !isnan( massPosition ) && ( verticalMode_ == VerticalControlIF::PITCH_ZERO || verticalMode_ == VerticalControlIF::PITCH || verticalMode_ == VerticalControlIF::DEPTH || verticalMode_ == VerticalControlIF::DEPTH_RATE || verticalMode_ == VerticalControlIF::SURFACE ) ) { float massPitchError( getMassPitchFilterError( pitch, pitchCmd, dt ) ); massPitchErrorWriter_->write( Units::RADIAN, massPitchError ); if( isnan( massPitchIntegral_ ) ) { massPitchIntegral_ = 0.; } massPitchIntegral_ += kiPitchMass_ * dt * massPitchError; // limit the integrator to prevent windup massPitchIntegral_ = AuvMath::Limit( massPitchIntegral_, maxPitchMassInt_, -maxPitchMassInt_ ); } if( elevatorAngleReader_->read( Units::RADIAN, elevatorAngle ) && !isnan( elevatorAngle ) && ( verticalMode_ == VerticalControlIF::PITCH_ZERO || verticalMode_ == VerticalControlIF::PITCH || useElevIntInDepthMode_ ) ) { if( isnan( elevatorPitchIntegral_ ) ) { elevatorPitchIntegral_ = 0.; } elevatorPitchIntegral_ += kiPitchElevator_ * dt * pitchError; // limit the integrator to prevent windup elevatorPitchIntegral_ = AuvMath::Limit( elevatorPitchIntegral_, maxPitchElevatorInt_, -maxPitchElevatorInt_ ); } elevatorAngleAction = pitchProportional + pitchDifferential + AuvMath::NanZero( elevatorPitchIntegral_ ); elevatorAngleAction = AuvMath::Limit( elevatorAngleAction, -elevatorLimit_, elevatorLimit_ ); massPositionAction = massDefault_ + AuvMath::NanZero( massPitchIntegral_ ); massPositionAction = AuvMath::Limit( massPositionAction, massPositionLimitAft_, massPositionLimitFwd_ ); lastPitch_ = pitch; } float VerticalControl::getMassPitchFilterError( float apitch, float pitchCmd, float dt ) { float pitchError( apitch - pitchCmd ); if( fabs( pitchError ) < massFilterLimit_ ) { massFilter_[massFilterPos_++] = apitch; if( massFilterPos_ >= massFilterBins_ ) { massFilterPos_ = 0; } int bins = 0; float filtPitch = 0.0f; for( ; bins < massFilterBins_; ++bins ) { if( isnan( massFilter_[bins] ) ) { break; } filtPitch += massFilter_[bins]; } filtPitch /= ( float )bins; pitchError = filtPitch - pitchCmd; } else { if( !isnan( massFilter_[0] ) || massFilterPos_ != 0 ) { for( int bin = 0 ; bin < massFilterBins_; ++bin ) { massFilter_[bin] = nanf( "" ); } massFilterPos_ = 0; } } return pitchError; } /// Apply buoyancy or drop weight overrides if necessary. void VerticalControl::overrideVertical() { // First check for requested motion bool upRequested( false ); bool flatRequested( false ); bool downRequested( false ); float depth( nanf( "" ) ); float depthCmd( nanf( "" ) ); float depthRateCmd( nanf( "" ) ); float pitchCmd( nanf( "" ) ); float pitchRateCmd( nanf( "" ) ); bool depthRead = depthReader_->read( Units::METER, depth ); float smoothDepth = depthTrajectory_.getX(); float depthError = depth - smoothDepth; switch( verticalMode_ ) { case VerticalControlIF::NONE: break; case VerticalControlIF::SURFACE: upRequested = true; break; case VerticalControlIF::DEPTH: if( depthRead && depthCmdReader_->read( Units::METER, depthCmd ) && !isnan( depth ) && !isnan( depthCmd ) ) { if( !isnan( failToGoDownDepth_ ) ) { if( failToGoDownDepth_ > smoothDepth ) { failToGoDownStart_ = Timestamp::Now(); failToGoDownDepth_ = smoothDepth; } } if( !isnan( failToGoUpDepth_ ) ) { if( failToGoUpDepth_ < smoothDepth ) { failToGoUpStart_ = Timestamp::Now(); failToGoUpDepth_ = smoothDepth; } } if( fabs( depthError ) > depthDeadband_ + minDepthExcursion_ ) { float reqDepthRate = depthError / dt_.asFloat(); upRequested = reqDepthRate > depthRateDeadband_; downRequested = reqDepthRate < -depthRateDeadband_; } // Not really requesting up if we are ultimately heading down if( depth - depthCmd < 0 ) { upRequested = false; } } else { upRequested = true; } break; case VerticalControlIF::DEPTH_RATE: if( depthRateCmdReader_->read( Units::METER_PER_SECOND, depthRateCmd ) && !isnan( depthRateCmd ) ) { upRequested = depthRateCmd < 0; flatRequested = depthRateCmd == 0; downRequested = depthRateCmd > 0; } else { upRequested = true; } break; case VerticalControlIF::PITCH_ZERO: flatRequested = true; break; case VerticalControlIF::PITCH: if( !pitchCmdReader_->isActive() || !pitchCmdReader_->wasTouchedSinceLastRun( this ) ) { flatRequested = true; } else if( pitchCmdReader_->read( Units::RADIAN, pitchCmd ) && !isnan( pitchCmd ) ) { upRequested = pitchCmd > minAscendPitchCfg_; downRequested = pitchCmd < -minAscendPitchCfg_; flatRequested = !upRequested && !downRequested; } else { upRequested = true; } break; case VerticalControlIF::PITCH_RATE: if( !pitchRateCmdReader_->isActive() || !pitchRateCmdReader_->wasTouchedSinceLastRun( this ) ) { flatRequested = true; } else if( pitchRateCmdReader_->read( Units::RADIAN, pitchRateCmd ) && !isnan( pitchRateCmd ) ) { pitchCmd = pitchRateCmd * dt_.asFloat() > 0; upRequested = pitchCmd > elevDeviation_; downRequested = pitchCmd < -elevDeviation_; flatRequested = !upRequested && !downRequested; } else { upRequested = true; } break; case VerticalControlIF::MASS_AND_ELEVATOR: break; case VerticalControlIF::FLOAT_ON_SURFACE: upRequested = true; break; default: upRequested = true; break; } float depthRate( nanf( "" ) ); if( depthRateReader_->wasTouchedSinceLastRun( this ) && depthRateReader_->read( Units::METER_PER_SECOND, depthRate ) && !isnan( depthRate ) ) { if( ok_ ) { depthRateFilter_[depthRateFilterIndex_] = depthRate; ++depthRateFilterIndex_; if( depthRateFilterIndex_ >= depthRateSamples_ ) { depthRateFilterIndex_ = 0; } } } // Now see what we are actually doing. if( ok_ ) { depthRate = 0; for( int i = 0; i < depthRateSamples_; ++i ) { depthRate += depthRateFilter_[i]; } depthRate /= depthRateSamples_; } bool goingUp( depthRate < -depthRateDeadband_ ); bool goingDown( depthRate > depthRateDeadband_ ); bool failToGoUp = ( !goingUp && upRequested ) || ( goingDown && flatRequested ); bool failToGoDown = ( !goingDown && downRequested ) || ( goingUp && flatRequested ); // Deal with surface false positives if( failToGoUp || failToGoDown ) { if( depthReader_->read( Units::METER, depth ) && !isnan( depth ) ) { if( failToGoUp && depth < surfaceThreshold_ ) { failToGoUp = false; } if( failToGoDown && depth < 0 && depthCmd <= 0.0 ) { failToGoDown = false; } } } /// In case smoothDepth is telling the vehicle to ascend slowly... if( verticalMode_ == VerticalControlIF::SURFACE || verticalMode_ == VerticalControlIF::DEPTH || verticalMode_ == VerticalControlIF::FLOAT_ON_SURFACE ) { if( depth < smoothDepth ) { failToGoUp = false; } } Timespan stopOverrideDelay( speedCmd_ == 0 ? stopOverrideDelayBuoy_ : stopOverrideDelay_ ); if( failToGoUp ) { failToGoDownStart_ = Timestamp::NOT_SET_TIME; failToGoDownDepth_ = nanf( "" ); if( failToGoUpStart_ == Timestamp::NOT_SET_TIME ) { failToGoUpStart_ = Timestamp::Now(); } if( isnan( failToGoUpDepth_ ) ) { failToGoUpDepth_ = depth; } if( failToGoUpStart_.elapsed() > stopOverrideDelay && !missionEnded_ ) { float pitch( nanf( "" ) ); pitchReader_->read( Units::DEGREE, pitch ); logger_.syslog( "Failure to ascend, stopping mission after " + Str( failToGoUpStart_.elapsed().asFloat() ) + " seconds, depthRate=" + depthRate + " m/s, pitch=" + pitch + " deg.", Syslog::CRITICAL ); // Float up missionEnded_ = true; failToGoUpStart_ = Timestamp::Now(); previouslyBelowExcursion_ = false; } if( depth - failToGoUpDepth_ > stopDepthExcursion_ && !missionEnded_ ) { if( ( previouslyBelowExcursion_ ) && ( excursionStartTime_.elapsed() > excursionDepthTimeout_ ) ) { float pitch( nanf( "" ) ); pitchReader_->read( Units::DEGREE, pitch ); logger_.syslog( "Excessive depth excursion=" + Str( depth - failToGoUpDepth_ ) + " m, failToGoUpDepth_=" + Str( failToGoUpDepth_ ) + " m, depthRate=" + Str( depthRate ) + " m/s, pitch=" + pitch + " deg.", Syslog::CRITICAL ); // Float up missionEnded_ = true; failToGoUpStart_ = Timestamp::Now(); previouslyBelowExcursion_ = false; } else if( !previouslyBelowExcursion_ ) { previouslyBelowExcursion_ = true; excursionStartTime_ = Timestamp::Now(); } } if( missionEnded_ && failToGoUpStart_.elapsed() > dropWtOverrideDelay_ && !burnwireActivated_ ) { float pitch( nanf( "" ) ); pitchReader_->read( Units::DEGREE, pitch ); logger_.syslog( "Failure to ascend, dropping drop weight after " + Str( failToGoUpStart_.elapsed().asFloat() ) + " seconds, depthRate=" + depthRate + " m/s, pitch=" + pitch + " deg.", Syslog::CRITICAL ); // Drop dropweight LPC3Reg::ActivateBurnwire(); burnwireActivated_ = true; } if( missionEnded_ && depth - failToGoUpDepth_ > dropWtDepthExcursion_ && !burnwireActivated_ ) { if( ( previouslyBelowExcursion_ ) && ( excursionStartTime_.elapsed() > excursionDepthTimeout_ ) ) { float pitch( nanf( "" ) ); pitchReader_->read( Units::DEGREE, pitch ); logger_.syslog( "Dropping drop weight at excessive depth excursion=" + Str( depth - failToGoUpDepth_ ) + ", failToGoUpDepth_=" + Str( failToGoUpDepth_ ) + ", depthRate=" + Str( depthRate ) + " m/s, pitch=" + pitch + " deg.", Syslog::CRITICAL ); // Drop dropweight LPC3Reg::ActivateBurnwire(); burnwireActivated_ = true; } else if( !previouslyBelowExcursion_ ) { previouslyBelowExcursion_ = true; excursionStartTime_ = Timestamp::Now(); } } } else if( failToGoDown ) { failToGoUpStart_ = Timestamp::NOT_SET_TIME; failToGoUpDepth_ = nanf( "" ); if( failToGoDownStart_ == Timestamp::NOT_SET_TIME ) { failToGoDownStart_ = Timestamp::Now(); } if( isnan( failToGoDownDepth_ ) ) { failToGoDownDepth_ = depth; } if( failToGoDownStart_.elapsed() > stopOverrideDelay ) { // Do nothing. } if( failToGoDownDepth_ - depth > stopDepthExcursion_ ) { // Do nothing. } if( failToGoUpStart_.elapsed() > dropWtOverrideDelay_ ) { // Do nothing. } if( failToGoDownDepth_ - depth > dropWtDepthExcursion_ ) { // Do nothing. } } else { failToGoDownStart_ = failToGoUpStart_ = Timestamp::NOT_SET_TIME; failToGoDownDepth_ = failToGoUpDepth_ = nanf( "" ); excursionStartTime_ = Timestamp::Now(); previouslyBelowExcursion_ = false; missionEnded_ = false; burnwireActivated_ = false; } }