servoCommsSa.h

#ifndef SERVOCOMMSSA_H
#define SERVOCOMMSSA_H

// Required C header files from the array control code

#include "gcp/control/code/unix/libunix_src/common/regmap.h" // RegMapBlock

#include "gcp/antenna/control/specific/AxisPositions.h"
#include "gcp/antenna/control/specific/Board.h"
#include "gcp/antenna/control/specific/Model.h"
#include "gcp/antenna/control/specific/ServoCommandSa.h"
#include "gcp/antenna/control/specific/PmacTarget.h"

#include "gcp/util/common/Angle.h"
#include "gcp/util/common/FdSet.h"

#include <termios.h>

#include <string>

#define SERVO_TIMEOUT_USEC 150000 

namespace gcp {
  namespace antenna {
    namespace control {

      class SpecificShare;
      
      class ServoCommsSa : public Board {
    
      public:
    
    ServoCommsSa(SpecificShare* share, std::string name, bool sim);
    
    ServoCommsSa();
    
    ~ServoCommsSa();

    void setAzEl(gcp::util::Angle& az, gcp::util::Angle& el);
    void setAzEl(gcp::util::Angle& az, gcp::util::Angle& el, gcp::util::Angle& az1, gcp::util::Angle& el1, gcp::util::Angle& az2, gcp::util::Angle& el2, gcp::util::TimeVal& mjd);
    void haltAntenna();
    void hardStopAntenna();
    void initializeAntenna();
    void queryStatus();
    void queryStatus(gcp::util::TimeVal& currTime);
    void queryAntPositions();
    void queryAntPositions(gcp::util::TimeVal& currTime);

    bool readPosition(AxisPositions* axes, Model* model);
    void commandNewPosition(PmacTarget* pmac);
    void commandNewPosition(PmacTarget* pmac, PmacTarget* pmac1, PmacTarget* pmac2, gcp::util::TimeVal& mjd);

    bool isCircuitBreakerTripped();
    bool isThermalTripped();
    bool isLidOpen();
    bool isBrakeOn();
    bool isInitialized();

    /* 
     * Check for first part of initialization
     */
    bool isPartOneComplete();

    void finishInitialization();
    
    void fillUtc(gcp::util::TimeVal& currTime);


    bool connect();
    
    void disconnect();

    bool servoIsConnected();
    
    void sendCommand(ServoCommandSa& command);
    
    int readResponse(ServoCommandSa& command);

    // Return true if the servo is not ready for another command

    bool isBusy();

    unsigned readPositionFault();

    //------------------------------------------------------------
    // Methods which will be used to read and write register values.
    // These methods will send a the appropriate command to the servo,
    // and time out waiting for a response.
    //------------------------------------------------------------
    
    int writeString(std::string message);

      private:

    bool sim_;

    int fd_;
    
    struct termios termioSave_;
    
    RegMapBlock* utc_;              
    RegMapBlock* azPositions_;          
    RegMapBlock* azErrors_;             
    RegMapBlock* elPositions_;          
    RegMapBlock* elErrors_;             
    RegMapBlock* slowAzPos_;        
    RegMapBlock* slowElPos_;        
    RegMapBlock* servoStatus_;      
    RegMapBlock* servoThermalCutouts_;  
    RegMapBlock* servoContactors_;      
    RegMapBlock* servoCircuitBreakers_; 
    RegMapBlock* servoBrakes_;      
    RegMapBlock* driveLids_;        
    RegMapBlock* azWrap_;               

    gcp::util::FdSet fdSet_;
    
    bool connected_;
    
      public:
    ServoCommandSa command_;

    ServoCommandSa issueCommand(ServoCommandSa::Request req, std::vector<float>& values);
    ServoCommandSa issueCommand(ServoCommandSa::Request req);
    
      private:
    bool responsePending_;

    bool initializationPartOne_;

    bool initializationComplete_;

    bool alarmStatus_;

    void serviceMsgQ();
    
    void zeroReadFds();
    
    void registerReadFd(int fd);
    
    int waitForNextMessage();

    int readTCPPort(ServoCommandSa& command);

    void waitForResponse();

    void recordStatusResponse(ServoCommandSa& command);

    void wait(long nsec=100000000);

    
      }; // End class ServoCommsSa
      
    }; // End namespace control
  }; // End namespace antenna
}; // End namespace gcp




#endif // End