RSeries astromech firmware
DomeSensorRing.h
Go to the documentation of this file.
1 #ifndef DomeSensorRing_h
2 #define DomeSensorRing_h
3 
4 #include "ReelTwo.h"
5 #include "core/SetupEvent.h"
6 #include "core/MedianSampleBuffer.h"
7 
8 #ifdef USE_DOME_SENSOR_DEBUG
9 #define DOME_SENSOR_PRINT(s) DEBUG_PRINT(s)
10 #define DOME_SENSOR_PRINTLN(s) DEBUG_PRINTLN(s)
11 #define DOME_SENSOR_PRINT_HEX(s) DEBUG_PRINT_HEX(s)
12 #define DOME_SENSOR_PRINTLN_HEX(s) DEBUG_PRINTLN_HEX(s)
13 #else
14 #define DOME_SENSOR_PRINT(s)
15 #define DOME_SENSOR_PRINTLN(s)
16 #define DOME_SENSOR_PRINT_HEX(s)
17 #define DOME_SENSOR_PRINTLN_HEX(s)
18 #endif
19 
20 #if defined(ARDUINO_ARCH_RP2040)
21 static constexpr uint8_t SENSORS[] {27, 28, 29, 3, 4, 6, 25, 24, 26};
22 // static constexpr uint8_t SENSORS[] {27, 28, 29, 3, 4, 6, 25, 24, 26};
23 #elif defined(ESP32) && defined(PIN_NEOPIXEL)
24 static constexpr uint8_t SENSORS[] {27, 25, 26, 13, 12, 14, 33, 4, 15};
25 #elif defined(ESP32)
26 static constexpr uint8_t SENSORS[] {18, 19, 21, 22, 32, 33, 25, 26, 27};
27 #endif
28 
29 class DomeSensorRing : public SetupEvent
30 {
31 public:
32  virtual void setup() override
33  {
34  #if defined(ESP32) || defined(ARDUINO_ARCH_RP2040)
35  for (auto i = 0; i < SizeOfArray(SENSORS); i++)
36  pinMode(SENSORS[i], INPUT_PULLUP);
37  #elif defined(DOME_CONTROLLER_BOARD)
38  /* Dome sensor is connected to A0-A8 */
39  for (uint8_t pin = A0; pin <= A8; pin++)
40  pinMode(pin, INPUT_PULLUP);
41  #else
42  /* Dome sensor is connected to 2-10 */
43  for (uint8_t pin = 2; pin <= 10; pin++)
44  pinMode(pin, INPUT_PULLUP);
45  #endif
46  }
47 
48  unsigned readSensors()
49  {
50  #if defined(ARDUINO_ARCH_RP2040)
51  unsigned mask = 0;
52  noInterrupts();
53  for (auto i = 0; i < SizeOfArray(SENSORS); i++)
54  mask |= (digitalRead(SENSORS[i]) << i);
55  interrupts();
56  #elif defined(ESP32)
57  unsigned mask = 0;
58  portDISABLE_INTERRUPTS();
59  for (auto i = 0; i < SizeOfArray(SENSORS); i++)
60  mask |= (digitalRead(SENSORS[i]) << i);
61  portENABLE_INTERRUPTS();
62  #elif defined(DOME_CONTROLLER_BOARD)
63  /* Dome controller board reading sensors directly */
64  unsigned pinF;
65  unsigned pinK;
66  cli();
67  pinF = PINF;
68  pinK = PINK;
69  sei();
70  unsigned mask =
71  ((pinF & (1<<0)) | /* A0 - 00000000X */
72  (pinF & (1<<1)) | /* A1 - 0000000X0 */
73  (pinF & (1<<2)) | /* A2 - 000000X00 */
74  (pinF & (1<<3)) | /* A3 - 00000X000 */
75  (pinF & (1<<4)) | /* A4 - 0000X0000 */
76  (pinF & (1<<5)) | /* A5 - 000X00000 */
77  (pinF & (1<<6)) | /* A6 - 00X000000 */
78  (pinF & (1<<7)) | /* A7 - 0X0000000 */
79  ((pinK & (1<<0)) << 8) /* A8 - X00000000 */
80  );
81  #elif defined(DOME_SENSOR_RING_FULL_SIZE)
82  // Sensors are oriented right to left
83  cli();
84  unsigned pinD { PIND };
85  unsigned pinB { PINB };
86  sei();
87  unsigned mask {
88  (((pinB >> 2) & 1) << 0) | /* D10 */
89  (((pinB >> 1) & 1) << 1) | /* D9 */
90  (((pinB >> 0) & 1) << 2) | /* D8 */
91  (((pinD >> 7) & 1) << 3) | /* D7 */
92  (((pinD >> 6) & 1) << 4) | /* D6 */
93  (((pinD >> 5) & 1) << 5) | /* D5 */
94  (((pinD >> 4) & 1) << 6) | /* D4 */
95  (((pinD >> 3) & 1) << 7) | /* D3 */
96  (((pinD >> 2) & 1) << 8) /* D2 */
97  };
98  #else
99  // Sensors are oriented left to right
100  cli();
101  unsigned pinD { PIND };
102  unsigned pinB { PINB };
103  sei();
104  unsigned mask {uint16_t(pinD >> 2) | uint16_t((pinB & 0b111) << 6)};
105  #endif
106  return mask;
107  }
108 
109  short getAngle()
110  {
111  auto mask = readSensors();
112  if (mask != fLastMask || fSampleCount < 6)
113  {
114  #ifdef USE_DOME_SENSOR_DEBUG
115  if (fSampleCount > 0 && fLastMask != ~0u && countChangedBits(mask, fLastMask) > 1)
116  {
117  DOME_SENSOR_PRINTLN();
118  DOME_SENSOR_PRINTLN();
119  DOME_SENSOR_PRINT("fLastMask=");
120  DOME_SENSOR_PRINTLN_HEX(fLastMask);
121  // More than one bit changed bad state
122  DOME_SENSOR_PRINTLN("BAD DOME POSITION STATE");
123  DOME_SENSOR_PRINTLN();
124  }
125  #endif
126  auto currentAngle = getDomeAngle(mask);
127  if (currentAngle != -1)
128  {
129  fSamples.append(currentAngle);
130  if (fSampleCount < 6)
131  {
132  // Return the raw angle
133  fLastPosition = currentAngle;
134  fSampleCount++;
135  }
136  else
137  {
138  // Return the filtered angle
139  fLastPosition = fSamples.median();
140  }
141  }
142  #ifdef USE_DOME_SENSOR_DEBUG
143  if (fSampleCount > 0)
144  {
145  DOME_SENSOR_PRINT(" ");
146  DOME_SENSOR_PRINT('\r');
147  DOME_SENSOR_PRINT_HEX(mask);
148  DOME_SENSOR_PRINT(" ");
149  printBinary(mask, 9);
150  DOME_SENSOR_PRINT(" - ");
151  DOME_SENSOR_PRINT(mask);
152  DOME_SENSOR_PRINT(" - ");
153  DOME_SENSOR_PRINT(countChangedBits(mask, fLastMask));
154  DOME_SENSOR_PRINT(" - ");
155  DOME_SENSOR_PRINT(fLastPosition);
156  if (currentAngle == -1)
157  {
158  DOME_SENSOR_PRINT(" *BAD*");
159  }
160  DOME_SENSOR_PRINT('\r');
161  }
162  #endif
163  fLastMask = mask;
164  }
165  return fLastPosition;
166  }
167 
168  inline bool ready()
169  {
170  return (fSampleCount > 1);
171  }
172 
173 private:
174  int fSampleCount = 0;
175  unsigned fLastMask = ~0;
176  short fLastPosition = -1;
177  MedianSampleBuffer<short, 5> fSamples;
178 
179 #ifdef USE_DOME_SENSOR_DEBUG
180  static unsigned countChangedBits(unsigned a, unsigned b)
181  {
182  unsigned n = 0;
183  for (unsigned i = 0; i < 9; i++)
184  {
185  if ((a & (1 << i)) != (b & (1 << i)))
186  n++;
187  }
188  return n;
189  }
190 
191  static void printBinary(unsigned num, unsigned places)
192  {
193  if (places)
194  printBinary(num >> 1, places-1);
195  DOME_SENSOR_PRINT((num & 1) ? '1' : '0');
196  }
197 #endif
198 
199  static short getDomeAngle(unsigned sensorMask)
200  {
201  static const short sDomeAngle[] PROGMEM = {
202  -1, 0, 40, 39, 80, 1, 79, 38, 120, 129, 41, 42, 119, 86, 78, 37, 160, 171, 169, 170, 81, 84, 82, 83,
203  159, 128, 126, 127, 118, 85, 77, 76, 200, 11, 211, 212, 209, 214, 210, 213, 121, 216, 124, 217, 122, 215, 123, 218,
204  199, 12, 168, -1, 166, -1, 167, -1, 158, -1, 125, -1, 117, -1, 116, 115, 240, 9, 51, 8, 251, 96, 252, -1,
205  249, -1, 254, -1, 250, 261, 253, 260, 161, 14, 256, -1, 164, -1, 257, -1, 162, -1, 255, -1, 163, -1, 258, 259,
206  239, 10, 52, -1, 208, -1, -1, -1, 206, 21, -1, -1, 207, 262, -1, -1, 198, 13, -1, -1, 165, 264, -1, 265,
207  157, 20, -1, -1, 156, 263, 155, 154, 280, 281, 49, 6, 91, 4, 48, 5, 291, 134, 136, 135, 292, -1, -1, -1,
208  289, 16, -1, 181, 294, -1, -1, -1, 290, -1, 301, 182, 293, 184, 300, 183, 201, 284, 54, -1, 296, -1, -1, 104,
209  204, -1, -1, 28, 297, 108, -1, 107, 202, 17, -1, -1, 295, 188, -1, 189, 203, -1, -1, 191, 298, 187, 299, 190,
210  279, 282, 50, 7, 92, 97, -1, -1, 248, -1, -1, -1, -1, -1, -1, -1, 246, 15, 61, 62, -1, 268, -1, 71,
211  247, -1, 302, -1, -1, 269, -1, 72, 238, 283, 53, -1, -1, -1, -1, 105, 205, -1, 304, 29, -1, 271, 305, 106,
212  197, 18, 60, -1, -1, 267, -1, 266, 196, 19, 303, 192, 195, 270, 194, 193, 320, 359, 321, 358, 89, 2, 46, 357,
213  131, 130, 44, 43, 88, 87, 45, 36, 331, 172, 174, 173, 176, 177, 175, 178, 332, -1, -1, -1, -1, -1, -1, 75,
214  329, 328, 56, -1, -1, -1, 221, 220, 334, -1, -1, -1, -1, -1, -1, 219, 330, -1, -1, -1, 341, -1, 222, -1,
215  333, -1, 224, 225, 340, -1, 223, 114, 241, 326, 324, 325, 94, 95, -1, -1, 336, 141, -1, -1, -1, 142, 144, 143,
216  244, -1, -1, 64, -1, 348, 68, 67, 337, -1, 148, 149, -1, 151, 147, 150, 242, 327, 57, -1, -1, -1, -1, -1,
217  335, 22, 228, 31, -1, -1, 229, 32, 243, -1, -1, 65, -1, 349, 231, 66, 338, -1, 227, 226, 339, 152, 230, 153,
218  319, 318, 322, 317, 90, 3, 47, 356, 132, 133, 137, 138, -1, -1, -1, 35, 288, -1, -1, 180, -1, -1, -1, 179,
219  -1, -1, -1, -1, -1, 185, -1, 74, 286, 285, 55, -1, 101, -1, 102, 103, -1, 24, 308, 27, -1, 109, 111, 110,
220  287, -1, -1, -1, 342, 351, -1, 352, -1, 25, 309, 26, -1, 186, 112, 113, 278, 277, 323, 316, 93, 98, -1, 355,
221  -1, 140, -1, 139, -1, -1, 145, 34, 245, -1, -1, 63, 344, 347, 69, 70, -1, -1, 311, 312, 345, 346, 146, 73,
222  237, 276, 58, 315, 100, 99, -1, 354, -1, 23, 307, 30, -1, 272, 306, 33, 236, 275, 59, 314, 343, 350, 232, 353,
223  235, 274, 310, 313, 234, 273, 233, -1
224  };
225  return pgm_read_word(&sDomeAngle[sensorMask & 0x1FF]);
226  }
227 };
228 
229 #endif
ReelTwo.h
SetupEvent.h
SetupEvent
Base class for all devices that require setup that cannot happen in the constructor....
Definition: SetupEvent.h:15
DOME_SENSOR_PRINTLN_HEX
#define DOME_SENSOR_PRINTLN_HEX(s)
Definition: DomeSensorRing.h:17
DomeSensorRing::setup
virtual void setup() override
Subclasses must implement this function to perform any necessary setup that cannot happen in the cons...
Definition: DomeSensorRing.h:32
DomeSensorRing::readSensors
unsigned readSensors()
Definition: DomeSensorRing.h:48
MedianSampleBuffer::append
void append(T val)
Definition: MedianSampleBuffer.h:21
DomeSensorRing
Definition: DomeSensorRing.h:29
DOME_SENSOR_PRINT_HEX
#define DOME_SENSOR_PRINT_HEX(s)
Definition: DomeSensorRing.h:16
DomeSensorRing::getAngle
short getAngle()
Definition: DomeSensorRing.h:109
MedianSampleBuffer< short, 5 >
DomeSensorRing::ready
bool ready()
Definition: DomeSensorRing.h:168
DOME_SENSOR_PRINT
#define DOME_SENSOR_PRINT(s)
Definition: DomeSensorRing.h:14
DOME_SENSOR_PRINTLN
#define DOME_SENSOR_PRINTLN(s)
Definition: DomeSensorRing.h:15
MedianSampleBuffer::median
T median()
Definition: MedianSampleBuffer.h:31
SizeOfArray
#define SizeOfArray(arr)
Definition: ReelTwo.h:213
MedianSampleBuffer.h