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