/****************************************************************************/ /* Copyright (c) 2000 MBARI */ /* MBARI Proprietary Information. All rights reserved. */ /****************************************************************************/ /* Summary : */ /* Filename : DynamicControlServer.cc */ /* Author : */ /* Project : */ /* Version : 1.0 */ /* Created : 02/07/2000 */ /* Modified : */ /* Archived : */ /****************************************************************************/ /* Modification History: */ /****************************************************************************/ #include #include "DynamicControlOutput.h" #include "DynamicControlServer.h" #include "VehicleConfigurationIF.h" #include "Syslog.h" #include "MathP.h" DynamicControlServer::DynamicControlServer() : DynamicControlIF_SK() { _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 Navigation "NewOutput" events subscribe(_navigation, NavigationIF::NewOutput, (EventCallback )DynamicControlServer::newNavDataCallback); // Subscribe to LayeredControl "MissionStarted" event subscribe(_layeredControl, LayeredControlIF::MissionStarted, (EventCallback )DynamicControlServer::missionStartCallback); // Subscribe to TailCone "Reset" event subscribe(_tailCone, DeviceIF::Initializing, (EventCallback )DynamicControlServer::tailConeCallback); _tailCone->enableThruster(); TailConeIF::Status status; if ((status =_tailCone->initialize()) != TailConeIF::Ok) { Syslog::write("DynamicControlServer::DynamicControl() - status %d from " "_tailCone->initialize()", status); } // // 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(); _kpp = vehicleConfig.kpPitch(); _kdp = vehicleConfig.kdPitch(); _kip = vehicleConfig.kiPitch(); _kpDepth = vehicleConfig.kpDepth(); _kiDepth = vehicleConfig.kiDepth(); _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(); _propEfficiency = vehicleConfig.propEfficiency(); _Xuabu = vehicleConfig.xuabu(); _decouple = (Boolean) vehicleConfig.decouple(); // // 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.; } DynamicControlServer::~DynamicControlServer() { delete _navigation; delete _tailCone; delete _layeredControl; delete _log; } void DynamicControlServer::setCommand(DynamicControlIF::Command *command) { memcpy( &_command, command, sizeof( DynamicControlIF::Command ) ); } void DynamicControlServer::setMaxHeadingRate(double rate) { _maxHeadingRate = rate; } double DynamicControlServer::maxHeadingRate() { return _maxHeadingRate; } void DynamicControlServer::setMaxDepthRate(double rate) { _maxDepthRate = rate; } double DynamicControlServer::maxDepthRate() { return _maxDepthRate; } void DynamicControlServer::setTau(float tau) { _tau = tau; } float DynamicControlServer::tau() { return _tau; } void DynamicControlServer::setPitchServoGains(double proportional, double integral, double derivative) { _pitchGains.p = proportional; _pitchGains.i = integral; _pitchGains.d = derivative; } void DynamicControlServer::pitchServoGains(double *proportional, double *integral, double *derivative) { *proportional = _pitchGains.p; *integral = _pitchGains.i; *derivative = _pitchGains.d; } void DynamicControlServer::setHeadingServoGains(double proportional, double integral, double derivative) { _headingGains.p = proportional; _headingGains.i = integral; _headingGains.d = derivative; } void DynamicControlServer::headingServoGains(double *proportional, double *integral, double *derivative) { *proportional = _headingGains.p; *integral = _headingGains.i; *derivative = _headingGains.d; } void DynamicControlServer::getLogData(DynamicControlIF::LogData *logData) { DynamicControlOutput::Data output; logData->propOmegaCmd = output.propOmegaCmd; logData->rudderCmd = output.rudderCmd; logData->elevatorCmd = output.elevatorCmd; } int DynamicControlServer::spawnAuxTasks() { return 0; } //========================================================================= void DynamicControlServer::process() { Boolean debug = False; double thruster_current, des_speed; double err_depth, hdg_err; double depth_cmd, pitch_cmd, elevator_cmd, heading, rudder; double xte, kxte, dn, de, heading_cmd, heading_cmd_smooth; double proportional, derivative, rudder_cmd; // // 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; } first = 1; dprintf("DynamicControlServer::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 dprintf("DynamicControlServer::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 // // Beware that _command.vertical can be either depth, pitch, or // elevator, depending on the mode. // depth_cmd = _command.vertical; // Extract the depth command //update( C_DEPTH, vvp->c[0].depth ); _des_depth = do_traj( depth_cmd, _maxDiveRate, &_depthTraj ); //update( T_DEPTH, _des_depth ); err_depth = _position.z - _des_depth; //monitor_msg( T_DEPTH, MonitorTraj ); /* FSH & DRY integrator prevents adjustments when desired depth rate is high. (Also clips at +-5 deg, BAM 10-Jan-1998) */ if ( fabs(_depthTraj.xdot) < .01 ) { _depthIntegral += _kiDepth * err_depth * _Ts; _depthIntegral = Math::limit( _depthIntegral, _maxDepthInt, -_maxDepthInt ); } mDes_pitch = _kpDepth * err_depth + _depthIntegral; mDes_pitch = Math::limit( mDes_pitch, _pitchLimit, -_pitchLimit ); //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; switch ( _command.horizontalMode ) { 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; xte = dn*sin(bearing) - de*cos(bearing); //update( WP_XTE, xte ); kxte = _kwph * xte; if ( fabs(kxte) < PIUPON2 ) heading_cmd = Math::modPi( bearing + kxte ); // // 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 ); // // doHeading is the PID heading control. It computes _rudder. // doHeading(heading, heading_cmd, &_attitude); break; case DynamicControlIF::Heading: /* heading control mode */ doHeading(heading, _command.horizontal, &_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; clock_gettime(CLOCK_REALTIME, &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); dprintf(" DynamicControl: Write to tailcone and terminate:\n"); dprintf(" Commanded Prop Speed = %.2f; Elevator = %.2f;" " Rudder = %.2f; radians\n", _propSpeedCmd, _elevator, _rudder); _tailCone->command( _propSpeedCmd, _elevator, _rudder ); } /*-----------------------------------------------------------------------* PITCH_CONTROL: Executes control of vehicle pitch. *-----------------------------------------------------------------------*/ double DynamicControlServer::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 DynamicControlServer::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 DynamicControlServer::delta_heading(double dh) { double result ; result = dh ; while(result > PI)result -= TWOPI ; while(result < -PI)result += TWOPI ; return(result) ; } void DynamicControlServer::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 DynamicControlServer::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 DynamicControlServer::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 DynamicControlServer::doHeading(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 DynamicControlServer::doHeading(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 DynamicControlServer::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 DynamicControlServer::tailConeCallback(TaskInterface *taskInterface, EventCode eventCode) { Syslog::write("DynamicControlServer - Got event %d from TailCone\n", eventCode); } void DynamicControlServer::missionStartCallback(TaskInterface *taskInterface, EventCode code) { Boolean debug = False; dprintf("*** DynamicControlServer::missionStartCallback()"); // LayeredControl says mission has started _missionStarted = True; _time = 0; } void DynamicControlServer::newNavDataCallback(TaskInterface *taskInterface, EventCode code) { if (!_navigationReady) { _navigationReady = True; // Inform rest of system that Navigation and DynamicControl are ready triggerEvent(DynamicControlIF::Ready); } Boolean debug = False; dprintf("DynamicControlServer::newNavDatacallback(): missionStarted=%d", _missionStarted); // // If the mission hasn't started yet return. // if (!_missionStarted) return; // // Measure the Navigation loop time, including the IPC lag from Nav to DynCnt. // struct timespec timeSpec; clock_gettime(CLOCK_REALTIME, &timeSpec); _clockTime = (timeSpec.tv_sec + timeSpec.tv_nsec/1.e9); _navLoopTimeDC = _clockTime - _lastTimeDC; _lastTimeDC = _clockTime; process(); _time += (SystemPeriodMillisec / 1000.); }