Diff: PEC.cpp

Revision:

8:21a33760bf10

Parent:

6:85b9d9a3903a

Child:

10:e356188d208e

--- 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);
+	}
 }
-