#include <math.h>
#include <i86.h>
#include "TailConeDriver.h"
#include "TailConeServer.h"
#include "Attributes.h"
#include "IntegerAttribute.h"
#include "FloatAttribute.h"
#include "BooleanAttribute.h"
#include "AttributeParser.h"
#include "System.h"
#include "TimeP.h"
#include "VehicleConfigurationIF.h"
#include "PeriodicTask.h"
#include "Syslog.h"
#include "MathP.h"
#include "stp100.h"
#include "sv203.h"

#define WATCHDOG_OUTPUT (8)
#define MOTOR_OFFSET (128)

const double in2m = .0254;        // meters / inch
const double m2s = 94488.189;     // steps / meter, number of steps to move linear actuators a meter.     

TailConeDriver::TailConeDriver(SerialDevice *device,
			       int period)
  : PeriodicTask("tailConeDriver")
{
  _period = period;
  _log = NULL;
  loadConfiguration();
  _rudderpos = _elevatorpos = 0.0;
  _maxElevatorCounts = elevatorSteps( _maxElevatorAngle );
  _minElevatorCounts = elevatorSteps( _minElevatorAngle );
  _maxRudderCounts = rudderSteps( _maxRudderAngle );
  _minRudderCounts = rudderSteps( _minRudderAngle );

  Syslog::write("TailConeServer -- "
		"Maximum Elevator Actuator Excursion is %d Counts", 
		_maxElevatorCounts);

  Syslog::write("TailConeServer:: -- "
		"Minimum Elevator Actuator Excursion is %d Counts", 
		_minElevatorCounts);

  Syslog::write("TailConeServer -- "
		"Maximum Rudder Actuator Excursion is %d Counts", 
		_maxRudderCounts);

  Syslog::write("TailConeServer -- "
		"Minimum Rudder Actuator Excursion is %d Counts", 
		_minRudderCounts);

  _commandCount = _count = 0;
  _device = device;
  _device->setLineFormat(19200, 8, 1, "NONE");
  _device->raw();


  //set up interfaces to periodic task
  _input = new TailConeInput(SharedData::Read);
  _output = new TailConeOutput(SharedData::Write);
  _input->read();
  _input->data.rudder = _input->data.elevator = _input->data.propOmega = 0;

  //create the log
  _log = new TailConeLog(this, DataLog::BinaryFormat);

  //grab device handles for rudder, elevator, and thruster
  _rudder = new stp100(_device,(int)_rudderBoardNum);
  _elevator = new stp100(_device, (int)_elevatorBoardNum);
  _propeller = new sv203(_device, (int)_propellerBoardNum);

  //since we can't create the control surface objects before we load the config,
  //we have to copy over some things here
  _rudder->Pot1Home(_rudderHome);
  _rudder->Pot1CntsToSteps(_rudderCntsToSteps);
  _elevator->Pot1Home(_elevatorHome);
  _elevator->Pot1CntsToSteps(_elevatorCntsToSteps);

  _elevator->selectBoard();
  initElevator();
  homeElevator();
  _rudder->selectBoard();
  initRudder();
  homeRudder();
  _propeller->selectBoard();
  initPropeller();

  //set up deadman;
  _deadman = 0; _deadmanCntDown = 0;

  addPeriodicCallback(period, 
		      (CallbackMethod )TailConeDriver::periodicCallback);


  _serverProxy = new TailConeIF("tailConeServer");


  // Subscribe to events from server
  _eventService->subscribe(_serverProxy,
			   TailConeIF::Initialized,
			   (EventService::Callback )TailConeDriver::eventCallback);
  

  _eventService->subscribe(_serverProxy,
			   TailConeIF::SetDeadman,
			   (EventService::Callback )TailConeDriver::eventCallback);
}


static int iabs(int i)
{
  if(i < 0)
    i = -1*i;
  return i;
}

TailConeDriver::~TailConeDriver()
{
  
  _propeller->selectBoard();
  _propeller->moveAbsolute(0);
  _elevator->selectBoard();
  setElevatorAngle(0.0);
  _rudder->selectBoard();
  setRudderAngle(0.0);

  if (_log) {
    delete _log;
  }

  delete _input;
  delete _output;
}

void TailConeDriver::periodicCallback()
{
static int i = 0, cnt = 0;
static unsigned int reindex = 0;
static unsigned int logWrite = 0;
struct timespec startTime, endTime;
double loopTime;

  clock_gettime(CLOCK_REALTIME, &startTime);

  // Read fin and prop commands from server (via shared memory)
  _input->read();

  //send new commands to boards and read back state  
  rudderControlLoop(!(reindex%5), _input->data.rudder);
  elevatorControlLoop(!((reindex+1)%5), _input->data.elevator);
  reindex++;


  //toggle propeller watchdog and execute rudder control loop
  if (_deadman && (_deadmanCntDown-- == 0)) {
    _deadmanCntDown = 0;
    _input->data.propOmega = 0;
  }


  if (!_propeller->selectBoard()) {
    propellerControlLoop(_input->data.propOmega);
    _propeller->pinToggle(WATCHDOG_OUTPUT);
  }

  clock_gettime(CLOCK_REALTIME, &endTime);
  loopTime = (double)(endTime.tv_sec+endTime.tv_nsec/1.0e9) -
    (double)(startTime.tv_sec+startTime.tv_nsec/1.0e9);
  //  printf("looptime %f\n", loopTime);


  _log->write();

  // Fill in output structure
  _output->data.elevator =
    rudderRadians(_rudder->pot1()*_rudder->Pot1CntsToSteps());
  _output->data.rudder = 
    elevatorRadians(_elevator->pot1()*_elevator->Pot1CntsToSteps());
  _output->data.propOmega = _currentSpeed;
  _output->data.status = DeviceIF::Ok;
  /*
  _output->data.elevatorCurrent = ...;
  _output->data.rudderCurrent = ...;
  */
  _output->data.propCurrent1 = _elevator->AD2();
  /*
  _output->data.propCurrent2 = ...;
  */

  //reset deadman on propeller
  Time::gettime(&_output->data.sampleTime);

  // Make output data available to server (via shared memory)
  _output->write();
}

DeviceIF::Status TailConeDriver::setDeadman(short msec)
{
  _deadman = msec;
  _deadmanCntDown = msec/_period+1;
  return DeviceIF::Ok;
}

DeviceIF::Status TailConeDriver::initialize()
{
  return DeviceIF::Ok;
}

void TailConeDriver::eventCallback(TaskInterface *taskIF,
				   EventCode event)
{
  switch (event) {

  case TailConeIF::Initialized:
    _output->data.status = initialize();
    break;

  case TailConeIF::SetDeadman:
    _input->read();
    if ((_output->data.status = setDeadman(_input->data.deadmanMillisec)) ==
	DeviceIF::Ok)
      // Successfully set deadman to input value
      _output->data.deadmanMillisec = _input->data.deadmanMillisec;

    break;

  default:
    return;
  }

  // Make output, status available to server
  _output->write();
}

//begin generic "control surface" functions
Boolean TailConeDriver::controlSurfaceReindex(stp100 *surface)
{
int errorSteps, potSteps;
int currentSteps, goalSteps;

    //reset if we see more than 2 A/D cnts error
    // convert to stepper motor steps here
    errorSteps = surface->Pot1CntsToSteps() * 2;

    //read current A/D, current counts, and goal coutns
    try {
      surface->readState();
    }
    catch(...) {
      printf("unable to read state\n");
      return False;
    }

    //convert from A/D counts to steps
    potSteps = surface->pot1() * surface->Pot1CntsToSteps();
    currentSteps = surface->currentSteps(); goalSteps = surface->goalSteps();
    if (goalSteps == currentSteps) { //have we stopped moving
      if (iabs(potSteps-(currentSteps+surface->offset())) >= errorSteps) {
	surface->offset(potSteps-currentSteps); //if we're not, them rehome
      } else {
	//	printf("offset set to 0\n");
	//	surface->offset(0);
      }
      return True;
    } else {
      return False;
    }

    return False;
}

////////////////////////////////////////////////////////////////////////////
// *************** BEGIN ELEVATOR CONTROL FUNCTIONS ***********************
void TailConeDriver::initElevator()
{
  int stepdelay = 0;
  int mindelay = 0;
  int accel = 0;
  int mode = 0;
  Boolean tryagain = False;
  int count = 0;
 
  do
    {
      try
	{
	  count++;
	  _elevator->setStepMode(STEPFULL);
	  delay(100);
	  _elevator->setDelay(_elevatorStepDelay);
	  delay(100);
	  _elevator->setMinStepDelayFactor(_elevatorMinStepDelay);
	  delay(100);
	  _elevator->setAccelFactor(_elevatorAccel);
	  delay(100);
	  
	  stepdelay =_elevator->readDelay();
	  delay(100);
	  mindelay =_elevator->readMinStepDelayFactor();
	  delay(100);
	  accel =_elevator->readAccelFactor();
	  delay(100);
          // this one is not quite definite
	  //  mode = _elevator->readRam(6);
         
	}
      catch(stp100Exception e)
	{
	  Syslog::write("TailConeServer::initElevator : Caught exception %s",e.msg);
	  tryagain = True;     
	}
      
      if((stepdelay != _elevatorStepDelay)||
	 (mindelay != _elevatorMinStepDelay)||
	 (accel != _elevatorAccel))
	{
	  Syslog::write("TailConeServer::initElevator : Didn't get the settings right, try again");
	  tryagain = True;
	}
      else
	tryagain = False;
      
    }while((tryagain)&&(count<10));
  
  if(tryagain == False)
    Syslog::write("TailConeServer::initElevator : Succeeded initializing elevator actuator, proceed");
  else
    {
      Syslog::write("TailConeServer::initElevator : Failure initializing elevator actuator, quit %i",count);
      exit(-1);
    }

}

void TailConeDriver::homeElevator()
{

  int pos1 = 0;
  int pos2 = 0;  
  int pot1 = 0;
  int pot = 0;
  int done = 0;

  // some high error values
  int poterr = 128;
  int poserr = 10000;
  int k = 23;
  int moveto = 0;
  int failures = 0;
  time_t t;
  do {
 
    try
      {
	_elevator->selectBoard();
	pot1 = _elevator->readA2D(1); delay(10);
	pos1 = _elevator->readPos(); delay(10);
	poterr = _elevatorHome - pot1;
	moveto = _elevator->Pot1CntsToSteps()*poterr;
	_elevator->incrementImmediate(moveto); delay(10);
	t = time(NULL);
	// wait till we get to commanded position
	// should use RT command here
	do{
	  pos2 = _elevator->readPos();	  
	  poserr = pos2 - (pos1 + moveto);	
          if(difftime(time(NULL),t) > 10.0)
	    throw stp100Exception("TailConeServer::homeElevator -- Failure to reach commanded position in 10 sec");
	  //  cout<<"Elev: poserr = "<<poserr<<"\n";
	}while(poserr != 0); 

      }    
    catch(stp100Exception e)
      {
	failures++;
	Syslog::write(e.msg);
        // make sure we don't exit loop if err happens to be zero
        poterr = 128;	
      }
    
  }while((iabs(poterr) > 1 )&&(failures < 10));
  
  if(failures == 10)
    {
      Syslog::write("BpauvTailConeServer::homeElevator -- Failed to initialize elevator");
      exit(-1);
    }

  _elevatorsteps = 0;
  try {_elevator->readState();}
  catch(...) {
	printf("weird address error happened in elevator initializtion\n");
  }
  _elevator->home(0);

}


/////////////////////////////////////////////////////////////////////////
// function to set elevator angle. Commanded angle is bounded by
// the maximum and minimum specified in the tailcone config file
// [input] desired elevator angle in radians

DeviceIF::Status TailConeDriver::setElevatorAngle(double desired_angle)
{
  Boolean debug = False;
  static long count = 0;
  dprintf("TailConeServer::setElevator()");
  TimeIF::TimeSpec time;
  double potSteps;  

  if (iabs(elevatorSteps(desired_angle)-_elevatorsteps) > 5) {
    _elevatorsteps = elevatorSteps(desired_angle);
    _elevatorpos = desired_angle;
    _elevator->moveImmediate(_elevatorsteps-_elevator->offset());
  } else {
    if (_elevator->currentSteps() != (_elevatorsteps-_elevator->offset())) {
      _elevator->moveImmediate(_elevatorsteps-_elevator->offset());
    }
  }

  /*
  if( _isSimulated  && _simulator ) {
       _simulator->command( _prevSpeed, desired_angle, _rudderpos );
  }
  */


  return DeviceIF::Ok;
}


DeviceIF::Status TailConeDriver::setElevatorCounts(short counts)
{
  Boolean debug = False;
  dprintf("setElevatorCounts()");

  _elevator->moveImmediate((int)counts);
  return DeviceIF::Ok;
}



DeviceIF::Status TailConeDriver::getComputedElevatorAngle(double *angle, 
					  TimeIF::TimeSpec *time)
{
  int pos;
  try
    {
     pos =_elevator->readPos();
    } 
  catch(Exception e)
    {
      Syslog::write("TailConeServer::getComputedElevatorAngle : caught excepetion %s",e.msg); 
    }
  
  
  *angle = elevatorRadians(pos);
  
  return DeviceIF::Ok;
}


DeviceIF::Status TailConeDriver::getElevatorAngle(double *angle, 
					  TimeIF::TimeSpec *time)
{
  try
    {
      *angle = elevatorRadians(_elevator->readA2D(1));     
    }
  catch(stp100Exception e)
    {
      Syslog::write("TailConeServer::getElevatorAngle : caught excpetion %s ",e.msg);
    }
  
  return DeviceIF::Ok;
}



/////////////////////////////////////////////////////////////////////////
// convert stp100  steps from home position to an angle in Radians

double TailConeDriver::elevatorRadians(short steps)
{
  double dR = (double)steps/m2s;           // how far from home position?
  double theta = 0.0;

  // should be ok here as long as wacky number of steps never gets input
  // positive angles occur when actuator gets shorter
  try{
    theta = _eTheta0 - acos((_eL1Sqrd+_eL2Sqrd - (_eR0+dR)*(_eR0+dR)) / (2*_eL1*_eL2) );
  } 
  catch(...){
    Syslog::write("TailConeServer: Caught exception in elevatorSteps");
    return 0;
  }

  return theta;
}

/////////////////////////////////////////////////////////////////
// function to compute the position to move to from the desired 
// angle specified for the BPAUV tailcone elevator

short TailConeDriver::elevatorSteps(double theta_desired)
{
  if(theta_desired > _maxElevatorAngle)
    theta_desired = _maxElevatorAngle;
  else if(theta_desired < _minElevatorAngle)
    theta_desired = _minElevatorAngle;
  
  
  double theta_command = _eTheta0 + theta_desired;
  double R_desired = 0.0;
  
  try{        
    R_desired = sqrt( _eL1Sqrd + _eL2Sqrd - (2.0*_eL1*_eL2*cos(theta_command)));
  }
  catch(...){
    Syslog::write("TailConeServer: Caught exception in elevatorSteps");
    return 0;
  }
  
  return ((short)( m2s * ( R_desired - _eR0 )));   // positive theta_desired vals cause actuator to get shorter
}

DeviceIF::Status TailConeDriver::elevatorControlLoop(int reindex,
						     double desired_angle)
{
  if (!reindex) return DeviceIF::Ok;

  if (_elevator->selectBoard()) return DeviceIF::Error;

  if (reindex) {
    if (controlSurfaceReindex(_elevator)) {
      return setElevatorAngle(desired_angle);
    } 
  }

  return DeviceIF::Ok;
}

//////////////////////////////////////////////////////////////////
// *************** BEGIN RUDDER CONTROL FUNCTIONS ***************
void TailConeDriver::initRudder()
{
  int stepdelay = 0;
  int mindelay = 0;
  int accel = 0;
  int mode = 0;
  Boolean tryagain = False;
  int count = 0;

  do
    {
      try
	{
	  count++;
	  delay(100);
	  _rudder->setStepMode(STEPFULL);
	  delay(100);
	  _rudder->setDelay(_rudderStepDelay);
	  delay(100);
	  _rudder->setMinStepDelayFactor(_rudderMinStepDelay);
	  delay(100);
	  _rudder->setAccelFactor(_rudderAccel);
	  delay(100);	  
	  stepdelay =_rudder->readDelay();
	  delay(100);
	  mindelay =_rudder->readMinStepDelayFactor();
	  delay(100);
	  accel =_rudder->readAccelFactor();
	  //  mode = _rudder->readRam(6);
	}
      catch(stp100Exception e)
	{
	  Syslog::write("TailConeServer::initRudder : Caught exception %s",e.msg);
	  tryagain = True;     
	}
      
      if((stepdelay != _rudderStepDelay)||(mindelay != _rudderMinStepDelay)||
	 (accel != _rudderAccel))
	{
	  tryagain = True;
	}
      else
	tryagain = False;
      
    }while((tryagain)&&(count<10));
  
  if(tryagain == False)
    Syslog::write("TailConeServer::initRudder : Succeeded initializing rudder actuator, proceed");
  else
    {
      Syslog::write("TailConeServer::initRudder : Failure initializing rudder actuator, quit");
      exit(-1);
    }

}

/////////////////////////////////////////////////////////////////////////
// convert stp100 steps from home position to an angle in Radians

double TailConeDriver::rudderRadians(short steps)
{
  double dR = (double)steps/m2s;           // how far from home position?
  double theta = 0.0;

  // should be ok here as long as wacky number of steps never gets input
  try{
    theta = acos( (_rL1Sqrd+_rL2Sqrd - (_rR0+dR)*(_rR0+dR)) / (2*_rL1*_rL2) ) - _rTheta0;
  }
  catch(...){
    Syslog::write("TailConeServer: Caught exception in rudderRadians");
    return 0.0;
  }
  
  return theta;

}

void TailConeDriver::homeRudder()
{

  int pos1 = 0;
  int pos2 = 0;  
  int pot1 = 0;
  int pot = 0;
  int done = 0;

  // some high error values
  int poterr = 128;
  int poserr = 10000;
  int moveto = 0;
  int failures = 0;
  time_t t;
  do {
 
    try
      {
	_rudder->selectBoard();
	pot1 = _rudder->readA2D(1); delay(10);
	pos1 = _rudder->readPos(); delay(10);
	poterr = _rudderHome - pot1;
	moveto = _rudder->Pot1CntsToSteps()*poterr;
	_rudder->incrementImmediate(moveto); delay(10);
	t = time(NULL);
	// wait till we get to commanded position
	do{
	  pos2 = _rudder->readPos();
	  poserr = pos2 - (pos1 + moveto);	
          if(difftime(time(NULL),t) > 10.0)
	    throw stp100Exception("TailConeServer::homeRudder -- Failure to reach commanded position in 10 sec");
	  //  cout<<"Rudd: poserr = "<<poserr<<"\n";
	}while(poserr != 0); 

      }    
    catch(stp100Exception e)
      {
	failures++;
	Syslog::write(e.msg);
        // make sure we don't exit loop if err happens to be zero
        poterr = 128;	      }
    
  }while((iabs(poterr) > 1 )&&(failures < 10));
  
  if(failures == 10)
    {
      Syslog::write("BpauvTailConeServer::homeRudder -- Failed to initialize rudder");
      exit(-1);
    }

  try {_rudder->readState();}
  catch(...) {
	printf("weird address happened in rudder initialization\n");
  }
  _rudder->home(0);

  
}
/////////////////////////////////////////////////////////////////////////
// function to set rudder angle. Commanded angle is bounded by
// the maximum and minimum specified in the tailcone config file
// [input] desired elevator angle in radians

DeviceIF::Status TailConeDriver::rudderControlLoop(int reindex, double desired_angle)
{
  if (!reindex) return DeviceIF::Ok;

  if (_rudder->selectBoard()) return DeviceIF::Error;

  if (reindex) {
    controlSurfaceReindex(_rudder);
  }

  return setRudderAngle(desired_angle);
}

DeviceIF::Status TailConeDriver::setRudderAngle(double desired_angle)
{
  Boolean debug = False;
  dprintf("TailConeDriver::setRudder()");
  static long count = 0;
  TimeIF::TimeSpec time;
  double current_angle;

  if (iabs(rudderSteps(desired_angle)-_ruddersteps) > 5) {
    _ruddersteps = rudderSteps(desired_angle);
    _rudderpos = desired_angle;
    _rudder->moveImmediate(_ruddersteps-_rudder->offset());
  } else {
    if (_rudder->currentSteps() != _ruddersteps-_rudder->offset()) {
      _rudder->moveImmediate(_ruddersteps);
    }
  }
  /*
  if( _isSimulated && _simulator ) {
       _simulator->command( _prevSpeed, _elevatorpos, desired_angle );
  }
  */


  return DeviceIF::Ok;
}


///////////////////////////////////////////////////////////////////////////
// function to set rudder motor position. Commanded movement is bounded by
// the maximum and minimum angles specified in the tailcone config file
// [input] desired linear motor position in steps from the zero
//         / home position.

DeviceIF::Status TailConeDriver::setRudderCounts(short counts)
{
  Boolean debug = False;
  dprintf("setRudderCounts()");

  _rudder->moveImmediate((int)counts);
  return DeviceIF::Ok;
}

//////////////////////////////////////////////////////////////////////
// Returns the current rudder angle in radians
// angle computed from the number of steps controller says the motor is 
// from the home position 

DeviceIF::Status TailConeDriver::getComputedRudderAngle(double *angle, 
					TimeIF::TimeSpec *time)
{
  int pos;
  try
    {
      pos = _rudder->readPos();
    }
  catch(Exception e)
    {
      Syslog::write("TailConeServer::getComputedElevatorAngle : caught excepetion %s",e.msg);            
    }
  
  *angle = rudderRadians(pos);

  return DeviceIF::Ok;
}


DeviceIF::Status TailConeDriver::getRudderAngle(double *angle, 
						TimeIF::TimeSpec *time)
{
  try
    {
      *angle = rudderRadians(_rudder->readA2D(1)*_rudder->Pot1CntsToSteps());     
    }
  catch(stp100Exception e)
    {
      Syslog::write("TailConeServer::getRudderAngle : exception caught, %s", e.msg);
    }
  
  return DeviceIF::Ok;
}

/////////////////////////////////////////////////////////////////
// function to compute the position to move to from the desired 
// angle specified for the BPAUV tailcone rudder 

short TailConeDriver::rudderSteps(double theta_desired)
{
  if(theta_desired > _maxRudderAngle)
    theta_desired = _maxRudderAngle;
  else if(theta_desired < _minRudderAngle)
     theta_desired = _minRudderAngle;

  
  double theta_command = _rTheta0 + theta_desired;
  double R_desired = 0.0;
  
  try{
    R_desired = sqrt( _rL1Sqrd + _rL2Sqrd - (2.0*_rL1*_rL2*cos(theta_command)));
  }  
  catch(...){
    Syslog::write("TailConeServer: Caught exception in rudderSteps");
    return 0;
  }
  
 
  return (short)( m2s * (R_desired - _rR0));  // positive theta_desired vals cause actuator to get longer

}
////////////////////////////////////////////////////////////////////////////
// *********** BEGIN PROPELLER CONTROL FUNCTIONS *************************


/////////////////////////////////////////////////////////////
// function to configure the propeller control system 

void TailConeDriver::initPropeller()
{
  // corresponds to the servo output of the sv203 at power up.
  // note that 128 and 0 correspond to the same output prop speed (0). I like to
  // set the throttle position to 0, though, since it turns off the amplifier
  _throttle_position = 128;
  _goalSpeed = 0;
  _propeller->selectBoard();
  _propeller->selectServo(1);
  _propeller->moveAbsolute(0); //turn prop off
  enablePropeller();

}


////////////////////////////////////////////////////////////////////
// function to implement rpm control of propeller motor.
// must choose throttle position that results in desired 
// rpm. Desired rpm may or may not be achievable depending
// on load etc.
// [input] desired speed in radians / sec.

DeviceIF::Status TailConeDriver::propellerControlLoop(double goalSpeed)
{
float accel, Perror, deltaThrottle;
int pwm;
static float scale = 1000.0;
int speed, temp;
static int coastCycles = 0;
Boolean debug = False;

  if (goalSpeed > 0.0 && _goalSpeed < 0.0) goalSpeed = 0.0;
  if (goalSpeed < 0.0 && _goalSpeed > 0.0) goalSpeed = 0.0;

 
 //if we want to stop, turn off the amplifier and let it coast to a stop
 if (goalSpeed == 0.0 && coastCycles == 0) {
   coastCycles = 10;
 }
 
 if (goalSpeed == 0.0 && coastCycles > 0) {
   _Perror = _Derror = _Ierror = _goalSpeed = 0.0;
   pwm = MOTOR_OFFSET - (_controlOutput/scale)*(coastCycles--/10);
   return setPropellerThrottle(pwm);
 }

  //limit acceleration from current goal speed to new goal speed
  accel = goalSpeed - _goalSpeed;
  if (accel > _maxDeltaRpm) accel = _maxDeltaRpm;
  if (accel < -_maxDeltaRpm) accel = -_maxDeltaRpm;
  _goalSpeed = _goalSpeed+accel;

  if ((_goalSpeed <= 0.1 && _goalSpeed >= -0.1) && (goalSpeed <=0.1 && _goalSpeed >=-0.1)) {
    _Perror = _Derror = _Ierror = _controlOutput = _goalSpeed = 0.0;
    return setPropellerThrottle(MOTOR_OFFSET);
  }

  //proportional error is _commandedSpeed - _currentSpeed
  _propeller->readState(&speed, &temp);
  _propTemp = temp;
  _currentSpeed = speed*4.0*(6.28/60.0);
  if (_throttle_position > 128) {
    _currentSpeed = -1.0*_currentSpeed;
  }
  printf("temp is %f C, rpm is %f\n", temp*1.5-273.15, speed*4.0);
  Perror = _goalSpeed - _currentSpeed;

  //integral error is just the sum of the proportional errors
  //we limit the integral error to avoid integrator wind-up
  _Ierror += Perror;
  if (_Ierror>_IntLimit) _Ierror = _IntLimit;
  if (_Ierror<-_IntLimit) _Ierror = -_IntLimit;

  //derivative error is the difference of the current proportional error and
  //last proportional error. We could do a windowed derivative, but we don't
  _Derror = Perror-_Perror;
  _Perror = Perror;

// dprintf("speed is %f RPM, perror is %f, accel is %f, goalspeed is %f\n",
//        speed*4.0, (float)_Perror, (float)accel, _goalSpeed); 

  //sum up all the control terms
  _controlOutput = _motorKmInv*_goalSpeed +
    _motorKp*_Perror + _motorKi*_Ierror + _motorKd*_Derror;


  //add in the offset
  //  printf("perror is %d\n, control output is %f\n", _Perror, _controlOutput);
  pwm = MOTOR_OFFSET - _controlOutput/scale;

   dprintf("speed is %f RPM, perror is %f, accel is %f, goalspeed is %f, pwm is %d\n",
        speed*4.0, (float)_Perror, (float)accel, _goalSpeed, pwm);                  
  return (setPropellerThrottle(pwm));
}


/////////////////////////////////////////////////////////////////////
// Fuction sends throttle position input directly to sv203 
// propeller motor controller.
// [input] desired throttle position in range [63..189]
//            63 is full reverse 
//            128 is set to be idle position.
//            189 is full ahead

DeviceIF::Status TailConeDriver::setPropellerThrottle(short pos)
{
  float deltaThrottle;
  Boolean debug = False;
  dprintf("TailConeServer::setPropellerThrottle()");

  if (!propellerEnabled())
    return DeviceIF::Offline;
 

  // Clamp the desired throttle position 
  if(pos > _maxThrottlePos)
    pos = _maxThrottlePos;
  else if(pos < _minThrottlePos)
    pos = _minThrottlePos;

  //final sanity check
  deltaThrottle = pos - _throttle_position;
  if (deltaThrottle > _maxDeltaThrottle) deltaThrottle = _maxDeltaThrottle;
  if (deltaThrottle < -_maxDeltaThrottle) deltaThrottle = -_maxDeltaThrottle;
  _throttle_position += deltaThrottle ;

  dprintf("moving prop to %d\n", _throttle_position);
  if (pos == MOTOR_OFFSET) {
    _propeller->moveAbsolute(0);
    _throttle_position = MOTOR_OFFSET;
  } else {
    _propeller->moveAbsolute(_throttle_position);
  }
 
  return DeviceIF::Ok;
}


DeviceIF::Status TailConeDriver::getPropellerThrottle(short *throttlepos)
{
  *throttlepos = _throttle_position;
  return DeviceIF::Ok;
}



//////////////////////////////////////////////////////////////////////////
// function to return the current rotational speed of the propeller
// and the sampletime.
// [output] propeller speed in rpm
// [output] sampletime 

DeviceIF::Status TailConeDriver::getPropellerRpm(short *rpm)
{
  double RPMperVolt = 4.1;   // change for dorado
  double throttle_offset = 128; 
  Boolean debug = False;
  double voltage;
  static long count = 0;

  try
    {
      count++;
      voltage = (double) (_propeller->readA2D(1));
    }
  catch(Exception e)
    {
      Syslog::write("TailConeServer::getPropellerRpm :  caught exception %s call # %i",e.msg,count); 
      return DeviceIF::Error;
    }  
    
  *rpm = (short) (RPMperVolt*voltage);
  
  return DeviceIF::Ok;
}


//////////////////////////////////////////////////////////////////////////
// function to return the current rotational speed of the propeller
// and the sampletime.
// [output] propeller speed in radians / sec
// [output] sampletime 

DeviceIF::Status TailConeDriver::getPropellerSpeed(double *radsPerSec)
{
  short rpm;
  getPropellerRpm(&rpm);
  *radsPerSec = Omega(rpm);

  return DeviceIF::Ok;
}


////////////////////////////////////////////////////////
// Convert revolutions / minute to radians / sec

double TailConeDriver::Omega(short rpm)
{
  return rpm * 60.0 * Math::RadsPerDeg;
}


////////////////////////////////////////////////////////
// Convert radians / sec to revolutions / minute

int TailConeDriver::Rpm(double omega)
{
  return (int)(omega * Math::DegsPerRad / 6.0);
}


// *************** END PROPELLER CONTROL FUCNTIONS ************************
////////////////////////////////////////////////////////////////////////////

void TailConeDriver::loadConfiguration()
{
  Attributes attributes("doradoTailCone");

  attributes.add(new IntegerAttribute("rudder_board_num",
				    "Board number of the stp100 rudder controller",
				    &_rudderBoardNum));    

  attributes.add(new FloatAttribute("rudder_L1",
				    "Rudder Actuator Parameter L1",
				    &_rL1,  0.333));     // meters

  attributes.add(new FloatAttribute("rudder_L2",
			     "Rudder Actuator Parameter L2",
			     &_rL2,  0.075));     // meter

  attributes.add(new FloatAttribute("rudder_R0",
				    "Rudder Actuator Parameter R0",
				    &_rR0,  0.302));     // meters

  attributes.add(new FloatAttribute("rudder_Theta_zero",
				    "Rudder Actuator Parameter theta_zero",
				    &_rTheta0,  1.045));   // radians

  attributes.add(new FloatAttribute("maxRudderAngle",
				    "Maximum Rudder Angle",
				    &_maxRudderAngle, 0.262)); // radians

  attributes.add(new FloatAttribute("minRudderAngle",
				    "Minimum Rudder Angle",
				    &_minRudderAngle, -0.262));  // radians

  attributes.add(new IntegerAttribute("rudderStepDelay",
				    "Board number of the stp100 rudder controller",
				      &_rudderStepDelay));    
  
  attributes.add(new IntegerAttribute("rudderMinStepDelay",
				    "Board number of the stp100 rudder controller",
				      &_rudderMinStepDelay));    
  
  attributes.add(new IntegerAttribute("rudderAccel",
				      "Board number of the stp100 rudder controller",
				     &_rudderAccel));    
  
  attributes.add(new IntegerAttribute("rudderHome",
				     "Board number of the stp100 rudder controller",
				      &_rudderHome));    

  attributes.add(new IntegerAttribute("rudderPot1CntsToSteps",
				     "Board number of the stp100 rudder controller",
				      &_rudderCntsToSteps));    
  
  attributes.add(new IntegerAttribute("elevator_board_num",
				    "Board number of the stp100 elevator controller",
				    &_elevatorBoardNum));    

  attributes.add(new FloatAttribute("elevator_L1",
				    "Elevator Actuator Parameter L1",
				    &_eL1,  0.35));      // meters

  attributes.add(new FloatAttribute("elevator_L2",
				    "Elevator Actuator Parameter L2",
				    &_eL2,  0.076));     // meters

  attributes.add(new FloatAttribute("elevator_R0",
				    "Elevator Actuator Parameter R0",
				    &_eR0,  0.311));     // meters

  attributes.add(new FloatAttribute("elevator_Theta_zero",
				    "Elevator Actuator Parameter theta_zero",
				    &_eTheta0,  0.936));   // meters

  attributes.add(new FloatAttribute("maxElevatorAngle",
				    "Maximum Elevator Angle",
				    &_maxElevatorAngle, 0.262));   // radians

  attributes.add(new FloatAttribute("minElevatorAngle",
				    "Minimum Elevator Angle",
				    &_minElevatorAngle, -0.262));  // radians

  attributes.add(new IntegerAttribute("elevatorStepDelay",
				    "Board number of the stp100 rudder controller",
				      &_elevatorStepDelay));    
  
  attributes.add(new IntegerAttribute("elevatorMinStepDelay",
				    "Board number of the stp100 rudder controller",
				      &_elevatorMinStepDelay));    
  
  attributes.add(new IntegerAttribute("elevatorAccel",
				      "Board number of the stp100 rudder controller",
				     &_elevatorAccel));    
  
  attributes.add(new IntegerAttribute("elevatorHome",
				     "Board number of the stp100 rudder controller",
				      &_elevatorHome));    

  attributes.add(new IntegerAttribute("elevatorPot1CntsToSteps",
				     "Board number of the stp100 rudder controller",
				      &_elevatorCntsToSteps));    


  attributes.add(new IntegerAttribute("propeller_board_num",
				    "Board number of the sv203 propeller controller",
				    &_propellerBoardNum));    

  attributes.add(new IntegerAttribute("minThrottlePos",
				    "Full reverse throttle position",
				    &_minThrottlePos));    

  attributes.add(new IntegerAttribute("maxThrottlePos",
				    "Full ahead throttle position",
				    &_maxThrottlePos));    

  attributes.add(new FloatAttribute("propPitch",
				    "Dist traveled per radian of prop turn",
				    &_propPitch));


  attributes.add(new IntegerAttribute("motorKmInv",
				      "",
				      &_motorKmInv));

  attributes.add(new IntegerAttribute("motorKp",
				      "",
				      &_motorKp));


  attributes.add(new IntegerAttribute("motorKi",
				      "",
				      &_motorKi));

  attributes.add(new IntegerAttribute("motorIntLimit",
				      "",
				      &_IntLimit));

  attributes.add(new IntegerAttribute("motorKd",
				      "",
				      &_motorKd));

  attributes.add(new FloatAttribute("maxDeltaRpm",
				      "Maximum allowed change in rpm",
				      &_maxDeltaRpm, 13.0));

  attributes.add(new FloatAttribute("maxDeltaThrottle",
				      "Maximum allowed change in throttle position",
				      &_maxDeltaThrottle, 5.0));


  attributes.add(new IntegerAttribute("logFrequency",
				      "Read and log telemetry every nth "
				      "command",
				      &_logFrequency, 1));

  attributes.add(new BooleanAttribute("simulated",
				      "",
				       &_isSimulated,
				       False ) );

  attributes.add(new BooleanAttribute("debug",
				      "",
				      &debug) );

  AttributeParser::parse(System::configurationFile("doradoTailCone.cfg"),
			 &attributes, debug);


  _eL1Sqrd = _eL1*_eL1;
  _eL2Sqrd = _eL2*_eL2;
  _rL1Sqrd = _rL1*_rL1;
  _rL2Sqrd = _rL2*_rL2;

}