Yuan Cao / pushtogo

Telescope Control Library

Dependents:   PushToGo-F429

EquatorialMount.cpp@19:fd854309cb4c, 2018-09-24 (annotated)

Committer:
caoyu@caoyuan9642-desktop.MIT.EDU
Date:
Mon Sep 24 19:36:48 2018 -0400
Revision:
19:fd854309cb4c
Parent:
17:7e47bc1630c0 
Fix bug in nudging with small speeds mentioned in the last commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
caoyuan9642 0:6cb2eaf8b133 1 #include <math.h>
caoyuan9642 0:6cb2eaf8b133 2 #include "EquatorialMount.h"
caoyuan9642 0:6cb2eaf8b133 3
caoyuan9642 2:2ee28add0821 4 #define EM_DEBUG 0
caoyuan9642 0:6cb2eaf8b133 5
caoyuan9642 0:6cb2eaf8b133 6 EquatorialMount::EquatorialMount(Axis& ra, Axis& dec, UTCClock& clk,
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 7 LocationProvider &loc) :
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 8 ra(ra), dec(dec), clock(clk), loc(loc), curr_pos(0, 0), curr_nudge_dir(
caoyuan9642 0:6cb2eaf8b133 9 NUDGE_NONE), nudgeSpeed(0), pier_side(PIER_SIDE_EAST), num_alignment_stars(
caoyu@caoyuan9642-desktop.MIT.EDU 10:e356188d208e 10 0), pec(ra) {
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 11 south = loc.getLatitude() < 0.0;
caoyuan9642 0:6cb2eaf8b133 12 // Get initial transformation
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 13 calibration.pa = AzimuthalCoordinates(loc.getLatitude(), 0);
caoyuan9642 0:6cb2eaf8b133 14 // Set RA and DEC positions to zero
caoyuan9642 0:6cb2eaf8b133 15 ra.setAngleDeg(0);
caoyuan9642 0:6cb2eaf8b133 16 dec.setAngleDeg(0);
caoyuan9642 0:6cb2eaf8b133 17 dec.setTrackSpeedSidereal(0); // Make sure dec doesn't move during tracking
caoyuan9642 0:6cb2eaf8b133 18 updatePosition();
caoyuan9642 0:6cb2eaf8b133 19
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 20 // Set PEC to RA axis
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 21 this->ra.setPEC(&pec);
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 22 this->ra.setPECEnabled(true);
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 23
caoyuan9642 0:6cb2eaf8b133 24 // Set in tracking mode
caoyuan9642 0:6cb2eaf8b133 25 startTracking();
caoyuan9642 0:6cb2eaf8b133 26 }
caoyuan9642 0:6cb2eaf8b133 27
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 28 osStatus EquatorialMount::goTo(double ra_dest, double dec_dest) {
caoyuan9642 0:6cb2eaf8b133 29 return goTo(EquatorialCoordinates(dec_dest, ra_dest));
caoyuan9642 0:6cb2eaf8b133 30 }
caoyuan9642 0:6cb2eaf8b133 31
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 32 osStatus EquatorialMount::goTo(EquatorialCoordinates dest) {
caoyuan9642 0:6cb2eaf8b133 33
caoyuan9642 0:6cb2eaf8b133 34 debug_if(EM_DEBUG, "dest ra=%.2f, dec=%.2f\n", dest.ra, dest.dec);
caoyuan9642 0:6cb2eaf8b133 35
caoyuan9642 0:6cb2eaf8b133 36 updatePosition(); // Get the latest position information
caoyuan9642 0:6cb2eaf8b133 37
caoyuan9642 0:6cb2eaf8b133 38 if (EM_DEBUG)
caoyuan9642 0:6cb2eaf8b133 39 printPosition();
caoyuan9642 0:6cb2eaf8b133 40
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 41 for (int i = 0; i < 2; i++) {
caoyuan9642 0:6cb2eaf8b133 42 // Convert to Mount coordinates. Automatically determine the pier side, then apply offset
caoyuan9642 0:6cb2eaf8b133 43 MountCoordinates dest_mount = convertToMountCoordinates(dest);
caoyuan9642 0:6cb2eaf8b133 44
caoyuan9642 0:6cb2eaf8b133 45 osStatus s = goToMount(dest_mount, (i > 0)); // Use correction only for the second time
caoyuan9642 0:6cb2eaf8b133 46 if (s != osOK)
caoyuan9642 0:6cb2eaf8b133 47 return s;
caoyuan9642 0:6cb2eaf8b133 48 }
caoyuan9642 0:6cb2eaf8b133 49
caoyuan9642 0:6cb2eaf8b133 50 if (EM_DEBUG)
caoyuan9642 0:6cb2eaf8b133 51 printPosition();
caoyuan9642 0:6cb2eaf8b133 52
caoyuan9642 0:6cb2eaf8b133 53 return osOK;
caoyuan9642 0:6cb2eaf8b133 54 }
caoyuan9642 0:6cb2eaf8b133 55
caoyuan9642 0:6cb2eaf8b133 56 osStatus EquatorialMount::goToMount(MountCoordinates dest_mount,
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 57 bool withCorrection) {
caoyuan9642 0:6cb2eaf8b133 58 mutex_execution.lock();
caoyuan9642 0:6cb2eaf8b133 59 bool was_tracking = false;
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 60 if ((status & MOUNT_TRACKING) && !(status & MOUNT_NUDGING)) {
caoyuan9642 2:2ee28add0821 61 // Tracking mode
caoyuan9642 0:6cb2eaf8b133 62 was_tracking = true;
caoyuan9642 0:6cb2eaf8b133 63 stopSync();
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 64 } else if (status != MOUNT_STOPPED) {
caoyuan9642 0:6cb2eaf8b133 65 debug("EM: goTo requested while mount is not stopped.\n");
caoyuan9642 0:6cb2eaf8b133 66 mutex_execution.unlock();
caoyuan9642 0:6cb2eaf8b133 67 return osErrorParameter;
caoyuan9642 0:6cb2eaf8b133 68 }
caoyuan9642 0:6cb2eaf8b133 69 debug_if(EM_DEBUG, "EM: goTo\n");
caoyuan9642 0:6cb2eaf8b133 70 debug_if(EM_DEBUG, "dstmnt ra=%.2f, dec=%.2f\n", dest_mount.ra_delta,
caoyuan9642 0:6cb2eaf8b133 71 dest_mount.dec_delta);
caoyuan9642 0:6cb2eaf8b133 72
caoyuan9642 0:6cb2eaf8b133 73 axisrotdir_t dec_dir, ra_dir;
caoyuan9642 0:6cb2eaf8b133 74 ra_dir =
caoyuan9642 0:6cb2eaf8b133 75 (remainder(dest_mount.ra_delta, 360.0)
caoyuan9642 0:6cb2eaf8b133 76 > remainder(curr_pos.ra_delta, 360.0)) ?
caoyuan9642 0:6cb2eaf8b133 77 AXIS_ROTATE_POSITIVE :
caoyuan9642 0:6cb2eaf8b133 78 (remainder(dest_mount.ra_delta, 360.0)
caoyuan9642 0:6cb2eaf8b133 79 < remainder(curr_pos.ra_delta, 360.0)) ?
caoyuan9642 0:6cb2eaf8b133 80 AXIS_ROTATE_NEGATIVE : AXIS_ROTATE_STOP;
caoyuan9642 0:6cb2eaf8b133 81 dec_dir =
caoyuan9642 0:6cb2eaf8b133 82 (remainder(dest_mount.dec_delta, 360.0)
caoyuan9642 0:6cb2eaf8b133 83 > remainder(curr_pos.dec_delta, 360.0)) ?
caoyuan9642 0:6cb2eaf8b133 84 AXIS_ROTATE_POSITIVE :
caoyuan9642 0:6cb2eaf8b133 85 (remainder(dest_mount.dec_delta, 360.0)
caoyuan9642 0:6cb2eaf8b133 86 < remainder(curr_pos.dec_delta, 360.0)) ?
caoyuan9642 0:6cb2eaf8b133 87 AXIS_ROTATE_NEGATIVE : AXIS_ROTATE_STOP;
caoyuan9642 0:6cb2eaf8b133 88
caoyuan9642 0:6cb2eaf8b133 89 debug_if(EM_DEBUG, "EM: start slewing\n");
caoyuan9642 0:6cb2eaf8b133 90 status = MOUNT_SLEWING;
caoyuan9642 0:6cb2eaf8b133 91 ra.startSlewTo(ra_dir, dest_mount.ra_delta, withCorrection);
caoyuan9642 0:6cb2eaf8b133 92 dec.startSlewTo(dec_dir, dest_mount.dec_delta, withCorrection);
caoyuan9642 0:6cb2eaf8b133 93
caoyuan9642 0:6cb2eaf8b133 94 int ret = (int) ra.waitForSlew();
caoyuan9642 0:6cb2eaf8b133 95 ret |= (int) dec.waitForSlew();
caoyuan9642 0:6cb2eaf8b133 96
caoyuan9642 0:6cb2eaf8b133 97 debug_if(EM_DEBUG, "EM: slewing finished\n");
caoyuan9642 0:6cb2eaf8b133 98
caoyuan9642 0:6cb2eaf8b133 99 status = MOUNT_STOPPED;
caoyuan9642 0:6cb2eaf8b133 100
caoyuan9642 0:6cb2eaf8b133 101 mutex_execution.unlock();
caoyuan9642 0:6cb2eaf8b133 102
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 103 if (was_tracking && !(ret & (FINISH_ERROR | FINISH_EMERG_STOPPED))) {
caoyuan9642 0:6cb2eaf8b133 104 startTracking();
caoyuan9642 0:6cb2eaf8b133 105 }
caoyuan9642 0:6cb2eaf8b133 106
caoyuan9642 0:6cb2eaf8b133 107 updatePosition(); // Update current position
caoyuan9642 0:6cb2eaf8b133 108
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 109 if (ret) {
caoyuan9642 0:6cb2eaf8b133 110 // Stopped during slew
caoyuan9642 0:6cb2eaf8b133 111 return ret;
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 112 } else
caoyuan9642 0:6cb2eaf8b133 113 return osOK;
caoyuan9642 0:6cb2eaf8b133 114 }
caoyuan9642 0:6cb2eaf8b133 115
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 116 osStatus EquatorialMount::startTracking() {
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 117 if (status != MOUNT_STOPPED) {
caoyuan9642 0:6cb2eaf8b133 118 debug("EM: tracking requested while mount is not stopped.\n");
caoyuan9642 0:6cb2eaf8b133 119 return osErrorParameter;
caoyuan9642 0:6cb2eaf8b133 120 }
caoyuan9642 0:6cb2eaf8b133 121
caoyuan9642 0:6cb2eaf8b133 122 mutex_execution.lock();
caoyuan9642 0:6cb2eaf8b133 123 axisrotdir_t ra_dir = AXIS_ROTATE_POSITIVE; // Tracking is always going to positive hour angle direction, which is defined as positive.
caoyuan9642 0:6cb2eaf8b133 124 status = MOUNT_TRACKING;
caoyuan9642 0:6cb2eaf8b133 125 osStatus sr, sd;
caoyuan9642 0:6cb2eaf8b133 126 sr = ra.startTracking(ra_dir);
caoyuan9642 0:6cb2eaf8b133 127 sd = dec.startTracking(AXIS_ROTATE_STOP);
caoyuan9642 0:6cb2eaf8b133 128 mutex_execution.unlock();
caoyuan9642 0:6cb2eaf8b133 129 if (sr != osOK || sd != osOK)
caoyuan9642 0:6cb2eaf8b133 130 return osErrorResource;
caoyuan9642 0:6cb2eaf8b133 131 else
caoyuan9642 0:6cb2eaf8b133 132 return osOK;
caoyuan9642 0:6cb2eaf8b133 133 }
caoyuan9642 0:6cb2eaf8b133 134
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 135 osStatus EquatorialMount::startNudge(nudgedir_t newdir) {
caoyuan9642 2:2ee28add0821 136 if (status & MOUNT_SLEWING) // Cannot be nudged in slewing mode
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 137 {
caoyuan9642 0:6cb2eaf8b133 138 return osErrorParameter;
caoyuan9642 0:6cb2eaf8b133 139 }
caoyuan9642 0:6cb2eaf8b133 140 osStatus s = osOK;
caoyuan9642 0:6cb2eaf8b133 141 mutex_execution.lock();
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 142 if (newdir == NUDGE_NONE) { // Stop nudging if being nudged
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 143 if (status & MOUNT_NUDGING) {
caoyuan9642 0:6cb2eaf8b133 144 mountstatus_t oldstatus = status;
caoyuan9642 0:6cb2eaf8b133 145 stopAsync(); // Stop the mount
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 146 if (oldstatus == MOUNT_NUDGING) {
caoyuan9642 0:6cb2eaf8b133 147 // Stop the mount
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 148 } else if (oldstatus == MOUNT_NUDGING_TRACKING) {
caoyuan9642 0:6cb2eaf8b133 149 // Get to tracking state
caoyuan9642 0:6cb2eaf8b133 150 ra.setSlewSpeed(nudgeSpeed); // restore the slew rate of RA
caoyuan9642 0:6cb2eaf8b133 151 startTracking();
caoyuan9642 0:6cb2eaf8b133 152 }
caoyuan9642 0:6cb2eaf8b133 153 }
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 154 } else { // newdir is not NUDGE_NONE
caoyuan9642 0:6cb2eaf8b133 155 updatePosition(); // Update current position, because we need to know the current pier side
caoyuan9642 0:6cb2eaf8b133 156 bool ra_changed = false, dec_changed = false;
caoyuan9642 0:6cb2eaf8b133 157 axisrotdir_t ra_dir, dec_dir;
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 158 if ((status & MOUNT_NUDGING) == 0) {
caoyuan9642 0:6cb2eaf8b133 159 // Initial nudge
caoyuan9642 0:6cb2eaf8b133 160 curr_nudge_dir = NUDGE_NONE; //Make sure the current nudging direction is cleared
caoyuan9642 0:6cb2eaf8b133 161 nudgeSpeed = getSlewSpeed(); // Get nudge speed and use it for ALL following nudge operations, until the nudge finishes
caoyuan9642 0:6cb2eaf8b133 162 }
caoyuan9642 0:6cb2eaf8b133 163 // see what has changed in RA
caoyuan9642 0:6cb2eaf8b133 164 if ((curr_nudge_dir & (NUDGE_WEST | NUDGE_EAST))
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 165 != (newdir & (NUDGE_WEST | NUDGE_EAST))) {
caoyuan9642 0:6cb2eaf8b133 166 // If something on east/west has changed, we need to stop the RA axis (or maybe enable it later to switch direction)
caoyuan9642 0:6cb2eaf8b133 167 ra_changed = true;
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 168 if (newdir & NUDGE_EAST) {
caoyuan9642 0:6cb2eaf8b133 169 // Nudge east
caoyuan9642 0:6cb2eaf8b133 170 ra_dir = AXIS_ROTATE_NEGATIVE;
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 171 } else if (newdir & NUDGE_WEST) {
caoyuan9642 0:6cb2eaf8b133 172 // Nudge west
caoyuan9642 0:6cb2eaf8b133 173 ra_dir = AXIS_ROTATE_POSITIVE;
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 174 } else {
caoyuan9642 0:6cb2eaf8b133 175 ra_dir = AXIS_ROTATE_STOP;
caoyuan9642 0:6cb2eaf8b133 176 }
caoyuan9642 0:6cb2eaf8b133 177 }
caoyuan9642 0:6cb2eaf8b133 178 // see what has changed in DEC
caoyuan9642 0:6cb2eaf8b133 179 if ((curr_nudge_dir & (NUDGE_SOUTH | NUDGE_NORTH))
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 180 != (newdir & (NUDGE_SOUTH | NUDGE_NORTH))) {
caoyuan9642 0:6cb2eaf8b133 181 // If something on east/west has changed, we need to stop the RA axis (or maybe enable it later to switch direction)
caoyuan9642 0:6cb2eaf8b133 182 dec_changed = true;
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 183 if (newdir & NUDGE_NORTH) {
caoyuan9642 0:6cb2eaf8b133 184 // Nudge north
caoyuan9642 0:6cb2eaf8b133 185 dec_dir =
caoyuan9642 0:6cb2eaf8b133 186 (curr_pos.side == PIER_SIDE_WEST) ?
caoyuan9642 0:6cb2eaf8b133 187 AXIS_ROTATE_NEGATIVE : AXIS_ROTATE_POSITIVE;
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 188 } else if (newdir & NUDGE_SOUTH) {
caoyuan9642 0:6cb2eaf8b133 189 // Nudge south
caoyuan9642 0:6cb2eaf8b133 190 dec_dir =
caoyuan9642 0:6cb2eaf8b133 191 (curr_pos.side == PIER_SIDE_WEST) ?
caoyuan9642 0:6cb2eaf8b133 192 AXIS_ROTATE_POSITIVE : AXIS_ROTATE_NEGATIVE;
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 193 } else {
caoyuan9642 0:6cb2eaf8b133 194 dec_dir = AXIS_ROTATE_STOP;
caoyuan9642 0:6cb2eaf8b133 195 }
caoyuan9642 0:6cb2eaf8b133 196 }
caoyuan9642 0:6cb2eaf8b133 197 curr_nudge_dir = newdir;
caoyuan9642 0:6cb2eaf8b133 198
caoyuan9642 0:6cb2eaf8b133 199 // Request stop as necessary
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 200 if (ra_changed) {
caoyuan9642 0:6cb2eaf8b133 201 ra.flushCommandQueue();
caoyuan9642 0:6cb2eaf8b133 202 if (ra.getSlewState() == AXIS_SLEW_DECELERATING
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 203 && ra_dir == ra.getCurrentDirection()) {
caoyuan9642 0:6cb2eaf8b133 204 ra.stopKeepSpeed();
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 205 } else {
caoyuan9642 0:6cb2eaf8b133 206 ra.stop();
caoyuan9642 0:6cb2eaf8b133 207 }
caoyuan9642 0:6cb2eaf8b133 208 }
caoyuan9642 0:6cb2eaf8b133 209
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 210 if (dec_changed) {
caoyuan9642 0:6cb2eaf8b133 211 dec.flushCommandQueue();
caoyuan9642 0:6cb2eaf8b133 212 if (dec.getSlewState() == AXIS_SLEW_DECELERATING
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 213 && dec_dir == dec.getCurrentDirection()) {
caoyuan9642 0:6cb2eaf8b133 214 dec.stopKeepSpeed();
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 215 } else {
caoyuan9642 0:6cb2eaf8b133 216 dec.stop();
caoyuan9642 0:6cb2eaf8b133 217 }
caoyuan9642 0:6cb2eaf8b133 218 }
caoyuan9642 0:6cb2eaf8b133 219
caoyuan9642 0:6cb2eaf8b133 220 // Wait for stop together
caoyuan9642 0:6cb2eaf8b133 221 // while ((ra_changed && ra.getStatus() != AXIS_STOPPED
caoyuan9642 0:6cb2eaf8b133 222 // && ra.getStatus() != AXIS_INERTIAL)
caoyuan9642 0:6cb2eaf8b133 223 // || (dec_changed && dec.getStatus() != AXIS_STOPPED
caoyuan9642 0:6cb2eaf8b133 224 // && dec.getStatus() != AXIS_INERTIAL))
caoyuan9642 0:6cb2eaf8b133 225 // {
caoyuan9642 0:6cb2eaf8b133 226 // Thread::yield();
caoyuan9642 0:6cb2eaf8b133 227 // }
caoyuan9642 0:6cb2eaf8b133 228
caoyuan9642 0:6cb2eaf8b133 229 // DEC axis is ok to start regardless of tracking state
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 230 if (dec_changed && dec_dir != AXIS_ROTATE_STOP) {
caoyuan9642 0:6cb2eaf8b133 231 s = dec.startSlewingIndefinite(dec_dir);
caoyuan9642 0:6cb2eaf8b133 232 }
caoyuan9642 0:6cb2eaf8b133 233
caoyuan9642 0:6cb2eaf8b133 234 // Now RA
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 235 if (ra_changed && s == osOK) {
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 236 if (status & MOUNT_TRACKING) { // In tracking mode now, need to calculate differential rate
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 237 if (ra_dir == AXIS_ROTATE_STOP) { // resume tracking
caoyuan9642 0:6cb2eaf8b133 238 s = ra.startTracking(AXIS_ROTATE_POSITIVE);
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 239 } else {
caoyuan9642 0:6cb2eaf8b133 240 // This is the complicated part
caoyuan9642 0:6cb2eaf8b133 241 double trackSpeed = ra.getTrackSpeedSidereal()
caoyuan9642 0:6cb2eaf8b133 242 * sidereal_speed;
caoyuan9642 0:6cb2eaf8b133 243 debug_if(EM_DEBUG > 1, "EM: ra, ns=%f, ts=%f\n", nudgeSpeed,
caoyuan9642 0:6cb2eaf8b133 244 trackSpeed);
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 245 if (ra_dir == AXIS_ROTATE_POSITIVE) {
caoyuan9642 0:6cb2eaf8b133 246 // Same direction as tracking
caoyuan9642 0:6cb2eaf8b133 247 ra.setSlewSpeed(nudgeSpeed + trackSpeed);
caoyuan9642 0:6cb2eaf8b133 248 s = ra.startSlewingIndefinite(AXIS_ROTATE_POSITIVE);
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 249 } else if (nudgeSpeed < trackSpeed) { // ra_dir == AXIS_ROTATE_NEGATIVE
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 250 // Partially canceling the tracking speed
caoyuan9642 0:6cb2eaf8b133 251 ra.setSlewSpeed(trackSpeed - nudgeSpeed);
caoyuan9642 0:6cb2eaf8b133 252 s = ra.startSlewingIndefinite(AXIS_ROTATE_POSITIVE);
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 253 } else if (nudgeSpeed > trackSpeed) {// ra_dir == AXIS_ROTATE_NEGATIVE
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 254 // Direction inverted
caoyuan9642 0:6cb2eaf8b133 255 ra.setSlewSpeed(nudgeSpeed - trackSpeed);
caoyuan9642 0:6cb2eaf8b133 256 ra.startSlewingIndefinite(AXIS_ROTATE_NEGATIVE);
caoyuan9642 0:6cb2eaf8b133 257 }
caoyuan9642 0:6cb2eaf8b133 258 // else would be nudgeSpeed == trackSpeed, and we don't need to start the RA Axis
caoyuan9642 0:6cb2eaf8b133 259 }
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 260 } else {
caoyuan9642 0:6cb2eaf8b133 261 // In non-tracking mode
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 262 if (ra_dir != AXIS_ROTATE_STOP) {
caoyuan9642 0:6cb2eaf8b133 263 s = ra.startSlewingIndefinite(ra_dir);
caoyuan9642 0:6cb2eaf8b133 264 }
caoyuan9642 0:6cb2eaf8b133 265 }
caoyuan9642 0:6cb2eaf8b133 266 }
caoyuan9642 0:6cb2eaf8b133 267
caoyuan9642 0:6cb2eaf8b133 268 // Update status
caoyuan9642 0:6cb2eaf8b133 269 if (status & MOUNT_TRACKING)
caoyuan9642 0:6cb2eaf8b133 270 status = MOUNT_NUDGING_TRACKING;
caoyuan9642 0:6cb2eaf8b133 271 else
caoyuan9642 0:6cb2eaf8b133 272 status = MOUNT_NUDGING;
caoyuan9642 0:6cb2eaf8b133 273
caoyuan9642 0:6cb2eaf8b133 274 debug_if(EM_DEBUG, "status=%d, ra_dir=%d, dec_dir=%d\r\n", (int) status,
caoyuan9642 0:6cb2eaf8b133 275 (int) ra_dir, (int) dec_dir);
caoyuan9642 0:6cb2eaf8b133 276 }
caoyuan9642 0:6cb2eaf8b133 277 mutex_execution.unlock();
caoyuan9642 0:6cb2eaf8b133 278
caoyuan9642 0:6cb2eaf8b133 279 return s;
caoyuan9642 0:6cb2eaf8b133 280 }
caoyuan9642 0:6cb2eaf8b133 281
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 282 osStatus EquatorialMount::stopNudge() {
caoyuan9642 0:6cb2eaf8b133 283 return startNudge(NUDGE_NONE);
caoyuan9642 0:6cb2eaf8b133 284 }
caoyuan9642 0:6cb2eaf8b133 285
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 286 double EquatorialMount::getSlewSpeed() {
caoyuan9642 0:6cb2eaf8b133 287 return dec.getSlewSpeed(); // Fix: RA speed changes if using NUDGE_TRACKING mode
caoyuan9642 0:6cb2eaf8b133 288 }
caoyuan9642 0:6cb2eaf8b133 289
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 290 double EquatorialMount::getTrackSpeedSidereal() {
caoyuan9642 0:6cb2eaf8b133 291 return ra.getTrackSpeedSidereal();
caoyuan9642 0:6cb2eaf8b133 292 }
caoyuan9642 0:6cb2eaf8b133 293
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 294 double EquatorialMount::getGuideSpeedSidereal() {
caoyuan9642 0:6cb2eaf8b133 295 return ra.getGuideSpeedSidereal();
caoyuan9642 0:6cb2eaf8b133 296 }
caoyuan9642 0:6cb2eaf8b133 297
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 298 osStatus EquatorialMount::stopTracking() {
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 299 if ((status & MOUNT_TRACKING) == 0) {
caoyuan9642 0:6cb2eaf8b133 300 return osErrorParameter;
caoyuan9642 0:6cb2eaf8b133 301 }
caoyuan9642 0:6cb2eaf8b133 302 mutex_execution.lock();
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 303 if (!(status & MOUNT_NUDGING)) {
caoyuan9642 2:2ee28add0821 304 // Tracking
caoyuan9642 0:6cb2eaf8b133 305 stopSync();
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 306 } else {
caoyuan9642 2:2ee28add0821 307 // Nudging
caoyuan9642 0:6cb2eaf8b133 308 status = MOUNT_NUDGING;
caoyuan9642 0:6cb2eaf8b133 309 }
caoyuan9642 0:6cb2eaf8b133 310 mutex_execution.unlock();
caoyuan9642 0:6cb2eaf8b133 311 return osOK;
caoyuan9642 0:6cb2eaf8b133 312 }
caoyuan9642 0:6cb2eaf8b133 313
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 314 void EquatorialMount::updatePosition() {
caoyuan9642 0:6cb2eaf8b133 315 // Lock the mutex to avoid race condition on the current position values
caoyuan9642 0:6cb2eaf8b133 316 mutex_update.lock();
caoyuan9642 0:6cb2eaf8b133 317 curr_pos = MountCoordinates(dec.getAngleDeg(), ra.getAngleDeg());
caoyuan9642 0:6cb2eaf8b133 318 // Update Eq coordinates
caoyuan9642 0:6cb2eaf8b133 319 curr_pos_eq = this->convertToEqCoordinates(curr_pos);
caoyuan9642 0:6cb2eaf8b133 320 mutex_update.unlock();
caoyuan9642 0:6cb2eaf8b133 321 }
caoyuan9642 0:6cb2eaf8b133 322
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 323 void EquatorialMount::emergencyStop() {
caoyuan9642 0:6cb2eaf8b133 324 ra.emergency_stop();
caoyuan9642 0:6cb2eaf8b133 325 dec.emergency_stop();
caoyuan9642 0:6cb2eaf8b133 326 status = MOUNT_STOPPED;
caoyuan9642 0:6cb2eaf8b133 327 curr_nudge_dir = NUDGE_NONE;
caoyuan9642 0:6cb2eaf8b133 328 }
caoyuan9642 0:6cb2eaf8b133 329
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 330 void EquatorialMount::stopAsync() {
caoyuan9642 0:6cb2eaf8b133 331 ra.stop();
caoyuan9642 0:6cb2eaf8b133 332 dec.stop();
caoyuan9642 0:6cb2eaf8b133 333 status = MOUNT_STOPPED;
caoyuan9642 0:6cb2eaf8b133 334 }
caoyuan9642 0:6cb2eaf8b133 335
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 336 void EquatorialMount::stopSync() {
caoyuan9642 0:6cb2eaf8b133 337 // Wait until they're fully stopped
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 338 while (ra.getStatus() != AXIS_STOPPED || dec.getStatus() != AXIS_STOPPED) {
caoyuan9642 0:6cb2eaf8b133 339 ra.stop();
caoyuan9642 0:6cb2eaf8b133 340 dec.stop();
caoyuan9642 0:6cb2eaf8b133 341 Thread::yield();
caoyuan9642 0:6cb2eaf8b133 342 }
caoyuan9642 0:6cb2eaf8b133 343 status = MOUNT_STOPPED;
caoyuan9642 0:6cb2eaf8b133 344 }
caoyuan9642 0:6cb2eaf8b133 345
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 346 osStatus EquatorialMount::recalibrate() {
caoyuan9642 0:6cb2eaf8b133 347 bool diverge = false;
caoyuan9642 0:6cb2eaf8b133 348
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 349 if (num_alignment_stars == 0) {
caoyuan9642 0:6cb2eaf8b133 350 return osOK;
caoyuan9642 0:6cb2eaf8b133 351 }
caoyuan9642 0:6cb2eaf8b133 352 EqCalibration newcalib = CelestialMath::align(num_alignment_stars,
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 353 alignment_stars, loc.getLocation(), diverge);
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 354 if (diverge) {
caoyuan9642 0:6cb2eaf8b133 355 return osErrorParameter;
caoyuan9642 0:6cb2eaf8b133 356 }
caoyuan9642 0:6cb2eaf8b133 357
caoyuan9642 0:6cb2eaf8b133 358 calibration = newcalib;
caoyuan9642 0:6cb2eaf8b133 359
caoyuan9642 0:6cb2eaf8b133 360 return osOK;
caoyuan9642 0:6cb2eaf8b133 361 }
caoyuan9642 0:6cb2eaf8b133 362
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 363 osStatus EquatorialMount::guide(guidedir_t dir, int ms) {
caoyuan9642 0:6cb2eaf8b133 364 // Check we are in tracking mode
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 365 if (status != MOUNT_TRACKING) {
caoyuan9642 0:6cb2eaf8b133 366 return osErrorResource;
caoyuan9642 0:6cb2eaf8b133 367 }
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 368 switch (dir) {
caoyuan9642 0:6cb2eaf8b133 369 case GUIDE_EAST:
caoyuan9642 0:6cb2eaf8b133 370 return ra.guide(AXIS_ROTATE_NEGATIVE, ms);
caoyuan9642 0:6cb2eaf8b133 371 case GUIDE_WEST:
caoyuan9642 0:6cb2eaf8b133 372 return ra.guide(AXIS_ROTATE_POSITIVE, ms);
caoyuan9642 0:6cb2eaf8b133 373 case GUIDE_NORTH:
caoyuan9642 0:6cb2eaf8b133 374 return dec.guide(
caoyuan9642 0:6cb2eaf8b133 375 (curr_pos.side == PIER_SIDE_WEST) ?
caoyuan9642 0:6cb2eaf8b133 376 AXIS_ROTATE_NEGATIVE : AXIS_ROTATE_POSITIVE, ms);
caoyuan9642 0:6cb2eaf8b133 377 case GUIDE_SOUTH:
caoyuan9642 0:6cb2eaf8b133 378 return dec.guide(
caoyuan9642 0:6cb2eaf8b133 379 (curr_pos.side == PIER_SIDE_WEST) ?
caoyuan9642 0:6cb2eaf8b133 380 AXIS_ROTATE_POSITIVE : AXIS_ROTATE_NEGATIVE, ms);
caoyuan9642 0:6cb2eaf8b133 381 default:
caoyuan9642 0:6cb2eaf8b133 382 return osErrorParameter;
caoyuan9642 0:6cb2eaf8b133 383 }
caoyuan9642 0:6cb2eaf8b133 384 }
caoyuan9642 0:6cb2eaf8b133 385
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 386 void EquatorialMount::setSlewSpeed(double rate) {
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 387 if (status & MOUNT_NUDGING) {
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 388 // Need to lock the mutex if mount is being nudged, to update the nudgeSpeed
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 389 mutex_execution.lock();
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 390 nudgeSpeed = rate; // Update nudgeSpeed
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 391 if ((status == MOUNT_NUDGING_TRACKING)
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 392 && ((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)
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 393 axisrotdir_t ra_dir = AXIS_ROTATE_STOP, curr_dir;
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 394 // Get RA rotation direction (on top of tracking rate)
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 395 if (curr_nudge_dir & NUDGE_EAST) {
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 396 // Nudge east
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 397 ra_dir = AXIS_ROTATE_NEGATIVE;
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 398 } else if (curr_nudge_dir & NUDGE_WEST) {
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 399 // Nudge west
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 400 ra_dir = AXIS_ROTATE_POSITIVE;
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 401 }
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 402
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 403 curr_dir = ra.getCurrentDirection(); // Current direction
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 404
caoyu@caoyuan9642-desktop.MIT.EDU 19:fd854309cb4c 405
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 406 double trackSpeed = ra.getTrackSpeedSidereal() * sidereal_speed;
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 407 double absSpeed = nudgeSpeed;
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 408
caoyu@caoyuan9642-desktop.MIT.EDU 19:fd854309cb4c 409
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 410 if (ra_dir == AXIS_ROTATE_POSITIVE) {
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 411 // Same direction as tracking
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 412 absSpeed = nudgeSpeed + trackSpeed;
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 413 // keep ra_dir
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 414 } else if (nudgeSpeed < trackSpeed) { // ra_dir == AXIS_ROTATE_NEGATIVE
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 415 // Partially canceling the tracking speed
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 416 absSpeed = trackSpeed - nudgeSpeed;
caoyu@caoyuan9642-desktop.MIT.EDU 19:fd854309cb4c 417 ra_dir = AXIS_ROTATE_POSITIVE; // Rotate in the tracking direction
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 418 } else if (nudgeSpeed > trackSpeed) {// ra_dir == AXIS_ROTATE_NEGATIVE
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 419 // Direction inverted
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 420 absSpeed = nudgeSpeed - trackSpeed;
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 421 ra_dir = AXIS_ROTATE_NEGATIVE; // Invert the rotation
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 422 } else { // nudgeSpeed == trackSpeed
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 423 absSpeed = 0; // Set speed to zero, although effective the speed would be a tiny value
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 424 }
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 425
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 426 if (ra_dir != curr_dir) {
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 427 ra.stop();
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 428 }
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 429 ra.setSlewSpeed(absSpeed);
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 430 if (ra_dir != curr_dir) {
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 431 ra.startSlewingIndefinite(ra_dir);
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 432 }
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 433
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 434 } else {
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 435 // Simply update rate
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 436 ra.setSlewSpeed(rate);
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 437 }
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 438 mutex_execution.unlock();
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 439 } else {
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 440 // Simply update rate
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 441 ra.setSlewSpeed(rate);
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 442 }
caoyu@caoyuan9642-desktop.MIT.EDU 17:7e47bc1630c0 443 // Simply update rate for DEC
caoyuan9642 0:6cb2eaf8b133 444 dec.setSlewSpeed(rate);
caoyuan9642 0:6cb2eaf8b133 445 }
caoyuan9642 0:6cb2eaf8b133 446
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 447 void EquatorialMount::setTrackSpeedSidereal(double rate) {
caoyuan9642 0:6cb2eaf8b133 448 // mutex_execution.lock();
caoyuan9642 0:6cb2eaf8b133 449 ra.setTrackSpeedSidereal(rate);
caoyuan9642 0:6cb2eaf8b133 450 // mutex_execution.unlock();
caoyuan9642 0:6cb2eaf8b133 451 }
caoyuan9642 0:6cb2eaf8b133 452
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 453 void EquatorialMount::setGuideSpeedSidereal(double rate) {
caoyuan9642 0:6cb2eaf8b133 454 // mutex_execution.lock();
caoyuan9642 0:6cb2eaf8b133 455 ra.setGuideSpeedSidereal(rate);
caoyuan9642 0:6cb2eaf8b133 456 dec.setGuideSpeedSidereal(rate);
caoyuan9642 0:6cb2eaf8b133 457 // mutex_execution.unlock();
caoyuan9642 0:6cb2eaf8b133 458 }
caoyuan9642 0:6cb2eaf8b133 459
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 460 mountstatus_t EquatorialMount::getStatus() {
caoyuan9642 2:2ee28add0821 461 mountstatus_t s = status;
caoyuan9642 2:2ee28add0821 462 if (ra.isGuiding() || dec.isGuiding())
caoyuan9642 2:2ee28add0821 463 s = (mountstatus_t) (s | MOUNT_GUIDING);
caoyuan9642 2:2ee28add0821 464 return s;
caoyuan9642 2:2ee28add0821 465 }
caoyuan9642 4:b8ad57bbf9e1 466
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 467 void EquatorialMount::forceAlignment() {
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 468 for (int i = 0; i < num_alignment_stars; i++) {
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 469 alignment_stars[i].star_meas = convertToMountCoordinates(
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 470 alignment_stars[i].star_ref);
caoyuan9642 4:b8ad57bbf9e1 471 alignment_stars[i].timestamp = clock.getTime();
caoyuan9642 4:b8ad57bbf9e1 472 }
caoyu@caoyuan9642-desktop.MIT.EDU 9:d0413a9b1386 473 }