Implements flexure compensation

Config/flexured.cfg.in

@@ -4,15 +4,34 @@
 LOGPATH=/data/logs/               # where to write logs
 BLKPORT=@FLEXURED_BLK_PORT@       # slitd server blocking port
 NBPORT=@FLEXURED_NB_PORT@         # slitd server non-blocking port
-DAEMON=no                         # run as daemon?
 ASYNCPORT=@MESSAGEPORT@           # asynchronous message port
 ASYNCGROUP=239.1.1.234            # asynchronous message broadcast group
-PUBLISHER_PORT="tcp://127.0.0.1:@FLEXURED_PUB_PORT@"  # my zeromq pub port
+
+# Message pub/sub
+#
+PUB_ENDPOINT="tcp://127.0.0.1:@MESSAGE_BROKER_SUB_PORT@"
+SUB_ENDPOINT="tcp://127.0.0.1:@MESSAGE_BROKER_PUB_PORT@"
 
 # this is the port number that the emulator listens to
 #
 EMULATOR_PORT=@FLEXURED_EMULATOR@
 
+# COLLIMATOR_POSITION
+# <chan> <axis> <a> <b> <c>
+#
+POSITION_COEFFICIENTS=(R X 21.2591 -0.275957 1.29479)
+POSITION_COEFFICIENTS=(R Y 0.266115 22.3092 -0.821104)
+POSITION_COEFFICIENTS=(I X 19.2199 0.149826 -0.159326)
+POSITION_COEFFICIENTS=(I Y 0.123447 -20.5831 0.307648)
+
+# FLEXURE_POLYNOMIALS
+# <chan> <axis> < ... > (expected 20 values)
+#
+FLEXURE_POLYNOMIALS=( I X 0.0135162 -0.00447609  -6.48415e-05 6.43345e-06 -4.00865e-08 0.0415762 -0.0701252 0.000227690 0 0 0.288840 -0.00161429 0 0 0 -0.0112647 -0.00343684 0.000147780 -1.68025e-06 7.84644e-09)
+FLEXURE_POLYNOMIALS=( I Y -0.00694281 0.00341489 -0.000852294 6.95515e-06 -6.85542e-08 0.00583450 -0.00415384 -5.64805e-05 0 0 2.30189 -0.0106356 0 0 0 -0.163970 0.000321176 8.95876e-06 0 0 )
+FLEXURE_POLYNOMIALS=( R X 0.00 0.00 0.0 0.0 0.0 -0.0641885 -0.0317361 -0.000197792 0 0 2.57404 -0.00327556 0 0 0 0.000445881 0.0107193 -0.000509618 9.11488e-6 -4.74720e-8)
+FLEXURE_POLYNOMIALS=( R Y -0.00644079 0.00907213 -0.000377799 1.49786e-05 -3.50103e-08 -0.0180188 -0.0165204 8.40640e-05 0 0 2.61190 -0.00353314 0 0 0 -0.00104779  0.000841425  4.02491e-06 0 0)
+
 # For each actuator's controller specify:
 # MOTOR_CONTROLLER="<name> <host> <port> <addr> <axes>"
 #   <name>  name of controller

common/common.h

@@ -7,7 +7,9 @@
 
 #pragma once
 
+#include <cstddef>
 #include <climits>
+#include <cstddef>
 #include <sstream>
 #include <queue>
 #include <string>
@@ -220,6 +222,13 @@ namespace Common {
           iface.subscriber_topics.push_back(topic);
         }
 
+        // check subscriber initialization (this would be a programming error)
+        //
+        if (!iface.subscriber) {
+          logwrite(function, "ERROR subscriber object is not initialized");
+          return ERROR;
+        }
+
         try {
           // connect to the message broker and wait for connection to establish
           //

flexured/CMakeLists.txt

@@ -4,6 +4,8 @@
 # @author  David Hale <dhale@caltech.edu>
 # ----------------------------------------------------------------------------
 
+# add_compile_options( -O0 -g -march=native -DNDEBUG)
+
 cmake_minimum_required( VERSION 3.12 )
 
 set( FLEXURED_DIR ${PROJECT_BASE_DIR}/flexured )
@@ -16,10 +18,16 @@ include_directories( ${PROJECT_BASE_DIR}/common )
 
 link_directories( ${PROJECT_BASE_DIR}/lib )
 
+# ZeroMQ
+#
+find_library( ZMQPP_LIB zmqpp NAMES libzmqpp PATHS /usr/local/lib )
+find_library( ZMQ_LIB zmq NAMES libzmq PATHS /usr/local/lib )
+
 add_executable(flexured 
         ${FLEXURED_DIR}/flexured.cpp 
         ${FLEXURED_DIR}/flexure_server.cpp 
         ${FLEXURED_DIR}/flexure_interface.cpp 
+        ${FLEXURED_DIR}/flexure_compensator.cpp
         )
 
 target_link_libraries(flexured
@@ -29,6 +37,8 @@ target_link_libraries(flexured
         logentry
         utilities
         ${CMAKE_THREAD_LIBS_INIT}
+        ${ZMQPP_LIB}
+        ${ZMQ_LIB}
         )
 
 # -- Port Definitions ----------------------------------------------------------

flexured/flexure_compensator.cpp

@@ -0,0 +1,246 @@
+/**
+ * @file    flexure_compensator.cpp
+ * @brief   this contains the flexure compensator code
+ * @author  David Hale <dhale@astro.caltech.edu> & Matt
+ *          Algorithms by Matt Matuszewski
+ *
+ */
+
+#include "tcs_info.h"
+#include "flexure_compensator.h"
+
+namespace Flexure {
+
+  /***** Flexure::Compensator::Compensator ************************************/
+  /**
+   * @brief      class constructor
+   * @param[in]  info  constructed with a reference to the TcsInfo object
+   *                   owned by Interface.
+   *
+   */
+  Compensator::Compensator(TcsInfo &info) : tcs_info(info) {
+    // position_coefficients and flexure_polynomials are maps
+    // indexed by a pair<chan,axis>. This initializes their indices.
+    // Values will be loaded by Compensator::load_position_coefficients().
+    //
+    for (const auto &chan : { "U", "G", "R", "I" }) {
+      for (const auto &axis : { X, Y }) {
+        position_coefficients[{chan,axis}] = std::vector<double>();
+        flexure_polynomials[{chan,axis}] = std::vector<double>();
+      }
+    }
+
+    this->trigfunction["R"] = TrigFunction::Sine;
+    this->trigfunction["I"] = TrigFunction::Cosine;
+
+  }
+  /***** Flexure::Compensator::Compensator ************************************/
+
+
+  /***** Flexure::Compensator::load_vector_from_config ************************/
+  /**
+   * @brief      loads position coefficients from configuration file
+   * @details    This parses a configuration file row given the specified VectorType
+   *             and loads the class map vector specified by VectorType. This will be
+   *             either a vector of POSITION_COEFFICIENTS or FLEXURE_POLYNOMIALS,
+   *             which are vectors assigned to a map indexed by pair { chan, axis }.
+   * @param[in]  config  configuration line
+   * @param[in]  type    one of VectorType enum to specify which vector map to load
+   * @return     ERROR|NO_ERROR
+   *
+   */
+  long Compensator::load_vector_from_config(std::string &config, VectorType type) {
+    const std::string function("Flexure::Compensator::load_vector_from_config");
+    std::vector<std::string> tokens;
+    Tokenize(config, tokens, " ");
+
+    size_t vecsize;
+    vector_map_t* vecmap;
+
+    // assign the vecmap pointer to the appropriate map based on VectorType,
+    // which also has a fixed vector size
+    if (type==VectorType::POSITION_COEFFICIENTS) {
+      vecmap  = &this->position_coefficients;
+      vecsize = 3;
+    }
+    else
+    if (type==VectorType::FLEXURE_POLYNOMIALS) {
+      vecmap  = &this->flexure_polynomials;
+      vecsize = 20;
+    }
+    else {
+      logwrite(function, "ERROR invalid vector type");
+      return ERROR;
+    }
+
+    // expect <CHAN> <AXIS> <COEFF> <COEFF> <COEFF> ...
+    if (tokens.size() != vecsize+2 ) {
+      std::ostringstream oss;
+      oss << "ERROR got \"" << config << "\" but expected <chan> <axis> ... (" << vecsize << " values)";
+      logwrite(function, oss.str());
+      return ERROR;
+    }
+
+    // the vector is in a map indexed by pair { chan, axis }
+    std::string chan = tokens[0];
+    std::string axis = tokens[1];
+
+    if (vecmap->find({chan,axis}) == vecmap->end()) {
+      logwrite(function, "ERROR invalid chan,axis: "+chan+","+axis);
+      return ERROR;
+    }
+
+    // erase the vector and load it with the values provided by the configuration row
+    try {
+      (*vecmap)[{chan,axis}].clear();
+      for (int tok=2; tok<vecsize+2; tok++) {
+        (*vecmap)[{chan,axis}].push_back(std::stod(tokens[tok]));
+      }
+    }
+    catch (const std::exception &e) {
+      logwrite(function, "ERROR parsing \""+config+"\"");
+      return ERROR;
+    }
+
+    return NO_ERROR;
+  }
+  /***** Flexure::Compensator::load_vector_from_config ************************/
+
+
+  /***** Flexure::Compensator::flexure_polynomial_fit *************************/
+  /**
+   * @brief      fits a 4th order polynomial to inputvar
+   * @details    Uses inputvar for the fitting variable and 4 coefficients from
+   *             supplied vector starting with offset. Requires that vector have
+   *             at least 5 coefficients. For example, if x=inputvar and the
+   *             five polynomials from this->flexure_polynomials[{chan,axis}]
+   *             are a,b,c,d,e then return a + bx + cx^2 + dx^3 + ex^4.
+   * @param[in]  which     pair { channel, axis }
+   * @param[in]  inputvar  independent input variable for the polynomial fit
+   * @param[in]  offset    offset in vector to start reading coefficients
+   * @return     double (a + bx + cx^2 + dx^3 + ex^4)
+   * @throws     std::out_of_range
+   *
+   */
+  double Compensator::flexure_polynomial_fit(const std::pair<std::string,std::string> &which, double inputvar, size_t offset) {
+
+    if (offset+5 > this->flexure_polynomials.at(which).size()) {
+      throw std::out_of_range("not enough flexure polynomial coefficients");
+    }
+
+    // a + bx + cx^2 + dx^3 + ex^4
+    //
+    return this->flexure_polynomials.at(which)[offset + 0]
+         + this->flexure_polynomials.at(which)[offset + 1] * inputvar
+         + this->flexure_polynomials.at(which)[offset + 2] * std::pow(inputvar, 2.0)
+         + this->flexure_polynomials.at(which)[offset + 3] * std::pow(inputvar, 3.0)
+         + this->flexure_polynomials.at(which)[offset + 4] * std::pow(inputvar, 4.0);
+  }
+  /***** Flexure::Compensator::flexure_polynomial_fit *************************/
+
+
+  /***** Flexure::Compensator::calculate_shift ********************************/
+  /**
+   * @brief      calculates the shift(chan,axis) of the spectrum on the detector
+   * @details    C + A1 * sin(cass-theta) + A2 * sin(2*(cass-theta)) or
+   *             C + A1 * cos(cass-theta) + A2 * cos(2*(cass-theta))
+   *             Input coefficients are a function of (chan,axis) so the output
+   *             shift will also be a function of (chan,axis).
+   * @param[in]  which     pair { channel, axis }
+   * @return     double (calculated shift)
+   * @throws     std::exception
+   *
+   */
+  double Compensator::calculate_shift(const std::pair<std::string,std::string> &which) {
+    const std::string function("Flexure::Compensator::calculate_shift");
+    double zenangle = this->tcs_info.get_zenangle();
+    double equivalentcass = this->tcs_info.get_equivalentcass();
+
+    try {
+      double c     = this->flexure_polynomial_fit(which, zenangle,  0);
+      double a1    = this->flexure_polynomial_fit(which, zenangle,  5);
+      double theta = this->flexure_polynomial_fit(which, zenangle, 10);
+      double a2    = this->flexure_polynomial_fit(which, zenangle, 15);
+
+      auto [ chan, axis ] = which;
+
+      if (this->trigfunction[chan] == TrigFunction::Sine) {
+        return c + a1 * std::sin(  (equivalentcass * DEGTORAD - theta))
+                 + a2 * std::sin(2*(equivalentcass * DEGTORAD - theta));
+      }
+      else
+      if (this->trigfunction[chan] == TrigFunction::Cosine) {
+        return c + a1 * std::cos(  (equivalentcass * DEGTORAD - theta))
+                 + a2 * std::cos(2*(equivalentcass * DEGTORAD - theta));
+      }
+      else {
+        logwrite(function, "ERROR undefined trig function for channel "+chan);
+        return NAN;
+      }
+    }
+    catch (const std::exception &e) {
+      logwrite(function, "ERROR: "+std::string(e.what()));
+      throw;
+    }
+  }
+  /***** Flexure::Compensator::calculate_shift ********************************/
+
+
+  /***** Flexure::Compensator::compensate_shift_to_delta *********************/
+  /**
+   * @brief      calculates the tip-tilt adjustment needed to compensate for shift
+   * @details    Given the spectral shift for a specific channel, this calculates
+   *             the adjustment needed to compensate for that shift.
+   * @param[in]  channel  string channel name
+   * @param[in]  shift    pair { sx, sy } representing shift in X, Y
+   * @param[out] delta    reference to pair { dx, dy } representing adjustments to X, Y
+   *
+   */
+  void Compensator::compensate_shift_to_delta(const std::string &channel,
+                                              const std::pair<double,double> &shift, std::pair<double,double> &delta) {
+
+    // delta.first is delta-X = [0]*x + [1]*y + [2]
+    delta.first  = this->position_coefficients.at({channel,X})[0] * shift.first  +
+                   this->position_coefficients.at({channel,X})[1] * shift.second +
+                   this->position_coefficients.at({channel,X})[2];
+
+    // delta.second is delta-Y = [0]*x + [1]*y + [2]
+    delta.second = this->position_coefficients.at({channel,Y})[0] * shift.first  +
+                   this->position_coefficients.at({channel,Y})[1] * shift.second +
+                   this->position_coefficients.at({channel,Y})[2];
+  }
+  /***** Flexure::Compensator::compensate_shift_to_delta *********************/
+
+
+  /***** Flexure::Compensator::calculate_compensation ************************/
+  /**
+   * @brief      calculates the adjustments needed to compensate for a shift
+   * @details    input is output of calculate_shift()
+   *             output is correction to apply to flexure actuator position
+   * @param[in]  which     pair { channel, axis }
+   * @param[out] delta     pair { dx, dy } representing adjustments to X, Y
+   *
+   */
+  void Compensator::calculate_compensation(const std::string &channel, std::pair<double,double> &delta) {
+
+    try {
+      // calculate shift of spectrum on detector
+      //
+      double shift_x = this->calculate_shift({channel, X});
+      double shift_y = this->calculate_shift({channel, Y});
+
+      std::pair<double, double> shift = { shift_x, shift_y };
+
+      // calculate tip-tilt adjustment needed to compenstate for shift
+      //
+      this->compensate_shift_to_delta(channel, shift, delta);
+    }
+    catch (const std::exception &e) {
+      logwrite("Flexure::Compensator::calculate_compensation", "ERROR: "+std::string(e.what()));
+      delta = { NAN, NAN };
+      throw;
+    }
+  }
+  /***** Flexure::Compensator::calculate_compensation ************************/
+
+}