Telescope Control Library
EquatorialMount.cpp
- Committer:
- caoyu@caoyuan9642-desktop.MIT.EDU
- Date:
- 2018-09-24
- Revision:
- 19:fd854309cb4c
- Parent:
- 17:7e47bc1630c0
File content as of revision 19:fd854309cb4c:
#include <math.h> #include "EquatorialMount.h" #define EM_DEBUG 0 EquatorialMount::EquatorialMount(Axis& ra, Axis& dec, UTCClock& clk, LocationProvider &loc) : ra(ra), dec(dec), clock(clk), loc(loc), curr_pos(0, 0), curr_nudge_dir( NUDGE_NONE), nudgeSpeed(0), pier_side(PIER_SIDE_EAST), num_alignment_stars( 0), pec(ra) { south = loc.getLatitude() < 0.0; // Get initial transformation calibration.pa = AzimuthalCoordinates(loc.getLatitude(), 0); // Set RA and DEC positions to zero ra.setAngleDeg(0); dec.setAngleDeg(0); dec.setTrackSpeedSidereal(0); // Make sure dec doesn't move during tracking updatePosition(); // Set PEC to RA axis this->ra.setPEC(&pec); this->ra.setPECEnabled(true); // Set in tracking mode startTracking(); } osStatus EquatorialMount::goTo(double ra_dest, double dec_dest) { return goTo(EquatorialCoordinates(dec_dest, ra_dest)); } osStatus EquatorialMount::goTo(EquatorialCoordinates dest) { debug_if(EM_DEBUG, "dest ra=%.2f, dec=%.2f\n", dest.ra, dest.dec); updatePosition(); // Get the latest position information if (EM_DEBUG) printPosition(); for (int i = 0; i < 2; i++) { // Convert to Mount coordinates. Automatically determine the pier side, then apply offset MountCoordinates dest_mount = convertToMountCoordinates(dest); osStatus s = goToMount(dest_mount, (i > 0)); // Use correction only for the second time if (s != osOK) return s; } if (EM_DEBUG) printPosition(); return osOK; } osStatus EquatorialMount::goToMount(MountCoordinates dest_mount, bool withCorrection) { mutex_execution.lock(); bool was_tracking = false; if ((status & MOUNT_TRACKING) && !(status & MOUNT_NUDGING)) { // Tracking mode was_tracking = true; stopSync(); } else if (status != MOUNT_STOPPED) { debug("EM: goTo requested while mount is not stopped.\n"); mutex_execution.unlock(); return osErrorParameter; } debug_if(EM_DEBUG, "EM: goTo\n"); debug_if(EM_DEBUG, "dstmnt ra=%.2f, dec=%.2f\n", dest_mount.ra_delta, dest_mount.dec_delta); axisrotdir_t dec_dir, ra_dir; ra_dir = (remainder(dest_mount.ra_delta, 360.0) > remainder(curr_pos.ra_delta, 360.0)) ? AXIS_ROTATE_POSITIVE : (remainder(dest_mount.ra_delta, 360.0) < remainder(curr_pos.ra_delta, 360.0)) ? AXIS_ROTATE_NEGATIVE : AXIS_ROTATE_STOP; dec_dir = (remainder(dest_mount.dec_delta, 360.0) > remainder(curr_pos.dec_delta, 360.0)) ? AXIS_ROTATE_POSITIVE : (remainder(dest_mount.dec_delta, 360.0) < remainder(curr_pos.dec_delta, 360.0)) ? AXIS_ROTATE_NEGATIVE : AXIS_ROTATE_STOP; debug_if(EM_DEBUG, "EM: start slewing\n"); status = MOUNT_SLEWING; ra.startSlewTo(ra_dir, dest_mount.ra_delta, withCorrection); dec.startSlewTo(dec_dir, dest_mount.dec_delta, withCorrection); int ret = (int) ra.waitForSlew(); ret |= (int) dec.waitForSlew(); debug_if(EM_DEBUG, "EM: slewing finished\n"); status = MOUNT_STOPPED; mutex_execution.unlock(); if (was_tracking && !(ret & (FINISH_ERROR | FINISH_EMERG_STOPPED))) { startTracking(); } updatePosition(); // Update current position if (ret) { // Stopped during slew return ret; } else return osOK; } osStatus EquatorialMount::startTracking() { if (status != MOUNT_STOPPED) { debug("EM: tracking requested while mount is not stopped.\n"); return osErrorParameter; } mutex_execution.lock(); axisrotdir_t ra_dir = AXIS_ROTATE_POSITIVE; // Tracking is always going to positive hour angle direction, which is defined as positive. status = MOUNT_TRACKING; osStatus sr, sd; sr = ra.startTracking(ra_dir); sd = dec.startTracking(AXIS_ROTATE_STOP); mutex_execution.unlock(); if (sr != osOK || sd != osOK) return osErrorResource; else return osOK; } osStatus EquatorialMount::startNudge(nudgedir_t newdir) { if (status & MOUNT_SLEWING) // Cannot be nudged in slewing mode { return osErrorParameter; } osStatus s = osOK; mutex_execution.lock(); if (newdir == NUDGE_NONE) { // Stop nudging if being nudged if (status & MOUNT_NUDGING) { mountstatus_t oldstatus = status; stopAsync(); // Stop the mount if (oldstatus == MOUNT_NUDGING) { // Stop the mount } else if (oldstatus == MOUNT_NUDGING_TRACKING) { // Get to tracking state ra.setSlewSpeed(nudgeSpeed); // restore the slew rate of RA startTracking(); } } } else { // newdir is not NUDGE_NONE updatePosition(); // Update current position, because we need to know the current pier side bool ra_changed = false, dec_changed = false; axisrotdir_t ra_dir, dec_dir; if ((status & MOUNT_NUDGING) == 0) { // Initial nudge curr_nudge_dir = NUDGE_NONE; //Make sure the current nudging direction is cleared nudgeSpeed = getSlewSpeed(); // Get nudge speed and use it for ALL following nudge operations, until the nudge finishes } // see what has changed in RA if ((curr_nudge_dir & (NUDGE_WEST | NUDGE_EAST)) != (newdir & (NUDGE_WEST | NUDGE_EAST))) { // If something on east/west has changed, we need to stop the RA axis (or maybe enable it later to switch direction) ra_changed = true; if (newdir & NUDGE_EAST) { // Nudge east ra_dir = AXIS_ROTATE_NEGATIVE; } else if (newdir & NUDGE_WEST) { // Nudge west ra_dir = AXIS_ROTATE_POSITIVE; } else { ra_dir = AXIS_ROTATE_STOP; } } // see what has changed in DEC if ((curr_nudge_dir & (NUDGE_SOUTH | NUDGE_NORTH)) != (newdir & (NUDGE_SOUTH | NUDGE_NORTH))) { // If something on east/west has changed, we need to stop the RA axis (or maybe enable it later to switch direction) dec_changed = true; if (newdir & NUDGE_NORTH) { // Nudge north dec_dir = (curr_pos.side == PIER_SIDE_WEST) ? AXIS_ROTATE_NEGATIVE : AXIS_ROTATE_POSITIVE; } else if (newdir & NUDGE_SOUTH) { // Nudge south dec_dir = (curr_pos.side == PIER_SIDE_WEST) ? AXIS_ROTATE_POSITIVE : AXIS_ROTATE_NEGATIVE; } else { dec_dir = AXIS_ROTATE_STOP; } } curr_nudge_dir = newdir; // Request stop as necessary if (ra_changed) { ra.flushCommandQueue(); if (ra.getSlewState() == AXIS_SLEW_DECELERATING && ra_dir == ra.getCurrentDirection()) { ra.stopKeepSpeed(); } else { ra.stop(); } } if (dec_changed) { dec.flushCommandQueue(); if (dec.getSlewState() == AXIS_SLEW_DECELERATING && dec_dir == dec.getCurrentDirection()) { dec.stopKeepSpeed(); } else { dec.stop(); } } // Wait for stop together // while ((ra_changed && ra.getStatus() != AXIS_STOPPED // && ra.getStatus() != AXIS_INERTIAL) // || (dec_changed && dec.getStatus() != AXIS_STOPPED // && dec.getStatus() != AXIS_INERTIAL)) // { // Thread::yield(); // } // DEC axis is ok to start regardless of tracking state if (dec_changed && dec_dir != AXIS_ROTATE_STOP) { s = dec.startSlewingIndefinite(dec_dir); } // Now RA if (ra_changed && s == osOK) { if (status & MOUNT_TRACKING) { // In tracking mode now, need to calculate differential rate if (ra_dir == AXIS_ROTATE_STOP) { // resume tracking s = ra.startTracking(AXIS_ROTATE_POSITIVE); } else { // This is the complicated part double trackSpeed = ra.getTrackSpeedSidereal() * sidereal_speed; debug_if(EM_DEBUG > 1, "EM: ra, ns=%f, ts=%f\n", nudgeSpeed, trackSpeed); if (ra_dir == AXIS_ROTATE_POSITIVE) { // Same direction as tracking ra.setSlewSpeed(nudgeSpeed + trackSpeed); s = ra.startSlewingIndefinite(AXIS_ROTATE_POSITIVE); } else if (nudgeSpeed < trackSpeed) { // ra_dir == AXIS_ROTATE_NEGATIVE // Partially canceling the tracking speed ra.setSlewSpeed(trackSpeed - nudgeSpeed); s = ra.startSlewingIndefinite(AXIS_ROTATE_POSITIVE); } else if (nudgeSpeed > trackSpeed) {// ra_dir == AXIS_ROTATE_NEGATIVE // Direction inverted ra.setSlewSpeed(nudgeSpeed - trackSpeed); ra.startSlewingIndefinite(AXIS_ROTATE_NEGATIVE); } // else would be nudgeSpeed == trackSpeed, and we don't need to start the RA Axis } } else { // In non-tracking mode if (ra_dir != AXIS_ROTATE_STOP) { s = ra.startSlewingIndefinite(ra_dir); } } } // Update status if (status & MOUNT_TRACKING) status = MOUNT_NUDGING_TRACKING; else status = MOUNT_NUDGING; debug_if(EM_DEBUG, "status=%d, ra_dir=%d, dec_dir=%d\r\n", (int) status, (int) ra_dir, (int) dec_dir); } mutex_execution.unlock(); return s; } osStatus EquatorialMount::stopNudge() { return startNudge(NUDGE_NONE); } double EquatorialMount::getSlewSpeed() { return dec.getSlewSpeed(); // Fix: RA speed changes if using NUDGE_TRACKING mode } double EquatorialMount::getTrackSpeedSidereal() { return ra.getTrackSpeedSidereal(); } double EquatorialMount::getGuideSpeedSidereal() { return ra.getGuideSpeedSidereal(); } osStatus EquatorialMount::stopTracking() { if ((status & MOUNT_TRACKING) == 0) { return osErrorParameter; } mutex_execution.lock(); if (!(status & MOUNT_NUDGING)) { // Tracking stopSync(); } else { // Nudging status = MOUNT_NUDGING; } mutex_execution.unlock(); return osOK; } void EquatorialMount::updatePosition() { // Lock the mutex to avoid race condition on the current position values mutex_update.lock(); curr_pos = MountCoordinates(dec.getAngleDeg(), ra.getAngleDeg()); // Update Eq coordinates curr_pos_eq = this->convertToEqCoordinates(curr_pos); mutex_update.unlock(); } void EquatorialMount::emergencyStop() { ra.emergency_stop(); dec.emergency_stop(); status = MOUNT_STOPPED; curr_nudge_dir = NUDGE_NONE; } void EquatorialMount::stopAsync() { ra.stop(); dec.stop(); status = MOUNT_STOPPED; } void EquatorialMount::stopSync() { // Wait until they're fully stopped while (ra.getStatus() != AXIS_STOPPED || dec.getStatus() != AXIS_STOPPED) { ra.stop(); dec.stop(); Thread::yield(); } status = MOUNT_STOPPED; } osStatus EquatorialMount::recalibrate() { bool diverge = false; if (num_alignment_stars == 0) { return osOK; } EqCalibration newcalib = CelestialMath::align(num_alignment_stars, alignment_stars, loc.getLocation(), diverge); if (diverge) { return osErrorParameter; } calibration = newcalib; return osOK; } osStatus EquatorialMount::guide(guidedir_t dir, int ms) { // Check we are in tracking mode if (status != MOUNT_TRACKING) { return osErrorResource; } switch (dir) { case GUIDE_EAST: return ra.guide(AXIS_ROTATE_NEGATIVE, ms); case GUIDE_WEST: return ra.guide(AXIS_ROTATE_POSITIVE, ms); case GUIDE_NORTH: return dec.guide( (curr_pos.side == PIER_SIDE_WEST) ? AXIS_ROTATE_NEGATIVE : AXIS_ROTATE_POSITIVE, ms); case GUIDE_SOUTH: return dec.guide( (curr_pos.side == PIER_SIDE_WEST) ? AXIS_ROTATE_POSITIVE : AXIS_ROTATE_NEGATIVE, ms); default: return osErrorParameter; } } void EquatorialMount::setSlewSpeed(double rate) { if (status & MOUNT_NUDGING) { // Need to lock the mutex if mount is being nudged, to update the nudgeSpeed mutex_execution.lock(); nudgeSpeed = rate; // Update nudgeSpeed if ((status == MOUNT_NUDGING_TRACKING) && ((curr_nudge_dir & (NUDGE_EAST | NUDGE_WEST)) != 0)) { // Need to take special care in nudging_tracking mode that affects RA (i.e. indefinite slew w/ tracking) axisrotdir_t ra_dir = AXIS_ROTATE_STOP, curr_dir; // Get RA rotation direction (on top of tracking rate) if (curr_nudge_dir & NUDGE_EAST) { // Nudge east ra_dir = AXIS_ROTATE_NEGATIVE; } else if (curr_nudge_dir & NUDGE_WEST) { // Nudge west ra_dir = AXIS_ROTATE_POSITIVE; } curr_dir = ra.getCurrentDirection(); // Current direction double trackSpeed = ra.getTrackSpeedSidereal() * sidereal_speed; double absSpeed = nudgeSpeed; if (ra_dir == AXIS_ROTATE_POSITIVE) { // Same direction as tracking absSpeed = nudgeSpeed + trackSpeed; // keep ra_dir } else if (nudgeSpeed < trackSpeed) { // ra_dir == AXIS_ROTATE_NEGATIVE // Partially canceling the tracking speed absSpeed = trackSpeed - nudgeSpeed; ra_dir = AXIS_ROTATE_POSITIVE; // Rotate in the tracking direction } else if (nudgeSpeed > trackSpeed) {// ra_dir == AXIS_ROTATE_NEGATIVE // Direction inverted absSpeed = nudgeSpeed - trackSpeed; ra_dir = AXIS_ROTATE_NEGATIVE; // Invert the rotation } else { // nudgeSpeed == trackSpeed absSpeed = 0; // Set speed to zero, although effective the speed would be a tiny value } if (ra_dir != curr_dir) { ra.stop(); } ra.setSlewSpeed(absSpeed); if (ra_dir != curr_dir) { ra.startSlewingIndefinite(ra_dir); } } else { // Simply update rate ra.setSlewSpeed(rate); } mutex_execution.unlock(); } else { // Simply update rate ra.setSlewSpeed(rate); } // Simply update rate for DEC dec.setSlewSpeed(rate); } void EquatorialMount::setTrackSpeedSidereal(double rate) { // mutex_execution.lock(); ra.setTrackSpeedSidereal(rate); // mutex_execution.unlock(); } void EquatorialMount::setGuideSpeedSidereal(double rate) { // mutex_execution.lock(); ra.setGuideSpeedSidereal(rate); dec.setGuideSpeedSidereal(rate); // mutex_execution.unlock(); } mountstatus_t EquatorialMount::getStatus() { mountstatus_t s = status; if (ra.isGuiding() || dec.isGuiding()) s = (mountstatus_t) (s | MOUNT_GUIDING); return s; } void EquatorialMount::forceAlignment() { for (int i = 0; i < num_alignment_stars; i++) { alignment_stars[i].star_meas = convertToMountCoordinates( alignment_stars[i].star_ref); alignment_stars[i].timestamp = clock.getTime(); } }