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