add PID (not tested yet)

LibSidServo/sidservo.h

@@ -49,6 +49,19 @@ extern "C"
 #define MCC_CONF_MAX_SPEEDINT   (2.)
 // minimal speed interval in parts of EncoderReqInterval
 #define MCC_CONF_MIN_SPEEDC     (3.)
+// PID I cycle time (analog of "RC" for PID on opamps)
+#define MCC_PID_CYCLE_TIME      (5.)
+// maximal PID refresh time interval (if larger all old data will be cleared)
+#define MCC_PID_MAX_DT          (1.)
+// normal PID refresh interval
+#define MCC_PID_REFRESH_DT      (0.1)
+// boundary conditions for axis state: "slewing/pointing/guiding"
+// if angle < MCC_MAX_POINTING_ERR, change state from "slewing" to "pointing": 12 degrees
+#define MCC_MAX_POINTING_ERR    (0.20943951)
+// if angle < MCC_MAX_GUIDING_ERR, chane state from "pointing" to "slewing": 15''
+#define MCC_MAX_GUIDING_ERR     (7.272205e-5)
+// if error less than this value we suppose that target is captured and guiding is good: 0.1''
+#define MCC_MAX_ATTARGET_ERR    (4.8481368e-7)
 
 // error codes
 typedef enum{
@@ -61,17 +74,25 @@ typedef enum{
 } mcc_errcodes_t;
 
 typedef struct{
-    char*   MountDevPath;       // path to mount device
-    int     MountDevSpeed;      // serial speed
-    char*   EncoderDevPath;     // path to encoder device
-    int     EncoderDevSpeed;    // serial speed
-    int     SepEncoder;         // ==1 if encoder works as separate serial device, ==2 if there's new version with two devices
-    char*   EncoderXDevPath;    // paths to new controller devices
+    double P, I, D;
+} PIDpar_t;
+
+typedef struct{
+    char*   MountDevPath;           // path to mount device
+    int     MountDevSpeed;          // serial speed
+    char*   EncoderDevPath;         // path to encoder device
+    int     EncoderDevSpeed;        // serial speed
+    int     SepEncoder;             // ==1 if encoder works as separate serial device, ==2 if there's new version with two devices
+    char*   EncoderXDevPath;        // paths to new controller devices
     char*   EncoderYDevPath;
-    double  MountReqInterval;   // interval between subsequent mount requests (seconds)
-    double  EncoderReqInterval; // interval between subsequent encoder requests (seconds)
-    double  EncoderSpeedInterval;// interval between speed calculations
-    int     RunModel;            // == 1 if you want to use model instead of real mount
+    double  MountReqInterval;       // interval between subsequent mount requests (seconds)
+    double  EncoderReqInterval;     // interval between subsequent encoder requests (seconds)
+    double  EncoderSpeedInterval;   // interval between speed calculations
+    int     RunModel;               // == 1 if you want to use model instead of real mount
+    PIDpar_t XPIDC;                 // gain parameters of PID for both axes (C - coordinate driven, V - velocity driven)
+    PIDpar_t XPIDV;
+    PIDpar_t YPIDC;
+    PIDpar_t YPIDV;
 } conf_t;
 
 // coordinates/speeds in degrees or d/s: X, Y
@@ -128,16 +149,16 @@ typedef struct{
 } extradata_t;
 
 typedef enum{
-    MNT_STOPPED,
-    MNT_SLEWING,
-    MNT_POINTING,
-    MNT_GUIDING,
-    MNT_ERROR,
-} mnt_status_t;
+    AXIS_STOPPED,
+    AXIS_SLEWING,
+    AXIS_POINTING,
+    AXIS_GUIDING,
+    AXIS_ERROR,
+} axis_status_t;
 
 typedef struct{
-    mnt_status_t Xstatus;
-    mnt_status_t Ystatus;
+    axis_status_t Xstate;
+    axis_status_t Ystate;
     coordval_t motXposition;
     coordval_t motYposition;
     coordval_t encXposition;
@@ -212,19 +233,19 @@ typedef struct{
     double backlspd;    // Backlash speed (rad/s)
 } hardware_configuration_t;
 
-// flags for slew function
+/* flags for slew function
 typedef struct{
     uint32_t slewNguide : 1; // ==1 to guide after slewing
 } slewflags_t;
-
+*/
 // mount class
 typedef struct{
     // TODO: on init/quit clear all XY-bits to default`
     mcc_errcodes_t  (*init)(conf_t *c); // init device
     void            (*quit)(); // deinit
     mcc_errcodes_t  (*getMountData)(mountdata_t *d); // get last data
-    mcc_errcodes_t  (*slewTo)(const coordpair_t *target, slewflags_t flags);
-    mcc_errcodes_t  (*correctTo)(coordval_pair_t *target);
+//    mcc_errcodes_t  (*slewTo)(const coordpair_t *target, slewflags_t flags);
+    mcc_errcodes_t  (*correctTo)(const coordval_pair_t *target, const coordpair_t *endpoint);
     mcc_errcodes_t  (*moveTo)(const coordpair_t *target); // move to given position and stop
     mcc_errcodes_t  (*moveWspeed)(const coordpair_t *target, const  coordpair_t *speed); // move with given max speed
     mcc_errcodes_t  (*setSpeed)(const coordpair_t *tagspeed); // set speed