diff --git a/Config/flexured.cfg.in b/Config/flexured.cfg.in index ea427718..23c15cc6 100644 --- a/Config/flexured.cfg.in +++ b/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 +# +# +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 +# < ... > (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 of controller diff --git a/common/common.h b/common/common.h index 1b1082da..ddfd42cb 100644 --- a/common/common.h +++ b/common/common.h @@ -7,7 +7,9 @@ #pragma once +#include #include +#include #include #include #include @@ -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 // diff --git a/flexured/CMakeLists.txt b/flexured/CMakeLists.txt index 8522b400..9665cb9d 100644 --- a/flexured/CMakeLists.txt +++ b/flexured/CMakeLists.txt @@ -4,6 +4,8 @@ # @author David Hale # ---------------------------------------------------------------------------- +# 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 ---------------------------------------------------------- diff --git a/flexured/flexure_compensator.cpp b/flexured/flexure_compensator.cpp new file mode 100644 index 00000000..2db5c884 --- /dev/null +++ b/flexured/flexure_compensator.cpp @@ -0,0 +1,246 @@ +/** + * @file flexure_compensator.cpp + * @brief this contains the flexure compensator code + * @author David Hale & 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. 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(); + flexure_polynomials[{chan,axis}] = std::vector(); + } + } + + 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 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 ... + if (tokens.size() != vecsize+2 ) { + std::ostringstream oss; + oss << "ERROR got \"" << config << "\" but expected ... (" << 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; tokflexure_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 &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 &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 &shift, std::pair &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 &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 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 ************************/ + +} diff --git a/flexured/flexure_compensator.h b/flexured/flexure_compensator.h new file mode 100644 index 00000000..9a602ec8 --- /dev/null +++ b/flexured/flexure_compensator.h @@ -0,0 +1,75 @@ +/** --------------------------------------------------------------------------- + * @file flexure_interface.h + * @brief flexure interface include + * @details defines the classes used by the flexure hardware interface + * @author David Hale + * + */ + +#pragma once + +#include "common.h" +#include "tcs_info.h" + +namespace Flexure { + + constexpr double PI = 3.14159265358979323846; + constexpr double DEGTORAD = PI/180.0; + + // PI actuator axis numbers + // + constexpr int AXIS_Z = 1; ///< piston + constexpr int AXIS_X = 2; ///< spectral + constexpr int AXIS_Y = 3; ///< spatial + + constexpr const char* X = "X"; + constexpr const char* Y = "Y"; + + namespace Channel { + constexpr const char* U = "U"; + constexpr const char* G = "G"; + constexpr const char* R = "R"; + constexpr const char* I = "I"; + } + + enum class TrigFunction { + Sine, + Cosine + }; + + enum VectorType : size_t { + POSITION_COEFFICIENTS, + FLEXURE_POLYNOMIALS + }; + + /***** Flexure::Compensator *************************************************/ + /** + * @brief contains functions and data for calculating compensation + * @details this does not compensate anything, just informs how to compensate + * + */ + class Compensator { + private: + using vector_map_t = std::map, std::vector>; + + TcsInfo &tcs_info; ///< reference to Interface's TcsInfo + + vector_map_t position_coefficients; + vector_map_t flexure_polynomials; + + std::map trigfunction; + + double flexure_polynomial_fit(const std::pair &which, double inputvar, size_t offset); + void compensate_shift_to_delta(const std::string &channel, + const std::pair &shift, std::pair &delta); + + public: + Compensator(TcsInfo &info); + + long load_vector_from_config(std::string &config, VectorType type); + double calculate_shift(const std::pair &which); + void calculate_compensation(const std::string &channel, std::pair &delta); + }; + /***** Flexure::Compensator *************************************************/ + +} diff --git a/flexured/flexure_interface.cpp b/flexured/flexure_interface.cpp index 55288f71..845330c1 100644 --- a/flexured/flexure_interface.cpp +++ b/flexured/flexure_interface.cpp @@ -43,6 +43,122 @@ namespace Flexure { /***** Flexure::Interface::initialize_class *********************************/ + /***** Flexure::Interface::handletopic_snapshot *****************************/ + /** + * @brief handler when subscriber receives a message with topic "_snapshot" + * @details This publishes a JSON message containing a snapshot of my + * telemetry info when the subscriber receives the "_snapshot" + * topic and the payload contains my daemon name. + * @param[in] jmessage subscribed-received JSON message + * + */ + void Interface::handletopic_snapshot(const nlohmann::json &jmessage) { + // If my name is in the jmessage then publish my snapshot + // + if (jmessage.contains(Flexure::DAEMON_NAME)) { + this->publish_snapshot(); + } + else + if (jmessage.contains("test")) { + logwrite("Flexure::Interface::handletopic_snapshot", jmessage.dump()); + } + } + /***** Flexure::Interface::handletopic_snapshot *****************************/ + + + /***** Flexure::Interface::handletopic_tcsd *********************************/ + /** + * @brief handler when subscriber receives a message with topic "tcsd" + * @details This loads the tcs_info class with values published by tcsd. + * @param[in] jmessage subscribed-received JSON message + * + */ + void Interface::handletopic_tcsd(const nlohmann::json &jmessage) { + { + std::lock_guard lock(snapshot_mutex); + this->tcs_snapshot_status = true; + } + // extract and store values in the class + double zenangle, casangle, pa; + Common::extract_telemetry_value(jmessage, "CASANGLE", casangle); + Common::extract_telemetry_value(jmessage, "ZENANGLE", zenangle); + Common::extract_telemetry_value(jmessage, "PA", pa); + this->tcs_info.store(zenangle, casangle, pa); + } + /***** Flexure::Interface::handletopic_tcsd *********************************/ + + + /***** Flexure::Interface::publish_snapshot *********************************/ + /** + * @brief publishes a snapshot of my telemetry + * + */ + void Interface::publish_snapshot() { + nlohmann::json jmessage_out; + jmessage_out["source"] = "flexured"; // informs subscriber of the source of this telemetry + + try { + this->publisher->publish(jmessage_out); + } + catch (const std::exception &e) { + logwrite("Flexure::Interface::publish_snapshot", "ERROR: "+std::string(e.what())); + } + } + /***** Flexure::Interface::publish_snapshot *********************************/ + + + /***** Flexure::Interface::request_tcs_snapshot *****************************/ + /** + * @brief requests tcsd to publish a snapshot of its telemetry + * @details This publishes a "_snapshot" message with "tcsd" topic, which + * will cause tcsd to publish its snapshot. + * @throws std::exception + * + */ + void Interface::request_tcs_snapshot() { + nlohmann::json jmessage; + { + std::lock_guard lock(snapshot_mutex); + this->tcs_snapshot_status = false; + jmessage["tcsd"] = false; + } + try { + this->publisher->publish(jmessage, "_snapshot"); + } + catch (const std::exception &e) { + throw; + } + } + /***** Flexure::Interface::request_tcs_snapshot *****************************/ + + + /***** Flexure::Interface::wait_for_tcs_snapshot ****************************/ + /** + * @brief wait for tcs snapshot data to be published + * @details This is used after request_tcs_snapshot to wait for that to be received. + * @return true when subscriber receives snapshot data + * @throws std::runtime_error on timeout + * + */ + bool Interface::wait_for_tcs_snapshot() { + auto start_time = std::chrono::steady_clock::now(); + auto timeout = std::chrono::seconds(3); + + while (true) { + { + std::lock_guard lock(snapshot_mutex); + if (this->tcs_snapshot_status) return true; + } + + if (std::chrono::steady_clock::now() - start_time > timeout) { + throw std::runtime_error("timeout waiting for TCS telemetry"); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + } + /***** Flexure::Interface::wait_for_tcs_snapshot ****************************/ + + /***** Flexure::Interface::open *********************************************/ /** * @brief opens the PI socket connection @@ -315,26 +431,102 @@ namespace Flexure { if ( args == "?" || args == "help" ) { retstring = FLEXURED_COMPENSATE; - retstring.append( " [ dryrun ]\n" ); - retstring.append( " Performs the flexure compensation. If the optional dryrun argument\n" ); - retstring.append( " is supplied then perform the calcuations and show the actions that\n" ); - retstring.append( " would be taken without actually moving anything.\n" ); + retstring.append( " [ ...] [ --dryrun ]\n" ); + retstring.append( " Performs the flexure compensation on one or more specified channels.\n" ); + retstring.append( " If the optional --dryrun argument is supplied then perform the calcuations\n" ); + retstring.append( " and show the actions that would be taken without actually moving anything.\n" ); return HELP; } - // get the needed telemetry (telescope position and temperatures) + // check for --dryrun // - this->get_external_telemetry(); + bool is_dryrun = false; + auto pos = args.find("--dryrun"); + if (pos != std::string::npos) { // if switch in the args + args.erase(pos, 8); // then remove it + is_dryrun = true; // and set is_dryrun + } - // perform the calculations + // args can contain a space-delimited list of channels // - retstring="not_yet_implemented"; + std::vector channels; + Tokenize(args, channels, " "); + if (channels.empty()) { + logwrite(function, "ERROR no channel specified"); + return ERROR; + } + + std::pair delta; + auto motormap = this->motorinterface.get_motormap(); - return ERROR; + // loop through list of specified channel(s) + // + for (const auto &chan : channels) { + try { + if (motormap.find(chan) == motormap.end()) throw std::runtime_error("unrecognized channel: "+chan); + this->validate_tcs_telemetry(); + this->compensator.calculate_compensation(chan, delta); + this->offset_tiptilt(chan, delta, is_dryrun); + } + catch (const std::exception &e) { + logwrite(function, "ERROR: "+std::string(e.what())); + return ERROR; + } + } + + return NO_ERROR; } /***** Flexure::Interface::compensate ***************************************/ + /***** Flexure::Interface::offset_tiptilt ***********************************/ + /** + * @brief offsets the X, Y axes for the specified channel by specified delta + * @details The compensation offsets (delta) must be calculated first, with + * a call to this->compensator.calculate_compensation() for a given + * channel. That returns a delta pair which this function adds to + * the nominal positions and then sends the modified position to the + * actuators. + * @param[in] chan string channel + * @param[in] delta double pair { X, Y } of calculated compensation offsets + * @param[in] is_dryrun if true then no motion is commanded + * @throws std::runtime_error + * + */ + void Interface::offset_tiptilt(const std::string &chan, const std::pair &delta, bool is_dryrun) { + + // nominal positions + auto motormap=this->motorinterface.get_motormap()[chan]; + float nominal_x=motormap.axes[AXIS_X].defpos; + float nominal_y=motormap.axes[AXIS_Y].defpos; + + // new positions + float newposition_x = nominal_x + delta.first; + float newposition_y = nominal_y - delta.second; + + // dryrun only logs what it would have done + if ( is_dryrun ) { + std::ostringstream oss; + oss << "dry run: would move chan " << chan << " by " << delta.first << ", " << delta.second + << " to X=" << newposition_x << " Y=" << newposition_y; + logwrite("Flexure::Interface::offset_tiptilt", oss.str()); + return; + } + + // move X-axis + std::string retstring; + if (this->motorinterface.moveto( chan, AXIS_X, newposition_x, retstring ) != NO_ERROR) { + throw std::runtime_error("moving X axis for channel "+chan); + } + + // move Y-axis + if (this->motorinterface.moveto( chan, AXIS_Y, newposition_y, retstring ) != NO_ERROR) { + throw std::runtime_error("moving Y axis for channel "+chan); + } + } + /***** Flexure::Interface::offset_tiptilt ***********************************/ + + /***** Flexure::Interface::stop *********************************************/ /** * @brief send the stop-all-motion command to all controllers @@ -423,9 +615,9 @@ namespace Flexure { std::string key; this->motorinterface.get_pos( chan, axis.second.axisnum, addr, position, posname ); switch ( axis.second.axisnum ) { - case 1 : key = "FLXPIS_" + chan; break; - case 2: key = "FLXSPE_" + chan; break; - case 3: key = "FLXSPA_" + chan; break; + case AXIS_Z : key = "FLXPIS_" + chan; break; + case AXIS_X: key = "FLXSPE_" + chan; break; + case AXIS_Y: key = "FLXSPA_" + chan; break; default: key = "error"; message.str(""); message << "ERROR unknown axis " << axis.second.axisnum; logwrite( function, message.str() ); @@ -461,19 +653,19 @@ namespace Flexure { // their telemetry which is returned as a serialized json string // held in retstring. // - // handle_json_message() will parse the serialized json string. + // parse_incoming_telemetry() will parse the serialized json string. // std::string retstring; for ( const auto &provider : this->telemetry_providers ) { Common::collect_telemetry( provider, retstring ); - handle_json_message(retstring); + parse_incoming_telemetry(retstring); } return; } /***** Flexure::Interface::get_external_telemetry ***************************/ - /***** Flexure::Interface::handle_json_message ******************************/ + /***** Flexure::Interface::parse_incoming_telemetry *************************/ /** * @brief parses incoming telemetry messages * @details Requesting telemetry from another daemon returns a serialized @@ -482,8 +674,8 @@ namespace Flexure { * @return ERROR | NO_ERROR * */ - long Interface::handle_json_message( std::string message_in ) { - const std::string function="Flexure::Interface::handle_json_message"; + long Interface::parse_incoming_telemetry( std::string message_in ) { + const std::string function="Flexure::Interface::parse_incoming_telemetry"; std::stringstream message; try { @@ -519,10 +711,11 @@ namespace Flexure { } else if ( messagetype == "tcsinfo" ) { - double casangle=NAN, alt=NAN; + double casangle=NAN, alt=NAN, pa=NAN; Common::extract_telemetry_value( message_in, "CASANGLE", casangle ); Common::extract_telemetry_value( message_in, "ALT", alt ); - message.str(""); message << "casangle=" << casangle << " alt=" << alt; + Common::extract_telemetry_value( message_in, "PA", pa ); + message.str(""); message << "casangle=" << casangle << " alt=" << alt << " PA=" << pa; logwrite( function, message.str() ); } else @@ -547,7 +740,7 @@ namespace Flexure { return NO_ERROR; } - /***** Flexure::Interface::handle_json_message ******************************/ + /***** Flexure::Interface::parse_incoming_telemetry *************************/ /***** Flexure::Interface::test *********************************************/ @@ -577,7 +770,7 @@ namespace Flexure { std::vector tokens; long error = NO_ERROR; - auto _motormap = this->motorinterface.get_motormap(); + auto motormap = this->motorinterface.get_motormap(); Tokenize( args, tokens, " " ); @@ -592,6 +785,11 @@ namespace Flexure { retstring.clear(); retstring.append( " motormap return definition of motormap\n" ); retstring.append( " posmap return definition of posmap\n" ); + retstring.append( " shift calculate shift(chan,axis) of spectrum on detector\n" ); + retstring.append( " comp calculates adjustments needed to compensate for shift\n" ); + retstring.append( " tcsinfo print current tcsinfo\n" ); + retstring.append( " ishift calculate shift for \n" ); + retstring.append( " icomp calculate adjustments for \n" ); return HELP; } else @@ -600,7 +798,7 @@ namespace Flexure { // if ( testname == "motormap" ) { retstring="name host:port addr naxes \n axisnum min max reftype defpos"; - for ( const auto &mot : _motormap ) { + for ( const auto &mot : motormap ) { retstring.append("\n"); message.str(""); message << mot.first << " " << mot.second.host << ":" @@ -615,6 +813,111 @@ namespace Flexure { } retstring.append("\n"); } + else + + // shift + // calculate shift of spectrum on detector + // + if (testname == "shift") { + if (tokens.size() != 3) { + logwrite(function, "ERROR expected "); + return ERROR; + } + message.str(""); + message << this->compensator.calculate_shift({tokens[1], tokens[2]}); + retstring = message.str(); + } + else + + // comp + // calculate adjustments needed to compensate for shift + // + if (testname == "comp") { + if (tokens.size() != 2) { + logwrite(function, "ERROR expected "); + return ERROR; + } + + try { + std::pair delta; + this->compensator.calculate_compensation(tokens[1], delta); + message.str(""); message << "delta X=" << delta.first << " Y=" << delta.second; + logwrite(function, message.str()); + this->offset_tiptilt(tokens[1], delta, true); // true = dry run + } + catch (const std::exception &e) { + logwrite(function, std::string(e.what())); + return ERROR; + } + } + else + + // get tcsinfo + // + if (testname=="tcsinfo") { + message.str(""); + message << "zenangle = " << this->tcs_info.get_zenangle() << "\n" + << "casangle = " << this->tcs_info.get_casangle() << "\n" + << "pa = " << this->tcs_info.get_pa() << "\n" + << "equivalent_cass = " << this->tcs_info.get_equivalentcass() << "\n"; + retstring=message.str(); + } + else + + // interactive shift + // calculate shift for specified tcsinfo + // + if (testname=="ishift") { + if (tokens.size()!=6) { + logwrite(function, "ERROR expected "); + return ERROR; + } + try { + double zenangle = std::stod(tokens[1]); + double casangle = std::stod(tokens[2]); + double pa = std::stod(tokens[3]); + { + std::lock_guard lock(snapshot_mutex); + this->tcs_info.store(zenangle, casangle, pa); + message.str(""); + message << this->compensator.calculate_shift({tokens[4], tokens[5]}); + } + retstring=message.str(); + } + catch (const std::exception &e) { + logwrite(function, "ERROR: "+std::string(e.what())); + return ERROR; + } + } + else + + // interactive comp + // calculate compensation adjustments for specified tcsinfo + // + if (testname=="icomp") { + if (tokens.size()!=5) { + logwrite(function, "ERROR expected "); + return ERROR; + } + try { + double zenangle = std::stod(tokens[1]); + double casangle = std::stod(tokens[2]); + double pa = std::stod(tokens[3]); + std::pair delta; + { + std::lock_guard lock(snapshot_mutex); + this->tcs_info.store(zenangle, casangle, pa); + this->compensator.calculate_compensation(tokens[4], delta); + message.str(""); message << "delta X=" << delta.first << " Y=" << delta.second; + } + retstring=message.str(); + this->offset_tiptilt(tokens[4], delta, true); // true = dry run + } + catch (const std::exception &e) { + logwrite(function, "ERROR: "+std::string(e.what())); + return ERROR; + } + } else { message.str(""); message << "ERROR: test " << testname << " unknown";; diff --git a/flexured/flexure_interface.h b/flexured/flexure_interface.h index 291f22e9..25c6e9aa 100644 --- a/flexured/flexure_interface.h +++ b/flexured/flexure_interface.h @@ -12,7 +12,9 @@ #include "pi.h" #include "logentry.h" #include "common.h" +#include "tcs_info.h" #include "flexured_commands.h" +#include "flexure_compensator.h" #include #include #include @@ -42,19 +44,71 @@ namespace Flexure { */ class Interface { private: + zmqpp::context context; size_t numdev; bool class_initialized; + public: + Interface() : + context(), + numdev(-1), + motorinterface(FLEXURE_MOVE_TIMEOUT, 0, FLEXURE_POSNAME_TOLERANCE), + subscriber(std::make_unique(context, Common::PubSub::Mode::SUB)), + is_subscriber_thread_running(false), + should_subscriber_thread_run(false), + tcs_snapshot_status(false), + tcs_info(), + compensator(tcs_info) + { + topic_handlers = { + { "_snapshot", std::function( + [this](const nlohmann::json &msg) { handletopic_snapshot(msg); } ) }, + { "tcsd", std::function( + [this](const nlohmann::json &msg) { handletopic_tcsd(msg); } ) } + }; + } + + // PI Interface class for the Piezo type + // + Physik_Instrumente::Interface motorinterface; + + std::unique_ptr publisher; ///< publisher object + std::string publisher_address; ///< publish socket endpoint + std::string publisher_topic; ///< my default topic for publishing + std::unique_ptr subscriber; ///< subscriber object + std::string subscriber_address; ///< subscribe socket endpoint + std::vector subscriber_topics; ///< list of topics I subscribe to + std::atomic is_subscriber_thread_running; ///< is my subscriber thread running? + std::atomic should_subscriber_thread_run; ///< should my subscriber thread run? + std::unordered_map> topic_handlers; + ///< maps a handler function to each topic + + bool tcs_snapshot_status; + std::mutex snapshot_mutex; + + // publish/subscribe functions + // + long init_pubsub(const std::initializer_list &topics={}) { + return Common::PubSubHandler::init_pubsub(context, *this, topics); + } + void start_subscriber_thread() { Common::PubSubHandler::start_subscriber_thread(*this); } + void stop_subscriber_thread() { Common::PubSubHandler::stop_subscriber_thread(*this); } - Interface() : numdev(-1), motorinterface( FLEXURE_MOVE_TIMEOUT, 0, FLEXURE_POSNAME_TOLERANCE ) {} + void handletopic_snapshot( const nlohmann::json &jmessage ); + void handletopic_tcsd( const nlohmann::json &jmessage ); + + void publish_snapshot(); + void request_tcs_snapshot(); + bool wait_for_tcs_snapshot(); std::map telemetry_providers; ///< map of port[daemon_name] for external telemetry providers Common::Queue async; - // PI Interface class for the Piezo type - // - Physik_Instrumente::Interface motorinterface; + TcsInfo tcs_info; ///< defined in tcs_info.h + + Compensator compensator; long initialize_class(); long open(); ///< opens the PI socket connection @@ -64,13 +118,14 @@ namespace Flexure { long set( std::string args, std::string &retstring ); ///< set the slit width and offset long get( std::string args, std::string &retstring ); ///< get the current width and offset long compensate( std::string args, std::string &retstring ); ///< perform flexure compensation + void offset_tiptilt(const std::string &chan, const std::pair &delta, bool is_dryrun); long stop(); ///< send the stop-all-motion command to all controllers long send_command( const std::string &name, std::string cmd ); ///< writes the raw command as received to the master controller, no reply long send_command( const std::string &name, std::string cmd, std::string &retstring ); ///< writes command?, reads reply void make_telemetry_message( std::string &retstring ); ///< assembles a telemetry message void get_external_telemetry(); ///< collect telemetry from other daemon(s) - long handle_json_message( std::string message_in ); ///< parses incoming telemetry messages + long parse_incoming_telemetry( std::string message_in ); ///< parses incoming telemetry messages long test( std::string args, std::string &retstring ); ///< test routines std::mutex pi_mutex; ///< mutex to protect multi-threaded access to PI controller @@ -80,6 +135,18 @@ namespace Flexure { std::mutex wait_mtx; ///< mutex object for waiting for threads std::condition_variable cv; ///< condition variable for waiting for threads + + void validate_tcs_telemetry() { + // if TCS telemetry is old then ask it to publish + // + if (this->tcs_info.is_older_than(std::chrono::seconds(10))) { + try { + this->request_tcs_snapshot(); + this->wait_for_tcs_snapshot(); + } + catch (const std::exception &e) { throw; } + } + } }; /***** Flexure::Interface ***************************************************/ diff --git a/flexured/flexure_server.cpp b/flexured/flexure_server.cpp index f7365737..d137f70a 100644 --- a/flexured/flexure_server.cpp +++ b/flexured/flexure_server.cpp @@ -9,6 +9,7 @@ namespace Flexure { + Server* Server::instance = nullptr; /***** Flexure::Server::exit_cleanly ****************************************/ /** @@ -24,6 +25,41 @@ namespace Flexure { /***** Flexure::Server::exit_cleanly ****************************************/ + /***** Flexure::Server::handle_signal ***************************************/ + /** + * @brief handles ctrl-C and other signals + * @param[in] signo + * + */ + void Server::handle_signal(int signo) { + const std::string function("Flexure::Server::handle_signal"); + std::ostringstream message; + + switch (signo) { + case SIGTERM: + case SIGINT: + logwrite(function, "received termination signal"); + message << "NOTICE:" << Flexure::DAEMON_NAME << " exit"; + Server::instance->interface.async.enqueue( message.str() ); + Server::instance->exit_cleanly(); // shutdown the daemon + break; + case SIGHUP: + logwrite(function, "ignored SIGHUP"); + break; + case SIGPIPE: + logwrite(function, "ignored SIGPIPE"); + break; + default: + message << "received unknown signal " << strsignal(signo); + logwrite( function, message.str() ); + message.str(""); message << "NOTICE:" << Flexure::DAEMON_NAME << " exit"; + Server::instance->interface.async.enqueue( message.str() ); + break; + } + } + /***** Flexure::Server::handle_signal ***************************************/ + + /***** Flexure::Server::configure_flexured **********************************/ /** * @brief read and apply the configuration file for the flexure daemon @@ -33,7 +69,7 @@ namespace Flexure { long Server::configure_flexured() { std::string function = "Flexure::Server::configure_flexured"; std::stringstream message; - int applied=0; + int numapplied=0, lastapplied=0; long error; // Clear the motormap map before loading new information from the config file @@ -44,6 +80,8 @@ namespace Flexure { // for (int entry=0; entry < this->config.n_entries; entry++) { + lastapplied=numapplied; + // NBPORT -- nonblocking listening port for the flexure daemon // if ( config.param[entry].find( "NBPORT" ) == 0 ) { @@ -60,10 +98,9 @@ namespace Flexure { return(ERROR); } this->nbport = port; - message.str(""); message << "FLEXURED:config:" << config.param[entry] << "=" << config.arg[entry]; - this->interface.async.enqueue_and_log( function, message.str() ); - applied++; + numapplied++; } + else // BLKPORT -- blocking listening port for the flexure daemon // @@ -81,10 +118,9 @@ namespace Flexure { return(ERROR); } this->blkport = port; - message.str(""); message << "FLEXURED:config:" << config.param[entry] << "=" << config.arg[entry]; - this->interface.async.enqueue_and_log( function, message.str() ); - applied++; + numapplied++; } + else // ASYNCPORT -- asynchronous broadcast message port for the flexure daemon // @@ -102,30 +138,43 @@ namespace Flexure { return(ERROR); } this->asyncport = port; - message.str(""); message << "FLEXURED:config:" << config.param[entry] << "=" << config.arg[entry]; - this->interface.async.enqueue_and_log( function, message.str() ); - applied++; + numapplied++; } + else // ASYNCGROUP -- asynchronous broadcast group for the flexure daemon // if ( config.param[entry].find( "ASYNCGROUP" ) == 0 ) { this->asyncgroup = config.arg[entry]; - message.str(""); message << "FLEXURED:config:" << config.param[entry] << "=" << config.arg[entry]; - this->interface.async.enqueue_and_log( function, message.str() ); - applied++; + numapplied++; } + else // MOTOR_CONTROLLER -- address and name of each PI motor controller in daisy-chain // Each CONTROLLER is stored in an STL map indexed by motorname // if ( config.param[entry].find( "MOTOR_CONTROLLER" ) == 0 ) { if ( this->interface.motorinterface.load_controller_config( config.arg[entry] ) == NO_ERROR ) { - message.str(""); message << "FLEXURED:config:" << config.param[entry] << "=" << config.arg[entry]; - this->interface.async.enqueue_and_log( function, message.str() ); - applied++; + numapplied++; } } + else + + // PUB_ENDPOINT -- my ZeroMQ socket endpoint for publishing + // + if ( config.param[entry] == "PUB_ENDPOINT" ) { + this->interface.publisher_address = config.arg[entry]; + this->interface.publisher_topic = DAEMON_NAME; // default publish topic is my name + numapplied++; + } + else + + // SUB_ENDPOINT + // + if ( config.param[entry] == "SUB_ENDPOINT" ) { + this->interface.subscriber_address = config.arg[entry]; + numapplied++; + } // MOTOR_AXIS -- axis info for specified MOTOR_CONTROLLER // @@ -145,9 +194,7 @@ namespace Flexure { if ( loc != _motormap.end() ) { this->interface.motorinterface.add_axis( AXIS ); - message.str(""); message << "FLEXURED:config:" << config.param[entry] << "=" << config.arg[entry]; - this->interface.async.enqueue_and_log( function, message.str() ); - applied++; + numapplied++; } else { message.str(""); message << "ERROR motor name \"" << AXIS.motorname << "\" " @@ -160,6 +207,29 @@ namespace Flexure { break; } } + else + + // POSITION_COEFFICIENTS + // + if ( config.param[entry] == "POSITION_COEFFICIENTS" ) { + error=interface.compensator.load_vector_from_config(config.arg[entry], VectorType::POSITION_COEFFICIENTS); + if (error==NO_ERROR) { + numapplied++; + } + else return ERROR; + } + else + + // FLEXURE_POLYNOMIALS + // + if ( config.param[entry] == "FLEXURE_POLYNOMIALS" ) { + error=interface.compensator.load_vector_from_config(config.arg[entry], VectorType::FLEXURE_POLYNOMIALS); + if (error==NO_ERROR) { + numapplied++; + } + else return ERROR; + } + else // TELEM_PROVIDER : contains daemon name and port to contact for header telemetry info // @@ -181,22 +251,27 @@ namespace Flexure { logwrite( function, message.str() ); return ERROR; } + numapplied++; + } + + if (numapplied>lastapplied) { message.str(""); message << "config:" << config.param[entry] << "=" << config.arg[entry]; this->interface.async.enqueue_and_log( to_uppercase(DAEMON_NAME), function, message.str() ); - applied++; } - } // end loop through the entries in the configuration file +//logwrite(function, "will start subscriber thread"); +// this->interface.start_subscriber_thread(); + message.str(""); - if (applied==0) { + if (numapplied==0) { message << "ERROR: "; error = ERROR; } else { error = NO_ERROR; } - message << "applied " << applied << " configuration lines to flexured"; + message << "applied " << numapplied << " configuration lines to flexured"; logwrite(function, message.str()); if ( error == NO_ERROR ) error = this->interface.initialize_class(); @@ -554,7 +629,7 @@ namespace Flexure { // Don't append anything nor log the reply if the command was just requesting help. // if (ret != NOTHING) { - if ( ! retstring.empty() ) retstring.append( " " ); + if ( ! retstring.empty() && ret != HELP ) retstring.append( " " ); if ( ret != HELP && ret != JSON ) retstring.append( ret == NO_ERROR ? "DONE" : "ERROR" ); if ( ret == JSON ) { diff --git a/flexured/flexure_server.h b/flexured/flexure_server.h index fa7a47f6..280ad267 100644 --- a/flexured/flexure_server.h +++ b/flexured/flexure_server.h @@ -46,11 +46,20 @@ namespace Flexure { class Server { private: public: + static Server* instance; + /***** Flexure::Server::Server ******************************************/ /** * @brief class constructor */ - Server() : nbport(-1), blkport(-1), asyncport(-1), cmd_num(0), threads_active(0), id_pool(Flexure::N_THREADS) { } + Server() : nbport(-1), blkport(-1), asyncport(-1), cmd_num(0), threads_active(0), id_pool(Flexure::N_THREADS) { + instance=this; + // register these signals + // + signal(SIGINT, this->signal_handler); + signal(SIGPIPE, this->signal_handler); + signal(SIGHUP, this->signal_handler); + } /***** Flexure::Server::~Server *****************************************/ /** @@ -102,6 +111,8 @@ namespace Flexure { long configure_flexured(); ///< read and apply the configuration file void doit(Network::TcpSocket &sock); ///< the workhorse of each thread connetion + void handle_signal(int signo); + static inline void signal_handler(int signo) { if (instance) { instance->handle_signal(signo); } } }; /***** Flexure::Server ******************************************************/ diff --git a/flexured/flexured.cpp b/flexured/flexured.cpp index f78cefb6..f705f097 100644 --- a/flexured/flexured.cpp +++ b/flexured/flexured.cpp @@ -17,18 +17,25 @@ * */ int main(int argc, char **argv) { - std::string function = "Flexure::main"; + const std::string function("main"); std::stringstream message; - std::string logpath; - long ret=NO_ERROR; - std::string daemon_in; // daemon setting read from config file - bool start_daemon = false; // don't start as daemon unless specifically requested - // capture these signals + // Unless specifically requested to run in foreground, + // immediately daemonize. // - signal(SIGINT, signal_handler); - signal(SIGPIPE, signal_handler); - signal(SIGHUP, signal_handler); + if ( !cmdOptionExists( argv, argv+argc, "--foreground" ) ) { + logwrite(function, "starting daemon"); + Daemon::daemonize( Flexure::DAEMON_NAME, "/tmp", "/dev/null", "/tmp/flexured.stderr", "", false ); + std::cerr << get_timestamp() << " (" << function << ") daemonized. child process running" << std::endl; + } + else logwrite(function, "starting"); + + // the child process instantiates a Server object + // + Flexure::Server flexured; + + std::string daemon_in; // daemon setting read from config file + bool start_daemon = false; // don't start as daemon unless specifically requested // check for "-f " command line option to specify config file // @@ -56,6 +63,8 @@ int main(int argc, char **argv) { flexured.exit_cleanly(); } + std::string logpath; + for (int entry=0; entry < flexured.config.n_entries; entry++) { if (flexured.config.param[entry] == "LOGPATH") logpath = flexured.config.arg[entry]; if (flexured.config.param[entry] == "DAEMON") daemon_in = flexured.config.arg[entry]; @@ -78,31 +87,11 @@ int main(int argc, char **argv) { flexured.exit_cleanly(); } - if ( !daemon_in.empty() && daemon_in == "yes" ) start_daemon = true; - else - if ( !daemon_in.empty() && daemon_in == "no" ) start_daemon = false; - else { - message.str(""); message << "ERROR: unrecognized argument DAEMON=" << daemon_in << ", expected { yes | no }"; - logwrite( function, message.str() ); - flexured.exit_cleanly(); - } - - // check for "-d" command line option last so that the command line - // can override the config file to start as daemon - // - if ( cmdOptionExists( argv, argv+argc, "-d" ) ) { - start_daemon = true; - } - - if ( start_daemon ) { - logwrite( function, "starting daemon" ); - Daemon::daemonize( Flexure::DAEMON_NAME, "/tmp", "", "", "" ); - } - if ( ( init_log( logpath, Flexure::DAEMON_NAME ) != 0 ) ) { // initialize the logging system logwrite(function, "ERROR: unable to initialize logging system"); flexured.exit_cleanly(); } + logwrite(function, "world"); message << "this version built " << BUILD_DATE << " " << BUILD_TIME; logwrite(function, message.str()); @@ -110,7 +99,7 @@ int main(int argc, char **argv) { message.str(""); message << flexured.config.n_entries << " lines read from " << flexured.config.filename; logwrite(function, message.str()); - if (ret==NO_ERROR) ret=flexured.configure_flexured(); // get needed values out of read-in configuration file for the daemon + long ret=flexured.configure_flexured(); // get needed values out of read-in configuration file for the daemon if (ret != NO_ERROR) { logwrite(function, "ERROR: unable to configure system"); @@ -122,6 +111,15 @@ int main(int argc, char **argv) { flexured.exit_cleanly(); } + // initialize the pub/sub handler + // + if ( flexured.interface.init_pubsub({"tcsd"}) == ERROR ) { + logwrite(function, "ERROR initializing publisher-subscriber handler"); + flexured.exit_cleanly(); + } + std::this_thread::sleep_for( std::chrono::milliseconds(500) ); + flexured.interface.publish_snapshot(); + // This will pre-thread N_THREADS threads. // The 0th thread is reserved for the blocking port, and the rest are for the non-blocking port. // Each thread gets a socket object. All of the socket objects are stored in a vector container. @@ -205,40 +203,3 @@ int main(int argc, char **argv) { return 0; } /***** main *******************************************************************/ - - -/***** signal_handler *********************************************************/ -/** - * @brief handles ctrl-C - * @param[in] signo - * - */ -void signal_handler(int signo) { - std::string function = "Flexure::signal_handler"; - std::stringstream message; - - switch (signo) { - case SIGTERM: - case SIGINT: - logwrite(function, "received termination signal"); - message << "NOTICE:" << Flexure::DAEMON_NAME << " exit"; - flexured.interface.async.enqueue( message.str() ); - flexured.exit_cleanly(); // shutdown the daemon - break; - case SIGHUP: - logwrite(function, "caught SIGHUP"); - break; - case SIGPIPE: - logwrite(function, "caught SIGPIPE"); - break; - default: - message << "received unknown signal " << strsignal(signo); - logwrite( function, message.str() ); - message.str(""); message << "NOTICE:" << Flexure::DAEMON_NAME << " exit"; - flexured.interface.async.enqueue( message.str() ); - flexured.exit_cleanly(); // shutdown the daemon - break; - } - return; -} -/***** signal_handler *********************************************************/ diff --git a/flexured/flexured.h b/flexured/flexured.h index 7fdafecc..afd040f0 100644 --- a/flexured/flexured.h +++ b/flexured/flexured.h @@ -29,7 +29,5 @@ #define CONN_TIMEOUT 3000 ///< incoming (non-blocking) connection timeout in milliseconds -Flexure::Server flexured; ///< global Flexure::Server object so that the main daemon can access the namespace - void signal_handler(int signo); ///< handles ctrl-C int main(int argc, char **argv); ///< the main function diff --git a/flexured/tcs_info.h b/flexured/tcs_info.h new file mode 100644 index 00000000..a0279e79 --- /dev/null +++ b/flexured/tcs_info.h @@ -0,0 +1,96 @@ +/** --------------------------------------------------------------------------- + * @file tcs_info.h + * @brief defines a class in Flexure for holding TCS info + * @author David Hale + * + */ + +#pragma once + +#include +#include + +namespace Flexure { + + /***** Flexure::TcsInfo *****************************************************/ + /** + * @brief contains TCS telemetry + * + */ + class TcsInfo { + private: + double zenangle; + double casangle; + double pa; + double equivalentcass; + + bool is_timeset; + std::chrono::system_clock::time_point telemetry_time; + + public: + + TcsInfo() + : zenangle(0), casangle(0), pa(0), equivalentcass(0), is_timeset(false) { } + + double get_zenangle() { return this->zenangle; } + double get_casangle() { return this->casangle; } + double get_pa() { return this->pa; } + double get_equivalentcass() { return this->equivalentcass; } + + /***** Flexure::TcsInfo:is_older_than ***********************************/ + /** + * @brief is the TCS telemetry more than seconds old? + * @details When the telemetry is stored in this class it is time-stamped. + * This returns true if that time stamp is more than + * seconds ago from now. + * @param[in] sec number of seconds + * @return true|false + */ + bool is_older_than(std::chrono::seconds sec) { + if (!this->is_timeset) return true; + if (std::chrono::system_clock::now() > this->telemetry_time+sec) { + return true; + } + return false; + } + /***** Flexure::TcsInfo:is_older_than ***********************************/ + + /***** Flexure::TcsInfo:set *********************************************/ + /** + * @brief timestamp telemetry + * @details Call this function when + * This returns true if that time stamp is more than + * + */ + void store(const double &_zenangle, + const double &_casangle, + const double &_pa, + const std::optional &_equivalentcass=std::nullopt) { + + // store the values received + this->zenangle = _zenangle; + this->casangle = _casangle; + this->pa = _pa; + + // if equivalentcass provided then use it + if (_equivalentcass) { + this->equivalentcass = *_equivalentcass; + } + // otherwise calculate it + else { + double angle = this->casangle - this->pa; + if (angle < -180.0) angle += 360.0; + if (angle > 180.0) angle -= 360.0; + this->equivalentcass = angle; + } + + // record the time that telemetry was received + this->telemetry_time = std::chrono::system_clock::now(); + this->is_timeset = true; + } + /***** Flexure::TcsInfo:set *********************************************/ + + }; + /***** Flexure::TcsInfo *****************************************************/ + +} diff --git a/sequencerd/sequence.cpp b/sequencerd/sequence.cpp index 90a8ade0..4a64a44f 100644 --- a/sequencerd/sequence.cpp +++ b/sequencerd/sequence.cpp @@ -756,6 +756,13 @@ namespace Sequencer { this->async.enqueue_and_log( function, "ERROR sending \""+camcmd.str()+"\": "+reply ); throw std::runtime_error( "camera returned "+reply ); } + camcmd.str(""); camcmd << CAMERAD_BIN << " col " << this->target.binspect; + if (error==NO_ERROR && (error=this->camerad.send( camcmd.str(), reply ))!=NO_ERROR) { + this->async.enqueue_and_log( function, "ERROR sending \""+camcmd.str()+"\": "+reply ); + throw std::runtime_error( "camera returned "+reply ); + } + + this->thread_error_manager.clear( THR_CAMERA_SET ); // success this->thread_error_manager.clear( THR_CAMERA_SET ); // success