/****************************************************************************/
/* Copyright 1995 to 1998 MBARI                                             */
/****************************************************************************/
/* Summary  : SemiPower Motor Controller Driver for vxWorks                 */
/* Filename : semiPower.c                                                   */
/* Author   : Andrew Pearce                                                 */
/* Project  : Tiburon                                                       */
/* Version  : Version 1.0                                                   */
/* Created  : 08/31/98                                                      */
/* Modified : 11/25/98                                                      */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/* $Header:
 * $Log:
 */
/****************************************************************************/

#include <vxWorks.h>                /* vxWorks system declarations          */
#include <string.h>                 /* vxWorks string library functions     */
#include <semLib.h>                 /* vxWorks semaphore functions          */
#include <taskLib.h>                /* vxWorks task management functions    */
#include <rebootLib.h>              /* vxWorks system reboot library funcs  */
#include <stdioLib.h>               /* vxWorks standard I/O functions       */
#include <ctype.h>                  /* vxWorks character functions          */
#include <msgQLib.h>                /* vxWorks Message Queue Library        */
#include <logLib.h>                 /* vxWorks log message library funcs    */

#include "mbariTypes.h"             /* MBARI style guide type declarations  */
#include "mbariConst.h"             /* Miscellaneous constants              */

#include "datamgr.h"                /* Data Manager declarations            */
#include "dm_errno.h"               /* Data Manager error declarations      */

#include "sio32Drv.h"               /* Sio32 hardware and driver information*/
#include "sio32Server.h"            /* Sio32 server communication services  */
#include "sio32Client.h"            /* Sio32 client communication services  */

#include "semiPower.h"              /* semiPower Motor Controller defs      */
#include "semiPowerVx.h"            /* semiPower vxWorks Library definitions*/
#include "thrusterIF.h"

#define CR 0x0d                     /* Carriage Return                      */

#define SEMI_POWER_BAUD_RATE  19200 /* Serial Port Baud Rate                */
#define SEMI_POWER_DRIVE_KV      65 /* Voltage scale factor for 240v unit   */
#define SEMI_POWER_DRIVE_KI     312 /* Current scale factor for 240v unit   */

STATUS semiPowerSendDriveCmd(semiPowerSerial *serialIO, Byte driveAdr,
             semiPowerDriveCmdBits driveCommand);

Void semiPowerDriveStatus( semiPowerSerial *serialIO, Byte driveAdr,
    Word *driveStatus );

/****************************************************************************/
/* Function    : asciiHexToInt                                              */
/* Purpose     : Converts an ASCII hex string to an integer                 */
/* Inputs      : ASCII string                                               */
/* Outputs     : Returns 32 bit value                                       */
/****************************************************************************/
    MLocal Nat32
asciiHexToInt(char *hexString)
{
  Byte hex;
  Nat32 integer = 0;
                                   /* Ignore "0x" at start of string       */
  if (( *hexString == '0') && ( toupper(* (hexString + 1)) == 'X'))
    hexString += 2;

  while (isxdigit(*hexString))
  {
    hex     = *(hexString++);
    integer = (integer << 4) + (hex - (isdigit(hex) ? '0' : 'A' - 10));
  } /* while */

  return (integer);
} /* asciiHexToInt() */

/****************************************************************************/
/* Function    : semiPowerChecksum                                          */
/* Purpose     : Calculates checksum for semi power motor controller        */
/* Inputs      : Message to calculate checksum                              */
/* Outputs     : Returns 8 bit checksum                                     */
/****************************************************************************/
    MLocal char
semiPowerChecksum(char *cmdString)
{
  Reg Nat16 ch;
  Reg char checksum = 0;

  for (ch = 0; ch < strlen(cmdString); ch++)
    checksum += cmdString[ch];

  return ((checksum & 0x7f) | 0x40);  /* Clear bits 7 and higher, set bit 6 */
} /* semiPowerChecksum() */

/****************************************************************************/
/* Function    : semiPowerFaultHandlerHookAdd                               */
/* Purpose     : Calculates checksum for semi power motor controller        */
/* Inputs      : Message to calculate checksum                              */
/* Outputs     : Returns 8 bit checksum                                     */
/****************************************************************************/
    Void
semiPowerFaultHandlerHookAdd(semiPowerSerial *serialIO, FUNCPTR faultHandler,
   Int32 faultParam)
{
  serialIO->faultHandler      = faultHandler;
  serialIO->faultHandlerParam = faultParam;
} /* semiPowerFaultHandlerHookAdd() */

/****************************************************************************/
/* Function    : semiPowerSendCmdReply                                      */
/* Purpose     : Writes a parameter to the semi power motor controller      */
/* Inputs      : Serial Channel, drive address, parameter ID and value      */
/* Outputs     : Returns OK or ERROR                                        */
/****************************************************************************/
     STATUS
semiPowerSendCmdReply( semiPowerSerial *serialIO, Byte driveAdr,
    char *cmdString, Word cmdLen, char *ackString, Word ackLen, Word *value )
{
    char checksum;
    STATUS len;
    STATUS status = ERROR;

    semiPowerFaultCode faultCode;

    FOREVER
    {
      sio32ChanRxFlush(&serialIO->sio32);  /* Discard any old packets       */

                                   /* Send Data Packet to Serial Port       */
      writeServerPacket(&serialIO->sio32, (Byte *) cmdString, cmdLen);

                                   /* Read received packet from serial port */
      len = readServerPacket(&serialIO->sio32, (Byte *) ackString, ackLen,
             serialIO->sio32.timeout);

      if ((len == ERROR) || (ackString[1] != driveAdr))
        break;

      if (ackString[0] == '!')
      {
        logMsg("Fault string %s\n", ackString);

        checksum = ackString[4];
        ackString[4] = NULL;
                                    /* Validate checksum against reply       */
        if (semiPowerChecksum(ackString) == checksum)
        {
#if DEBUG
          printf ("Fault Recvd 0x%c%c\n", ackString[2], ackString[3]);
          printf ("Drive Status 0x%x\n", driveStatus);
#endif
          if (serialIO->faultHandler != NULL)
          {

            faultCode = (Word) asciiHexToInt(ackString + 2);
            serialIO->faultHandler(serialIO, driveAdr, faultCode,
                                     serialIO->faultHandlerParam);
          } /* if */
        } /* if */
      } /* if */

      else if (ackString[0] == '#')
      {                              /* Validate write cmd response          */
        checksum = ackString[2];
        ackString[2] = NULL;
                                     /* Validate checksum against reply      */
        if (semiPowerChecksum(ackString) == checksum)
        {
          status = OK;
          break;
        } /* if */
      } /* else */

      else if (ackString[0] == '%')
      {                             /* Validate read cmd response            */
        if (strncmp(&ackString[2], &cmdString[2], 3) != 0)
          break;

        checksum = ackString[9];
        ackString[9] = NULL;
                                    /* Validate checksum against reply       */
        if (semiPowerChecksum(ackString) == checksum)
        {
          *value = (Word) asciiHexToInt(ackString + 5);
          status = OK;
          break;
        } /* else */
      } /* else */

      else if (ackString[0] == '?')
    return (ERROR);             /* Drive reported an error condition    */

    } /* FOREVER */

    return(status);
} /* semiPowerSendCmdReply() */

/****************************************************************************/
/* Function    : semiPowerWriteCmd                                          */
/* Purpose     : Writes a parameter to the semi power motor controller      */
/* Inputs      : Serial Channel, drive address, parameter ID and value      */
/* Outputs     : Returns OK or ERROR                                        */
/****************************************************************************/
     STATUS
semiPowerWriteCmd( semiPowerSerial *serialIO, Byte driveAdr, char *paramId,
    Word value )
{
    Word temp;
    STATUS status, cmdLen;

    char ackString[6];
    char cmdString[12];

    bzero(ackString, 6);
    bzero(cmdString, 12);

    cmdLen = sprintf(cmdString, "&%c%s%04x", driveAdr, paramId, value);
    cmdString[cmdLen++] = semiPowerChecksum(cmdString);
    cmdString[cmdLen++] = CR;

#if DEBUG
    printf ("Write Cmd %s\n", cmdString);
#endif

    if (semTake(serialIO->sio32.mutexSem, serialIO->sio32.timeout) == ERROR)
        return(ERROR);

    status = semiPowerSendCmdReply(serialIO, driveAdr, cmdString, cmdLen,
                          ackString, 6, &temp);

    semGive(serialIO->sio32.mutexSem); /* Release access to serial channel */

    return(status);
} /* semiPowerWriteCmd() */

/****************************************************************************/
/* Function    : semiPowerReadCmd                                           */
/* Purpose     : Reads a parameter from the semi power motor controller     */
/* Inputs      : Serial Channel, drive address, parameter ID and value      */
/* Outputs     : Returns OK or ERROR and writes parameter to value          */
/****************************************************************************/
    STATUS
semiPowerReadCmd( semiPowerSerial *serialIO, Byte driveAdr, char *paramId,
    Word *value )
{
    STATUS status, cmdLen;

    char ackString[12];
    char cmdString[12];

    bzero(ackString, 12);
    bzero(cmdString, 12);

    cmdLen = sprintf(cmdString, "$%c%s", driveAdr, paramId);
    cmdString[cmdLen++] = semiPowerChecksum(cmdString);
    cmdString[cmdLen++] = CR;

#if DEBUG
    printf ("Read Cmd %s\n", cmdString);
#endif

    if (semTake(serialIO->sio32.mutexSem, serialIO->sio32.timeout) == ERROR)
        return(ERROR);

    status = semiPowerSendCmdReply(serialIO, driveAdr, cmdString, cmdLen,
                          ackString, 11, value);

    semGive(serialIO->sio32.mutexSem); /* Release access to serial channel */

    return(status);
} /* semiPowerReadCmd() */

/****************************************************************************/
/* Function    : semiPowerEEPROMValid                                       */
/* Purpose     : Reads EEPROM Valid parameter from semi power controller    */
/* Inputs      : Serial Channel, drive address, pointer to eeprom valid     */
/* Outputs     : Returns OK or ERROR and eeprom valid                       */
/****************************************************************************/
    Void
semiPowerEEPROMValid( semiPowerSerial *serialIO, Byte driveAdr,
    Word *eepromValid )
{
  semiPowerReadCmd(serialIO, driveAdr, "A00", eepromValid);
} /* semiPowerEEPROMValid() */

/****************************************************************************/
/* Function    : semiPowerSetDriveBaudRate                                  */
/* Purpose     : Writes serial I/O Baud Rate to semi power controller       */
/* Inputs      : Serial Channel, drive address, baud rate                   */
/* Outputs     : Returns OK or ERROR                                        */
/****************************************************************************/
    MLocal STATUS
semiPowerSetDriveBaudRate( semiPowerSerial *serialIO, Byte driveAdr,
    Nat16 baud )
{
  if ((baud < 300) || (baud > 19200))
    return(ERROR);

  if (semiPowerWriteCmd(serialIO, driveAdr, "B01", baud) == ERROR)
    return(ERROR);

  if (semiPowerSendDriveCmd(serialIO, driveAdr, processorReset) == ERROR)
    return(ERROR);

  return (OK);
} /* semiPowerSetDriveBaudRate() */

/****************************************************************************/
/* Function    : semiPowerSendDriveCmd                                      */
/* Purpose     : Write Drive Command to semi power motor controller         */
/* Inputs      : Serial Channel, drive address, Drive Command               */
/* Outputs     : Returns OK or ERROR and Drive State                        */
/****************************************************************************/
    STATUS
semiPowerSendDriveCmd(semiPowerSerial *serialIO, Byte driveAdr,
    semiPowerDriveCmdBits driveCommand)
{
  Word driveCmd = clearBit(0xffff, driveCommand);

  return (semiPowerWriteCmd(serialIO, driveAdr, "C00", driveCmd));
} /* semiPowerSendDriveCmd() */

/****************************************************************************/
/* Function    : semiPowerSetDriveModeBit                                   */
/* Purpose     : Write Drive Mode Command to semi power motor controller    */
/* Inputs      : Serial Channel, drive address, Drive Mode Bit              */
/* Outputs     : Returns OK or ERROR and Drive State                        */
/****************************************************************************/
    STATUS
semiPowerSetDriveModeBit( semiPowerSerial *serialIO, Byte driveAdr,
    semiPowerDriveMode driveModeBit, Word *driveModeCmd )
{
  Word driveMode;

  semiPowerReadCmd(serialIO, driveAdr, "C01", driveModeCmd);

  if (driveModeBit & BIT_SET)
    driveMode = setBit(  *driveModeCmd, driveModeBit & ~BIT_SET);
  else
    driveMode = clearBit(*driveModeCmd, driveModeBit & ~BIT_SET);

  *driveModeCmd = driveMode;

  return (OK);
} /* semiPowerSetDriveModeBit() */

/****************************************************************************/
/* Function    : semiPowerSendDriveMode                                     */
/* Purpose     : Write Drive Mode Command to semi power motor controller    */
/* Inputs      : Serial Channel, drive address                              */
/* Outputs     : Returns OK or ERROR and Drive State                        */
/****************************************************************************/
    STATUS
semiPowerSendDriveMode( semiPowerSerial *serialIO, Byte driveAdr, Word driveModeCmd )
{
  return (semiPowerWriteCmd(serialIO, driveAdr, "C01", driveModeCmd));
} /* semiPowerSendDriveMode() */

/****************************************************************************/
/* Function    : semiPowerDriveState                                        */
/* Purpose     : Reads semi power motor controller Drive State Parameter    */
/* Inputs      : Serial Channel, drive address, pointer to Drive State      */
/* Outputs     : Returns OK or ERROR and Drive State                        */
/****************************************************************************/
    STATUS
semiPowerDriveState( semiPowerSerial *serialIO, Byte driveAdr,
    Word *driveState )
{
  return (semiPowerReadCmd(serialIO, driveAdr, "D00", driveState));
} /* semiPowerDriveState() */

/****************************************************************************/
/* Function    : semiPowerDriveStatus                                       */
/* Purpose     : Reads semi power motor controller Drive Status Parameter   */
/* Inputs      : Serial Channel, drive address, pointer to Drive Status     */
/* Outputs     : Returns OK or ERROR and Drive Status                       */
/****************************************************************************/
    Void
semiPowerDriveStatus( semiPowerSerial *serialIO, Byte driveAdr,
    Word *driveStatus )
{
  semiPowerReadCmd(serialIO, driveAdr, "D01", driveStatus);
} /* semiPowerDriveStatus() */

/****************************************************************************/
/* Function    : semiPowerClearDriveFault                                   */
/* Purpose     : Reads semi power motor controller Drive Status Parameter   */
/* Inputs      : Serial Channel, drive address, pointer to Drive Status     */
/* Outputs     : Returns OK or ERROR and Drive Status                       */
/****************************************************************************/
    STATUS
semiPowerClearDriveFault( semiPowerSerial *serialIO, Byte driveAdr,
    Word *driveStatus )
{
    char paramId[4] = "D01";

    char ackString[12];
    char cmdString[12];

    STATUS cmdLen;

    bzero(ackString, 12);
    bzero(cmdString, 12);

    cmdLen = sprintf(cmdString, "$%c%s", driveAdr, paramId);
    cmdString[cmdLen++] = semiPowerChecksum(cmdString);
    cmdString[cmdLen++] = CR;

#if DEBUG
    printf ("Clear Drive Fault\n");
#endif

    return(semiPowerSendCmdReply(serialIO, driveAdr, cmdString, cmdLen,
                          ackString, 11, driveStatus));
} /* semiPowerDriveStatus() */

/****************************************************************************/
/* Function    : semiPowerInputVoltage                                      */
/* Purpose     : Reads semi power motor controller input voltage            */
/* Inputs      : Serial Channel, drive address, pointer to voltage          */
/* Outputs     : Returns OK or ERROR and voltage in volts                   */
/****************************************************************************/
    STATUS
semiPowerInputVoltage( semiPowerSerial *serialIO, Byte driveAdr, Int16 *volts )
{
  Word value;
  *volts  = 0;

  if (semiPowerReadCmd(serialIO, driveAdr, "D07", &value) == ERROR)
    return(ERROR);

  *volts = value / SEMI_POWER_DRIVE_KV;

  return(OK);
} /* semiPowerInputVoltage() */

/****************************************************************************/
/* Function    : semiPowerOutputPower                                       */
/* Purpose     : Reads semi power motor controller output power             */
/* Inputs      : Serial Channel, drive address, pointer to power            */
/* Outputs     : Returns OK or ERROR and power in watts                     */
/****************************************************************************/
    STATUS
semiPowerOutputPower( semiPowerSerial *serialIO, Byte driveAdr, Int16 *watts )
{
  Word value;
  *watts  = 0;

  if (semiPowerReadCmd(serialIO, driveAdr, "D0C", &value) == ERROR)
    return(ERROR);

  *watts = value;

  return(OK);
} /* semiPowerOutputPower() */

/****************************************************************************/
/* Function    : semiPowerMotorCurrent                                      */
/* Purpose     : Reads semiPower motor controller motor current             */
/* Inputs      : Serial Channel, drive address, pointer to current          */
/* Outputs     : Returns OK or ERROR and current in mA                      */
/****************************************************************************/
    STATUS
semiPowerMotorCurrent( semiPowerSerial *serialIO, Byte driveAdr,
    Int16 *currentmA )
{
  Word value;

  *currentmA = 0;

  if (semiPowerReadCmd(serialIO, driveAdr, "D03", &value) == ERROR)
    return(ERROR);

                                       /* sqrt(2) converts current from RMS */
  *currentmA = (Int16) (((Flt32) (value * 1000))
		     / (SEMI_POWER_DRIVE_KI * sqrt(2)));

  return(OK);
} /* semiPowerMotorCurrent() */

/****************************************************************************/
/* Function    : semiPowerMotorTorque                                       */
/* Purpose     : Reads semiPower actual motor torque                        */
/* Inputs      : Serial Channel, drive address, pointer to torque           */
/* Outputs     : Returns OK or ERROR and torque in Nm * 100                 */
/****************************************************************************/
    STATUS
semiPowerMotorTorque(semiPowerSerial *serialIO, Byte driveAdr, Int16 *torqueNm)
{
  Word value;
  *torqueNm = 0;

  if (semiPowerReadCmd(serialIO, driveAdr, "D0B", &value) == ERROR)
    return(ERROR);

  *torqueNm = value;

  return(OK);
} /* semiPowerMotorTorque() */

/****************************************************************************/
/* Function    : semiPowerElapsedHours                                      */
/* Purpose     : Reads semi power motor controller elapsed hours            */
/* Inputs      : Serial Channel, drive address, pointer to hours            */
/* Outputs     : Returns OK or ERROR and elapsed hours in hours             */
/****************************************************************************/
    STATUS
semiPowerElapsedHours( semiPowerSerial *serialIO, Byte driveAdr, Int16 *hours )
{
  Word value;
  *hours = 0;

  if (semiPowerReadCmd(serialIO, driveAdr, "D0F", &value) == ERROR)
    return(ERROR);

  *hours = value;

  return(OK);
} /* semiPowerElapsedHours() */

/****************************************************************************/
/* Function    : semiPowerDynamicBrake                                      */
/* Purpose     : Reads semi power motor controller dynamic break accumulator*/
/* Inputs      : Serial Channel, drive address, pointer to accumulator      */
/* Outputs     : Returns OK or ERROR and Dynamic Brake Accumulator DB_PT    */
/****************************************************************************/
    STATUS
semiPowerDynamicBrake( semiPowerSerial *serialIO, Byte driveAdr, Int16 *DB_PT )
{
  Word value;
  *DB_PT = 0;

  if (semiPowerReadCmd(serialIO, driveAdr, "D13", &value) == ERROR)
    return(ERROR);

  *DB_PT = value;

  return(OK);
} /* semiPowerDynamicBrake() */

/****************************************************************************/
/* Function    : semiPowerHumidity                                          */
/* Purpose     : Reads semi power motor controller humidity sensor          */
/* Inputs      : Serial Channel, drive address, pointer to humidity         */
/* Outputs     : Returns OK or ERROR and humidity in %RH                    */
/****************************************************************************/
    STATUS
semiPowerHumidity( semiPowerSerial *serialIO, Byte driveAdr, Nat16 *humidity )
{
  Word bits;
  Nat16 rh;
  Flt32 volts;

  *humidity = 0;

  if (semiPowerReadCmd(serialIO, driveAdr, "D21", &bits) == ERROR)
    return(ERROR);
                                   /* Vout = 5.00V x ((.0062 x RH%) + 0.16) */
                                   /* RH% = ((Vout/5.0) - 0.16) / .0062     */
  volts = ((Flt32) bits) * (10.0 / 32768.0);
  rh = (Nat16) (((volts / 5.0) - 0.16) / 0.0062);

  *humidity = rh;

  return(OK);
} /* semiPowerHumidity() */

/****************************************************************************/
/* Function    : semiPowerHeatsinkTemp                                      */
/* Purpose     : Reads semi power motor controller heatsink temperature     */
/* Inputs      : Serial Channel, drive address, pointer to temperature      */
/* Outputs     : Returns OK or ERROR and temperature in degrees             */
/****************************************************************************/
    STATUS
semiPowerHeatsinkTemp( semiPowerSerial *serialIO, Byte driveAdr, Flt32 *degs )
{
  Word value;
  *degs  = 0.0;

  if (semiPowerReadCmd(serialIO, driveAdr, "D0A", &value) == ERROR)
    return(ERROR);

  *degs = ((Flt32) value) / 10.0;

  return(OK);
} /* semiPowerHeatsinkTemp() */

/****************************************************************************/
/* Function    : semiPowerReadFaultQueue                                    */
/* Purpose     : Reads EEPROM Valid parameter from semi power controller    */
/* Inputs      : Serial Channel, drive address, pointer to eeprom valid     */
/* Outputs     : Returns OK or ERROR and eeprom valid                       */
/****************************************************************************/
    STATUS
semiPowerReadFaultQueue( semiPowerSerial *serialIO, Byte driveAdr,
    struct faultQueueEntry faultQueue[FAULT_QUEUE_LENGTH] )
{
  char cmd[10];
  Int32 fault;
  Word numFaults, driveStatusSave;
  Word elapsedTimeHigh, elapsedTimeLow;
  Nat16 i_mag, vdc;
  Int16 rpm;

  for (fault = 0; fault < FAULT_QUEUE_LENGTH; fault++)
  {
    faultQueue[fault].driveStatus = 0xffff;
    faultQueue[fault].elapsedTime = 0;
    faultQueue[fault].i_mag     = 0;
    faultQueue[fault].vdc       = 0;
    faultQueue[fault].rpm       = 0;
    faultQueue[fault].paRiscPad = 0;
  } /* for */

  if (semiPowerReadCmd(serialIO, driveAdr, "E01", &numFaults) != OK)
    return(ERROR);

  for (fault = 0; fault < numFaults; fault++)
  {
    sprintf(cmd, "E%02X", 0x0b + (fault * 0x09));
    semiPowerReadCmd(serialIO, driveAdr, cmd, &driveStatusSave);
    faultQueue[fault].driveStatus = driveStatusSave;

    sprintf(cmd, "E%02X", 0x0c + (fault * 0x09));
    semiPowerReadCmd(serialIO, driveAdr, cmd, &elapsedTimeLow);

    sprintf(cmd, "E%02X", 0x0d + (fault * 0x09));
    semiPowerReadCmd(serialIO, driveAdr, cmd, &elapsedTimeHigh);
    faultQueue[fault].elapsedTime = (elapsedTimeHigh << 16) + elapsedTimeLow;

    sprintf(cmd, "E%02X", 0x0f + (fault * 0x09));
    semiPowerReadCmd(serialIO, driveAdr, cmd, &i_mag);
    faultQueue[fault].i_mag = (i_mag * 1000) / SEMI_POWER_DRIVE_KI; /* mA */

    sprintf(cmd, "E%02X", 0x10 + (fault * 0x09));
    semiPowerReadCmd(serialIO, driveAdr, cmd, &vdc);
    faultQueue[fault].vdc = vdc / SEMI_POWER_DRIVE_KV;

    sprintf(cmd, "E%02X", 0x11 + (fault * 0x09));
    semiPowerReadCmd(serialIO, driveAdr, cmd, (Nat16 *) &rpm);
    faultQueue[fault].rpm = rpm;
  } /* for */

  return (OK);
} /* semiPowerReadFaultQueue() */

#if 0
/****************************************************************************/
/* Function    : semiPowerSetMotorSpeed                                     */
/* Purpose     : Sends Motor Speed Command to Motor                         */
/* Inputs      : Serial Channel, drive address, desired torque value        */
/* Outputs     : Returns OK or ERROR                                        */
/****************************************************************************/
    STATUS
semiPowerSetMotorSpeed( semiPowerSerial *serialIO, Byte driveAdr, Int16 speed,
    Word *driveModeCmd )
{                                   /* Send Motor Speed Command to Motor  */

                                    /* Send Motor Direction based on sign */
  semiPowerSetDriveModeBit( serialIO, driveAdr,
     (speed < 0 ? reverseDirectionCmd : forwardDirectionCmd), driveModeCmd);

  semiPowerSendDriveMode( serialIO, driveAdr, *driveModeCmd );

  semiPowerWriteCmd(serialIO, driveAdr, "C02", (Word) abs(speed));

  return(OK);
} /* semiPowerSetMotorSpeed() */
#endif

/****************************************************************************/
/* Function    : semiPowerSetMotorSpeed                                     */
/* Purpose     : Sends Motor Speed Command to Motor                         */
/* Inputs      : Serial Channel, drive address, desired torque value        */
/* Outputs     : Returns OK or ERROR                                        */
/****************************************************************************/
    STATUS
semiPowerSetMotorSpeed( semiPowerSerial *serialIO, Byte driveAdr, Int16 speed,
    Word *driveModeCmd )
{                                   /* Send Motor Speed Command to Motor  */
  semiPowerWriteCmd(serialIO, driveAdr, "C02", speed);
  return(OK);
} /* semiPowerSetMotorSpeed() */

/****************************************************************************/
/* Function    : semiPowerSetMotorTorque                                    */
/* Purpose     : Sends Motor Torque Command to Motor                        */
/* Inputs      : Serial Channel, drive address, desired torque value        */
/* Outputs     : Returns OK or ERROR                                        */
/****************************************************************************/
    STATUS
semiPowerSetMotorTorque( semiPowerSerial *serialIO, Byte driveAdr, Int16 torque )
{                                   /* Send Motor Torque Command to Motor */
  semiPowerWriteCmd(serialIO, driveAdr, "C03", torque);
  return(OK);
} /* semiPowerSetMotorTorque() */

/****************************************************************************/
/* Function    : semiPowerSetMotorEnable                                    */
/* Purpose     : Sends Motor Enable/Disable Command to Motor                */
/* Inputs      : Serial Channel, drive address, desired enable/disable      */
/* Outputs     : Returns OK or ERROR                                        */
/****************************************************************************/
    STATUS
semiPowerSetMotorEnable( semiPowerSerial *serialIO, Byte driveAdr, MBool enable )
{
#ifdef DEBUG
  printf ("Motor %s\n", (enable ? "Enabled" : "Disabled"));
#endif

  return (semiPowerSendDriveCmd( serialIO, driveAdr,
             (enable ? startRequest : stopRequest) ));
} /* semiPowerSetMotorEnable() */

/****************************************************************************/
/* Function    : semiPowerClearFaultQueue                                   */
/* Purpose     : Sends command to controller to clear fault queue           */
/* Inputs      : Serial Channel, drive address                              */
/* Outputs     : Returns OK or ERROR                                        */
/****************************************************************************/
    STATUS
semiPowerClearFaultQueue( semiPowerSerial *serialIO, Byte driveAdr )
{
  return (semiPowerSendDriveCmd( serialIO, driveAdr, clearFaultQueue ));
} /* semiPowerClearFaultQueue() */

/****************************************************************************/
/* Function    : semiPowerMotorVelocity                                     */
/* Purpose     : Reads signed motor velocity command from motor controller  */
/* Inputs      : Serial Channel, drive address, pointer to motor velocity   */
/* Outputs     : Returns OK or ERROR                                        */
/****************************************************************************/
    STATUS
semiPowerMotorVelocity( semiPowerSerial *serialIO, Byte driveAdr, Int16 *motorRPM )
{
  Int16 rpm;

  if (semiPowerReadCmd(serialIO, driveAdr, "D12", &rpm) == ERROR)
    return(ERROR);

  *motorRPM = rpm;

  return(OK);
} /* semiPowerMotorVelocity() */

/****************************************************************************/
/* Function    : semiPowerMotorInit                                         */
/* Purpose     : Initializes Semipower motor controller                     */
/* Inputs      : Serial Channel, drive address                              */
/* Outputs     : Returns OK or ERROR                                        */
/****************************************************************************/
    Void
semiPowerMotorInit( semiPowerSerial *serialIO, Byte driveAdr, Word *dMode )
{
    Word driveStatus;

    semiPowerDriveStatus( serialIO, driveAdr, &driveStatus );
    semiPowerDriveStatus( serialIO, driveAdr, &driveStatus );

    semiPowerSetDriveModeBit( serialIO, driveAdr, useStopMode,         dMode );
    semiPowerSetDriveModeBit( serialIO, driveAdr, speedRegulatorMode,  dMode );
    semiPowerSetDriveModeBit( serialIO, driveAdr, flyingStartsDisable, dMode );
    semiPowerSetDriveModeBit( serialIO, driveAdr, efficiencyDisable,   dMode );
    semiPowerSetDriveModeBit( serialIO, driveAdr, dontCalcMotorParams, dMode );
    semiPowerSetDriveModeBit( serialIO, driveAdr, forwardDirectionCmd, dMode );
    semiPowerSetDriveModeBit( serialIO, driveAdr, normalCommMode,      dMode );
    semiPowerSetDriveModeBit( serialIO, driveAdr, noExtendCommission,  dMode);
    semiPowerSetDriveModeBit( serialIO, driveAdr, BLDCMode,            dMode );
    semiPowerSetDriveModeBit( serialIO, driveAdr, closedLoopMode,      dMode );

    semiPowerSendDriveMode( serialIO, driveAdr, *dMode );
} /* semiPowerMotorInit() */

/****************************************************************************/
/* Function    : semiPowerMotorFeedback                                     */
/* Purpose     : Reads and prints telemetry values from Semipower controller*/
/* Inputs      : Serial Channel, drive address                              */
/* Outputs     : None                                                       */
/****************************************************************************/
    Void
semiPowerMotorFeedback( semiPowerSerial *serialIO, Byte driveAdr )
{
    Int16 velocity, volts, watts, hours, DB_PT;
    Nat16 humidity;
    Flt32 heatsinkTemp;

    semiPowerInputVoltage( serialIO, driveAdr, &volts );
    semiPowerOutputPower(  serialIO, driveAdr, &watts );
    semiPowerElapsedHours( serialIO, driveAdr, &hours );
    semiPowerDynamicBrake( serialIO, driveAdr, &DB_PT );
    semiPowerHeatsinkTemp( serialIO, driveAdr, &heatsinkTemp );
    semiPowerHumidity(     serialIO, driveAdr, &humidity );
    semiPowerMotorVelocity(serialIO, driveAdr, &velocity );

    printf ("%d Volts  ", volts);
    printf ("%d Watts  ", watts);
    printf ("%d Hours  ", hours);
    printf ("%d RPM  ",   velocity);
    printf ("Temp = %4.1f degs  ", heatsinkTemp);
    printf ("Humidity = %d %RH  ", humidity);
    printf ("\n");
} /* semiPowerMotorFeedback() */