Telescope Control Library
Diff: PEC.cpp
- Revision:
- 8:21a33760bf10
- Parent:
- 6:85b9d9a3903a
- Child:
- 10:e356188d208e
diff -r 9daa589c92c8 -r 21a33760bf10 PEC.cpp --- a/PEC.cpp Sun Sep 09 16:28:03 2018 -0400 +++ b/PEC.cpp Sun Sep 09 17:31:20 2018 -0400 @@ -7,12 +7,65 @@ #include <PEC.h> -PEC::PEC() { - // TODO Auto-generated constructor stub +PEC::PEC(Axis a) : + axis(a), enabled(false), indexOffset(0) { + thread = new Thread(osPriorityAboveNormal, + OS_STACK_SIZE, NULL, "PEC Thread"); + thread->start(callback(this, &PEC::task)); + granularity = TelescopeConfiguration::getInt("pec_granularity"); + pecData = new float[granularity]; + for (int i = 0; i < granularity; i++) { + pecData = 0.0f; + } } -PEC::~PEC() { - // TODO Auto-generated destructor stub +void PEC::task() { + + Timer t; + t.start(); + while (true) { + if (enabled) { + double degTeeth = 360.0 + / TelescopeConfiguration::getDouble("worm_teeth"); // Degree per teeth + double period = degTeeth + / (axis.getTrackSpeedSidereal() * sidereal_speed); // Worm rotation period + + int currentIndex = (int) round( + remainder((axis.getAngleDeg() - indexOffset), degTeeth) + * granularity); + if (currentIndex < 0) + currentIndex += granularity; + + double interval = period / granularity; // Time to wait between executing each step + + uint64_t startTimeUs = t.read_high_resolution_us(); + uint64_t count = 0; + double intervalUs = interval * 1000000.0; + + while (enabled) { + float correction = pecData[currentIndex]; // Correction in arcseconds + if (++currentIndex >= granularity) + currentIndex -= granularity; + + if (correction != 0.0f) { + // Calculate guide time in milliseconds + // 3.6 = 3600 (arcsec/degree) * 1000 (ms/s) + int guidetime = correction / float(axis.guideSpeed) / 3.6f; + + // Positive guidetime means needs to correct towards east + // Negative guidetime means needs to correct towards west + axis.guide(AXIS_ROTATE_NEGATIVE, guidetime); + } + + // Make sure mean interval is very accurate + count++; + while ((t.read_high_resolution_us() - startTimeUs) + < intervalUs * count) { + Thread::wait(1); + } + } + } + Thread::wait(10); + } } -