/****************************************************************************/ /* Copyright (c) 2000 MBARI */ /* MBARI Proprietary Information. All rights reserved. */ /****************************************************************************/ /* Summary : */ /* Filename : DynamicControl.cc */ /* Author : */ /* Project : */ /* Version : 1.0 */ /* Created : 02/07/2000 */ /* Modified : */ /* Archived : */ /****************************************************************************/ /* Modification History: */ /****************************************************************************/ #include #include #include "DynamicControlOutput.h" #include "DynamicControl.h" #include "Syslog.h" #include "MathP.h" #include "TimeP.h" #include "LowAltitudeFollowing.h" DynamicControl::DynamicControl() : _vehicleConfig("vehicleConfig") { Boolean debug = True; dprintf("DynamicControl default ctor"); _tau = 0.; _maxHeadingRate = 0.; _maxDepthRate = 0.; // Set this to True when first event is received from Navigation _navigationReady = False; _missionStarted = False; _navigation = new NavigationIF("navigation"); // _layeredControl = new LayeredControlIF("layeredControl"); _log = new DynamicControlLog(this, DataLog::BinaryFormat); _tailCone = new TailConeIF(TailConeIFServerName,10); // Subscribe to LayeredControl "OutputGenerated" events in place of NavigatioIF::NewOutput events // Then: // subscribe(_navigation, NavigationIF::NewOutput, // (EventCallback )DynamicControl::newNavDataCallback); // Now: // subscribe(_layeredControl, LayeredControlIF::OutputGenerated, // (EventCallback )DynamicControl::newNavDataCallback); // Subscribe to LayeredControl "MissionStarted" event // subscribe(_layeredControl, LayeredControlIF::MissionStarted, // (EventCallback )DynamicControl::missionStartCallback); // Subscribe to TailCone "Reset" event // subscribe(_tailCone, DeviceIF::Initializing, // (EventCallback )DynamicControl::tailConeCallback); _tailCone->enableThruster(); TailConeIF::Status status; if ((status =_tailCone->initialize()) != TailConeIF::Ok) { Syslog::write("DynamicControl::DynamicControl() - status %d from " "_tailCone->initialize()", status); } _terrainAidIF = 0; _enableTRN = True; if( _enableTRN ) { try { _terrainAidIF = new TerrainAidIF("TerrainAidIFServer", 10); } catch (...) { _terrainAidIF = 0; Syslog::write("DynamicControl - Could not open TerrainAid IF."); } } _lastTerNavTime = 0.; _dx = 0.; //Terrain Nav biases _dy = 0.; // // Get vehicle parameters // //VehicleConfigurationIF vehicleConfig("vehicleConfig"); // // Initialize constants: // _ductArea = _vehicleConfig.ductArea(); _propPitch = _vehicleConfig.propPitch(); _headingGains.p = _vehicleConfig.kpHeading(); _headingGains.d = _vehicleConfig.kdHeading(); _headingGains.i = _vehicleConfig.kiHeading(); _kwph = _vehicleConfig.kwpHeading(); _maxHdgRate = _vehicleConfig.maxHdgRate(); _kiwp = _vehicleConfig.kiwp(); _kpp = _vehicleConfig.kpPitch(); _kdp = _vehicleConfig.kdPitch(); _kip = _vehicleConfig.kiPitch(); _kpDepth = _vehicleConfig.kpDepth(); _kiDepth = _vehicleConfig.kiDepth(); _kiDepthOff = _vehicleConfig.kiDepthOff(); _kiWPDepthOff= _vehicleConfig.kiWPDepthOff(); _kdDepth = _vehicleConfig.kdDepth(); _pitchLimit = _vehicleConfig.pitchLimit(); _effDragCoef = _vehicleConfig.effDragCoef(); _tau = _vehicleConfig.tau(); _maxRudder = _vehicleConfig.rudLimit(); _maxElevator = _vehicleConfig.elevLimit(); _maxDiveRate = _vehicleConfig.maxDiveRate(); _Ts = _vehicleConfig.Ts(); _maxDepthInt = _vehicleConfig.maxDepthInt(); _maxHdgInt = _vehicleConfig.maxHdgInt(); _maxPitchInt = _vehicleConfig.maxPitchInt(); _maxXteInt = _vehicleConfig.maxXteInt(); _maxXteOff = _vehicleConfig.maxXteOff(); _propEfficiency = _vehicleConfig.propEfficiency(); _Xuabu = _vehicleConfig.xuabu(); _decouple = (Boolean) _vehicleConfig.decouple(); _pitchTrim = _vehicleConfig.pitchTrim(); // // Initialize integrator states to zero. // _depthIntegral = 0.; _headingIntegral = 0.; _pitchIntegral = 0.; _rudder = 0.; _elevator = 0.; _propOmega = 0.; _wasLimited = 0; _time = 0.; _lastTimeDC = 0.; _propSpeedCmd = 0.; _cmd_depth = 0.; _xte = 0.; _xteWay = 0.; _err_depth = 0.; _thetaFF = 0.; _first = True; _wallBearing = 0.; _firstWall = True; _wallFeedForward = False; _useWallEstimator = False; _rhtOnly = False; _inTheFOV = True; _lastSwitches = 0; _initializeWallBearing = False; _wallPresent = False; _turnReset = False; _clockTime0 = 0; _gains.kwpHeading = -1.; _gains.kiwp = -1.; _gains.kpHeading = -1.; _gains.kiHeading = -1.; _gains.kdHeading = -1.; _gains.kpDepth = -1.; _gains.kiDepth = -1.; _gains.kdDepth = -1.; _gains.kpPitch = -1.; _gains.kiPitch = -1.; _gains.kdPitch = -1.; _gainsChanged = False; _intLatched = False; } DynamicControl::~DynamicControl() { Syslog::write("DynamicControl::Destructor. Difference between " "clock time and accumulated sample periods is %.2f, at t=%.2f.", _clockTime -_clockTime0 - _time, _clockTime - _clockTime0); delete _navigation; delete _tailCone; // delete _layeredControl; delete _log; if( _terrainAidIF ) delete _terrainAidIF; } void DynamicControl::setCommand(DynamicControlIF::Command *command) { memcpy( &_command, command, sizeof( DynamicControlIF::Command ) ); //log the time of the command struct timespec timeSpec; Time::gettime(&timeSpec); _commandTime = (timeSpec.tv_sec + timeSpec.tv_nsec/1.e9); } void DynamicControl::revertGains(void) { //log the time of the command struct timespec timeSpec; Time::gettime(&timeSpec); _gainTime = (timeSpec.tv_sec + timeSpec.tv_nsec/1.e9); Syslog::write("DynamicControl::Reverting the control gains " "to the configuration file values at t = %-15.2f\n", _gainTime - _clockTime0); _kwph = _vehicleConfig.kwpHeading(); _kiwp = _vehicleConfig.kiwp(); _headingGains.p = _vehicleConfig.kpHeading(); _headingGains.i = _vehicleConfig.kiHeading(); _headingGains.d = _vehicleConfig.kdHeading(); _kpDepth = _vehicleConfig.kpDepth(); _kiDepth = _vehicleConfig.kiDepth(); _kdDepth = _vehicleConfig.kdDepth(); _kpp = _vehicleConfig.kpPitch(); _kdp = _vehicleConfig.kdPitch(); _kip = _vehicleConfig.kiPitch(); Syslog::write("DynamicControl::Setting kwpHeading " "to %8.2e at t = %-15.2f\n", _kwph, _gainTime - _clockTime0); Syslog::write("DynamicControl::Setting kiwp " "to %8.2e at t = %-15.2f\n", _kiwp, _gainTime - _clockTime0); Syslog::write("DynamicControl::Setting kpHeading " "to %8.2e at t = %-15.2f\n", _headingGains.p, _gainTime - _clockTime0); Syslog::write("DynamicControl::Setting kiHeading " "to %8.2e at t = %-15.2f\n", _headingGains.i, _gainTime - _clockTime0); Syslog::write("DynamicControl::Setting kdHeading " "to %8.2e at t = %-15.2f\n", _headingGains.d, _gainTime - _clockTime0); Syslog::write("DynamicControl::Setting kpDepth " "to %8.2e at t = %-15.2f\n", _kpDepth, _gainTime - _clockTime0); Syslog::write("DynamicControl::Setting kiDepth " "to %8.2e at t = %-15.2f\n", _kiDepth, _gainTime - _clockTime0); Syslog::write("DynamicControl::Setting kdDepth " "to %8.2e at t = %-15.2f\n", _kdDepth, _gainTime - _clockTime0); Syslog::write("DynamicControl::Setting kpPitch " "to %8.2e at t = %-15.2f\n", _kpp, _gainTime - _clockTime0); Syslog::write("DynamicControl::Setting kiPitch " "to %8.2e at t = %-15.2f\n", _kip, _gainTime - _clockTime0); Syslog::write("DynamicControl::Setting kdPitch " "to %8.2e at t = %-15.2f\n", _kdp, _gainTime - _clockTime0); _gainsChanged = False; } Boolean DynamicControl::newGains() { if( _gainsChanged ) return True; else return False; } void DynamicControl::setGains(DynamicControlIF::Gains *gains) { memcpy( &_gains, gains, sizeof( DynamicControlIF::Gains ) ); //log the time of the command struct timespec timeSpec; Time::gettime(&timeSpec); _gainTime = (timeSpec.tv_sec + timeSpec.tv_nsec/1.e9); _gainsChanged = True; // // Too bad this isn't in an array. // if( _gains.kwpHeading >= 0. ) { _kwph = _gains.kwpHeading; Syslog::write("DynamicControl::Setting kwpHeading " "to %8.2e at t = %-15.2f\n", _kwph, _gainTime - _clockTime0); } if( _gains.kiwp >= 0. ) { _kiwp = _gains.kiwp; Syslog::write("DynamicControl::Setting kiwp " "to %8.2e at t = %-15.2f\n", _kiwp, _gainTime - _clockTime0); } if( _gains.kpHeading >= 0. ) { _headingGains.p = _gains.kpHeading; Syslog::write("DynamicControl::Setting kpHeading " "to %8.2e at t = %-15.2f\n", _headingGains.p, _gainTime - _clockTime0); } if( _gains.kiHeading >= 0. ) { _headingGains.i = _gains.kiHeading; Syslog::write("DynamicControl::Setting kiHeading " "to %8.2e at t = %-15.2f\n", _headingGains.i, _gainTime - _clockTime0); } if( _gains.kdHeading >= 0. ) { _headingGains.d = _gains.kdHeading; Syslog::write("DynamicControl::Setting kdHeading " "to %8.2e at t = %-15.2f\n", _headingGains.d, _gainTime - _clockTime0); } if( _gains.kpDepth >= 0. ) { _kpDepth = _gains.kpDepth; Syslog::write("DynamicControl::Setting kpDepth " "to %8.2e at t = %-15.2f\n", _kpDepth, _gainTime - _clockTime0); } if( _gains.kiDepth >= 0. ) { _kiDepth = _gains.kiDepth; Syslog::write("DynamicControl::Setting kiDepth " "to %8.2e at t = %-15.2f\n", _kiDepth, _gainTime - _clockTime0); } if( _gains.kdDepth >= 0. ) { _kdDepth = _gains.kdDepth; Syslog::write("DynamicControl::Setting kdDepth " "to %8.2e at t = %-15.2f\n", _kdDepth, _gainTime - _clockTime0); } if( _gains.kpPitch >= 0. ) { _kpp = _gains.kpPitch; Syslog::write("DynamicControl::Setting kpPitch " "to %8.2e at t = %-15.2f\n", _kpp, _gainTime - _clockTime0); } if( _gains.kiPitch >= 0. ) { _kip = _gains.kiPitch; Syslog::write("DynamicControl::Setting kiPitch " "to %8.2e at t = %-15.2f\n", _kip, _gainTime - _clockTime0); } if( _gains.kdPitch >= 0. ) { _kdp = _gains.kdPitch; Syslog::write("DynamicControl::Setting kdPitch " "to %8.2e at t = %-15.2f\n", _kdp, _gainTime - _clockTime0); } } void DynamicControl::setMaxHeadingRate(double rate) { _maxHeadingRate = rate; } double DynamicControl::maxHeadingRate() { return _maxHeadingRate; } void DynamicControl::setMaxDepthRate(double rate) { _maxDepthRate = rate; } double DynamicControl::maxDepthRate() { return _maxDepthRate; } void DynamicControl::setTau(float tau) { _tau = tau; } float DynamicControl::tau() { return _tau; } void DynamicControl::setPitchServoGains(double proportional, double integral, double derivative) { _pitchGains.p = proportional; _pitchGains.i = integral; _pitchGains.d = derivative; } void DynamicControl::pitchServoGains(double *proportional, double *integral, double *derivative) { *proportional = _pitchGains.p; *integral = _pitchGains.i; *derivative = _pitchGains.d; } void DynamicControl::setHeadingServoGains(double proportional, double integral, double derivative) { _headingGains.p = proportional; _headingGains.i = integral; _headingGains.d = derivative; } void DynamicControl::headingServoGains(double *proportional, double *integral, double *derivative) { *proportional = _headingGains.p; *integral = _headingGains.i; *derivative = _headingGains.d; } void DynamicControl::getWallData(double *lastWallTime, double *wallBearing) { *lastWallTime = _lastWallTime; *wallBearing = _lastWallBearing; } void DynamicControl::getLogData(DynamicControlIF::LogData *logData) { DynamicControlOutput::Data output; logData->propOmegaCmd = output.propOmegaCmd; logData->rudderCmd = output.rudderCmd; logData->elevatorCmd = output.elevatorCmd; } //int DynamicControl::spawnAuxTasks() //{ // return 0; //} //========================================================================= void DynamicControl::process() { Boolean debug = True; double thruster_current, des_speed; double depth_cmd, pitch_cmd, elevator_cmd, heading, rudder; double dn, de, heading_cmd, heading_cmd_smooth, altitude_cmd; double proportional, derivative, rudder_cmd; double fairInDist = 0.; // // Read latest input command from Layered Control: // // _input->read(&_command); //xxx // // Debug. Try raising priority. Put input->read here to avoid // uninitialized data. // static int first = 0; //zzz if( first == 0 ) { // setprio(0,19); _lastTime = 0; _lastHMode = _command.horizontalMode; _lastVMode = _command.verticalMode; _clockTime0 = _clockTime; } first = 1; _clockTimeSinceStart = _clockTime - _clockTime0; //Syslog::write("DynamicControl:: Executing at %.3f seconds.", // _clockTimeSinceStart); if( _lastHMode != _command.horizontalMode ) { Syslog::write("DynamicControl::Begin horizontal mode " "%d at t = %.2f sec.", _command.horizontalMode, _clockTimeSinceStart); _lastHMode = _command.horizontalMode; } if( _lastVMode != _command.verticalMode ) { Syslog::write("DynamicControl::Begin vertical mode " "%d at t = %.2f sec.", _command.verticalMode, _clockTimeSinceStart); _lastVMode = _command.verticalMode; } // dprintf("DynamicControl::process() - commands:\n"); // // dprintf("verticalMode=%d, horizontalMode=%d, speedMode=%d\n", // _command.verticalMode, _command.horizontalMode, // _command.speedMode); // // dprintf("vertical=%.2f, horizontal=%.2f, " // "speed=%.2f\n", // _command.vertical, _command.horizontal, // _command.speed); // // Read latest vehicle state from NavigationIF: // _navigation->state( &_position, &_attitude ); //xxx if( _terrainAidIF ) { _terrainAidIF->get( &_terrainNavData ); if( _terrainNavData.time != _lastTerNavTime ) { // // There is new data from TerrainAid. Syslog::write("DynamicControl:: TerrainNav has new data. " "Valid = %d\n", _terrainNavData.valid ); _lastTerNavTime = _terrainNavData.time; if( _terrainNavData.valid ) { _dx = _terrainNavData.x - _position.x; _dy = _terrainNavData.y - _position.y; Syslog::write("DynamicControl:: TerrainNav correction:\n" "\tdNorthing = %.2f dEasting = %.2f", _dx, _dy); } } _position.x += _dx; _position.y += _dy; } // dprintf("DynamicControl::process() - state:\n"); // dprintf("x=%.2f, y=%.2f, z=%.2f\n", _position.x, _position.y, _position.z); // dprintf("u=%.2f, v=%.2f, w=%.2f\n", _position.x, _position.y, _position.z); // // dprintf("phi=%.2f, theta=%.2f, psi=%.2f\n", // _attitude.roll, _attitude.pitch, _attitude.yaw); // // dprintf("omega_Bx=%.2f, omega_By=%.2f, omega_Bz=%.2f\n", // _attitude.omega_B_x, _attitude.omega_B_y, _attitude.omega_B_z); /*------------------------------------------------------------------- BEGIN SPEED CONTROL (Open Loop) Different modes of speed control are indicated by the value of variable "_command.speedMode" and control is handled accordingly. value of (_command.speedMode) 0: Current: carries the desired thruster amperage. 1: Speed carries the desired vehicle speed (m/s), control is open loop... The following are not yet implemented. 2: vvp->c[0].speed carries the desired vehicle speed (m/s), the thruster rotation rate is used for feedback... 3: vvp->c[0].speed carries the desired vehicle speed (m/s), the water speed sensor is used for feedback... *-------------------------------------------------------------------*/ // // switch ( _command.speedMode ) { case DynamicControlIF::Current: // // Apparently they intend to the variable _command.speed to hold // current in this case. // thruster_current = _command.speed; // // Log thruster current // break; case DynamicControlIF::Speed: des_speed = _command.speed; // // propSpeedCmd is in units of rad/sec. // _propSpeedCmd = des_speed / _propPitch; } /*------------------------------------------------------------------- Different modes of depth control are indicated by the value of "vvp->c[0].depth_mode" and control is handled accordingly. value of (vvp->c[0].depth_mode) DM_ELEVATOR: vvp->c[0].depth carries desired elevator angle. D_DEPTH: vvp->c[0].depth carries desired depth D_PITCH: vvp->c[0].depth carries desired pitch angle Trajectory generator added 8 March 1996 F. Hover (FSH). Integrator security features by B. A. Moran. -------------------------------------------------------------------*/ // dprintf(" DynamicControl: Begin pitch control:\n"); if ( _command.verticalMode != DynamicControlIF::VmInitial && !_depthTraj.init ) { init_traj( &_depthTraj, _position.z, _tau, _Ts ); } switch ( _command.verticalMode ) { case DynamicControlIF::Depth: // map desired depth to pitch cmd case DynamicControlIF::DepthImmediate: case DynamicControlIF::Altitude: // // Beware that _command.vertical can be either depth, altitude, // pitch, or elevator, depending on the mode. // depth_cmd = _command.vertical; // Extract the depth command _thetaFF = _command.thetaFF; // Extract pitch feedforward if( _command.verticalMode == DynamicControlIF::Altitude ) { // Use the depth slot for the commanded altitude: altitude_cmd = _command.vertical; } if (_command.verticalMode==DynamicControlIF::DepthImmediate && _cmd_depth != _command.vertical) { _cmd_depth = _command.vertical; _depthTraj.init = False; init_traj(&_depthTraj,_position.z,_tau, _Ts); _depthIntegral = 0.; Syslog::write("resetting depth trajectory"); } _des_depth = do_traj( depth_cmd, _maxDiveRate, &_depthTraj ); _err_depth = _position.z - _des_depth; if( _command.verticalMode == DynamicControlIF::Altitude ) { // // Use _des_depth to pass out the measured altitude. _des_depth = _position.z + _position.altitude - altitude_cmd; // // Compute depth error from the unsmoothed altitude. _err_depth = altitude_cmd - _position.altitude; // // Will rely on LayeredControl to handle the case of // INVALID_ALTITUDE. The INVALID_ALTITUDE check here is to prevent // a glitch for a Ts or two before LayeredControl switches us out of // altitude mode. if( _position.altitude == INVALID_ALTITUDE ) { _des_depth = _position.z; _err_depth = 0.; } } /* FSH & DRY integrator prevents adjustments when desired depth rate is high. (Also clips at +-5 deg, BAM 10-Jan-1998) */ // // RSM 7 Oct 05. Moved the integrator shut-off threshold to // vehicle.cfg. It was hardcoded to .01 m/s. Added a second // shut-off value for WaypointDepth, because in this case we would // like the integrator to operate during climbs or descents. //if ( fabs(_depthTraj.xdot) < .01 ) if( !_intLatched ) { if ( fabs(_depthTraj.xdot) < _kiDepthOff || ( (fabs(_depthTraj.xdot) < _kiWPDepthOff) && _command.horizontalMode==DynamicControlIF::WaypointDepth) ) { _depthIntegral += _kiDepth * _err_depth * _Ts; _depthIntegral = Math::limit( _depthIntegral, _maxDepthInt, -_maxDepthInt ); } else { _depthIntegral = 0.; } } _kdZdot = _kdDepth * _position.depthRate; mDes_pitch = _kpDepth * _err_depth + _depthIntegral + _kdZdot + _thetaFF + _pitchTrim; _lowerPitchLimit = -_pitchLimit; // // Active if maxLowerPitch and deltaDepthRestart are enabled in // DepthEnvelope. It prevents the vehicle from pitching too far // down after a deltaDepth restart. Also applies during altitude tracking. // if( _command.maxLowerPitch != 0. ) { _lowerPitchLimit = -_command.maxLowerPitch; if(fairInDist) { double fairInAngle = atan( _err_depth/fairInDist ); if( fairInAngle > _lowerPitchLimit ) { Syslog::write("DynamicControl: lowerPitchLimit reset: " "from %.2f to %.2f at t = %.2f", _lowerPitchLimit, fairInAngle, _clockTimeSinceStart); _lowerPitchLimit = fairInAngle; } } if( mDes_pitch < _lowerPitchLimit ) Syslog::write("DynamicControl: mDes_pitch = %.2f; " "limited to %.2f at t = %.2f", mDes_pitch, _lowerPitchLimit, _clockTimeSinceStart); } if( mDes_pitch > _pitchLimit || mDes_pitch < _lowerPitchLimit ) _intLatched = True; else _intLatched = False; mDes_pitch = Math::limit( mDes_pitch, _pitchLimit, _lowerPitchLimit ); //update( C_PITCH, des_pitch ); _elevator = pitch_control( _attitude.pitch, mDes_pitch, _attitude.omega_B_y); _elevBefLim = _elevator; /* rsm Debug zzz */ break; case DynamicControlIF::Pitch: /* dynamic control of pitch */ pitch_cmd = _command.vertical; //Extract the pitch command mDes_pitch = Math::limit( pitch_cmd, _pitchLimit, -_pitchLimit ); _elevator = pitch_control( _attitude.pitch, mDes_pitch, _attitude.omega_B_y); _elevBefLim = _elevator; /* rsm Debug zzz */ _depthTraj.init = False; break; case DynamicControlIF::Elevator: // // Here the variable _command.vertical is the elevator angle. // _elevator = _command.vertical; _depthTraj.init = False; break; } // update( C_ELEVATOR_ANGLE, _elevator ); /*-------------------------------------------------------------------* PID HEADING CONTROL LAW ALGORITHM Different modes of speed control are indicated by the value of variable "vvp->c[0].heading_mode" and control is handled accordingly. *-------------------------------------------------------------------*/ // // Extract the measured heading angle from the navigation algorithm. // I'm assuming that _navigation->heading() and attitude.yaw are // not necessarily the same. They would be if the Euler Angle sequence // had yaw first, for example, 3-1-2. // // Also, beware that (_command.) bearing is atan2(east/north). // // dprintf(" DynamicControl: Begin heading control:\n"); _navigation->heading(&heading); //Extract the heading zzz if ( _command.horizontalMode != DynamicControlIF::HmInitial && !_headingTraj.init ) { init_traj( &_headingTraj, heading, _tau, _Ts ); } double bearing; double radius; long cntr=0; Boolean outOfTheLane = False; double deltaPsiFF = 0.; Boolean LHT = False; Boolean RHT = False; Boolean resetWallBearing = False; unsigned int switches = 0; unsigned int changed = 0; double dPsiUnlimited = 0.; // // newBearing passes the angle of the wall wrto the vehicle when hMode // == WaypointWall. psiWallwrtoVeh = 0 when the vehicle is flying // parallel to the wall. // double psiWallwrtoVeh = _command.newBearing; switch ( _command.horizontalMode ) { case DynamicControlIF::Circle: case DynamicControlIF::WaypointDepth: case DynamicControlIF::Waypoint: /* waypoint control mode, must appear before heading control mode */ dn = _position.x - _command.north; de = _position.y - _command.east; bearing = _command.horizontal; // // Now compute the cross-track error based on the mode: // _xteWay = dn*sin(bearing) - de*cos(bearing); _xte = _xteWay - _command.xTrkOffset; if ( _command.horizontalMode == DynamicControlIF::Circle ) { radius = sqrt( dn*dn + de*de ); _xte = radius - _command.radius; if( debug && ( (cntr % 20) == 0 ) ) { printf("radius=%.2f, command=%.2f " "bearing= %.2f, xte = %.2f\n", radius, _command.radius, bearing*180/PI, _xte); } cntr++; } _kxte = _kwph * _xte; // // Integrate the cross track error and feed it back. // if ( fabs(_xte) < _maxXteOff ) { _xteIntegral += _xte * _kiwp * _Ts; _xteIntegral = Math::limit( _xteIntegral, _maxXteInt, -_maxXteInt ); } else _xteIntegral = 0.; _dPsi = _kxte + _xteIntegral; //if ( fabs(kxte) < PIUPON2 ) _dPsi = Math::limit(_dPsi, PIUPON2, -PIUPON2); heading_cmd = Math::modPi( bearing + _dPsi ); // // Goats change by H. Schmidt & Reiffel commented out below. // Apparently the above relation has a flaw where if the // vehicle misses the capture radius it keeps going on the // previous bearing, rather than turning to the next waypoint. // // heading_cmd = Math::modPi( atan2(-de, -dn) + kxte ); //else // heading_cmd = atan2( -de, -dn ); // // HeadingControl is the PID heading control. It computes _rudder. // HeadingControl(heading, heading_cmd, &_attitude); break; case DynamicControlIF::WaypointWall: switches = (unsigned int) _command.newEasting; bearing = _command.horizontal; if( _firstWall ) { _wallBearing = bearing; //_wallBearing = _attitude.yaw; //_wallBearing = behavior argument; // Use existing value _firstWall = False; } // // Unpack the switches: if( switches & (1<<0) ) _useWallEstimator = True; else _useWallEstimator = False; if( switches & (1<<1) ) _wallFeedForward = True; else _wallFeedForward = False; if( switches & (1<<2) ) outOfTheLane = True; else outOfTheLane = False; if( switches & (1<<3) ) LHT = True; else LHT = False; if( switches & (1<<4) ) RHT = True; else RHT = False; if( switches & (1<<5) ) _initializeWallBearing = True; else _initializeWallBearing = False; if( switches & (1<<6) ) _wallPresent = True; else _wallPresent = False; // // See which have changed and log it: // changed = switches^_lastSwitches; if( changed ) { if( changed & (1<<0) ) Syslog::write("DynamicControl:: " "useWallEstimator set to %d at %.2f", _useWallEstimator, _clockTimeSinceStart); if( changed & (1<<1) ) Syslog::write("DynamicControl:: " "wallFeedForward set to %d at %.2f", _wallFeedForward, _clockTimeSinceStart); if( changed & (1<<2) ) Syslog::write("DynamicControl:: " "outOfTheLane set to %d at %.2f", outOfTheLane, _clockTimeSinceStart); if( changed & (1<<3) ) Syslog::write("DynamicControl:: " "LHT set to %d at %.2f", LHT, _clockTimeSinceStart); if( changed & (1<<4) ) Syslog::write("DynamicControl:: " "RHT set to %d at %.2f", RHT, _clockTimeSinceStart); if( changed & (1<<5) ) Syslog::write("DynamicControl:: " "initializeWallBearing set to %d at %.2f", _initializeWallBearing, _clockTimeSinceStart); if( changed & (1<<6) ) Syslog::write("DynamicControl:: " "wallPresent set to %d at %.2f", _wallPresent, _clockTimeSinceStart); _lastSwitches = switches; } if(LHT && !outOfTheLane) { // // Turn left at a controlled rate. HeadingControl(heading, heading-PI/4., &_attitude); _turnReset = True; break; } if(RHT && !outOfTheLane) { // // Turn right at a controlled rate. HeadingControl(heading, heading+PI/4., &_attitude); _turnReset = True; break; } if( _wallFeedForward ) { deltaPsiFF = psiWallwrtoVeh; if( _rhtOnly ) { if( deltaPsiFF < 0.) deltaPsiFF = 0.; } if( outOfTheLane ) deltaPsiFF = 0.; } // // Compute cross-track error from corridor centerline: // dn = _position.x - _command.north; de = _position.y - _command.east; _xteWay = dn*sin(bearing) - de*cos(bearing); // // Extract the stand-off error: // _xte = _command.xTrkOffset; // // Close the outer loop. Covert cross-track error to heading delta. // _kxte = _kwph * _xte; dPsiUnlimited = _kxte; _dPsi = Math::limit(dPsiUnlimited, PIUPON2, -PIUPON2); if( _dPsi != dPsiUnlimited ) resetWallBearing = True; _dPsi += deltaPsiFF; //_dPsi = _kxte + deltaPsiFF; //_dPsi = _kxte; // // Don't let the outer loop tell the vehicle to turn too far from // the psi_ref if we're out of the lane. // if( outOfTheLane ) _dPsi = Math::limit(_dPsi, PIUPON4, -PIUPON4); // // Estimate the wall bearing from the stand-off error: // if( _useWallEstimator ) { if( _wallPresent ) { _wallBearing += _xte * _kiwp * _Ts; _wallBearing = Math::modPi( _wallBearing ); // // if( resetWallBearing ) { _wallBearing = Math::modPi(_attitude.yaw + psiWallwrtoVeh); } if( _initializeWallBearing ) { _wallBearing = Math::modPi(_attitude.yaw + psiWallwrtoVeh); // // Don't let it persist. _initializeWallBearing = False; Syslog::write("DynamicControl:: " "_wallBearing initialized to %.2f at %.2f", _attitude.yaw + psiWallwrtoVeh, _clockTimeSinceStart); } if( _turnReset ) { _wallBearing = Math::modPi(_attitude.yaw + psiWallwrtoVeh); // // Don't let it persist. _turnReset = False; Syslog::write("DynamicControl:: " "_wallBearing reset to %.2f at %.2f by L/RHT.", _attitude.yaw + psiWallwrtoVeh, _clockTimeSinceStart); } // // Safety: If the vehicle is out of the lane, run along the // edge on the bearing to the next waypoint. if( outOfTheLane ){ _wallBearing = Math::modPi(_command.horizontal); } // set latest known wall time/bearing [klh] _lastWallBearing=_wallBearing; _lastWallTime=_clockTime; } // // Else, do nothing. This latches _wallBearing at its last // value until the wall reappears. // } else { _wallBearing = Math::modPi(_command.horizontal); } heading_cmd = Math::modPi( _wallBearing + _dPsi ); HeadingControl(heading, heading_cmd, &_attitude); break; case DynamicControlIF::Heading: /* heading control mode */ HeadingControl(heading, _command.horizontal, &_attitude); break; case DynamicControlIF::Homing: /* Homing control mode */ // // This is pursuit guidance. The vehicle just continually points // its nose at the target. // // Latch the heading until the Usbl gets a hit. (FIX: This should // reset at the end of each homing behavior.) // if( _first ) { _first = False; _bearingToTarget = heading; } if( _position.usblValid && _position.usblNewData ) { // // Convert body-referenced usblBearing to the standard // North-referenced bearing. Maintain this bearing until // the Usbl updates again. // // Beware that "heading" updates at 5 Hz, and usblBearing at // about one Hz. usblValid = False between updates, thus the // heading control maintains the usblBearing between updates. _bearingToTarget = heading + _position.usblBearing; } HeadingControl(heading, _bearingToTarget, &_attitude); break; case DynamicControlIF::Rudder: /* direct rudder */ _rudder = _command.horizontal; //Extract the rudder command //update( C_RUDDER_ANGLE, Del13 ); _headingTraj.init = False; break; } #if 0 // // Debug: Print out loop time. // // Note: This is equivalent to a back difference on the first column // of the log file created by _log->write() below. // struct timespec timeSpec; //zzz double cTime; Time::gettime(&timeSpec); cTime = (timeSpec.tv_sec + timeSpec.tv_nsec/1.e9); printf (" %10.6f\n", cTime - _lastTime ); _lastTime = cTime; #endif // // Transform the rudder and elevator angles from the body frame to the // local vertical frame. // const double maxRoll = 20.*PI/180.; const double maxPitch = 20.*PI/180.; // // Scope this debug to be local. // { Boolean debug = False; dprintf(" _decouple = %d.\n", _decouple ); } if( _decouple ) { double cPhi, sPhi, phi, deltaR, deltaE, roll, pitch; roll = Math::limit( _attitude.roll , maxRoll, -maxRoll ); pitch = Math::limit( _attitude.pitch, maxRoll, -maxPitch ); // // BEWARE: The AHRS puts out projected, or space-fixed, roll and pitch // angles. THESE ARE NOT EULER ANGLES. The following relation converts // the space-fixed roll angle into a 2-1 Euler. See Wertz, p.226. // // At 20 Deg pitch, the Euler roll and the space-fixed roll differ // by about 20 milli-Radians. // phi = atan( cos(_attitude.pitch)*tan(_attitude.roll) ); deltaR = _rudder; deltaE = _elevator; cPhi = cos(phi); sPhi = sin(phi); // // Perform the matrix multiply directly, without a subroutine call: // _rudder = cPhi * deltaR - sPhi * deltaE; _elevator = sPhi * deltaR + cPhi * deltaE; } // // Limit the deflections: // _rudder = Math::limit( _rudder, _maxRudder, -_maxRudder ); _elevator = Math::limit( _elevator, _maxElevator, -_maxElevator ); // // Write to the log: // _log->write(); // // WARNING: The following write to tailCone MUST be the LAST statement // before the end of this routine. This routine does not complete until // tailCone has completed. This includes anything that tailCone may call // like "simulator". For example, if the _log->write() statement is after // _tailCone->command, the timestamps in dynamicControl.log will have a // lot of jitter, because they are not recorded until after the call to // simulation (and motion(), etc.) complete. // // Syslog::write("DynamicControl cmd: speed=%f, elev=%f, rudder=%f", // des_speed, _elevator, _rudder); // debug=True; // dprintf(" DynamicControl: Write to tailcone and terminate:\n"); // dprintf(" Commanded Prop Speed = %.2f; Elevator = %.2f;" // " Rudder = %.2f; radians\n", // _propSpeedCmd, _elevator, _rudder); //zero out the speed command if the difference between the time of the //command and now is greater than 1 second if (fabs(_clockTime - _commandTime) > 1.0) { _propSpeedCmd = 0.0; } _tailCone->setDeadman(1000); _tailCone->command( _propSpeedCmd, _elevator, _rudder ); } /*-----------------------------------------------------------------------* PITCH_CONTROL: Executes control of vehicle pitch. *-----------------------------------------------------------------------*/ double DynamicControl::pitch_control( double pitch, double des_pitch, double pitch_rate ) { double elevator; _pitchProportional = _kpp * (pitch - des_pitch); _pitchRate = _kdp * pitch_rate; elevator = _elevator; //From last time if ((fabs(elevator) < _maxElevator) || (fabs(_kip) < 1.e-6)) //(_wasLimited && (fabs(_pitchIntegral) < _maxPitchInt )) ) { _p_dbg = _kip*_Ts*(pitch - des_pitch); _pitchIntegral += _p_dbg; _wasLimited = 0; } else { _pitchIntegral = Math::sgn(elevator)*_maxElevator - _pitchProportional - _pitchRate ; _p_dbg = 0.; _wasLimited = 1; } // limit the integrator to prevent windup _pitchIntegral = Math::limit(_pitchIntegral, _maxPitchInt, -_maxPitchInt ); return (_pitchProportional + _pitchRate + _pitchIntegral); } /*-----------------------------------------------------------------------* | File: whoi_traj.c | Authors: Dana Yoeger, Franz Hover of WHOI. | Created: 3-11/96 *-----------------------------------------------------------------------*/ /* keeps heading between 0-2PI, only corrects once though */ void DynamicControl::wrap_heading(double *h) { while(*h > TWOPI)*h -= TWOPI ; while(*h < 0)*h += TWOPI ; } /* corrects the difference between 2 headings to keep the value between +- pi. In other words, it takes the shortest distance around the circle */ double DynamicControl::delta_heading(double dh) { double result ; result = dh ; while(result > PI)result -= TWOPI ; while(result < -PI)result += TWOPI ; return(result) ; } void DynamicControl::init_traj( Trajectory *t, double x, double tau, double dt ) { if ( t->init ) return; t->x = x; t->tau = tau; t->dt = dt; t->xdot = 0.0; t->alpha = exp(-dt/tau) ; t->beta = 1.0 - t->alpha ; t->init = True; } /*-----------------------------------------------------------------------* do_hdg_traj: executes the hdg trajectory generator for one time step inputs: xf is the final, or goal heading in radians (0-2PI) xdotmax is the max desired hdg rate (rad/sec) outputs: the trajectory structure will have updated x and xdot elements *-----------------------------------------------------------------------*/ double DynamicControl::do_hdg_traj( double xf, double xdotmax, Trajectory *t ) { double dx, new_x, last_x, speed ; speed = fabs( xdotmax ); dx = delta_heading( xf - t->x ); last_x = xf - dx ; t->xdot = t->alpha*t->xdot + t->beta*speed*Math::sgn(dx) ; if ( fabs(dx/t->tau) > speed ) new_x = last_x + t->xdot*t->dt ; else { new_x = t->alpha*last_x + t->beta*xf ; t->xdot = delta_heading(new_x - t->x) / t->dt ; } wrap_heading(&new_x) ; t->x = new_x ; return new_x ; } /*-----------------------------------------------------------------------* executes the general trajectory generator for one time step inputs: xf is the final, or goal position xdotmax is the max desired rate outputs: the trajectory structure will have updated x and xdot elements *-----------------------------------------------------------------------*/ double DynamicControl::do_traj( double xf, double xdotmax, Trajectory *t ) { double dx, new_x, last_x, speed; speed = fabs(xdotmax); dx = xf - t->x ; last_x = t->x ; t->xdot = t->alpha*t->xdot + t->beta*speed*Math::sgn(dx); if(fabs(dx/t->tau) > speed) new_x = last_x + t->xdot*t->dt ; else { new_x = t->alpha*last_x + t->beta*xf ; t->xdot = (new_x-t->x)/t->dt ; } t->x = new_x ; return new_x; } void DynamicControl::HeadingControl(double heading, double headingCmd, NavigationIF::Attitude *attitude) { mT_Psi = do_hdg_traj( headingCmd, _maxHdgRate, &_headingTraj ); double hdg_err = Math::modPi( heading - mT_Psi ); mPsiProp = _headingGains.p * hdg_err; mPsiRate = _headingGains.d * ( attitude->omega_B_z - _headingTraj.xdot); if ( fabs(_headingTraj.xdot) < .0175 ) { //Fix later. .017 rad = 1 deg. if ( (fabs(_rudder) < _maxRudder) || (fabs(_headingGains.i) <= 1e-6) ) _headingIntegral += _headingGains.i * _Ts * hdg_err; else _headingIntegral = Math::sgn(_rudder) * _maxRudder - mPsiProp - mPsiRate; // // Limit the integral term // _headingIntegral = Math::limit(_headingIntegral, _maxHdgInt, -_maxHdgInt); } _rudder = mPsiProp + mPsiRate + _headingIntegral; } #if 0 void DynamicControl::HeadingControl(double heading, double headingCmd, NavigationIF::Attitude *attitude) { mT_Psi = do_hdg_traj( headingCmd, _maxHdgRate, &_headingTraj ); double hdg_err = Math::modPi( heading - mT_Psi ); double proportional = _headingGains.p * hdg_err; double derivative = _headingGains.d * ( attitude->omega_B_z - _headingTraj.xdot); if ( fabs(_headingTraj.xdot) < .0175 ) { //Fix later. .017 rad = 1 deg. if ( (fabs(_rudder) < _maxRudder) || (fabs(_headingGains.i) <= 1e-6) ) _headingIntegral += _headingGains.i * _Ts * hdg_err; else _headingIntegral = Math::sgn(_rudder) * _maxRudder - proportional - derivative; // // Limit the integral term // _headingIntegral = Math::limit(_headingIntegral, _maxHdgInt, -_maxHdgInt); } _rudder = proportional + derivative + _headingIntegral; _rudder = Math::limit( _rudder, _maxRudder, -_maxRudder ); } #endif #ifdef ZILCH int DynamicControl::Log::write(DynamicControl *obj) { fprintf(file(), "%.2f " "%7.3e %7.3e %7.3e\n", obj->_time, obj->_pitchProportional, obj->_pitchIntegral, obj->_pitchRate /* "%7.3e %7.3e %7.3e " "%7.3e %7.3e %7.3e " "%7.3e %7.3e %7.3e " "%7.3e %7.3e %7.3e\n", obj->_position.x, obj->_position.y, obj->_position.z, obj->_position.xRate, obj->_position.yRate, obj->_position.zRate, obj->_attitude.roll, obj->_attitude.pitch, obj->_attitude.yaw, obj->_attitude.omega_B_x, obj->_attitude.omega_B_y, obj->_attitude.omega_B_z */ ); return 0; } #endif //========================================================================= void DynamicControl::tailConeCallback(TaskInterface *taskInterface, EventCode eventCode) { Syslog::write("DynamicControl - Got event %d from TailCone\n", eventCode); } void DynamicControl::missionStartCallback(TaskInterface *taskInterface, EventCode code) { Boolean debug = False; dprintf("*** DynamicControl::missionStartCallback()"); // LayeredControl says mission has started _missionStarted = True; _time = 0; } void DynamicControl::newNavDataCallback(TaskInterface *taskInterface, EventCode code) { //Syslog::write("DynamicControl::newNavDataCallback() - triggered"); if (!_navigationReady) { // We infer navigation is ready because navigation triggers LayeredControl // which in turn triggers me. _navigationReady = True; // Inform rest of system that Navigation and DynamicControl are ready // triggerEvent(DynamicControlIF::Ready); } Boolean debug = True; // // If the mission hasn't started yet return. // if (!_missionStarted){ dprintf("DynamicControl::newNavDatacallback(): mission not started - returning\n"); return; } // // Measure the Navigation loop time, including the IPC lag from Nav to DynCnt. // struct timespec timeSpec; Time::gettime(&timeSpec); _clockTime = (timeSpec.tv_sec + timeSpec.tv_nsec/1.e9); _navLoopTimeDC = _clockTime - _lastTimeDC; _lastTimeDC = _clockTime; process(); _time += (SystemPeriodMillisec / 1000.); } void DynamicControl::run(){} void DynamicControl::notifyEvent(EventCode event) { Boolean debug = False; dprintf("DynamicControl::notifyEvent - event[%d]\n",event); // LayeredControl "MissionStarted" event if (event==LayeredControlIF::MissionStarted){ dprintf("DynamicControl::notifyEvent - LayeredControlIF::MissionStarted\n"); missionStartCallback(NULL, event); } // LayeredControl "OutputGenerated" event if (event==LayeredControlIF::OutputGenerated){ dprintf("DynamicControl::notifyEvent - LayeredControlIF::OutputGenerated\n"); newNavDataCallback(NULL, event); } // TailCone "Reset" event if (event==DeviceIF::Initializing){ debug=True; dprintf("DynamicControl::notifyEvent - DeviceIF::Initializing\n"); tailConeCallback(NULL, event); } }