Telescope Control Library
PEC.cpp
- Committer:
- caoyu@caoyuan9642-desktop.MIT.EDU
- Date:
- 2018-09-24
- Revision:
- 19:fd854309cb4c
- Parent:
- 12:35f5cb75743e
File content as of revision 19:fd854309cb4c:
/* * PEC.cpp * * Created on: Sep 9, 2018 * Author: caoyu */ #include "PEC.h" #include "Axis.h" #include "TelescopeConfiguration.h" #include <math.h> 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[i] = 0.0f; } } 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) floor( 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); } }