Telescope Control Library
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?
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 | 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 | } |