timur/mcc
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

...

Browse Source
This commit is contained in:
Timur A. Fatkhullin
2026-02-19 18:49:30 +03:00
parent 18e6a99267
commit 099c6056d7
3 changed files with 136 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files

191
include/mcc/mcc_movement_controls.h
View File

@@ -257,7 +257,7 @@ protected:
return true; return true;
} }
fst.open(_filename); fst.open(_filename, std::ios::app);
if (!fst.is_open()) { if (!fst.is_open()) {
return false; return false;
@@ -312,13 +312,16 @@ public:
using STATUS_T = std::decay_t<traits::mcc_func_arg1_t<CallbackFuncT>>; using STATUS_T = std::decay_t<traits::mcc_func_arg1_t<CallbackFuncT>>;
auto send_to_hardware = [hardware, logger](typename HARDWARE_T::hardware_state_t const& hw_state) -> error_t { auto send_to_hardware = [hardware, logger](typename HARDWARE_T::hardware_state_t const& hw_state) -> error_t {
auto tp =
std::chrono::duration_cast<std::chrono::milliseconds>(hw_state.XY.epoch().UTC().time_since_epoch());
if constexpr (std::derived_from<decltype(hw_state.XY), MccCoordPair<MccAngleX, MccAngleY>>) { if constexpr (std::derived_from<decltype(hw_state.XY), MccCoordPair<MccAngleX, MccAngleY>>) {
logger->logDebug(std::format("Send to hardware: X = {} degs, Y = {} degs", hw_state.XY.x().degrees(), logger->logDebug(std::format("Send to hardware: X = {} degs, Y = {} degs (timepoint: {})",
hw_state.XY.y().degrees())); hw_state.XY.x().degrees(), hw_state.XY.y().degrees(), tp));
} else { // user defined mcc_coord_pair_c } else { // user defined mcc_coord_pair_c
logger->logDebug(std::format("Send to hardware: X = {} degs, Y = {} degs", logger->logDebug(std::format("Send to hardware: X = {} degs, Y = {} degs (timepoint: {})",
MccAngle((double)hw_state.XY.x()).degrees(), MccAngle((double)hw_state.XY.x()).degrees(),
MccAngle((double)hw_state.XY.y()).degrees())); MccAngle((double)hw_state.XY.y()).degrees(), tp));
} }
auto hw_err = hardware->hardwareSetState(hw_state); auto hw_err = hardware->hardwareSetState(hw_state);
@@ -438,11 +441,13 @@ public:
}; };
auto log_pos = [logger, this](typename TELEMETRY_T::telemetry_data_t const& tdata) -> error_t { auto log_pos = [logger, this](typename TELEMETRY_T::telemetry_data_t const& tdata,
std::tuple<double, double, double>& dist) -> error_t {
double x_mnt, y_mnt, x_tag, y_tag; double x_mnt, y_mnt, x_tag, y_tag;
std::conditional_t<mccIsEquatorialMount(HARDWARE_T::hwMountType), MccSkyHADEC_OBS, std::conditional_t<
std::conditional_t<mccIsAltAzMount(HARDWARE_T::hwMountType), MccSkyAZZD, std::nullptr_t>> mccIsEquatorialMount(HARDWARE_T::hwMountType), MccSkyHADEC_OBS,
std::conditional_t<mccIsAltAzMount(HARDWARE_T::hwMountType), MccSkyAZALT, std::nullptr_t>>
coord_pair; coord_pair;
static_assert(!std::is_null_pointer_v<decltype(coord_pair)>, "UNKNOWN MOUNT TYPE!"); static_assert(!std::is_null_pointer_v<decltype(coord_pair)>, "UNKNOWN MOUNT TYPE!");
@@ -451,7 +456,7 @@ public:
: mccIsAltAzMount(HARDWARE_T::hwMountType) ? "AZ" : mccIsAltAzMount(HARDWARE_T::hwMountType) ? "AZ"
: "GEN_X"; : "GEN_X";
const std::string_view y_str = mccIsEquatorialMount(HARDWARE_T::hwMountType) ? "DEC" const std::string_view y_str = mccIsEquatorialMount(HARDWARE_T::hwMountType) ? "DEC"
: mccIsAltAzMount(HARDWARE_T::hwMountType) ? "ZD" : mccIsAltAzMount(HARDWARE_T::hwMountType) ? "ALT"
: "GEN_Y"; : "GEN_Y";
; ;
@@ -477,9 +482,22 @@ public:
coord_pair.x().sexagesimal(true), y_str, coord_pair.y().sexagesimal(), coord_pair.x().sexagesimal(true), y_str, coord_pair.y().sexagesimal(),
tdata.hwState.XY.x().sexagesimal(), tdata.hwState.XY.y().sexagesimal())); tdata.hwState.XY.x().sexagesimal(), tdata.hwState.XY.y().sexagesimal()));
x_mnt = coord_pair.x();
y_mnt = coord_pair.y();
dist = utils::distanceOnSphere(x_tag, y_tag, x_mnt, y_mnt);
_pathFile << tdata.mountPos.epoch().UTC().time_since_epoch().count() << " " << x_tag << " " << y_tag << " " _pathFile << tdata.mountPos.epoch().UTC().time_since_epoch().count() << " " << x_tag << " " << y_tag << " "
<< x_mnt << " " << y_mnt << " " << (x_tag - x_mnt) << " " << (y_tag - y_mnt) << " " << x_mnt << " " << y_mnt << " " << std::get<0>(dist) << " " << std::get<1>(dist) << " "
<< (int)tdata.hwState.movementState << "\n"; << std::get<2>(dist) << " " << (int)tdata.hwState.movementState << "\n";
// _pathFile << tdata.mountPos.epoch().UTC().time_since_epoch().count() << " " << x_tag << " " << y_tag << "
// "
// << x_mnt << " " << y_mnt << " " << (x_tag - x_mnt) << " " << (y_tag - y_mnt) << " "
// << (int)tdata.hwState.movementState << "\n";
logger->logTrace(std::format(" target-to-mount distance: {} (dx = {}, dy = {})",
MccAngleFancyString(std::get<2>(dist)), MccAngleFancyString(std::get<0>(dist)),
MccAngleFancyString(std::get<1>(dist))));
return MccSimpleMovementControlsErrorCode::ERROR_OK; return MccSimpleMovementControlsErrorCode::ERROR_OK;
}; };
@@ -530,6 +548,7 @@ public:
std::chrono::duration_cast<std::chrono::duration<double>>(_currentParams.minTimeToPZone).count(); std::chrono::duration_cast<std::chrono::duration<double>>(_currentParams.minTimeToPZone).count();
if (!_currentParams.slewingPathFilename.empty()) { // open slewing trajectory file if (!_currentParams.slewingPathFilename.empty()) { // open slewing trajectory file
logger->logInfo(std::format("Set slewing path filename to {}", _currentParams.slewingPathFilename));
_pathFile.setFilename(_currentParams.slewingPathFilename); _pathFile.setFilename(_currentParams.slewingPathFilename);
} else { } else {
logger->logWarn("Slewing path filename is empty! Do not save it!"); logger->logWarn("Slewing path filename is empty! Do not save it!");
@@ -559,7 +578,7 @@ public:
_pathFile << "# \n"; _pathFile << "# \n";
_pathFile << "# Format (time is in nanoseconds, coordinates are in radians): \n"; _pathFile << "# Format (time is in nanoseconds, coordinates are in radians): \n";
_pathFile << "# <UNIXTIME> <target X> <target Y> <mount X> <mount Y> <dX_{target-mount}> " _pathFile << "# <UNIXTIME> <target X> <target Y> <mount X> <mount Y> <dX_{target-mount}> "
"<dY_{target-mount}> <moving state>\n"; "<dY_{target-mount}> <taget-mount-distance> <moving state>\n";
typename TELEMETRY_T::telemetry_data_t tdata; typename TELEMETRY_T::telemetry_data_t tdata;
@@ -584,9 +603,12 @@ public:
return; return;
} }
using ep_t = decltype(hw_state.XY.epoch());
hw_state.movementState = HARDWARE_T::hardware_movement_state_t::HW_MOVE_SLEWING; hw_state.movementState = HARDWARE_T::hardware_movement_state_t::HW_MOVE_SLEWING;
hw_state.XY.setX(tdata.targetXY.x()); hw_state.XY.setX(tdata.targetXY.x());
hw_state.XY.setY(tdata.targetXY.y()); hw_state.XY.setY(tdata.targetXY.y());
hw_state.XY.setEpoch(ep_t::now());
// start slewing ... // start slewing ...
@@ -607,6 +629,8 @@ public:
mcc_deduced_coord_pair_t<HARDWARE_T::hwMountType> tag_cp, mnt_cp; mcc_deduced_coord_pair_t<HARDWARE_T::hwMountType> tag_cp, mnt_cp;
std::tuple<double, double, double> dist;
*_stopMoving = false; *_stopMoving = false;
while (!*_stopMoving) { while (!*_stopMoving) {
@@ -618,7 +642,12 @@ public:
return; return;
} }
log_pos(tdata); log_pos(tdata, dist);
// logger->logInfo("\tMNT.XY {} {}; MNT.HADEC {} {}", (double)tdata.hwState.XY.x(),
// (double)tdata.hwState.XY.y(), tdata.mountPos.co_lon(), tdata.mountPos.co_lat());
// logger->logInfo("\tTAG.XY {} {}; TAG.HADEC {} {}", (double)tdata.targetXY.x(),
// (double)tdata.targetXY.y(), tdata.targetPos.co_lon(), tdata.targetPos.co_lat());
if (*_stopMoving) { if (*_stopMoving) {
*_lastError = MccSimpleMovementControlsErrorCode::ERROR_STOPPED; *_lastError = MccSimpleMovementControlsErrorCode::ERROR_STOPPED;
@@ -643,8 +672,21 @@ public:
if (slew_and_stop) { // just wait until the mount stops if (slew_and_stop) { // just wait until the mount stops
if (tdata.hwState.movementState == HARDWARE_T::hardware_movement_state_t::HW_MOVE_STOPPED) { if (tdata.hwState.movementState == HARDWARE_T::hardware_movement_state_t::HW_MOVE_STOPPED) {
logger->logInfo("mount movement state is STOPPED! Exit from slewing process!"); if (std::get<2>(dist) > _currentParams.slewToleranceRadius) {
break; // resend new position since target coordinates are changed in time
hw_state.movementState = HARDWARE_T::hardware_movement_state_t::HW_MOVE_SLEWING;
hw_state.XY.setX(tdata.targetXY.x());
hw_state.XY.setY(tdata.targetXY.y());
hw_state.XY.setEpoch(ep_t::now());
*_lastError = send_to_hardware(hw_state);
if (_lastError->load()) {
break;
}
} else {
logger->logInfo("mount movement state is STOPPED! Exit from slewing process!");
break;
}
} }
} else { } else {
if (last_hw_time == tdata.hwState.XY.epoch().UTC()) { if (last_hw_time == tdata.hwState.XY.epoch().UTC()) {
@@ -654,20 +696,21 @@ public:
last_hw_time = tdata.hwState.XY.epoch().UTC(); last_hw_time = tdata.hwState.XY.epoch().UTC();
auto ccte_err = tdata.targetPos.to(tag_cp); // auto ccte_err = tdata.targetPos.toAtSameEpoch(tag_cp);
if (ccte_err) { // if (ccte_err) {
*_lastError = mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP); // *_lastError = mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP);
} // }
ccte_err = tdata.mountPos.to(mnt_cp); // ccte_err = tdata.mountPos.toAtSameEpoch(mnt_cp);
if (ccte_err) { // if (ccte_err) {
*_lastError = mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP); // *_lastError = mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP);
} // }
auto dist = utils::distanceOnSphere(tag_cp.x(), tag_cp.y(), mnt_cp.x(), mnt_cp.y()); // auto dist = utils::distanceOnSphere(tag_cp.x(), tag_cp.y(), mnt_cp.x(), mnt_cp.y());
logger->logTrace(std::format( // logger->logTrace(std::format(
" target-to-mount distance: {} (dx = {}, dy = {})", MccAngleFancyString(std::get<2>(dist)), // " target-to-mount distance: {} (dx = {}, dy = {})",
MccAngleFancyString(std::get<0>(dist)), MccAngleFancyString(std::get<1>(dist)))); // MccAngleFancyString(std::get<2>(dist)), MccAngleFancyString(std::get<0>(dist)),
// MccAngleFancyString(std::get<1>(dist))));
// stop slewing and exit from the cycle? // stop slewing and exit from the cycle?
@@ -694,20 +737,24 @@ public:
hw_state.movementState = HARDWARE_T::hardware_movement_state_t::HW_MOVE_SLEWING; hw_state.movementState = HARDWARE_T::hardware_movement_state_t::HW_MOVE_SLEWING;
hw_state.XY.setX(tdata.targetXY.x()); hw_state.XY.setX(tdata.targetXY.x());
hw_state.XY.setY(tdata.targetXY.y()); hw_state.XY.setY(tdata.targetXY.y());
hw_state.XY.setEpoch(ep_t::now());
*_lastError = send_to_hardware(hw_state); *_lastError = send_to_hardware(hw_state);
if (_lastError->load()) { if (_lastError->load()) {
break; break;
} }
// sleep here
std::this_thread::sleep_for(_currentParams.slewingTelemetryInterval);
} }
// sleep here
std::this_thread::sleep_for(_currentParams.slewingTelemetryInterval);
} }
if (_stopMoving->load()) { // external stop if (_stopMoving->load()) { // external stop
logger->logWarn("Slewing was stopped!"); logger->logWarn("Slewing was stopped!");
*_lastError = MccSimpleMovementControlsErrorCode::ERROR_OK; *_lastError = MccSimpleMovementControlsErrorCode::ERROR_OK;
_pathFile.save();
return;
} }
*_stopMoving = true; *_stopMoving = true;
@@ -729,27 +776,27 @@ public:
return; return;
} }
auto ccte_err = tdata.targetPos.to(tag_cp); // auto ccte_err = tdata.targetPos.to(tag_cp);
if (ccte_err) { // if (ccte_err) {
*_lastError = mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP); // *_lastError = mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP);
(*cb_sptr)(STATUS_T::MOUNT_STATUS_ERROR); // (*cb_sptr)(STATUS_T::MOUNT_STATUS_ERROR);
return; // return;
} // }
ccte_err = tdata.mountPos.to(mnt_cp); // ccte_err = tdata.mountPos.to(mnt_cp);
if (ccte_err) { // if (ccte_err) {
*_lastError = mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP); // *_lastError = mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP);
(*cb_sptr)(STATUS_T::MOUNT_STATUS_ERROR); // (*cb_sptr)(STATUS_T::MOUNT_STATUS_ERROR);
return; // return;
} // }
auto dist = utils::distanceOnSphere(tag_cp.x(), tag_cp.y(), mnt_cp.x(), mnt_cp.y()); // auto dist = utils::distanceOnSphere(tag_cp.x(), tag_cp.y(), mnt_cp.x(), mnt_cp.y());
log_pos(tdata); log_pos(tdata, dist);
logger->logDebug( // logger->logDebug(
std::format(" target-to-mount distance {}", MccAngleFancyString(std::get<2>(dist)))); // std::format(" target-to-mount distance {}", MccAngleFancyString(std::get<2>(dist))));
if (!slew_and_stop) { // start tracking if (!slew_and_stop) { // start tracking
_trackingFunc(); _trackingFunc();
@@ -760,11 +807,14 @@ public:
}; };
/* tracking function */
_trackingFunc = [log_pos, check_pzones, send_to_hardware, telemetry, cb_sptr, logger, this]() { _trackingFunc = [log_pos, check_pzones, send_to_hardware, telemetry, cb_sptr, logger, this]() {
double braking_accelX, braking_accelY; double braking_accelX, braking_accelY;
double min_time_to_pzone_in_secs; double min_time_to_pzone_in_secs;
logger->logInfo("Start tracking");
{ {
// std::lock_guard lock{*_currentParamsMutex}; // std::lock_guard lock{*_currentParamsMutex};
if (mcc::utils::isEqual(_currentParams.brakingAccelX, 0.0)) { if (mcc::utils::isEqual(_currentParams.brakingAccelX, 0.0)) {
@@ -783,13 +833,14 @@ public:
std::chrono::duration_cast<std::chrono::duration<double>>(_currentParams.minTimeToPZone).count(); std::chrono::duration_cast<std::chrono::duration<double>>(_currentParams.minTimeToPZone).count();
if (!_currentParams.trackingPathFilename.empty()) { // open slewing trajectory file if (!_currentParams.trackingPathFilename.empty()) { // open slewing trajectory file
logger->logInfo(
std::format(" Set tracking filename to {}", _currentParams.trackingPathFilename));
_pathFile.setFilename(_currentParams.trackingPathFilename); _pathFile.setFilename(_currentParams.trackingPathFilename);
} else { } else {
logger->logWarn("Tracking path filename is empty! Do not save it!"); logger->logWarn(" Tracking path filename is empty! Do not save it!");
} }
} }
logger->logInfo("Start tracking");
logger->logInfo(std::format(" braking acceleration X: {} degs/s^2 (in config: {} rads/s^2)", logger->logInfo(std::format(" braking acceleration X: {} degs/s^2 (in config: {} rads/s^2)",
MccAngle(braking_accelX).degrees(), _currentParams.brakingAccelX)); MccAngle(braking_accelX).degrees(), _currentParams.brakingAccelX));
logger->logInfo(std::format(" braking acceleration Y: {} degs/s^2 (in config: {} rads/s^2)", logger->logInfo(std::format(" braking acceleration Y: {} degs/s^2 (in config: {} rads/s^2)",
@@ -802,7 +853,7 @@ public:
_pathFile << "# \n"; _pathFile << "# \n";
_pathFile << "# Format (time is in nanoseconds, coordinates are in radians): \n"; _pathFile << "# Format (time is in nanoseconds, coordinates are in radians): \n";
_pathFile << "# <UNIXTIME> <target X> <target Y> <mount X> <mount Y> <dX_{target-mount}> " _pathFile << "# <UNIXTIME> <target X> <target Y> <mount X> <mount Y> <dX_{target-mount}> "
"<dY_{target-mount}> <moving state>\n"; "<dY_{target-mount}> <taget-mount-distance> <moving state>\n";
typename TELEMETRY_T::telemetry_data_t tdata; typename TELEMETRY_T::telemetry_data_t tdata;
@@ -832,6 +883,8 @@ public:
mcc_deduced_coord_pair_t<HARDWARE_T::hwMountType> tag_cp, mnt_cp; mcc_deduced_coord_pair_t<HARDWARE_T::hwMountType> tag_cp, mnt_cp;
std::tuple<double, double, double> dist;
*_stopMoving = false; *_stopMoving = false;
while (!_stopMoving->load()) { while (!_stopMoving->load()) {
@@ -843,7 +896,7 @@ public:
return; return;
} }
log_pos(tdata); log_pos(tdata, dist);
if (*_stopMoving) { if (*_stopMoving) {
*_lastError = MccSimpleMovementControlsErrorCode::ERROR_STOPPED; *_lastError = MccSimpleMovementControlsErrorCode::ERROR_STOPPED;
@@ -863,25 +916,26 @@ public:
last_hw_time = tdata.hwState.XY.epoch().UTC(); last_hw_time = tdata.hwState.XY.epoch().UTC();
auto ccte_err = tdata.targetPos.to(tag_cp); // auto ccte_err = tdata.targetPos.to(tag_cp);
if (ccte_err) { // if (ccte_err) {
*_lastError = mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP); // *_lastError = mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP);
} // }
ccte_err = tdata.mountPos.to(mnt_cp); // ccte_err = tdata.mountPos.to(mnt_cp);
if (ccte_err) { // if (ccte_err) {
*_lastError = mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP); // *_lastError = mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP);
} // }
auto dist = utils::distanceOnSphere(tag_cp.x(), tag_cp.y(), mnt_cp.x(), mnt_cp.y()); // auto dist = utils::distanceOnSphere(tag_cp.x(), tag_cp.y(), mnt_cp.x(), mnt_cp.y());
logger->logTrace(std::format( // logger->logTrace(std::format(
" target-to-mount distance: {} (dx = {}, dy = {})", MccAngleFancyString(std::get<2>(dist)), // " target-to-mount distance: {} (dx = {}, dy = {})", MccAngleFancyString(std::get<2>(dist)),
MccAngleFancyString(std::get<0>(dist)), MccAngleFancyString(std::get<1>(dist)))); // MccAngleFancyString(std::get<0>(dist)), MccAngleFancyString(std::get<1>(dist))));
// resend new position since target coordinates are changed in time // resend new position since target coordinates are changed in time
hw_state.movementState = HARDWARE_T::hardware_movement_state_t::HW_MOVE_TRACKING; hw_state.movementState = HARDWARE_T::hardware_movement_state_t::HW_MOVE_TRACKING;
hw_state.XY.setX(tdata.targetXY.x()); hw_state.XY.setX(tdata.targetXY.x());
hw_state.XY.setY(tdata.targetXY.y()); hw_state.XY.setY(tdata.targetXY.y());
hw_state.XY.setEpoch(tdata.hwState.XY.epoch());
*_lastError = send_to_hardware(hw_state); *_lastError = send_to_hardware(hw_state);
if (_lastError->load()) { if (_lastError->load()) {
@@ -899,14 +953,15 @@ public:
if (_stopMoving->load()) { // external stop if (_stopMoving->load()) { // external stop
logger->logWarn("Tracking was stopped!"); logger->logWarn("Tracking was stopped!");
*_lastError = MccSimpleMovementControlsErrorCode::ERROR_OK; *_lastError = MccSimpleMovementControlsErrorCode::ERROR_OK;
} else {
*_stopMoving = true;
logger->logInfo("Tracking finished");
auto err = _lastError->load();
logger->logInfo(
std::format(" exit code: {} {} {}", err.value(), err.category().name(), err.message()));
} }
*_stopMoving = true;
logger->logInfo("Tracking finished");
auto err = _lastError->load();
logger->logInfo(std::format(" exit code: {} {} {}", err.value(), err.category().name(), err.message()));
_pathFile.save(); _pathFile.save();
}; };
} }

2
include/mcc/mcc_serializer.h
View File

@@ -518,7 +518,7 @@ struct MccSerializer<VT> : MccSerializerBase {
MccSerializerBase::addElemDelimiter(output, pars_h); MccSerializerBase::addElemDelimiter(output, pars_h);
err = ang_sr(output, rd_obs.y(), pars_h); err = ang_sr(output, rd_obs.y(), pars_d);
if (err) { if (err) {
return mcc_deduced_err(err, MccSerializerErrorCode::ERROR_UNDERLYING_SERIALIZER); return mcc_deduced_err(err, MccSerializerErrorCode::ERROR_UNDERLYING_SERIALIZER);
} }

20
include/mcc/mcc_utils.h
View File

@@ -395,16 +395,20 @@ static std::tuple<double, double, double> distanceOnSphere(double co_lon1,
std::get<0>(res) = co_lon1 - co_lon2; std::get<0>(res) = co_lon1 - co_lon2;
std::get<1>(res) = co_lat1 - co_lat2; std::get<1>(res) = co_lat1 - co_lat2;
double cosDco_lon = cos(std::get<0>(res)); double cosDco_lon = std::cos(std::abs(std::get<0>(res)));
double cos1 = cos(co_lat1);
double sin1 = sin(co_lat1);
double cos2 = cos(co_lat2);
double sin2 = sin(co_lat2);
double term1 = cos1 * sin(std::get<0>(res)); double cos1 = std::cos(co_lat1);
double term2 = cos2 * sin1 - sin2 * cos1 * cosDco_lon; double sin1 = std::sin(co_lat1);
std::get<2>(res) = atan2(sqrt(term1 * term1 + term2 * term2), sin2 * sin1 + cos2 * cos1 * cosDco_lon); double cos2 = std::cos(co_lat2);
double sin2 = std::sin(co_lat2);
double term1 = cos2 * std::sin(std::abs(std::get<0>(res)));
double term2 = cos1 * sin2 - sin1 * cos2 * cosDco_lon;
std::get<2>(res) = std::atan2(sqrt(term1 * term1 + term2 * term2), sin1 * sin2 + cos1 * cos2 * cosDco_lon);
// std::get<2>(res) = acos(sin1 * sin2 + cos1 * cos2 * cosDco_lon);
return res; return res;
} }