Telescope Control Library
EquatorialMount.cpp@17:7e47bc1630c0, 2018-09-14 (annotated)
- Committer:
- caoyu@caoyuan9642-desktop.MIT.EDU
- Date:
- Fri Sep 14 21:09:02 2018 -0400
- Revision:
- 17:7e47bc1630c0
- Parent:
- 10:e356188d208e
- Child:
- 19:fd854309cb4c
Fixed two bugs related to nudging
Who changed what in which revision?
User | Revision | Line number | New 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 | 17:7e47bc1630c0 | 405 | double trackSpeed = ra.getTrackSpeedSidereal() * sidereal_speed; |
caoyu@caoyuan9642-desktop.MIT.EDU | 17:7e47bc1630c0 | 406 | double absSpeed = nudgeSpeed; |
caoyu@caoyuan9642-desktop.MIT.EDU | 17:7e47bc1630c0 | 407 | |
caoyu@caoyuan9642-desktop.MIT.EDU | 17:7e47bc1630c0 | 408 | if (ra_dir == AXIS_ROTATE_POSITIVE) { |
caoyu@caoyuan9642-desktop.MIT.EDU | 17:7e47bc1630c0 | 409 | // Same direction as tracking |
caoyu@caoyuan9642-desktop.MIT.EDU | 17:7e47bc1630c0 | 410 | absSpeed = nudgeSpeed + trackSpeed; |
caoyu@caoyuan9642-desktop.MIT.EDU | 17:7e47bc1630c0 | 411 | // keep ra_dir |
caoyu@caoyuan9642-desktop.MIT.EDU | 17:7e47bc1630c0 | 412 | } else if (nudgeSpeed < trackSpeed) { // ra_dir == AXIS_ROTATE_NEGATIVE |
caoyu@caoyuan9642-desktop.MIT.EDU | 17:7e47bc1630c0 | 413 | // Partially canceling the tracking speed |
caoyu@caoyuan9642-desktop.MIT.EDU | 17:7e47bc1630c0 | 414 | absSpeed = trackSpeed - nudgeSpeed; |
caoyu@caoyuan9642-desktop.MIT.EDU | 17:7e47bc1630c0 | 415 | } else if (nudgeSpeed > trackSpeed) {// ra_dir == AXIS_ROTATE_NEGATIVE |
caoyu@caoyuan9642-desktop.MIT.EDU | 17:7e47bc1630c0 | 416 | // Direction inverted |
caoyu@caoyuan9642-desktop.MIT.EDU | 17:7e47bc1630c0 | 417 | absSpeed = nudgeSpeed - trackSpeed; |
caoyu@caoyuan9642-desktop.MIT.EDU | 17:7e47bc1630c0 | 418 | ra_dir = AXIS_ROTATE_NEGATIVE; // Invert the rotation |
caoyu@caoyuan9642-desktop.MIT.EDU | 17:7e47bc1630c0 | 419 | } else { // nudgeSpeed == trackSpeed |
caoyu@caoyuan9642-desktop.MIT.EDU | 17:7e47bc1630c0 | 420 | absSpeed = 0; // Set speed to zero, although effective the speed would be a tiny value |
caoyu@caoyuan9642-desktop.MIT.EDU | 17:7e47bc1630c0 | 421 | } |
caoyu@caoyuan9642-desktop.MIT.EDU | 17:7e47bc1630c0 | 422 | |
caoyu@caoyuan9642-desktop.MIT.EDU | 17:7e47bc1630c0 | 423 | if (ra_dir != curr_dir) { |
caoyu@caoyuan9642-desktop.MIT.EDU | 17:7e47bc1630c0 | 424 | ra.stop(); |
caoyu@caoyuan9642-desktop.MIT.EDU | 17:7e47bc1630c0 | 425 | } |
caoyu@caoyuan9642-desktop.MIT.EDU | 17:7e47bc1630c0 | 426 | ra.setSlewSpeed(absSpeed); |
caoyu@caoyuan9642-desktop.MIT.EDU | 17:7e47bc1630c0 | 427 | if (ra_dir != curr_dir) { |
caoyu@caoyuan9642-desktop.MIT.EDU | 17:7e47bc1630c0 | 428 | ra.startSlewingIndefinite(ra_dir); |
caoyu@caoyuan9642-desktop.MIT.EDU | 17:7e47bc1630c0 | 429 | } |
caoyu@caoyuan9642-desktop.MIT.EDU | 17:7e47bc1630c0 | 430 | |
caoyu@caoyuan9642-desktop.MIT.EDU | 17:7e47bc1630c0 | 431 | } else { |
caoyu@caoyuan9642-desktop.MIT.EDU | 17:7e47bc1630c0 | 432 | // Simply update rate |
caoyu@caoyuan9642-desktop.MIT.EDU | 17:7e47bc1630c0 | 433 | ra.setSlewSpeed(rate); |
caoyu@caoyuan9642-desktop.MIT.EDU | 17:7e47bc1630c0 | 434 | } |
caoyu@caoyuan9642-desktop.MIT.EDU | 17:7e47bc1630c0 | 435 | mutex_execution.unlock(); |
caoyu@caoyuan9642-desktop.MIT.EDU | 17:7e47bc1630c0 | 436 | } else { |
caoyu@caoyuan9642-desktop.MIT.EDU | 17:7e47bc1630c0 | 437 | // Simply update rate |
caoyu@caoyuan9642-desktop.MIT.EDU | 17:7e47bc1630c0 | 438 | ra.setSlewSpeed(rate); |
caoyu@caoyuan9642-desktop.MIT.EDU | 17:7e47bc1630c0 | 439 | } |
caoyu@caoyuan9642-desktop.MIT.EDU | 17:7e47bc1630c0 | 440 | // Simply update rate for DEC |
caoyuan9642 | 0:6cb2eaf8b133 | 441 | dec.setSlewSpeed(rate); |
caoyuan9642 | 0:6cb2eaf8b133 | 442 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 443 | |
caoyu@caoyuan9642-desktop.MIT.EDU | 9:d0413a9b1386 | 444 | void EquatorialMount::setTrackSpeedSidereal(double rate) { |
caoyuan9642 | 0:6cb2eaf8b133 | 445 | // mutex_execution.lock(); |
caoyuan9642 | 0:6cb2eaf8b133 | 446 | ra.setTrackSpeedSidereal(rate); |
caoyuan9642 | 0:6cb2eaf8b133 | 447 | // mutex_execution.unlock(); |
caoyuan9642 | 0:6cb2eaf8b133 | 448 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 449 | |
caoyu@caoyuan9642-desktop.MIT.EDU | 9:d0413a9b1386 | 450 | void EquatorialMount::setGuideSpeedSidereal(double rate) { |
caoyuan9642 | 0:6cb2eaf8b133 | 451 | // mutex_execution.lock(); |
caoyuan9642 | 0:6cb2eaf8b133 | 452 | ra.setGuideSpeedSidereal(rate); |
caoyuan9642 | 0:6cb2eaf8b133 | 453 | dec.setGuideSpeedSidereal(rate); |
caoyuan9642 | 0:6cb2eaf8b133 | 454 | // mutex_execution.unlock(); |
caoyuan9642 | 0:6cb2eaf8b133 | 455 | } |
caoyuan9642 | 0:6cb2eaf8b133 | 456 | |
caoyu@caoyuan9642-desktop.MIT.EDU | 9:d0413a9b1386 | 457 | mountstatus_t EquatorialMount::getStatus() { |
caoyuan9642 | 2:2ee28add0821 | 458 | mountstatus_t s = status; |
caoyuan9642 | 2:2ee28add0821 | 459 | if (ra.isGuiding() || dec.isGuiding()) |
caoyuan9642 | 2:2ee28add0821 | 460 | s = (mountstatus_t) (s | MOUNT_GUIDING); |
caoyuan9642 | 2:2ee28add0821 | 461 | return s; |
caoyuan9642 | 2:2ee28add0821 | 462 | } |
caoyuan9642 | 4:b8ad57bbf9e1 | 463 | |
caoyu@caoyuan9642-desktop.MIT.EDU | 9:d0413a9b1386 | 464 | void EquatorialMount::forceAlignment() { |
caoyu@caoyuan9642-desktop.MIT.EDU | 9:d0413a9b1386 | 465 | for (int i = 0; i < num_alignment_stars; i++) { |
caoyu@caoyuan9642-desktop.MIT.EDU | 9:d0413a9b1386 | 466 | alignment_stars[i].star_meas = convertToMountCoordinates( |
caoyu@caoyuan9642-desktop.MIT.EDU | 9:d0413a9b1386 | 467 | alignment_stars[i].star_ref); |
caoyuan9642 | 4:b8ad57bbf9e1 | 468 | alignment_stars[i].timestamp = clock.getTime(); |
caoyuan9642 | 4:b8ad57bbf9e1 | 469 | } |
caoyu@caoyuan9642-desktop.MIT.EDU | 9:d0413a9b1386 | 470 | } |