Yuan Cao / pushtogo

Telescope Control Library

Dependents:   PushToGo-F429

PEC.cpp@10:e356188d208e, 2018-09-14 (annotated)

Committer:
caoyu@caoyuan9642-desktop.MIT.EDU
Date:
Fri Sep 14 01:30:55 2018 -0400
Revision:
10:e356188d208e
Parent:
8:21a33760bf10
Child:
12:35f5cb75743e
Resolved circular dependency. Slewing rate now only applies to indefinite slews. GOTO use goto_slew_speed in the configuration.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
caoyu@caoyuan9642-desktop.MIT.EDU 6:85b9d9a3903a 1 /*
caoyu@caoyuan9642-desktop.MIT.EDU 6:85b9d9a3903a 2 * PEC.cpp
caoyu@caoyuan9642-desktop.MIT.EDU 6:85b9d9a3903a 3 *
caoyu@caoyuan9642-desktop.MIT.EDU 6:85b9d9a3903a 4 * Created on: Sep 9, 2018
caoyu@caoyuan9642-desktop.MIT.EDU 6:85b9d9a3903a 5 * Author: caoyu
caoyu@caoyuan9642-desktop.MIT.EDU 6:85b9d9a3903a 6 */
caoyu@caoyuan9642-desktop.MIT.EDU 6:85b9d9a3903a 7
caoyu@caoyuan9642-desktop.MIT.EDU 10:e356188d208e 8 #include "PEC.h"
caoyu@caoyuan9642-desktop.MIT.EDU 10:e356188d208e 9 #include "Axis.h"
caoyu@caoyuan9642-desktop.MIT.EDU 10:e356188d208e 10 #include "TelescopeConfiguration.h"
caoyu@caoyuan9642-desktop.MIT.EDU 6:85b9d9a3903a 11
caoyu@caoyuan9642-desktop.MIT.EDU 10:e356188d208e 12 PEC::PEC(Axis &a) :
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 13 axis(a), enabled(false), indexOffset(0) {
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 14 thread = new Thread(osPriorityAboveNormal,
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 15 OS_STACK_SIZE, NULL, "PEC Thread");
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 16 thread->start(callback(this, &PEC::task));
caoyu@caoyuan9642-desktop.MIT.EDU 6:85b9d9a3903a 17
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 18 granularity = TelescopeConfiguration::getInt("pec_granularity");
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 19 pecData = new float[granularity];
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 20 for (int i = 0; i < granularity; i++) {
caoyu@caoyuan9642-desktop.MIT.EDU 10:e356188d208e 21 pecData[i] = 0.0f;
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 22 }
caoyu@caoyuan9642-desktop.MIT.EDU 6:85b9d9a3903a 23 }
caoyu@caoyuan9642-desktop.MIT.EDU 6:85b9d9a3903a 24
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 25 void PEC::task() {
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 26
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 27 Timer t;
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 28 t.start();
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 29 while (true) {
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 30 if (enabled) {
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 31 double degTeeth = 360.0
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 32 / TelescopeConfiguration::getDouble("worm_teeth"); // Degree per teeth
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 33 double period = degTeeth
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 34 / (axis.getTrackSpeedSidereal() * sidereal_speed); // Worm rotation period
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 35
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 36 int currentIndex = (int) round(
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 37 remainder((axis.getAngleDeg() - indexOffset), degTeeth)
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 38 * granularity);
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 39 if (currentIndex < 0)
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 40 currentIndex += granularity;
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 41
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 42 double interval = period / granularity; // Time to wait between executing each step
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 43
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 44 uint64_t startTimeUs = t.read_high_resolution_us();
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 45 uint64_t count = 0;
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 46 double intervalUs = interval * 1000000.0;
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 47
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 48 while (enabled) {
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 49 float correction = pecData[currentIndex]; // Correction in arcseconds
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 50 if (++currentIndex >= granularity)
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 51 currentIndex -= granularity;
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 52
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 53 if (correction != 0.0f) {
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 54 // Calculate guide time in milliseconds
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 55 // 3.6 = 3600 (arcsec/degree) * 1000 (ms/s)
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 56 int guidetime = correction / float(axis.guideSpeed) / 3.6f;
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 57
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 58 // Positive guidetime means needs to correct towards east
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 59 // Negative guidetime means needs to correct towards west
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 60 axis.guide(AXIS_ROTATE_NEGATIVE, guidetime);
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 61 }
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 62
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 63 // Make sure mean interval is very accurate
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 64 count++;
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 65 while ((t.read_high_resolution_us() - startTimeUs)
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 66 < intervalUs * count) {
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 67 Thread::wait(1);
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 68 }
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 69 }
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 70 }
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 71 Thread::wait(10);
caoyu@caoyuan9642-desktop.MIT.EDU 8:21a33760bf10 72 }
caoyu@caoyuan9642-desktop.MIT.EDU 6:85b9d9a3903a 73 }