Yuan Cao / pushtogo

Telescope Control Library

Dependents:   PushToGo-F429

Axis.cpp@0:6cb2eaf8b133, 2018-08-19 (annotated)

Committer:
caoyuan9642
Date:
Sun Aug 19 05:21:20 2018 +0000
Revision:
0:6cb2eaf8b133
Child:
2:2ee28add0821
v0.1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
caoyuan9642 0:6cb2eaf8b133 1 /*
caoyuan9642 0:6cb2eaf8b133 2 * Axis.cpp
caoyuan9642 0:6cb2eaf8b133 3 *
caoyuan9642 0:6cb2eaf8b133 4 * Created on: 2018Äê2ÔÂ24ÈÕ
caoyuan9642 0:6cb2eaf8b133 5 * Author: caoyuan9642
caoyuan9642 0:6cb2eaf8b133 6 */
caoyuan9642 0:6cb2eaf8b133 7
caoyuan9642 0:6cb2eaf8b133 8 #include <Axis.h>
caoyuan9642 0:6cb2eaf8b133 9
caoyuan9642 0:6cb2eaf8b133 10 #define AXIS_DEBUG 1
caoyuan9642 0:6cb2eaf8b133 11
caoyuan9642 0:6cb2eaf8b133 12 static inline double min(double x, double y)
caoyuan9642 0:6cb2eaf8b133 13 {
caoyuan9642 0:6cb2eaf8b133 14 return (x < y) ? x : y;
caoyuan9642 0:6cb2eaf8b133 15 }
caoyuan9642 0:6cb2eaf8b133 16 Axis::Axis(double stepsPerDeg, StepperMotor *stepper, const char *name) :
caoyuan9642 0:6cb2eaf8b133 17 stepsPerDeg(stepsPerDeg), stepper(stepper), axisName(name), currentSpeed(
caoyuan9642 0:6cb2eaf8b133 18 0), currentDirection(AXIS_ROTATE_POSITIVE), slewSpeed(
caoyuan9642 0:6cb2eaf8b133 19 TelescopeConfiguration::getDouble("default_slew_speed")), trackSpeed(
caoyuan9642 0:6cb2eaf8b133 20 TelescopeConfiguration::getDouble(
caoyuan9642 0:6cb2eaf8b133 21 "default_track_speed_sidereal") * sidereal_speed), guideSpeed(
caoyuan9642 0:6cb2eaf8b133 22 TelescopeConfiguration::getDouble(
caoyuan9642 0:6cb2eaf8b133 23 "default_guide_speed_sidereal") * sidereal_speed), status(
caoyuan9642 0:6cb2eaf8b133 24 AXIS_STOPPED), slewState(AXIS_NOT_SLEWING), slew_finish_sem(0,
caoyuan9642 0:6cb2eaf8b133 25 1), slew_finish_state(FINISH_COMPLETE)
caoyuan9642 0:6cb2eaf8b133 26 {
caoyuan9642 0:6cb2eaf8b133 27 if (stepsPerDeg <= 0)
caoyuan9642 0:6cb2eaf8b133 28 error("Axis: steps per degree must be > 0");
caoyuan9642 0:6cb2eaf8b133 29
caoyuan9642 0:6cb2eaf8b133 30 if (!stepper)
caoyuan9642 0:6cb2eaf8b133 31 error("Axis: stepper must be defined");
caoyuan9642 0:6cb2eaf8b133 32
caoyuan9642 0:6cb2eaf8b133 33 taskName = new char[strlen(name) + 10];
caoyuan9642 0:6cb2eaf8b133 34 strcpy(taskName, name);
caoyuan9642 0:6cb2eaf8b133 35 strcat(taskName, " task");
caoyuan9642 0:6cb2eaf8b133 36 /*Start the task-handling thread*/
caoyuan9642 0:6cb2eaf8b133 37 task_thread = new Thread(osPriorityAboveNormal,
caoyuan9642 0:6cb2eaf8b133 38 OS_STACK_SIZE, NULL, taskName);
caoyuan9642 0:6cb2eaf8b133 39 task_thread->start(callback(this, &Axis::task));
caoyuan9642 0:6cb2eaf8b133 40 tim.start();
caoyuan9642 0:6cb2eaf8b133 41 }
caoyuan9642 0:6cb2eaf8b133 42
caoyuan9642 0:6cb2eaf8b133 43 Axis::~Axis()
caoyuan9642 0:6cb2eaf8b133 44 {
caoyuan9642 0:6cb2eaf8b133 45 // Wait until the axis is stopped
caoyuan9642 0:6cb2eaf8b133 46 if (status != AXIS_STOPPED)
caoyuan9642 0:6cb2eaf8b133 47 {
caoyuan9642 0:6cb2eaf8b133 48 stop();
caoyuan9642 0:6cb2eaf8b133 49 while (status != AXIS_STOPPED)
caoyuan9642 0:6cb2eaf8b133 50 {
caoyuan9642 0:6cb2eaf8b133 51 Thread::wait(100);
caoyuan9642 0:6cb2eaf8b133 52 }
caoyuan9642 0:6cb2eaf8b133 53 }
caoyuan9642 0:6cb2eaf8b133 54
caoyuan9642 0:6cb2eaf8b133 55 // Terminate the task thread to prevent illegal access to destroyed objects.
caoyuan9642 0:6cb2eaf8b133 56 task_thread->terminate();
caoyuan9642 0:6cb2eaf8b133 57 delete task_thread;
caoyuan9642 0:6cb2eaf8b133 58 delete taskName;
caoyuan9642 0:6cb2eaf8b133 59 }
caoyuan9642 0:6cb2eaf8b133 60
caoyuan9642 0:6cb2eaf8b133 61 void Axis::task()
caoyuan9642 0:6cb2eaf8b133 62 {
caoyuan9642 0:6cb2eaf8b133 63
caoyuan9642 0:6cb2eaf8b133 64 /*Main loop of RotationAxis*/
caoyuan9642 0:6cb2eaf8b133 65 while (true)
caoyuan9642 0:6cb2eaf8b133 66 {
caoyuan9642 0:6cb2eaf8b133 67 /*Get next message*/
caoyuan9642 0:6cb2eaf8b133 68 msg_t *message;
caoyuan9642 0:6cb2eaf8b133 69 enum msg_t::sig_t signal;
caoyuan9642 0:6cb2eaf8b133 70 float value;
caoyuan9642 0:6cb2eaf8b133 71 axisrotdir_t dir;
caoyuan9642 0:6cb2eaf8b133 72 bool wc;
caoyuan9642 0:6cb2eaf8b133 73
caoyuan9642 0:6cb2eaf8b133 74 // Wait for next message
caoyuan9642 0:6cb2eaf8b133 75 osEvent evt = task_queue.get();
caoyuan9642 0:6cb2eaf8b133 76 if (evt.status == osEventMessage)
caoyuan9642 0:6cb2eaf8b133 77 {
caoyuan9642 0:6cb2eaf8b133 78 /*New message arrived. Copy the data and free is asap*/
caoyuan9642 0:6cb2eaf8b133 79 message = (msg_t*) evt.value.p;
caoyuan9642 0:6cb2eaf8b133 80 signal = message->signal;
caoyuan9642 0:6cb2eaf8b133 81 value = message->value;
caoyuan9642 0:6cb2eaf8b133 82 dir = message->dir;
caoyuan9642 0:6cb2eaf8b133 83 wc = message->withCorrection;
caoyuan9642 0:6cb2eaf8b133 84 task_pool.free(message);
caoyuan9642 0:6cb2eaf8b133 85 }
caoyuan9642 0:6cb2eaf8b133 86 else
caoyuan9642 0:6cb2eaf8b133 87 {
caoyuan9642 0:6cb2eaf8b133 88 /*Error*/
caoyuan9642 0:6cb2eaf8b133 89 debug("%s: Error fetching the task queue.\n", axisName);
caoyuan9642 0:6cb2eaf8b133 90 continue;
caoyuan9642 0:6cb2eaf8b133 91 }
caoyuan9642 0:6cb2eaf8b133 92 debug_if(AXIS_DEBUG, "%s: MSG %d %f %d 0x%8x\n", axisName, signal,
caoyuan9642 0:6cb2eaf8b133 93 value, dir);
caoyuan9642 0:6cb2eaf8b133 94
caoyuan9642 0:6cb2eaf8b133 95 /*Check the type of the signal, and start corresponding operations*/
caoyuan9642 0:6cb2eaf8b133 96 switch (signal)
caoyuan9642 0:6cb2eaf8b133 97 {
caoyuan9642 0:6cb2eaf8b133 98 case msg_t::SIGNAL_SLEW_TO:
caoyuan9642 0:6cb2eaf8b133 99 if (status == AXIS_STOPPED)
caoyuan9642 0:6cb2eaf8b133 100 {
caoyuan9642 0:6cb2eaf8b133 101 slew(dir, value, false, wc);
caoyuan9642 0:6cb2eaf8b133 102 }
caoyuan9642 0:6cb2eaf8b133 103 else
caoyuan9642 0:6cb2eaf8b133 104 {
caoyuan9642 0:6cb2eaf8b133 105 debug("%s: being slewed while not in STOPPED mode.\n",
caoyuan9642 0:6cb2eaf8b133 106 axisName);
caoyuan9642 0:6cb2eaf8b133 107 }
caoyuan9642 0:6cb2eaf8b133 108 debug_if(0, "%s: SIG SLEW 0x%08x\n", axisName, Thread::gettid());
caoyuan9642 0:6cb2eaf8b133 109 slew_finish_sem.release(); /*Send a signal so that the caller is free to run*/
caoyuan9642 0:6cb2eaf8b133 110 break;
caoyuan9642 0:6cb2eaf8b133 111 case msg_t::SIGNAL_SLEW_INDEFINITE:
caoyuan9642 0:6cb2eaf8b133 112 if (status == AXIS_STOPPED || status == AXIS_INERTIAL)
caoyuan9642 0:6cb2eaf8b133 113 {
caoyuan9642 0:6cb2eaf8b133 114 slew(dir, 0.0, true, false);
caoyuan9642 0:6cb2eaf8b133 115 }
caoyuan9642 0:6cb2eaf8b133 116 else
caoyuan9642 0:6cb2eaf8b133 117 {
caoyuan9642 0:6cb2eaf8b133 118 debug("%s: being slewed while not in STOPPED mode.\n",
caoyuan9642 0:6cb2eaf8b133 119 axisName);
caoyuan9642 0:6cb2eaf8b133 120 }
caoyuan9642 0:6cb2eaf8b133 121 break;
caoyuan9642 0:6cb2eaf8b133 122 case msg_t::SIGNAL_TRACK:
caoyuan9642 0:6cb2eaf8b133 123 if (status == AXIS_STOPPED)
caoyuan9642 0:6cb2eaf8b133 124 {
caoyuan9642 0:6cb2eaf8b133 125 track(dir);
caoyuan9642 0:6cb2eaf8b133 126 }
caoyuan9642 0:6cb2eaf8b133 127 else
caoyuan9642 0:6cb2eaf8b133 128 {
caoyuan9642 0:6cb2eaf8b133 129 debug("%s: trying to track while not in STOPPED mode.\n",
caoyuan9642 0:6cb2eaf8b133 130 axisName);
caoyuan9642 0:6cb2eaf8b133 131 }
caoyuan9642 0:6cb2eaf8b133 132 break;
caoyuan9642 0:6cb2eaf8b133 133 default:
caoyuan9642 0:6cb2eaf8b133 134 debug("%s: undefined signal %d\n", axisName, message->signal);
caoyuan9642 0:6cb2eaf8b133 135 }
caoyuan9642 0:6cb2eaf8b133 136 }
caoyuan9642 0:6cb2eaf8b133 137 }
caoyuan9642 0:6cb2eaf8b133 138
caoyuan9642 0:6cb2eaf8b133 139 void Axis::slew(axisrotdir_t dir, double dest, bool indefinite,
caoyuan9642 0:6cb2eaf8b133 140 bool useCorrection)
caoyuan9642 0:6cb2eaf8b133 141 {
caoyuan9642 0:6cb2eaf8b133 142 if (!indefinite && (isnan(dest) || isinf(dest)))
caoyuan9642 0:6cb2eaf8b133 143 {
caoyuan9642 0:6cb2eaf8b133 144 debug("%s: invalid angle.\n", axisName);
caoyuan9642 0:6cb2eaf8b133 145 return;
caoyuan9642 0:6cb2eaf8b133 146 }
caoyuan9642 0:6cb2eaf8b133 147 if (dir == AXIS_ROTATE_STOP)
caoyuan9642 0:6cb2eaf8b133 148 {
caoyuan9642 0:6cb2eaf8b133 149 debug("%s: skipped.\n", axisName);
caoyuan9642 0:6cb2eaf8b133 150 return;
caoyuan9642 0:6cb2eaf8b133 151 }
caoyuan9642 0:6cb2eaf8b133 152
caoyuan9642 0:6cb2eaf8b133 153 slew_mode(); // Switch to slew mode
caoyuan9642 0:6cb2eaf8b133 154 Thread::signal_clr(
caoyuan9642 0:6cb2eaf8b133 155 AXIS_STOP_SIGNAL | AXIS_EMERGE_STOP_SIGNAL | AXIS_SPEEDCHANGE_SIGNAL); // Clear flags
caoyuan9642 0:6cb2eaf8b133 156 bool isInertial = (status == AXIS_INERTIAL);
caoyuan9642 0:6cb2eaf8b133 157 status = AXIS_SLEWING;
caoyuan9642 0:6cb2eaf8b133 158 slewState = AXIS_NOT_SLEWING;
caoyuan9642 0:6cb2eaf8b133 159 slew_finish_state = FINISH_COMPLETE;
caoyuan9642 0:6cb2eaf8b133 160 currentDirection = dir;
caoyuan9642 0:6cb2eaf8b133 161 stepdir_t sd = (dir == AXIS_ROTATE_POSITIVE) ? STEP_FORWARD : STEP_BACKWARD;
caoyuan9642 0:6cb2eaf8b133 162
caoyuan9642 0:6cb2eaf8b133 163 /* Calculate the angle to rotate*/
caoyuan9642 0:6cb2eaf8b133 164 bool skip_slew = false;
caoyuan9642 0:6cb2eaf8b133 165 double angleDeg = getAngleDeg();
caoyuan9642 0:6cb2eaf8b133 166 double delta;
caoyuan9642 0:6cb2eaf8b133 167 delta = (dest - angleDeg) * (dir == AXIS_ROTATE_POSITIVE ? 1 : -1); /*delta is the actual angle to rotate*/
caoyuan9642 0:6cb2eaf8b133 168 if (fabs(delta) < 1e-5)
caoyuan9642 0:6cb2eaf8b133 169 { // FIX: delta 0->360degrees when angleDeg is essentially equal to dest
caoyuan9642 0:6cb2eaf8b133 170 delta = 0;
caoyuan9642 0:6cb2eaf8b133 171 }
caoyuan9642 0:6cb2eaf8b133 172 delta = remainder(delta - 180.0, 360.0) + 180.0; /*Shift to 0-360 deg*/
caoyuan9642 0:6cb2eaf8b133 173 debug_if(AXIS_DEBUG, "%s: start=%f, end=%f, delta=%f\n", axisName, angleDeg,
caoyuan9642 0:6cb2eaf8b133 174 dest, delta);
caoyuan9642 0:6cb2eaf8b133 175
caoyuan9642 0:6cb2eaf8b133 176 double startSpeed = 0;
caoyuan9642 0:6cb2eaf8b133 177 double endSpeed = slewSpeed, waitTime;
caoyuan9642 0:6cb2eaf8b133 178 unsigned int ramp_steps;
caoyuan9642 0:6cb2eaf8b133 179 double acceleration = TelescopeConfiguration::getDouble("acceleration");
caoyuan9642 0:6cb2eaf8b133 180
caoyuan9642 0:6cb2eaf8b133 181 if (!indefinite)
caoyuan9642 0:6cb2eaf8b133 182 {
caoyuan9642 0:6cb2eaf8b133 183 // Ensure that delta is more than the minimum slewing angle, calculate the correct endSpeed and waitTime
caoyuan9642 0:6cb2eaf8b133 184 if (delta > TelescopeConfiguration::getDouble("min_slew_angle"))
caoyuan9642 0:6cb2eaf8b133 185 {
caoyuan9642 0:6cb2eaf8b133 186 /*The motion angle is decreased to ensure the correction step is in the same direction*/
caoyuan9642 0:6cb2eaf8b133 187 delta = delta
caoyuan9642 0:6cb2eaf8b133 188 - 0.5 * TelescopeConfiguration::getDouble("min_slew_angle");
caoyuan9642 0:6cb2eaf8b133 189
caoyuan9642 0:6cb2eaf8b133 190 double angleRotatedDuringAcceleration;
caoyuan9642 0:6cb2eaf8b133 191
caoyuan9642 0:6cb2eaf8b133 192 /* The actual endSpeed we get might be different than this due to the finite time resolution of the stepper driver
caoyuan9642 0:6cb2eaf8b133 193 * Here we set the dummy frequency and obtain the actual frequency it will be set to, so that the slewing time will be more accurate
caoyuan9642 0:6cb2eaf8b133 194 */
caoyuan9642 0:6cb2eaf8b133 195
caoyuan9642 0:6cb2eaf8b133 196 // Calculate the desired endSpeed. If delta is small, then endSpeed will correspondingly be reduced
caoyuan9642 0:6cb2eaf8b133 197 endSpeed = min(sqrt(delta * acceleration),
caoyuan9642 0:6cb2eaf8b133 198 stepper->setFrequency(stepsPerDeg * endSpeed)
caoyuan9642 0:6cb2eaf8b133 199 / stepsPerDeg);
caoyuan9642 0:6cb2eaf8b133 200 ramp_steps = (unsigned int) (endSpeed
caoyuan9642 0:6cb2eaf8b133 201 / (TelescopeConfiguration::getInt("acceleration_step_time")
caoyuan9642 0:6cb2eaf8b133 202 / 1000.0) / acceleration);
caoyuan9642 0:6cb2eaf8b133 203 if (ramp_steps < 1)
caoyuan9642 0:6cb2eaf8b133 204 ramp_steps = 1;
caoyuan9642 0:6cb2eaf8b133 205
caoyuan9642 0:6cb2eaf8b133 206 angleRotatedDuringAcceleration = 0;
caoyuan9642 0:6cb2eaf8b133 207 /*Simulate the slewing process to get an accurate estimate of the actual angle that will be slewed*/
caoyuan9642 0:6cb2eaf8b133 208 for (unsigned int i = 1; i <= ramp_steps; i++)
caoyuan9642 0:6cb2eaf8b133 209 {
caoyuan9642 0:6cb2eaf8b133 210 double speed = stepper->setFrequency(
caoyuan9642 0:6cb2eaf8b133 211 stepsPerDeg * endSpeed / ramp_steps * i) / stepsPerDeg;
caoyuan9642 0:6cb2eaf8b133 212 angleRotatedDuringAcceleration += speed
caoyuan9642 0:6cb2eaf8b133 213 * (TelescopeConfiguration::getInt(
caoyuan9642 0:6cb2eaf8b133 214 "acceleration_step_time") / 1000.0)
caoyuan9642 0:6cb2eaf8b133 215 * (i == ramp_steps ? 1 : 2); // Count both acceleration and deceleration
caoyuan9642 0:6cb2eaf8b133 216 }
caoyuan9642 0:6cb2eaf8b133 217
caoyuan9642 0:6cb2eaf8b133 218 waitTime = (delta - angleRotatedDuringAcceleration) / endSpeed;
caoyuan9642 0:6cb2eaf8b133 219 if (waitTime < 0.0)
caoyuan9642 0:6cb2eaf8b133 220 waitTime = 0.0; // With the above calculations, waitTime should no longer be zero. But if it happens to be so, let the correction do the job
caoyuan9642 0:6cb2eaf8b133 221
caoyuan9642 0:6cb2eaf8b133 222 debug_if(AXIS_DEBUG, "%s: endspeed = %f deg/s, time=%f, acc=%f\n",
caoyuan9642 0:6cb2eaf8b133 223 axisName, endSpeed, waitTime, acceleration);
caoyuan9642 0:6cb2eaf8b133 224 }
caoyuan9642 0:6cb2eaf8b133 225 else
caoyuan9642 0:6cb2eaf8b133 226 {
caoyuan9642 0:6cb2eaf8b133 227 // Angle difference is too small, skip slewing
caoyuan9642 0:6cb2eaf8b133 228 skip_slew = true;
caoyuan9642 0:6cb2eaf8b133 229 }
caoyuan9642 0:6cb2eaf8b133 230 }
caoyuan9642 0:6cb2eaf8b133 231 else
caoyuan9642 0:6cb2eaf8b133 232 {
caoyuan9642 0:6cb2eaf8b133 233 // Indefinite slewing mode
caoyuan9642 0:6cb2eaf8b133 234 waitTime = INFINITY;
caoyuan9642 0:6cb2eaf8b133 235 // If was in inertial mode, use startSpeed
caoyuan9642 0:6cb2eaf8b133 236 if (isInertial)
caoyuan9642 0:6cb2eaf8b133 237 startSpeed = currentSpeed;
caoyuan9642 0:6cb2eaf8b133 238 // No need for correction
caoyuan9642 0:6cb2eaf8b133 239 useCorrection = false;
caoyuan9642 0:6cb2eaf8b133 240 }
caoyuan9642 0:6cb2eaf8b133 241
caoyuan9642 0:6cb2eaf8b133 242 /*Slewing -> accel, wait, decel*/
caoyuan9642 0:6cb2eaf8b133 243 if (!skip_slew)
caoyuan9642 0:6cb2eaf8b133 244 {
caoyuan9642 0:6cb2eaf8b133 245 int wait_ms;
caoyuan9642 0:6cb2eaf8b133 246 uint32_t flags;
caoyuan9642 0:6cb2eaf8b133 247 /*Acceleration*/
caoyuan9642 0:6cb2eaf8b133 248 slewState = AXIS_SLEW_ACCELERATING;
caoyuan9642 0:6cb2eaf8b133 249 ramp_steps = (unsigned int) ((endSpeed - startSpeed)
caoyuan9642 0:6cb2eaf8b133 250 / (TelescopeConfiguration::getInt("acceleration_step_time")
caoyuan9642 0:6cb2eaf8b133 251 / 1000.0) / acceleration);
caoyuan9642 0:6cb2eaf8b133 252
caoyuan9642 0:6cb2eaf8b133 253 if (ramp_steps < 1)
caoyuan9642 0:6cb2eaf8b133 254 ramp_steps = 1;
caoyuan9642 0:6cb2eaf8b133 255
caoyuan9642 0:6cb2eaf8b133 256 debug_if(AXIS_DEBUG, "%s: accelerate in %d steps\n", axisName,
caoyuan9642 0:6cb2eaf8b133 257 ramp_steps); // TODO: DEBUG
caoyuan9642 0:6cb2eaf8b133 258
caoyuan9642 0:6cb2eaf8b133 259 for (unsigned int i = 1; i <= ramp_steps; i++)
caoyuan9642 0:6cb2eaf8b133 260 {
caoyuan9642 0:6cb2eaf8b133 261 currentSpeed = stepper->setFrequency(
caoyuan9642 0:6cb2eaf8b133 262 stepsPerDeg
caoyuan9642 0:6cb2eaf8b133 263 * ((endSpeed - startSpeed) / ramp_steps * i
caoyuan9642 0:6cb2eaf8b133 264 + startSpeed)) / stepsPerDeg; // Set and update currentSpeed with actual speed
caoyuan9642 0:6cb2eaf8b133 265
caoyuan9642 0:6cb2eaf8b133 266 if (i == 1)
caoyuan9642 0:6cb2eaf8b133 267 stepper->start(sd);
caoyuan9642 0:6cb2eaf8b133 268
caoyuan9642 0:6cb2eaf8b133 269 /*Monitor whether there is a stop/emerge stop signal*/
caoyuan9642 0:6cb2eaf8b133 270 uint32_t flags = osThreadFlagsWait(
caoyuan9642 0:6cb2eaf8b133 271 AXIS_STOP_SIGNAL | AXIS_EMERGE_STOP_SIGNAL, osFlagsWaitAny,
caoyuan9642 0:6cb2eaf8b133 272 TelescopeConfiguration::getInt("acceleration_step_time"));
caoyuan9642 0:6cb2eaf8b133 273
caoyuan9642 0:6cb2eaf8b133 274 if (flags == osFlagsErrorTimeout)
caoyuan9642 0:6cb2eaf8b133 275 {
caoyuan9642 0:6cb2eaf8b133 276 /*Nothing happened, we're good*/
caoyuan9642 0:6cb2eaf8b133 277 continue;
caoyuan9642 0:6cb2eaf8b133 278 }
caoyuan9642 0:6cb2eaf8b133 279 else if ((flags & osFlagsError) == 0)
caoyuan9642 0:6cb2eaf8b133 280 {
caoyuan9642 0:6cb2eaf8b133 281 // We're stopped!
caoyuan9642 0:6cb2eaf8b133 282 useCorrection = false;
caoyuan9642 0:6cb2eaf8b133 283 if (flags & AXIS_EMERGE_STOP_SIGNAL)
caoyuan9642 0:6cb2eaf8b133 284 {
caoyuan9642 0:6cb2eaf8b133 285 slew_finish_state = FINISH_EMERG_STOPPED;
caoyuan9642 0:6cb2eaf8b133 286 goto emerge_stop;
caoyuan9642 0:6cb2eaf8b133 287 }
caoyuan9642 0:6cb2eaf8b133 288 else if (flags & AXIS_STOP_SIGNAL)
caoyuan9642 0:6cb2eaf8b133 289 {
caoyuan9642 0:6cb2eaf8b133 290 slew_finish_state = FINISH_STOPPED;
caoyuan9642 0:6cb2eaf8b133 291 goto stop;
caoyuan9642 0:6cb2eaf8b133 292 }
caoyuan9642 0:6cb2eaf8b133 293 }
caoyuan9642 0:6cb2eaf8b133 294 }
caoyuan9642 0:6cb2eaf8b133 295
caoyuan9642 0:6cb2eaf8b133 296 /*Keep slewing and wait*/
caoyuan9642 0:6cb2eaf8b133 297 slewState = AXIS_SLEW_CONSTANT_SPEED;
caoyuan9642 0:6cb2eaf8b133 298 debug_if(AXIS_DEBUG, "%s: wait for %f\n", axisName, waitTime); // TODO
caoyuan9642 0:6cb2eaf8b133 299 wait_ms = (isinf(waitTime)) ? osWaitForever : (int) (waitTime * 1000);
caoyuan9642 0:6cb2eaf8b133 300
caoyuan9642 0:6cb2eaf8b133 301 tim.reset();
caoyuan9642 0:6cb2eaf8b133 302
caoyuan9642 0:6cb2eaf8b133 303 while (wait_ms)
caoyuan9642 0:6cb2eaf8b133 304 {
caoyuan9642 0:6cb2eaf8b133 305 flags = osThreadFlagsWait(
caoyuan9642 0:6cb2eaf8b133 306 AXIS_STOP_SIGNAL | AXIS_EMERGE_STOP_SIGNAL
caoyuan9642 0:6cb2eaf8b133 307 | (indefinite ? AXIS_SPEEDCHANGE_SIGNAL : 0),
caoyuan9642 0:6cb2eaf8b133 308 osFlagsWaitAny, wait_ms); /*Wait the remaining time*/
caoyuan9642 0:6cb2eaf8b133 309 if (flags != osFlagsErrorTimeout)
caoyuan9642 0:6cb2eaf8b133 310 {
caoyuan9642 0:6cb2eaf8b133 311 if (flags & AXIS_EMERGE_STOP_SIGNAL)
caoyuan9642 0:6cb2eaf8b133 312 {
caoyuan9642 0:6cb2eaf8b133 313 slew_finish_state = FINISH_EMERG_STOPPED;
caoyuan9642 0:6cb2eaf8b133 314 useCorrection = false;
caoyuan9642 0:6cb2eaf8b133 315 goto emerge_stop;
caoyuan9642 0:6cb2eaf8b133 316 }
caoyuan9642 0:6cb2eaf8b133 317 else if (flags & AXIS_STOP_SIGNAL)
caoyuan9642 0:6cb2eaf8b133 318 {
caoyuan9642 0:6cb2eaf8b133 319 slew_finish_state = FINISH_STOPPED;
caoyuan9642 0:6cb2eaf8b133 320 useCorrection = false;
caoyuan9642 0:6cb2eaf8b133 321 goto stop;
caoyuan9642 0:6cb2eaf8b133 322 }
caoyuan9642 0:6cb2eaf8b133 323 else if (flags & AXIS_SPEEDCHANGE_SIGNAL)
caoyuan9642 0:6cb2eaf8b133 324 {
caoyuan9642 0:6cb2eaf8b133 325 // Change speed, therefore also changing waittime. Only applies to indefinite slew
caoyuan9642 0:6cb2eaf8b133 326 double newSpeed = slewSpeed;
caoyuan9642 0:6cb2eaf8b133 327 startSpeed = currentSpeed;
caoyuan9642 0:6cb2eaf8b133 328 endSpeed = newSpeed;
caoyuan9642 0:6cb2eaf8b133 329
caoyuan9642 0:6cb2eaf8b133 330 ramp_steps = (unsigned int) (fabs(endSpeed - startSpeed)
caoyuan9642 0:6cb2eaf8b133 331 / (TelescopeConfiguration::getInt(
caoyuan9642 0:6cb2eaf8b133 332 "acceleration_step_time") / 1000.0)
caoyuan9642 0:6cb2eaf8b133 333 / acceleration);
caoyuan9642 0:6cb2eaf8b133 334
caoyuan9642 0:6cb2eaf8b133 335 if (ramp_steps < 1)
caoyuan9642 0:6cb2eaf8b133 336 ramp_steps = 1;
caoyuan9642 0:6cb2eaf8b133 337
caoyuan9642 0:6cb2eaf8b133 338 debug_if(AXIS_DEBUG, "%s: accelerate in %d steps\n",
caoyuan9642 0:6cb2eaf8b133 339 axisName, ramp_steps); // TODO: DEBUG
caoyuan9642 0:6cb2eaf8b133 340
caoyuan9642 0:6cb2eaf8b133 341 for (unsigned int i = 1; i <= ramp_steps; i++)
caoyuan9642 0:6cb2eaf8b133 342 {
caoyuan9642 0:6cb2eaf8b133 343 currentSpeed = stepper->setFrequency(
caoyuan9642 0:6cb2eaf8b133 344 stepsPerDeg
caoyuan9642 0:6cb2eaf8b133 345 * ((endSpeed - startSpeed) / ramp_steps
caoyuan9642 0:6cb2eaf8b133 346 * i + startSpeed))
caoyuan9642 0:6cb2eaf8b133 347 / stepsPerDeg; // Set and update currentSpeed with actual speed
caoyuan9642 0:6cb2eaf8b133 348
caoyuan9642 0:6cb2eaf8b133 349 /*Monitor whether there is a stop/emerge stop signal*/
caoyuan9642 0:6cb2eaf8b133 350 uint32_t flags = osThreadFlagsWait(
caoyuan9642 0:6cb2eaf8b133 351 AXIS_STOP_SIGNAL | AXIS_EMERGE_STOP_SIGNAL,
caoyuan9642 0:6cb2eaf8b133 352 osFlagsWaitAny,
caoyuan9642 0:6cb2eaf8b133 353 TelescopeConfiguration::getInt(
caoyuan9642 0:6cb2eaf8b133 354 "acceleration_step_time"));
caoyuan9642 0:6cb2eaf8b133 355
caoyuan9642 0:6cb2eaf8b133 356 if (flags == osFlagsErrorTimeout)
caoyuan9642 0:6cb2eaf8b133 357 {
caoyuan9642 0:6cb2eaf8b133 358 /*Nothing happened, we're good*/
caoyuan9642 0:6cb2eaf8b133 359 continue;
caoyuan9642 0:6cb2eaf8b133 360 }
caoyuan9642 0:6cb2eaf8b133 361 else if ((flags & osFlagsError) == 0)
caoyuan9642 0:6cb2eaf8b133 362 {
caoyuan9642 0:6cb2eaf8b133 363 // We're stopped!
caoyuan9642 0:6cb2eaf8b133 364 useCorrection = false;
caoyuan9642 0:6cb2eaf8b133 365 if (flags & AXIS_EMERGE_STOP_SIGNAL)
caoyuan9642 0:6cb2eaf8b133 366 {
caoyuan9642 0:6cb2eaf8b133 367 slew_finish_state = FINISH_EMERG_STOPPED;
caoyuan9642 0:6cb2eaf8b133 368 goto emerge_stop;
caoyuan9642 0:6cb2eaf8b133 369 }
caoyuan9642 0:6cb2eaf8b133 370 else if (flags & AXIS_STOP_SIGNAL)
caoyuan9642 0:6cb2eaf8b133 371 {
caoyuan9642 0:6cb2eaf8b133 372 slew_finish_state = FINISH_STOPPED;
caoyuan9642 0:6cb2eaf8b133 373 goto stop;
caoyuan9642 0:6cb2eaf8b133 374 }
caoyuan9642 0:6cb2eaf8b133 375 }
caoyuan9642 0:6cb2eaf8b133 376 }
caoyuan9642 0:6cb2eaf8b133 377 }
caoyuan9642 0:6cb2eaf8b133 378 }
caoyuan9642 0:6cb2eaf8b133 379 if (!indefinite)
caoyuan9642 0:6cb2eaf8b133 380 {
caoyuan9642 0:6cb2eaf8b133 381 wait_ms -= tim.read_ms();
caoyuan9642 0:6cb2eaf8b133 382 tim.reset();
caoyuan9642 0:6cb2eaf8b133 383 }
caoyuan9642 0:6cb2eaf8b133 384 }
caoyuan9642 0:6cb2eaf8b133 385
caoyuan9642 0:6cb2eaf8b133 386 stop:
caoyuan9642 0:6cb2eaf8b133 387 /*Now deceleration*/
caoyuan9642 0:6cb2eaf8b133 388 slewState = AXIS_SLEW_DECELERATING;
caoyuan9642 0:6cb2eaf8b133 389 endSpeed = currentSpeed;
caoyuan9642 0:6cb2eaf8b133 390 ramp_steps = (unsigned int) (currentSpeed
caoyuan9642 0:6cb2eaf8b133 391 / (TelescopeConfiguration::getInt("acceleration_step_time")
caoyuan9642 0:6cb2eaf8b133 392 / 1000.0) / acceleration);
caoyuan9642 0:6cb2eaf8b133 393
caoyuan9642 0:6cb2eaf8b133 394 if (ramp_steps < 1)
caoyuan9642 0:6cb2eaf8b133 395 ramp_steps = 1;
caoyuan9642 0:6cb2eaf8b133 396
caoyuan9642 0:6cb2eaf8b133 397 debug_if(AXIS_DEBUG, "%s: decelerate in %d steps from %f\n", axisName,
caoyuan9642 0:6cb2eaf8b133 398 ramp_steps, endSpeed); // TODO: DEBUG
caoyuan9642 0:6cb2eaf8b133 399
caoyuan9642 0:6cb2eaf8b133 400 for (unsigned int i = ramp_steps - 1; i >= 1; i--)
caoyuan9642 0:6cb2eaf8b133 401 {
caoyuan9642 0:6cb2eaf8b133 402 currentSpeed = stepper->setFrequency(
caoyuan9642 0:6cb2eaf8b133 403 stepsPerDeg * endSpeed / ramp_steps * i) / stepsPerDeg; // set and update accurate speed
caoyuan9642 0:6cb2eaf8b133 404 // Wait. Now we only handle EMERGENCY STOP signal, since stop has been handled already
caoyuan9642 0:6cb2eaf8b133 405 flags = osThreadFlagsWait(
caoyuan9642 0:6cb2eaf8b133 406 AXIS_EMERGE_STOP_SIGNAL | AXIS_STOP_KEEPSPEED_SIGNAL,
caoyuan9642 0:6cb2eaf8b133 407 osFlagsWaitAny,
caoyuan9642 0:6cb2eaf8b133 408 TelescopeConfiguration::getInt("acceleration_step_time"));
caoyuan9642 0:6cb2eaf8b133 409
caoyuan9642 0:6cb2eaf8b133 410 if (flags != osFlagsErrorTimeout)
caoyuan9642 0:6cb2eaf8b133 411 {
caoyuan9642 0:6cb2eaf8b133 412 if (flags & AXIS_EMERGE_STOP_SIGNAL)
caoyuan9642 0:6cb2eaf8b133 413 {
caoyuan9642 0:6cb2eaf8b133 414 // We're stopped!
caoyuan9642 0:6cb2eaf8b133 415 useCorrection = false;
caoyuan9642 0:6cb2eaf8b133 416 slew_finish_state = FINISH_EMERG_STOPPED;
caoyuan9642 0:6cb2eaf8b133 417 goto emerge_stop;
caoyuan9642 0:6cb2eaf8b133 418 }
caoyuan9642 0:6cb2eaf8b133 419 else if (flags & AXIS_STOP_KEEPSPEED_SIGNAL)
caoyuan9642 0:6cb2eaf8b133 420 {
caoyuan9642 0:6cb2eaf8b133 421 // Keep current speed
caoyuan9642 0:6cb2eaf8b133 422 status = AXIS_INERTIAL;
caoyuan9642 0:6cb2eaf8b133 423 slewState = AXIS_NOT_SLEWING;
caoyuan9642 0:6cb2eaf8b133 424 return;
caoyuan9642 0:6cb2eaf8b133 425 }
caoyuan9642 0:6cb2eaf8b133 426 }
caoyuan9642 0:6cb2eaf8b133 427 }
caoyuan9642 0:6cb2eaf8b133 428
caoyuan9642 0:6cb2eaf8b133 429 emerge_stop:
caoyuan9642 0:6cb2eaf8b133 430 /*Fully pull-over*/
caoyuan9642 0:6cb2eaf8b133 431 slewState = AXIS_NOT_SLEWING;
caoyuan9642 0:6cb2eaf8b133 432 stepper->stop();
caoyuan9642 0:6cb2eaf8b133 433 currentSpeed = 0;
caoyuan9642 0:6cb2eaf8b133 434 }
caoyuan9642 0:6cb2eaf8b133 435
caoyuan9642 0:6cb2eaf8b133 436 if (useCorrection)
caoyuan9642 0:6cb2eaf8b133 437 {
caoyuan9642 0:6cb2eaf8b133 438 // Switch mode
caoyuan9642 0:6cb2eaf8b133 439 correction_mode();
caoyuan9642 0:6cb2eaf8b133 440 double correctionSpeed = TelescopeConfiguration::getDouble(
caoyuan9642 0:6cb2eaf8b133 441 "correction_speed_sidereal") * sidereal_speed;
caoyuan9642 0:6cb2eaf8b133 442 /*Use correction to goto the final angle with high resolution*/
caoyuan9642 0:6cb2eaf8b133 443 angleDeg = getAngleDeg();
caoyuan9642 0:6cb2eaf8b133 444 debug_if(AXIS_DEBUG, "%s: correct from %f to %f deg\n", axisName,
caoyuan9642 0:6cb2eaf8b133 445 angleDeg, dest); // TODO: DEBUG
caoyuan9642 0:6cb2eaf8b133 446
caoyuan9642 0:6cb2eaf8b133 447 double diff = remainder(angleDeg - dest, 360.0);
caoyuan9642 0:6cb2eaf8b133 448 if (diff > TelescopeConfiguration::getDouble("max_correction_angle"))
caoyuan9642 0:6cb2eaf8b133 449 {
caoyuan9642 0:6cb2eaf8b133 450 debug(
caoyuan9642 0:6cb2eaf8b133 451 "%s: correction too large: %f. Check hardware configuration.\n",
caoyuan9642 0:6cb2eaf8b133 452 axisName, diff);
caoyuan9642 0:6cb2eaf8b133 453 stop();
caoyuan9642 0:6cb2eaf8b133 454 idle_mode();
caoyuan9642 0:6cb2eaf8b133 455 currentSpeed = 0;
caoyuan9642 0:6cb2eaf8b133 456 slew_finish_state = FINISH_EMERG_STOPPED;
caoyuan9642 0:6cb2eaf8b133 457 status = AXIS_STOPPED;
caoyuan9642 0:6cb2eaf8b133 458 return;
caoyuan9642 0:6cb2eaf8b133 459 }
caoyuan9642 0:6cb2eaf8b133 460
caoyuan9642 0:6cb2eaf8b133 461 int nTry = 3; // Try 3 corrections at most
caoyuan9642 0:6cb2eaf8b133 462 while (--nTry
caoyuan9642 0:6cb2eaf8b133 463 && fabsf(diff)
caoyuan9642 0:6cb2eaf8b133 464 > TelescopeConfiguration::getDouble(
caoyuan9642 0:6cb2eaf8b133 465 "correction_tolerance"))
caoyuan9642 0:6cb2eaf8b133 466 {
caoyuan9642 0:6cb2eaf8b133 467 /*Determine correction direction and time*/
caoyuan9642 0:6cb2eaf8b133 468 sd = (diff > 0.0) ? STEP_BACKWARD : STEP_FORWARD;
caoyuan9642 0:6cb2eaf8b133 469
caoyuan9642 0:6cb2eaf8b133 470 /*Perform correction*/
caoyuan9642 0:6cb2eaf8b133 471 currentSpeed = stepper->setFrequency(stepsPerDeg * correctionSpeed)
caoyuan9642 0:6cb2eaf8b133 472 / stepsPerDeg; // Set and update actual speed
caoyuan9642 0:6cb2eaf8b133 473
caoyuan9642 0:6cb2eaf8b133 474 int correctionTime_ms = (int) (fabs(diff) / currentSpeed * 1000); // Use the accurate speed for calculating time
caoyuan9642 0:6cb2eaf8b133 475
caoyuan9642 0:6cb2eaf8b133 476 debug_if(AXIS_DEBUG,
caoyuan9642 0:6cb2eaf8b133 477 "%s: correction: from %f to %f deg. time=%d ms\n", axisName,
caoyuan9642 0:6cb2eaf8b133 478 angleDeg, dest, correctionTime_ms); //TODO: DEBUG
caoyuan9642 0:6cb2eaf8b133 479 if (correctionTime_ms
caoyuan9642 0:6cb2eaf8b133 480 < TelescopeConfiguration::getInt("min_correction_time"))
caoyuan9642 0:6cb2eaf8b133 481 {
caoyuan9642 0:6cb2eaf8b133 482 break;
caoyuan9642 0:6cb2eaf8b133 483 }
caoyuan9642 0:6cb2eaf8b133 484
caoyuan9642 0:6cb2eaf8b133 485 /*Start, wait, stop*/
caoyuan9642 0:6cb2eaf8b133 486 stepper->start(sd);
caoyuan9642 0:6cb2eaf8b133 487 uint32_t flags = osThreadFlagsWait(AXIS_EMERGE_STOP_SIGNAL,
caoyuan9642 0:6cb2eaf8b133 488 osFlagsWaitAny, correctionTime_ms);
caoyuan9642 0:6cb2eaf8b133 489 stepper->stop();
caoyuan9642 0:6cb2eaf8b133 490 if (flags != osFlagsErrorTimeout)
caoyuan9642 0:6cb2eaf8b133 491 {
caoyuan9642 0:6cb2eaf8b133 492 // Emergency stop!
caoyuan9642 0:6cb2eaf8b133 493 slew_finish_state = FINISH_EMERG_STOPPED;
caoyuan9642 0:6cb2eaf8b133 494 goto emerge_stop2;
caoyuan9642 0:6cb2eaf8b133 495 }
caoyuan9642 0:6cb2eaf8b133 496
caoyuan9642 0:6cb2eaf8b133 497 angleDeg = getAngleDeg();
caoyuan9642 0:6cb2eaf8b133 498 diff = remainder(angleDeg - dest, 360.0);
caoyuan9642 0:6cb2eaf8b133 499 }
caoyuan9642 0:6cb2eaf8b133 500
caoyuan9642 0:6cb2eaf8b133 501 if (!nTry)
caoyuan9642 0:6cb2eaf8b133 502 {
caoyuan9642 0:6cb2eaf8b133 503 debug("%s: correction failed. Check hardware configuration.\n",
caoyuan9642 0:6cb2eaf8b133 504 axisName);
caoyuan9642 0:6cb2eaf8b133 505 }
caoyuan9642 0:6cb2eaf8b133 506
caoyuan9642 0:6cb2eaf8b133 507 debug_if(AXIS_DEBUG, "%s: correction finished: %f deg\n", axisName,
caoyuan9642 0:6cb2eaf8b133 508 angleDeg); //TODO:DEBUG
caoyuan9642 0:6cb2eaf8b133 509 }
caoyuan9642 0:6cb2eaf8b133 510 emerge_stop2:
caoyuan9642 0:6cb2eaf8b133 511 // Set status to stopped
caoyuan9642 0:6cb2eaf8b133 512 currentSpeed = 0;
caoyuan9642 0:6cb2eaf8b133 513 status = AXIS_STOPPED;
caoyuan9642 0:6cb2eaf8b133 514 idle_mode();
caoyuan9642 0:6cb2eaf8b133 515 }
caoyuan9642 0:6cb2eaf8b133 516
caoyuan9642 0:6cb2eaf8b133 517 void Axis::track(axisrotdir_t dir)
caoyuan9642 0:6cb2eaf8b133 518 {
caoyuan9642 0:6cb2eaf8b133 519 track_mode();
caoyuan9642 0:6cb2eaf8b133 520 if (trackSpeed != 0 && dir != AXIS_ROTATE_STOP)
caoyuan9642 0:6cb2eaf8b133 521 {
caoyuan9642 0:6cb2eaf8b133 522 stepdir_t sd =
caoyuan9642 0:6cb2eaf8b133 523 (dir == AXIS_ROTATE_POSITIVE) ? STEP_FORWARD : STEP_BACKWARD;
caoyuan9642 0:6cb2eaf8b133 524 currentSpeed = stepper->setFrequency(trackSpeed * stepsPerDeg)
caoyuan9642 0:6cb2eaf8b133 525 / stepsPerDeg;
caoyuan9642 0:6cb2eaf8b133 526 currentDirection = dir;
caoyuan9642 0:6cb2eaf8b133 527 stepper->start(sd);
caoyuan9642 0:6cb2eaf8b133 528 }
caoyuan9642 0:6cb2eaf8b133 529 else
caoyuan9642 0:6cb2eaf8b133 530 {
caoyuan9642 0:6cb2eaf8b133 531 // For DEC axis
caoyuan9642 0:6cb2eaf8b133 532 dir = AXIS_ROTATE_STOP;
caoyuan9642 0:6cb2eaf8b133 533 trackSpeed = 0;
caoyuan9642 0:6cb2eaf8b133 534 currentSpeed = 0;
caoyuan9642 0:6cb2eaf8b133 535 currentDirection = AXIS_ROTATE_POSITIVE;
caoyuan9642 0:6cb2eaf8b133 536 }
caoyuan9642 0:6cb2eaf8b133 537 status = AXIS_TRACKING;
caoyuan9642 0:6cb2eaf8b133 538 Thread::signal_clr(
caoyuan9642 0:6cb2eaf8b133 539 AXIS_STOP_SIGNAL | AXIS_EMERGE_STOP_SIGNAL | AXIS_GUIDE_SIGNAL);
caoyuan9642 0:6cb2eaf8b133 540 // Empty the guide queue
caoyuan9642 0:6cb2eaf8b133 541 while (!guide_queue.empty())
caoyuan9642 0:6cb2eaf8b133 542 guide_queue.get();
caoyuan9642 0:6cb2eaf8b133 543
caoyuan9642 0:6cb2eaf8b133 544 while (true)
caoyuan9642 0:6cb2eaf8b133 545 {
caoyuan9642 0:6cb2eaf8b133 546 // Now we wait for SOMETHING to happen - either STOP, EMERGE_STOP or GUIDE
caoyuan9642 0:6cb2eaf8b133 547 uint32_t flags = osThreadFlagsWait(
caoyuan9642 0:6cb2eaf8b133 548 AXIS_GUIDE_SIGNAL | AXIS_STOP_SIGNAL | AXIS_EMERGE_STOP_SIGNAL,
caoyuan9642 0:6cb2eaf8b133 549 osFlagsWaitAny, osWaitForever);
caoyuan9642 0:6cb2eaf8b133 550 if ((flags & osFlagsError) == 0) // has flag
caoyuan9642 0:6cb2eaf8b133 551 {
caoyuan9642 0:6cb2eaf8b133 552 if (flags & (AXIS_STOP_SIGNAL | AXIS_EMERGE_STOP_SIGNAL))
caoyuan9642 0:6cb2eaf8b133 553 {
caoyuan9642 0:6cb2eaf8b133 554 // We stop tracking
caoyuan9642 0:6cb2eaf8b133 555 break;
caoyuan9642 0:6cb2eaf8b133 556 }
caoyuan9642 0:6cb2eaf8b133 557 else if (flags & AXIS_GUIDE_SIGNAL)
caoyuan9642 0:6cb2eaf8b133 558 {
caoyuan9642 0:6cb2eaf8b133 559 bool stopped = false;
caoyuan9642 0:6cb2eaf8b133 560 // Guide. Process all commands in the queue
caoyuan9642 0:6cb2eaf8b133 561 while (true)
caoyuan9642 0:6cb2eaf8b133 562 {
caoyuan9642 0:6cb2eaf8b133 563 osEvent evt = guide_queue.get(0); // try to get a message
caoyuan9642 0:6cb2eaf8b133 564 if (evt.status == osEventMessage)
caoyuan9642 0:6cb2eaf8b133 565 {
caoyuan9642 0:6cb2eaf8b133 566 int guideTime_ms = (int) evt.value.p;
caoyuan9642 0:6cb2eaf8b133 567 if (guideTime_ms == 0)
caoyuan9642 0:6cb2eaf8b133 568 continue; // Nothing to guide
caoyuan9642 0:6cb2eaf8b133 569 // Determine guide direction
caoyuan9642 0:6cb2eaf8b133 570 axisrotdir_t guide_dir =
caoyuan9642 0:6cb2eaf8b133 571 (guideTime_ms > 0) ?
caoyuan9642 0:6cb2eaf8b133 572 AXIS_ROTATE_POSITIVE :
caoyuan9642 0:6cb2eaf8b133 573 AXIS_ROTATE_NEGATIVE;
caoyuan9642 0:6cb2eaf8b133 574
caoyuan9642 0:6cb2eaf8b133 575 double newSpeed =
caoyuan9642 0:6cb2eaf8b133 576 (guide_dir == currentDirection) ?
caoyuan9642 0:6cb2eaf8b133 577 trackSpeed + guideSpeed :
caoyuan9642 0:6cb2eaf8b133 578 trackSpeed - guideSpeed; /*Determine speed in the original direction (currentDirection)*/
caoyuan9642 0:6cb2eaf8b133 579
caoyuan9642 0:6cb2eaf8b133 580 // Clamp to maximum guide time
caoyuan9642 0:6cb2eaf8b133 581 guideTime_ms = abs(guideTime_ms);
caoyuan9642 0:6cb2eaf8b133 582 if (guideTime_ms
caoyuan9642 0:6cb2eaf8b133 583 > TelescopeConfiguration::getInt(
caoyuan9642 0:6cb2eaf8b133 584 "max_guide_time"))
caoyuan9642 0:6cb2eaf8b133 585 {
caoyuan9642 0:6cb2eaf8b133 586 debug("Axis: Guiding time too long: %d ms\n",
caoyuan9642 0:6cb2eaf8b133 587 abs(guideTime_ms));
caoyuan9642 0:6cb2eaf8b133 588 guideTime_ms = TelescopeConfiguration::getInt(
caoyuan9642 0:6cb2eaf8b133 589 "max_guide_time");
caoyuan9642 0:6cb2eaf8b133 590 }
caoyuan9642 0:6cb2eaf8b133 591
caoyuan9642 0:6cb2eaf8b133 592 bool dirswitch = false;
caoyuan9642 0:6cb2eaf8b133 593 if (newSpeed > 0)
caoyuan9642 0:6cb2eaf8b133 594 {
caoyuan9642 0:6cb2eaf8b133 595 currentSpeed = stepper->setFrequency(
caoyuan9642 0:6cb2eaf8b133 596 newSpeed * stepsPerDeg) / stepsPerDeg; //set and update accurate speed
caoyuan9642 0:6cb2eaf8b133 597 if (trackSpeed == 0)
caoyuan9642 0:6cb2eaf8b133 598 {
caoyuan9642 0:6cb2eaf8b133 599 // For DEC, we also need to start the motor
caoyuan9642 0:6cb2eaf8b133 600 stepper->start(STEP_FORWARD); // Reverse direction
caoyuan9642 0:6cb2eaf8b133 601 }
caoyuan9642 0:6cb2eaf8b133 602 }
caoyuan9642 0:6cb2eaf8b133 603 else if (newSpeed < 0)
caoyuan9642 0:6cb2eaf8b133 604 {
caoyuan9642 0:6cb2eaf8b133 605 //
caoyuan9642 0:6cb2eaf8b133 606 stepper->stop();
caoyuan9642 0:6cb2eaf8b133 607 currentSpeed = stepper->setFrequency(
caoyuan9642 0:6cb2eaf8b133 608 -newSpeed * stepsPerDeg) / stepsPerDeg; //set and update accurate speed
caoyuan9642 0:6cb2eaf8b133 609 stepper->start(
caoyuan9642 0:6cb2eaf8b133 610 (currentDirection == AXIS_ROTATE_POSITIVE) ?
caoyuan9642 0:6cb2eaf8b133 611 STEP_BACKWARD : STEP_FORWARD); // Reverse direction
caoyuan9642 0:6cb2eaf8b133 612 dirswitch = true;
caoyuan9642 0:6cb2eaf8b133 613 }
caoyuan9642 0:6cb2eaf8b133 614 else
caoyuan9642 0:6cb2eaf8b133 615 {
caoyuan9642 0:6cb2eaf8b133 616 // newSpeed == 0, just stop the motor
caoyuan9642 0:6cb2eaf8b133 617 currentSpeed = 0;
caoyuan9642 0:6cb2eaf8b133 618 stepper->stop();
caoyuan9642 0:6cb2eaf8b133 619 dirswitch = true; // Make sure to recover the original speed
caoyuan9642 0:6cb2eaf8b133 620 }
caoyuan9642 0:6cb2eaf8b133 621
caoyuan9642 0:6cb2eaf8b133 622 uint32_t flags = osThreadFlagsWait(
caoyuan9642 0:6cb2eaf8b133 623 AXIS_STOP_SIGNAL | AXIS_EMERGE_STOP_SIGNAL,
caoyuan9642 0:6cb2eaf8b133 624 osFlagsWaitAny, guideTime_ms);
caoyuan9642 0:6cb2eaf8b133 625 if (flags != osFlagsErrorTimeout)
caoyuan9642 0:6cb2eaf8b133 626 {
caoyuan9642 0:6cb2eaf8b133 627 //break and stop;
caoyuan9642 0:6cb2eaf8b133 628 stopped = true;
caoyuan9642 0:6cb2eaf8b133 629 break;
caoyuan9642 0:6cb2eaf8b133 630 }
caoyuan9642 0:6cb2eaf8b133 631 if (dirswitch || trackSpeed == 0)
caoyuan9642 0:6cb2eaf8b133 632 {
caoyuan9642 0:6cb2eaf8b133 633 stepper->stop();
caoyuan9642 0:6cb2eaf8b133 634 // Restart the motor in original direction
caoyuan9642 0:6cb2eaf8b133 635 if (trackSpeed != 0)
caoyuan9642 0:6cb2eaf8b133 636 {
caoyuan9642 0:6cb2eaf8b133 637 stepper->start(
caoyuan9642 0:6cb2eaf8b133 638 (currentDirection
caoyuan9642 0:6cb2eaf8b133 639 == AXIS_ROTATE_POSITIVE) ?
caoyuan9642 0:6cb2eaf8b133 640 STEP_FORWARD : STEP_BACKWARD); // Reverse direction
caoyuan9642 0:6cb2eaf8b133 641 }
caoyuan9642 0:6cb2eaf8b133 642 }
caoyuan9642 0:6cb2eaf8b133 643 // Restore to normal speed
caoyuan9642 0:6cb2eaf8b133 644 if (trackSpeed != 0)
caoyuan9642 0:6cb2eaf8b133 645 currentSpeed = stepper->setFrequency(
caoyuan9642 0:6cb2eaf8b133 646 trackSpeed * stepsPerDeg) / stepsPerDeg;
caoyuan9642 0:6cb2eaf8b133 647 else
caoyuan9642 0:6cb2eaf8b133 648 currentSpeed = 0;
caoyuan9642 0:6cb2eaf8b133 649
caoyuan9642 0:6cb2eaf8b133 650 // End guiding
caoyuan9642 0:6cb2eaf8b133 651 }
caoyuan9642 0:6cb2eaf8b133 652 else
caoyuan9642 0:6cb2eaf8b133 653 {
caoyuan9642 0:6cb2eaf8b133 654 // No more message to get. Break out
caoyuan9642 0:6cb2eaf8b133 655 break;
caoyuan9642 0:6cb2eaf8b133 656 }
caoyuan9642 0:6cb2eaf8b133 657 }
caoyuan9642 0:6cb2eaf8b133 658 if (stopped)
caoyuan9642 0:6cb2eaf8b133 659 {
caoyuan9642 0:6cb2eaf8b133 660 //Complete break out
caoyuan9642 0:6cb2eaf8b133 661 break;
caoyuan9642 0:6cb2eaf8b133 662 }
caoyuan9642 0:6cb2eaf8b133 663 }
caoyuan9642 0:6cb2eaf8b133 664 }
caoyuan9642 0:6cb2eaf8b133 665 }
caoyuan9642 0:6cb2eaf8b133 666
caoyuan9642 0:6cb2eaf8b133 667 // Stop
caoyuan9642 0:6cb2eaf8b133 668 currentSpeed = 0;
caoyuan9642 0:6cb2eaf8b133 669 stepper->stop();
caoyuan9642 0:6cb2eaf8b133 670 status = AXIS_STOPPED;
caoyuan9642 0:6cb2eaf8b133 671 idle_mode();
caoyuan9642 0:6cb2eaf8b133 672 }
caoyuan9642 0:6cb2eaf8b133 673
caoyuan9642 0:6cb2eaf8b133 674