...

asibfm700/asibfm700_servocontroller.cpp

@@ -106,27 +106,26 @@ AsibFM700ServoController::error_t AsibFM700ServoController::hardwareInit()
 
 AsibFM700ServoController::error_t AsibFM700ServoController::hardwareSetState(hardware_state_t state)
 {
-    static thread_local coordval_pair_t cvalpair{.X{0.0, 0.0}, .Y{0.0, 0.0}};
+    std::lock_guard lock{*_setStateMutex};
-    static thread_local coordpair_t cpair{.X = 0.0, .Y = 0.0};
+
+    // static thread_local coordval_pair_t cvalpair{.X{0.0, 0.0}, .Y{0.0, 0.0}};
+    // static thread_local coordpair_t cpair{.X = 0.0, .Y = 0.0};
+
+    // cvalpair.X = {.val = state.Y, .t = tp};
+    // cvalpair.Y = {.val = state.X, .t = tp};
+
+    // cpair.X = state.tagY;
+    // cpair.Y = state.tagX;
 
     // time point from sidservo library is 'double' number represented UNIXTIME with
     // microseconds/nanoseconds precision
     double tp = std::chrono::duration<double>(state.time_point.time_since_epoch()).count();
 
-    std::lock_guard lock{*_setStateMutex};
-
     // according to"SiTech protocol notes" X is DEC-axis and Y is HA-axis
-    // coordval_pair_t cvalpair{.X{.val = state.Y, .t = tp}, .Y{.val = state.X, .t = tp}};
+    coordval_pair_t cvalpair{.X{.val = state.Y, .t = tp}, .Y{.val = state.X, .t = tp}};
-    // coordpair_t cpair{.X = state.Y, .Y = state.X};
+    coordpair_t cpair{.X = state.endptY, .Y = state.endptX};
-    // coordpair_t cpair{.X = state.Y + mcc::MccAngle(1.0_degs), .Y = state.X + mcc::MccAngle(1.0_degs)};
 
 
-    cvalpair.X = {.val = state.Y, .t = tp};
-    cvalpair.Y = {.val = state.X, .t = tp};
-
-    cpair.X = state.Y;
-    cpair.Y = state.X;
-
     // correctTo is asynchronous function!!!
     //
     // according to the Eddy's implementation of the LibSidServo library it is safe
@@ -154,11 +153,13 @@ AsibFM700ServoController::error_t AsibFM700ServoController::hardwareGetState(har
         using secs_t = std::chrono::duration<double>;
 
         secs_t secs = secs_t{mdata.encXposition.t};
-        if (mcc::utils::isEqual(secs.count(), 0.0)) {  // model mode?
+        state->time_point = tp_t{std::chrono::duration_cast<tp_t::duration>(secs)};
-            state->time_point = decltype(state->time_point)::clock::now();
+
-        } else {
+        // if (mcc::utils::isEqual(secs.count(), 0.0)) {  // model mode?
-            state->time_point = tp_t{std::chrono::duration_cast<tp_t::duration>(secs)};
+        //     state->time_point = decltype(state->time_point)::clock::now();
-        }
+        // } else {
+        //     state->time_point = tp_t{std::chrono::duration_cast<tp_t::duration>(secs)};
+        // }
         // WARNING: TEMPORARY (WAIT FOR Eddy fix its implementation of LibSidServo)!!!
         //        state->time_point = decltype(state->time_point)::clock::now();
 

asibfm700/asibfm700_servocontroller.h

@@ -78,6 +78,12 @@ public:
         axis_status_t stateX, stateY;  // Eddy's LibSidServo axis state
 
         hardware_moving_state_t moving_state;
+
+        // endpoint: a point on the trajectory of movement behind the guidance point (X,Y), taking into account
+        // the movement vector (i.e. sign of movement speed)
+        // this point is needed as Sidereal controller commands require not only moving speed but
+        // also 'target' point (point at which mount will stop)
+        double endptX, endptY;
     };
 
 

mcc/mcc_slewing_model.h

@@ -85,6 +85,8 @@ struct MccSimpleSlewingModelCategory : public std::error_category {
                 return "already slewing";
             case MccSimpleSlewingModelErrorCode::ERROR_ALREADY_STOPPED:
                 return "slewing is already stopped";
+            case MccSimpleSlewingModelErrorCode::ERROR_STOPPED:
+                return "slewing was stopped";
             default:
                 return "UNKNOWN";
         }
@@ -176,6 +178,8 @@ public:
                 }
             }
 
+            auto last_hw_time = tdata.time_point;
+
             bool in_zone;
             std::vector<bool> in_zone_vec;
             auto pz_err = controls->inPZone(tdata.target, &in_zone, &in_zone_vec);
@@ -262,8 +266,12 @@ public:
                 return mcc_deduce_error_code(hw_err, MccSimpleSlewingModelErrorCode::ERROR_HW_GETSTATE);
             }
 
+
             hw_state.X = (double)tdata.target.X;
             hw_state.Y = (double)tdata.target.Y;
+            hw_state.endptX = (double)tdata.target.X;
+            hw_state.endptY = (double)tdata.target.Y;
+
             {
                 std::lock_guard lock{*_currentParamsMutex};
 
@@ -313,7 +321,7 @@ public:
             // double dist, dx, dy, sinY, rate2, xrate;
             // std::chrono::duration<double> dtx, dty;  // seconds in double
 
-            double dist;
+            double dist, dx, dy;
 
             // bool adjust_mode = false;
             // static constexpr auto sideral_rate2 = slewing_params_t::sideralRate * slewing_params_t::sideralRate;
@@ -323,18 +331,11 @@ public:
             last_adjust_tp = start_slewing_tp;
 
             std::pair<double, double> distXY;
+            bool tag_var_coord = true;
 
-
+            if (tdata.target.pair_kind == MccCoordPairKind::COORDS_KIND_AZALT ||
-            if constexpr (mccIsEquatorialMount(CONTROLS_T::mountType)) {
+                tdata.target.pair_kind == MccCoordPairKind::COORDS_KIND_AZZD) {
-                if (tdata.target.pair_kind != MccCoordPairKind::COORDS_KIND_HADEC_APP) {
+                tag_var_coord = false;
-                    // here, HA and DEC are changed during slewing process!!
-                    slew_and_stop = false;
-                }
-            } else if constexpr (mccIsAltAzMount(CONTROLS_T::mountType)) {
-                if (!(tdata.target.pair_kind == MccCoordPairKind::COORDS_KIND_AZALT &&
-                      tdata.target.pair_kind == MccCoordPairKind::COORDS_KIND_AZZD)) {
-                    slew_and_stop = false;
-                }
             }
 
 
@@ -463,12 +464,19 @@ public:
                 logger.logTrace(std::format("hw state was updated ({}, {})", MccAngle(hw_state.X).sexagesimal(true),
                                             MccAngle(hw_state.Y).sexagesimal()));
 
-                if (slew_and_stop) {  // just wait for mount to be stopped
+                if (slew_and_stop && !tag_var_coord) {  // just wait for mount to be stopped
                     if (hw_state.moving_state == CONTROLS_T::hardware_moving_state_t::HW_MOVE_STOPPED) {
                         logger.logInfo("mount moving state is STOPPED - exit!");
                         break;
                     }
                 } else {
+                    if (last_hw_time == tdata.time_point) {
+                        logger.logTrace("Same hardware timepoint! Just continue to polling!\n\n\n\n");
+                        continue;
+                    }
+
+                    last_hw_time = tdata.time_point;
+
                     t_err = controls->targetToMountDist(&dist);
                     if (t_err) {
                         *_stopSlewing = true;
@@ -477,10 +485,11 @@ public:
 
                     logger.logTrace(std::format("    target-to-mount distance: {}", mcc::MccAngleFancyString(dist)));
 
-                    // if (dist < _currentParams.adjustCoordDiff) {
-                    // if (dist < 1.0_degs) {
                     if (dist <= _currentParams.slewToleranceRadius) {  // stop slewing and exit from cycle
                         logger.logInfo("target-to-mount distance is lesser than slew tolerance radius - exit!");
+                        if (slew_and_stop) {
+                            controls->hardwareStop();
+                        }
                         break;
                     }
 
@@ -494,10 +503,18 @@ public:
                     hw_state.X = (double)tdata.target.X;
                     hw_state.Y = (double)tdata.target.Y;
 
-                    logger.logTrace(
+                    controls->targetToMountDiff(tdata.pair_kind, &dx, &dy);
-                        std::format("Send to hardware: X = {} degs, Y = {} degs ({}, {})",
+
-                                    mcc::MccAngle{hw_state.X}.degrees(), mcc::MccAngle{hw_state.Y}.degrees(),
+                    // hw_state.endptX = hw_state.X + std::copysign(1.0_degs, dx);
-                                    MccAngle(hw_state.X).sexagesimal(true), MccAngle(hw_state.Y).sexagesimal()));
+                    // hw_state.endptY = hw_state.Y + std::copysign(1.0_degs, dy);
+                    hw_state.endptX = hw_state.X + std::copysign(10.0_degs, dx);
+                    hw_state.endptY = hw_state.Y + std::copysign(10.0_degs, dy);
+
+                    logger.logTrace(std::format(
+                        "Send to hardware: {}, {}, tag: {}, {} (X = {} degs, Y = {} degs)",
+                        MccAngle(hw_state.X).sexagesimal(true), MccAngle(hw_state.Y).sexagesimal(),
+                        MccAngle(hw_state.endptX).sexagesimal(true), MccAngle(hw_state.endptY).sexagesimal(),
+                        mcc::MccAngle{hw_state.X}.degrees(), mcc::MccAngle{hw_state.Y}.degrees()));
 
                     hw_err = controls->hardwareSetState(hw_state);
                     if (hw_err) {
@@ -507,11 +524,10 @@ public:
 
 
                     logger.logDebug("    the 'hardwareSetState' method performed successfully!");
-                    // }
 
 
                     // FOR DEBUG PURPOSE!!!!
-                    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+                    // std::this_thread::sleep_for(std::chrono::milliseconds(50));
                     logger.logTrace(std::format("get hw state right after hardwareSetState ..."));
 
                     hw_err = controls->hardwareGetState(&hw_state);