Yuan Cao / pushtogo

Telescope Control Library

Dependents:   PushToGo-F429

PEC.cpp@8:21a33760bf10, 2018-09-09 (annotated)

Committer:
caoyu@caoyuan9642-desktop.MIT.EDU
Date:
Sun Sep 09 17:31:20 2018 -0400
Revision:
8:21a33760bf10
Parent:
6:85b9d9a3903a
Child:
10:e356188d208e
Implementing PEC module

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