satctl.h

Go to the documentation of this file.

#ifndef _SATCTL_H
#define _SATCTL_H
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <termios.h>
#include <string.h>
#include <math.h>
#include <sys/time.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/fcntl.h>
#include <sys/ioctl.h>
#include <asm/ioctls.h>
#include <bits/types.h>

/* Data structure definitions: */

/* A single momentum wheel: */
typedef struct {
  int wnum;             /* Wheel number */
  char *portfn;         /* Port filename */
  speed_t brate;        /* Baudrate as a speed_t */
  int portfd;           /* Open port fdes */
  float cvel;           /* Current velocity (rps) */
  float pvel;           /* Previous velocity (rps) */
  int status;           /* Current status */
  int type;             /* Motor type */
  float maxaccel;       /* Maximum acceleration (rps^2) */
  float maxvel;         /* Maximum velocity (rps) */
  float maxcur;         /* Maximum motor current */
                        /* Add position info */
                        /* Add moment info */
} wheel_t;

typedef struct {
  int lades;            /* LA number */
  char *serno;          /* LA serial number */
  char *portfn;         /* Port filename */
  int portfd;           /* Open port fdes */
  int hi_l;             /* Hi limit (on LA's step counter) */
  int lo_l;             /* Low limit (on LA's step counter) */
  int travel;           /* Total travel in steps (hi_l - lo_l)*/
  int mtt;              /* Estimated max end-to-end travel time (usec) */
  int curpos;           /* Current commanded abs position (0 @ neg end)
                         * Note that this is in a shifted range, whereas hi_l
                         * and lo_l are in the LA's real range.  So at the
                         * lower limit, curpos = 0, and at the high end,
                         * curpos = hi_l - lo_l.  In general, curpos is
                         * realpos - lo_l (where realpos is the position
                         * according to the LA's step counter as reported
                         * by SP and IP.  This means that hi_l and lo_l
                         * MUST be calculated before the LA is practically
                         * usable.  The shifted range limits then become
                         * 0 at the low end, and 'travel' at the high end.
                         */
  int posvel;           /* Positioning velocity ( x20,000 steps/sec) */
  int posacc;           /* Positioning acceleration (max 20 recommended) */
  int posdec;           /* Positioning deceleration (max 20 recommended) */
  int jogvel;           /* Jog velocity */
  int jogacc;           /* Jog accel/decel */
} la_t;
  

/* wheel_t.status - 0 - Not initialized
 *                  1 - Initialized
 *                  2 - Initialized but did not reach tvel (but did move)
 *                  3 - Initialized but stuck (did not move at all)
 */


/* A single delta-V command action: */
typedef struct {
  float vtarget;        /* Target velocity */
  float atarget;        /* Target acceleration */
  float actime;         /* Action time (depends on flags) */
  int flags;            /* Bit-mapped flags: See below*/
} dva_t;


/* A single set of attitude data: */
typedef struct {
  float xrate;
  float yrate;
  float zrate;
  float xaccel;
  float yaccel;
  float zaccel;
  float temp;
  int deltat;
  long serial;
} adat_t;

/* The master configuration structure */
typedef struct {
  char *mwPort;         /* MW port filename */
  speed_t mwSpeed;      /* MW port speeds */
  /* This is not the most compact way to do this, but it's less work, and
   * we're not so short on memory that we need to scrounge for the space
   * of two and half int's.  This way it's more straightforward, which is
   * probably more important, since this could potentially be confusing.
   * The motors will be assigned addresses serially based on their cabling
   * order.  Then, the first element of this array will contain the
   * address of the X motor, the second of the Y, and the third of the Z.
   */
  int mwOrder[3];       /* MW motor cabling order */
  char **laPort;        /* LA port filenames */
  speed_t *laSpeed;     /* LA port speeds */
  char *dmuPort;        /* DMU port filename */
  speed_t dmuSpeed;     /* DMU port speed */
  int regChan;          /* Thruster regulator channel (out)*/
  int thlChan;          /* Thruster Hi/Lo relay (out)*/
  int thAChan;          /* Thruster A (out)*/
  int thBChan;          /* Thruster B (out)*/
  int batChan[8];       /* Battery volt channels (in)*/
  int dmuRate[3];       /* DMU xyz turn rates */
  int dmuAccel[3];      /* DMU xyz accelerations */
  int taChan;
  int tbChan;
  int batchan[8];
} mcfg_t;



/* End of data structure definitions */


/* Global constants (and vars ;-> ) */

#define CMD_STRLEN 80   /* A default buffer length for serial comm */
#define MICROMO 1       /* Motor type for MicroMo (MVP2001A01 w/ 3863H024C) */
#define SMARTMOTOR 2    /* Motor type for SmartMotor (SM3420) */

#define DFLT_TVAL 1000000 /* Default timerval in seconds */
#define SS_TVEL 30.0    /* Spinup target velocity */
#define SS_TACC 30.0    /* Spinup target acceleration */
#define SU_TVTOL 2.0    /* Spinup target velocity error tolerance */


#ifdef INMAIN
const char cfgWords[11][9] = {
        "mwport",
        "mwspeed",
        "mworder",
        "laport",
        "laspeed",
        "dmuport",
        "dmuspeed",
        "dmudata",
        "thchan",
        "batchan",
        ""};
  struct itimerval mtmr1;       /* Timers for use in measuring acceleration */
  struct itimerval mtmr2;
  struct timeval dflt_tv;       /* The default settings for mtmr1 and mtmr2 */
  const float MM_SR = 2000.0;           /* 1/(500usec) = 1/(5e-4) = 2e+3 */
/*  const float MM_SR = 2000.0;          1/(1000usec) = 1/(1e-3) = 1e+3 */
  const float MM_ENC_RES = 15.15576;    /* Effective CPR to calibrate w/ SM */
  const float SM_VELSF = 64424.5;       /* cts / rev/sec */
  const float SM_ACCSF = 15.84;         /* cts/sec^2 / rev/sec^2 */
  const int LA_ACC = 20;                /* LA accel */
  const int LA_VEL = 5;                 /* LA velocity */
  const char *hex_charset = "0123456789ABCDEF";
  int debug = 0;
#else
  extern struct itimerval mtmr1;
  extern struct itimerval mtmr2;
  extern struct timeval dflt_tv;
  extern const float MM_ENC_RES;
  extern const float MM_SR;
/*  extern const float MM_RPM_COUNT;
  extern const float MM_RPS_COUNT;*/
  extern const float SM_VELSF;
  extern const float SM_ACCSF;
  extern const int LA_ACC;
  extern const int LA_VEL;
  extern const char *hex_charset;
  extern const int debug;
#endif

extern const char cfgWords[11][9];

#define MM_RPS_COUNT 2000/MM_SR*2000/MM_ENC_RES
                        /* 2000/MM_SR * 4 * (2000 / (4 * MM_ENC_RES)) */
#define MM_RPM_COUNT MM_RPS_COUNT/60.0
/* End of global constants */



/* Function prototypes and options defines: */

/**********************************************************************
 * Function: mwInit()
 * Description:  Initialize a momentum wheel.  Open the serial port,
 *      verify presence of configured motor type, and perform a variety
 *      of spinup tests.
 * Arguments:
 *      wheel  - Wheel to initialize
 *      action - Bit-mapped flag of startup tests to run
 * Return Values:
 *        -1 - Failed to open port
 *         0 - Initialized OK
 *         1 - Failed init tests
 *         2 - Motor not responding
 * Bugs: none known
 * Notes: Stable through several program revisions.
 * Last Update: 6/15/02
 * Last Updated By: Cengiz Akinli
 **********************************************************************/

int mwInit(wheel_t *wheel, int actions);

/* Spinup testing will proceed in order down this list.  Simple spinup should
 * be more than adequate most of the time.  Because of potential for motor
 * damage, shaking modes will not be immediately coded.
 */
#define MWIT_SSPINUP     0x0001 /* Simple spinup to a preset +RPM and check */
#define MWIT_BDSSPINUP   0x0002 /* Bidirectional simple spinup */

/* The below should ONLY be used in failure modes- i.e. to free a stuck motor */
#define MWIT_RSPINUP_3   0x0004 /* 3-stage ramped current spinup (to 60% max) */
#define MWIT_BDRSPINUP_3 0x0008 /* Bidirectional 3-stage ramped current */
#define MWIT_RSPINUP_5   0x0010 /* 5-stage ramped current (to max current) */
#define MWIT_BDRSPINUP_5 0x0020 /* Bidirectional 5-stage ramped current */

/* Last resort only- not implemented as of 11/25/02 */
#define MWIT_SHAKE_25    0x0040 /* 2 Hz shaking at 25% max current */
#define MWIT_SHAKE_50    0x0080 /* 2 Hz shaking at 50% max current */
#define MWIT_SHAKE_75    0x0100 /* 2 Hz shaking at 75% max current */
#define MWIT_SHAKE_100   0x0200 /* 2 Hz shaking at max current */



/**********************************************************************
 * Function: mwStop()
 * Description:  Shut down a momentum wheel (must be at v=0 to shutdown)
 * Arguments:
 *      wheel  - Wheel to initialize
 *      status - Failure status
 * Returns:
 *         0 - Shut down OK
 *         1 - Motor not at 0 velocity - no action taken
 *         2 - Motor not responding
 * Bugs: none known
 * Notes: Stable through several program revisions.
 * Last Update: 6/15/02
 * Last Updated By: Cengiz Akinli
 **********************************************************************/

int mwStop(wheel_t *wheel);



/**********************************************************************
 * Function: mwChangeVel()
 * Description:  Change a wheel's target velocity
 * Arguments:
 *      wheel   - Designated wheel
 *      params  - Delta-v action parameter structure
 *      status  - Failure status
 * Returns:
 *        -1 - Some failure occured (see status)
 *         0 - Initialized OK
 * Bugs: none known
 * Notes: Transience allows a new velocity to be set for a fixed period
 *      of time, after which the wheel is returned to the previous
 *      velocity.  Flagging an action as transient affects how blocking
 *      is implemented.  When you set the blocking flag for a transient
 *      action, the function blocks until the wheel returns to the
 *      previous velocity.  For a non-transient motion, the function
 *      blocks only for the number of microseconds in the actime
 *      parameter, which is then counted starting from the beginning of
 *      the action OR from the time the wheel reaches the target
 *      velocity, depending on whether the MWCV_TTA or MWCV_TAV flags
 *      are set respectively.
 * Last Update: 6/15/02
 * Last Updated By: Cengiz Akinli
 **********************************************************************/

int mwChangeVel(wheel_t *wheel, dva_t *params, int *status);

/* dva_t flags: */
#define MWCV_TTA 0x01   /* actime is total time for action (includes accel) */
#define MWCV_TAV 0x02   /* actime is only time at target vel (excludes accel) */
#define MWCV_FAOR 0x04  /* Fail on acceleration out of range (and do nothing) */
#define MWCV_FVOR 0x08  /* Fail on velocity out of range (and do nothing) */
#define MWCV_BLOCK 0x10 /* Block until end of action.  If non-transient, this
                         * will end up being the acceleration time. */
#define MWCV_TRANS 0x20 /* Motion is transient.  Return to previous velocity
                         * along same acceleration after actime seconds.
                         * MWCV_BLOCK is implied.  MWCV_TTA and MWCV_TAV
                         * control behavior in this case.
                         */
/* mwChangeVel() status bitmap: */
#define MWCV_AOR_S 0x01 /* Acceleration overrange failure */
#define MWCV_VOR_S 0x02 /* Velocity overrange failure */


/* Motion Pak data acquisition
 * This will be a standalone process forked by the dmuInit call and killed
 * by the dmuStop call.  It will poll the MP at the selected rate, calculate
 * the calibrated data and write it to a shared memory segment for pickup by
 * dmuReadData().  Currently, there is no overflow checking.  Thus, care
 * should be taken to ensure that the read interval is sufficiently short,
 * and that the shared mem segment large enough such that the chance of
 * overwriting unread data is minimized.  Overflow checking, and perhaps
 * dynamic reallocation, cacheing, and other more sophisticated handling
 * of data would be a more robust solution, but also could be involved and,
 * if the existing system is well implemented, it would also be unnecessary.
 */

/* Motion pak data read: */

/**********************************************************************
 * Function: dmuInit()
 * Description:  Initialize the DMU
 * Arguments:
 *     maxBufSz - Maximum number of data sets to buffer (Note: bytesize
 *                per set will be 7*sizeof(float)).
 *     rate     - Read rate in reads per second (0 for max allowed by baudrate)
 *     rpid     - Read process PID - out
 *     flags    - Bit-mapped flags
 *
 * Returns:
 *        -1 - Could not create FIFO
 *         0 - Initialized OK
 *         1 - rate exceeds max
 *         2 - open() failed on port
 *         3 - open() failed on data pipe
 *         4 - open() failed on control pipe
 *         5 - fork() failed
 * Bugs: 
 * Notes: See notes above regarding shared mem segment size vs read rates.
 * Last Update: 7/8/02
 * Last Updated By: Cengiz Akinli
 **********************************************************************/

int dmuInit(long maxBufferSize, int rate, int flags);

#define DMUI_FBA 0x01   /* On full data buffer, begin averaging data */
#define DMUI_FBD 0x02   /* On full data buffer, begin dropping data */
#define DMU_PORT "/dev/mopak"


/**********************************************************************
 * Function: dmuStop()
 * Description:  Shutdown the DMU read process.
 * Arguments:
 *         rpid - Read process PID
 *
 * Returns:
 *         0 - OK
 *         1 - Error on close()
 * Bugs: 
 * Notes:
 * Last Update: 7/8/02
 * Last Updated By: Cengiz Akinli
 **********************************************************************/

int dmuStop(pid_t rpid);



/**********************************************************************
 * Function: dmuReadData()
 * Description:  Read data from the DMU.  This will read ALL buffered
 *              data with each call.  accData AND ALL of its members
 *              must be statically allocated in advance.
 * Arguments:
 *      accData - Acceleration data struct
 *      fp      - File pointer to data pipe
 *      maxsets - Maximum number of sets to read (accData should be
 *                  pre-allocated for this many sets
 *
 *   Returns:
 *         Data sets read
 *        -2 - Buffer filled
 *        -1 - Pipe closed
 * Bugs: 
 * Notes:
 * Last Update: 7/8/02
 * Last Updated By: Cengiz Akinli
 **********************************************************************/

int dmuReadData(adat_t **accData, int maxsets);


__uint16_t mbcrc_16(__uint8_t *ptr, int len);


/**********************************************************************
 * Function: laInit()
 * Description:  Initialize a linear actuator
 * Arguments:
 *      lades  - actuator descriptor
 *      action - Bit-mapped flag of startup tests to run
 * Return Values:
 *        -1 - Failed to open port
 *         0 - Initialized OK
 *         1 - Move/Wait in progress (sleep and try back later)
 *         2 - Servo fault requiring power cycle of controller
 *         3 - Controller not responding
 *         4 - SP cmd fail during LAIT_CTRA
 *         5 - Failed to reach limit in la->mtt
 * Bugs: none known
 * Notes: 
 * Last Update: 11/25/02
 * Last Updated By: Cengiz Akinli
 **********************************************************************/

int laInit(la_t *lades, int action);

/**********************************************************************
 * Function: laMove()
 * Description:  Move a linear actuator relative to it current position
 * Arguments:
 *      lades  - actuator descriptor
 *      steps  - distance to move in counts
 * Return Values:
 *        -1 - Failed to open port
 *         0 - Initialized OK
 *         1 - Move/Wait in progress (sleep and try back later)
 *         2 - Servo fault requiring power cycle of controller
 *         3 - Controller not responding
 *         4 - SP cmd fail during LAIT_CTRA
 *         5 - Failed to reach limit in la->mtt
 * Bugs: none known
 * Notes: 
 * Last Update: 11/25/02
 * Last Updated By: Cengiz Akinli
 **********************************************************************/

int laMove(la_t *lades, int steps);

#define LAIT_COMM       0x01    /* Test comm */
#define LAIT_PRGP       0x02    /* Program presets */
#define LAIT_JPOS       0x04    /* Jog to positive end */
#define LAIT_JNEG       0x08    /* Jog to negative end */
#define LAIT_CTRA       0x10    /* Calc total travel */
#define LAIT_MOLD       0x20    /* Move to old position (after CTRA) */
#define LAIT_MPOS       0x40    /* Move to positive end (after CTRA) */
#define LAIT_MNEG       0x80    /* Move to negative end (after CTRA) */
#define LAIT_SLIM       5000    /* Soft limit cushion.  This is the closest
                                 * to the limit switches that the software will
                                 * allow the actuators to move (except during
                                 * init, where the switches are set).  Counter
                                 * resolution is ~ 20,000/inch.*/

#endif

---