/** \file
 *
 *  Contains the ThrusterHE class implementation.
 *
 *  Copyright (c) 2007,2008,2009 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 */

/*
The thruster HE board needs to be configured prior to use.
At the time of writing the script in Tools/configureThrusterHE
was used to configure the board. See the interface guide for
further configuration info.
*/

#include "ThrusterHE.h"
#include "ThrusterHEIF.h"

#include <cstdlib>

#include "controlModule/SpeedControlIF.h"
#include "data/ConfigReader.h"
#include "data/SimSlate.h"
#include "data/Slate.h"
#include "data/StrValue.h"
#include "data/UniversalDataWriter.h"
#include "units/Units.h"

#define RATED_SPEED_DIVISOR (511)       // commanded RPM = command/ratedSpeed*511
#define SPEED_MULTIPLIER (0.53)         // When speed is read, multiply by this to get Hz
#define NUM_POLES ( 8.0)                // The number of poles the motor has

ThrusterHE::ThrusterHE( const Module* module )
    : EZServoServo( ThrusterHEIF::NAME, ThrusterHEIF::UART, ThrusterHEIF::BAUD, module, THRUSTER ),
      loadControl_( ThrusterHEIF::LOAD_CONTROL, !simulateHardware(), logger_, this ),
      startup_( INITIALIZE ),
      // indicates whether things are ok to run
      ok_( false ),
      // Debugging outputs
      debug_( false ),
      lastCmdPropOmega_( nanf( "" ) ), // Stores the previous command
      cmdPropOmega_( nanf( "" ) ), // Stores the commanded propeller rotational velocity
      motorPosCmd_( nanf( "" ) ), // Stores the commanded motor speed
      controllerVelocityCmd_( nanf( "" ) ), // Stores the commanded velocity
      // Allowable Number of bad velocity reads
      allowableBadVelocityCfg_( 5 ),
      // Number of bad velocity reads
      badVelocityCount_( 0 ),
      // Current speed is within the bounds of what was commanded
      thrustSpdGood_( false ),
      // Velocity good after the command has been issues and the thruster is run this long without fault
      velocityTimeout_( 10.0 ),
      // Time it takes to respond to a FW/RV command. The firmware waits up to 1.5 seconds
      commandTimeout_( 1.6 )

{
    // Create the outputs
    propOmegaWriter_ = newUniversalWriter( UniversalURI::PLATFORM_PROPELLER_ROTATION_RATE, Units::RADIAN_PER_SECOND, 0.25 );

    // Create the inputs from Dynamic Control
    propOmegaReader_ = newDataReader( SpeedControlIF::PROP_OMEGA_ACTION );
    propOmegaReader_->setImplementor( true );

    // Config settings shared by all Servos:
    powerOnTimeoutCfgReader_ = newConfigReader( ThrusterHEIF::POWER_ON_TIMEOUT_CFG ); // Time to allow system to power up before commanding

    ratedSpeedCfgReader_ = newConfigReader( ThrusterHEIF::RATED_SPEED_CFG );
    deviationCfgReader_ = newConfigReader( ThrusterHEIF::DEVIATION_CFG );
    allowableBadVelocityCfgReader_ = newConfigReader( ThrusterHEIF::ALLOWABLE_BAD_VELOCITY_CFG ); // Allowable Number of bad velocity reads

    // This configures the advanced run modes.
    setRunState( START );
    persistentPause_ = true;
}


ThrusterHE::~ThrusterHE()
{}


// Load Thruster Servo parameters
bool ThrusterHE::readConfig( void )
{
    // Check if all the parameters are read correctly
    bool ok = EZServoServo::readConfig();

    // Thruster values
    ok &= ratedSpeedCfgReader_->read( Units::COUNT, ratedSpeedCfg_ );                     // Holds the rated speed set in the AMT49406
    ok &= deviationCfgReader_->read( Units::RADIAN_PER_SECOND, deviationCfg_ );           // Deviation value allowed between expected and actual
    ok &= allowableBadVelocityCfgReader_->read( Units::COUNT, allowableBadVelocityCfg_ ); // Allowable Number of bad velocity reads
    return ok;
}


void ThrusterHE::run( void )
{
}


void ThrusterHE::uninitialize( void )
{
    logger_.syslog( "Uninitialize Thruster Servo." );
    uninitializeStart();

    if( !simulateHardware() )
    {
        logger_.syslog( "Powering down", Syslog::INFO );
        if( !loadControl_.powerDown() )
        {
            logger_.syslog( "Failed to power down", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
        }
        startup_ = INITIALIZE;
    }
    uninitializeEnd();
}

/// Should return [myNamespace]::SIMULATE_HARDWARE, or [myNamespace]::POWER, etc
ConfigURI ThrusterHE::getConfigURI( ConfigOption configOption ) const
{
    return configOption == CONFIG_SIMULATE_HARDWARE ? ThrusterHEIF::SIMULATE_HARDWARE : ConfigURI::NO_CONFIG_URI;
}

// Thruster Servo Initialization Routines
bool ThrusterHE::initThruster( void )
{
    bool retVal( false );

    if( !simulateHardware() )
    {
        if( uart_.canReadUntil( '>' ) ) // Normal power up
        {
            uart_ << "WU\r"; // Make sure the AMT49406 is awake
            retVal = true;
            if( debug_ ) logger_.syslog( "Normal Prompt. Good Init", Syslog::INFO );

            if( uart_.hasError() )
            {
                logger_.syslog( "Thruster initialization uart error ", uart_.errorString(), Syslog::ERROR );
                retVal = false;
            }
        }
        else if( uart_.canReadUntil( '<' ) )  // Internal Error reported
        {
            logger_.syslog( "Internal error reported. Error codes are not defined in interface spec.", Syslog::ERROR );
            retVal = false;
        }
    }
    thrusterHEFault_ = NO_ERROR;
    return retVal;
}


// Do what needs to be done to run
// Similar to initialize, in old init/run/uninit sequence
Component::RunState ThrusterHE::start()
{
    if( debug_ ) logger_.syslog( "Start", Syslog::INFO );

    this->setAllowableFailures( 5 );
    this->setRetryTimeout( 45 );

    if( !simulateHardware() )
    {
        if( !loadControl_.powerUp() )
        {
            logger_.syslog( Str( "Error: Thruster load controller failed to power up.\n" ), Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            return START;
        }

        initializeStart();
    }
    logger_.syslog( "Initializing ThrusterHE." );

    ok_ = true;
    if( !readConfig() )
    {
        ok_ = false;
        // Critical error
        logger_.syslog( Str( "Error: Error loading parameters in initialization routine. Returning.\n" ), Syslog::CRITICAL );
        return START;
    }
    startTime_ = Timestamp::Now();
    return STARTING;
}


/// Might follow a STOP...START sequence
Component::RunState ThrusterHE::starting()
{
    if( debug_ ) logger_.syslog( "Starting", Syslog::INFO );

    badVelocityCount_ = 0;

    // Wait until PO reset has occurred
    if( startTime_.elapsed() < powerOnTimeoutCfg_ )
    {
        return STARTING;
    }

    if( simulateHardware() )
    {
        return RUNNABLE;
    }

    thrustSpdGood_ = false;

    switch( startup_ )
    {
    case INITIALIZE:
        if( initThruster() )
        {
            startTime_ = Timestamp::Now();
            startup_ = WAIT;
        }
        else
        {
            logger_.syslog( Str( "Thruster failed to initialize" ), Syslog::FAULT );
            this->setFailure( FailureMode::COMMUNICATIONS );
            return STOP;
        }
        break;
    case WAIT:
        if( startTime_.elapsed() >= eepromWriteTimeout_ )
        {
            startup_ = EXECUTE;
        }
        break;
    case HOME:
        break;
    case EXECUTE:
        uart_ << "BD\r"; // parking brake off!

        uart_.flushCRLF().readLine( uartResponse_, sizeof( uartResponse_ ) );

        if( !uart_.hasError() )
        {
            startup_ = DONE;
        }
        else
        {
            logger_.syslog( "Thruster initialization uart error:", uart_.errorString(), Syslog::FAULT );
            this->setFailure( FailureMode::COMMUNICATIONS );
            return STOP;
        }
        break;
    case VERIFY:
        break;
    case DONE:
        startup_ = INITIALIZE;
        uart_.flush();
        return RUNNABLE;
        break;
    }
    return STARTING;
}


// Pause for a short period (indicated by pauseTime)
Component::RunState ThrusterHE::pause()
{
    if( debug_ ) logger_.syslog( "Pause", Syslog::INFO );
    if( !simulateHardware() )
    {
        if( !simulateHardware() && !loadControl_.powerDown() )
        {
            logger_.syslog( "Failed to power down", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            return STOP;
        }
        uart_.close();
    }
    propOmegaWriter_->write( Units::RADIAN_PER_SECOND, 0 );
    thrustSpdGood_ = false; // Let's force a command to be sent the next time we resume.

    return PAUSED;
}


/// Should eventually follow a PAUSE request: should set continueTime
Component::RunState ThrusterHE::paused()
{
    //if( debug_ ) logger_.syslog( "Paused", Syslog::INFO );
    if( isNeeded() )
    {
        return resume();
    }
    return PAUSED;
}


Component::RunState ThrusterHE::resume()
{
    if( debug_ ) logger_.syslog( "Resume", Syslog::INFO );
    if( !simulateHardware() )
    {
        uart_.open();
        if( uart_.hasError() )
        {
            logger_.syslog( "Error opening port on resume: ", uart_.errorString() );
        }
        if( !loadControl_.powerUp() )
        {
            logger_.syslog( "Failed to power up", Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
            return STOP;
        }
    }
    startTime_ = Timestamp::Now();
    return RESUMING;
}


Component::RunState ThrusterHE::resuming()
{
    if( debug_ ) logger_.syslog( "Resuming", Syslog::INFO );

    if( startTime_.elapsed() < powerOnTimeoutCfg_ )
    {
        return RESUMING;
    }
    initThruster();
    readConfig();
    startTime_ = Timestamp::Now();
    uart_.flush();
    return RUNNABLE;
}


Component::RunState ThrusterHE::runnable()
{
    if( debug_ ) logger_.syslog( "Runnable", Syslog::INFO );

    // Check for async data on the serial port first in the event a fault was reported.
    if( uart_.dataAvailable() )
    {
        uart_.read( uartResponse_, uart_.dataAvailable() );
        if( debug_ ) logger_.syslog( "Unexpected data on serial port:" + ( Str ) uartResponse_, Syslog::INFO );

        if( getControllerFaults() ) // Parse/report any faults
        {
            if( debug_ ) logger_.syslog( "Fault detected", Syslog::INFO );
        }
    }


    // Processing for the thruster
    if( propOmegaReader_->isActive() )
    {
        propOmegaReader_->read( Units::RADIAN_PER_SECOND, cmdPropOmega_ );
        if( !isnan( cmdPropOmega_ ) )
        {

            // Check to make sure we aren't attempting a command greater than the rated speed.
            int rpmCmd = ( ( R2D( fabs( cmdPropOmega_ ) ) / 360.0 ) * 60.0 );
            if( rpmCmd > ratedSpeedCfg_ )
            {
                logger_.syslog( "Commanded speed of:" + Str( rpmCmd ) + " rpm exceeds configured ratedSpeed value.", Syslog::FAULT );
                this->setFailure( FailureMode::HARDWARE );
                return STOP;
            }

            float actualPropOmega;
            bool haveActualPropOmega = false;
            if( !simulateHardware() )
            {
                // Log voltage and current and check for any faults
                loadControl_.requestVoltageAndCurrent();
                if( loadControl_.hasError() )
                {
                    // Put anything that isn't an actual fault first
                    if( loadControl_.errorString().find( "Software Overcurrent" ) )
                    {
                        if( debug_ )logger_.syslog( "LCB error:" + loadControl_.errorString(), Syslog::ERROR );
                    }
                    // And things that set failures second
                    else
                    {
                        logger_.syslog( "LCB fault: " + loadControl_.errorString(), Syslog::FAULT );
                        this->setFailure( FailureMode::HARDWARE );
                        return STOP;
                    }
                }

                // Async faults from the controller are handled at the top, but some issues are masked in the
                // controller firmware. For example, in the event the prop isn't able to spin at all
                // the controller only shows one async "lock Detect" and then goes silent and does not
                // report futher failures to turn. So we need to check the status register by polling it
                // after handling any async faults that turn up (above).
                if( startTime_.elapsed() > commandTimeout_ ) // Wait for any velocity command to return prior to querying
                {
                    int status = getOperationState();
                    uart_.flush(); // Clear out any echo or responses so we don't grab them as async data

                    if( status != 0 ) // Check for a value
                    {
                        if( status == 5000 )
                        {
                            ++badVelocityCount_;
                            logger_.syslog( "Status lock Detected.", Syslog::IMPORTANT );
                        }
                        else if( status == 1100 || status == 1200 )
                        {
                            logger_.syslog( "Over current/temp status detected. Status reporting:" + ( Str ) status, Syslog::IMPORTANT );
                        }
                        else if( status == 1300 )
                        {
                            logger_.syslog( "Under voltage status detected. Status reporting:" + ( Str ) status, Syslog::IMPORTANT );
                        }
                    }
                }

                // Calculate commanded motor velocity
                // E.g. 300 rpm = 300 / 600 * 511 = 255 (00FF)
                // Where rated speed is 600 and the divisor is 511
                // Convert rad/s to rpm and then crank out the formula above to appease the controller.
                motorPosCmd_ = ( ( R2D( fabs( cmdPropOmega_ ) ) / 360.0 ) * 60.0 ) / ratedSpeedCfg_ * RATED_SPEED_DIVISOR;
                char hexPosCmd[5];
                sprintf( hexPosCmd, "%04X", ( int )motorPosCmd_ );

                // Send to controller if necessary.
                if( !thrustSpdGood_ )
                {
                    // Start the clock so we know when the last speed command was issued
                    startTime_ = Timestamp::Now();

                    // And now set direction here
                    if( cmdPropOmega_ >= 0 )
                    {
                        //Direction FW is forward for the vehicle (RH prop - clockwise spin)
                        uart_ << "FW," << Str( hexPosCmd ) << "\r";
                        if( debug_ ) logger_.syslog( "Send speed command forward:" + Str( hexPosCmd ), Syslog::INFO );
                    }
                    else
                    {
                        //Direction RV is reverse for the vehicle (RH prop - counter-clockwise spin)
                        uart_ << "RV," << Str( hexPosCmd ) << "\r";
                        if( debug_ ) logger_.syslog( "Send speed command reverse:" + Str( hexPosCmd ), Syslog::INFO );
                    }

                    thrustSpdGood_ = true; // Set to true unless proven otherwise below
                    lastCmdPropOmega_ = cmdPropOmega_; // Store the last command that was sent
                    uart_.flush(); // *** DEBUG
                }

                // Otherwise, check velocity if no velocity command is going to be sent
                else
                {
                    if( startTime_.elapsed() > commandTimeout_ ) // Wait for the velocity command to return prior to querying again
                    {
                        controllerVelocityCmd_ = getSpeed();
                        // If we're under way, try once more before failing so we don't stop the prop due to a single comms error
                        if( isnan( controllerVelocityCmd_ ) )
                        {
                            ++badVelocityCount_;
                            if( debug_ ) logger_.syslog( "Speed is bad", Syslog::INFO );
                        }

                        if( !uart_.hasError() )
                        {

                            if( !isnan( controllerVelocityCmd_ ) )
                            {
                                haveActualPropOmega = true;
                                actualPropOmega = controllerVelocityCmd_;
                                // Check to see that we're within the deviation bounds of the commanded speed of the motor
                                // and that the command hasn't flipped to the same value with a different sign.
                                if( ( ( fabs( actualPropOmega - fabs( cmdPropOmega_ ) ) <= deviationCfg_ ) ) && ( cmdPropOmega_ == lastCmdPropOmega_ ) )
                                {
                                    thrustSpdGood_ = true;
                                    badVelocityCount_ = 0;
                                    if( debug_ ) logger_.syslog( "Speed Good:" + Str( actualPropOmega ), Syslog::INFO );

                                    // Reset the fail count if the prop is turning within bounds
                                    if( startTime_.elapsed() > velocityTimeout_ )
                                    {
                                        this->resetFailCount();
                                    }
                                }
                                else
                                {
                                    if( ( startTime_.elapsed() > velocityTimeout_ ) && ( cmdPropOmega_ != 0 ) ) // Give the motor some time to spin up before we call foul.
                                    {
                                        // Need to adjust speed next time through
                                        thrustSpdGood_ = false;
                                        ++badVelocityCount_;
                                        if( debug_ ) logger_.syslog( "Reported speed not within cmd bounds. Speed:" + Str( actualPropOmega ), Syslog::INFO );
                                    }
                                }
                            }
                            // getVelocityCmd is returning nan so attempt to re-send the prop command
                            else
                            {
                                thrustSpdGood_ = false;
                            }
                        }
                        // There was a UART error reported
                        else
                        {
                            logger_.syslog( "Thruster uart error: ", uart_.errorString(), Syslog::FAULT );
                            this->setFailure( FailureMode::COMMUNICATIONS );
                            return STOP;
                        }
                        if( badVelocityCount_ > allowableBadVelocityCfg_ )
                        {
                            // Attempt to adjust speed next time through
                            thrustSpdGood_ = false;
                            return STOP;
                        }
                    }
                }
                uart_.flush(); // Clear out any echo or responses so we don't grab them as async data
            }
            else
            {
                haveActualPropOmega = SimSlate::Read( SimSlate::PROPELLER_OMEGA_RADIAN_PER_SECOND, actualPropOmega );
            }

            if( haveActualPropOmega )
            {
                propOmegaWriter_->write( Units::RADIAN_PER_SECOND, actualPropOmega );
            }
            // If we don't have actualPropOmega, maybe the controller is turned off?
            else if( loadControl_.getPowerState() == LoadControl::OFF )
            {
                propOmegaWriter_->write( Units::RADIAN_PER_SECOND, 0 );
            }
        }
    }


    // Pause if we don't want data
    if( !isNeeded() )
    {
        return PAUSE;
    }

    return RUNNABLE;
}


Component::RunState ThrusterHE::stop()
{
    if( debug_ ) logger_.syslog( "Stop", Syslog::INFO );
    if( !simulateHardware() )
    {
        uninitialize(); // First power down then query for faults next cycle
    }
    return STOPPING;
}


Component::RunState ThrusterHE::stopping()
{
    if( debug_ ) logger_.syslog( "Stopping", Syslog::INFO );

    if( !simulateHardware() )
    {
        loadControl_.readFaults(); // See if anything went wrong that may have caused this request for uninitialize
        if( loadControl_.hasError() )
        {
            logger_.syslog( "LCB fault: " + loadControl_.errorString(), Syslog::FAULT );
            this->setFailure( FailureMode::HARDWARE );
        }

    }
    // Currently used to delay one more cycle to allow the EZ Servo to fully power down
    return STOPPED;
}


Component::RunState ThrusterHE::stopped()
{
    if( debug_ ) logger_.syslog( "Stopped", Syslog::INFO );
    if( isNeeded() )
    {
        return start();
    }

    if( ( loadControl_.getPowerState() != LoadControl::OFF ) && ( loadControl_.getPowerState() != LoadControl::POWER_DOWN ) )
    {
        return stop();
    }

    // Close if the uart if it is open
    if( uart_.isReadable() )
    {
        uart_.close();
    }
    return STOPPED;
}


bool ThrusterHE::isDataRequested()
{
    return propOmegaWriter_->isDataRequested();
}


bool ThrusterHE::isNeeded()
{
    bool retVal = true;
    if( propOmegaReader_->isActive() && propOmegaReader_->read( Units::RADIAN_PER_SECOND, cmdPropOmega_ ) && !isnan( cmdPropOmega_ ) )
    {
        motorPosCmd_ = ( ( R2D( cmdPropOmega_ ) / 360 ) * 60 ) / ratedSpeedCfg_ * RATED_SPEED_DIVISOR;
        if( motorPosCmd_ == 0 ) retVal = false;
    }
    return retVal;
}


bool ThrusterHE::getControllerFaults()
{
    bool retVal = true; // Assume there's a fault
    if( strstr( uartResponse_, "Unrec" ) )
    {
        thrusterHEFault_ = UNRECOGNIZED_CMD_ERROR;
    }
    else if( strstr( uartResponse_, "Bad" ) )
    {
        thrusterHEFault_ = BAD_FORM_ERROR;
    }
    else if( strstr( uartResponse_, "LDetect" ) )
    {
        thrusterHEFault_ = LOCK_DETECT_ERROR;
    }
    else if( strstr( uartResponse_, "ZSpeed" ) )
    {
        thrusterHEFault_ = ZERO_SPEED_ERROR;
    }
    else if( strstr( uartResponse_, "OCP" ) )
    {
        thrusterHEFault_ = OVERCURRENT_ERROR;
    }
    else if( strstr( uartResponse_, "OTP" ) )
    {
        thrusterHEFault_ = OVERTEMP_ERROR;
    }
    else if( strstr( uartResponse_, "SysEr" ) )
    {
        thrusterHEFault_ = SYSTEM_ERROR;
    }
    else if( strstr( uartResponse_, "OVP" ) )
    {
        thrusterHEFault_ = OVERVOLTAGE_ERROR;
    }
    else
    {
        retVal = false; // No fault we know of was reported
    }
    // Log any faults from the controller
    // These are asynchronous and show up between commands
    switch( thrusterHEFault_ )
    {
    case NO_ERROR:
        break;

    // It's not so sure what that command is
    case UNRECOGNIZED_CMD_ERROR:
        logger_.syslog( "Unrecognized Command", Syslog::FAULT );
        this->setFailure( FailureMode::HARDWARE );
        break;

    // Bad Form! Otherwise known as a syntax error
    case BAD_FORM_ERROR:
        logger_.syslog( "Bad Form", Syslog::FAULT );
        this->setFailure( FailureMode::HARDWARE );
        break;

    case LOCK_DETECT_ERROR:
        logger_.syslog( "Lock Detect. Motor stopped spinning or could not start spinning.", Syslog::FAULT );
        this->setFailure( FailureMode::HARDWARE );
        break;

    // Not so much a fault in this case but noteworthy nonetheless.
    case ZERO_SPEED_ERROR:
        logger_.syslog( "Zero Speed Commanded.", Syslog::ERROR );
        break;

    case OVERCURRENT_ERROR:
        logger_.syslog( "Over Current Protection Reported.", Syslog::FAULT );
        this->setFailure( FailureMode::HARDWARE );
        break;

    case OVERTEMP_ERROR:
        logger_.syslog( "Over Temperature Protection Reported.", Syslog::FAULT );
        this->setFailure( FailureMode::HARDWARE );
        break;

    case SYSTEM_ERROR:
        logger_.syslog( "System Error Reported.", Syslog::FAULT );
        this->setFailure( FailureMode::HARDWARE );
        break;

    case OVERVOLTAGE_ERROR:
        logger_.syslog( "Over Voltage Protection Reported.", Syslog::FAULT );
        this->setFailure( FailureMode::HARDWARE );
        break;

    default:
        logger_.syslog( "Unexpected data: " + ( Str ) uartResponse_, Syslog::ERROR );
        break;
    }
    thrusterHEFault_ = NO_ERROR; // Clear this on our end and let the unit tell us if there's more.
    return retVal;
}


int ThrusterHE::getOperationState()
{
    int retVal = 0;

    uart_ << "RW,7F\r"; // Queries for status
    uart_.readLines( uartResponse_, sizeof( uartResponse_ ), 3 ); // Grab the echo, response, and next prompt.

    if( !uart_.hasError() )
    {
        if( 1 == sscanf( uartResponse_, "RW,7F\r%d", &retVal ) )
        {
            if( debug_ ) logger_.syslog( "Status Value: " + ( Str ) retVal, Syslog::INFO );
        }
    }
    else
    {
        logger_.syslog( "Could not get status:", uart_.errorString(), Syslog::FAULT );
    }
    return retVal;
}


// Returns rotation speed in rad/s
float ThrusterHE::getSpeed()
{
    int response;
    float retVal = nanf( "" );

    uart_ << "RW,78\r"; // Queries for speed in Hz
    uart_.readLines( uartResponse_, sizeof( uartResponse_ ), 3 ); // Grab the echo, response, and next prompt.

    if( !uart_.hasError() )
    {
        // grab the response and multiply by SPEED_MULTIPLIER to get the answer in Hz
        if( 1 == sscanf( uartResponse_, "RW,78\r%x", &response ) )
        {
            // And then return in rad/s
            retVal = ( ( response * SPEED_MULTIPLIER ) / NUM_POLES ) * ( 2 * M_PI );
        }
    }
    else
    {
        logger_.syslog( "getSpeed error ", uart_.errorString(), Syslog::FAULT );
    }
    return retVal;
}