Arnaud VALLEY / Mbed 2 deprecated Pinscape_Controller_V2_arnoz

Mirror with some correction

Dependencies:   mbed FastIO FastPWM USBDevice

Plunger/plunger.h@104:6e06e0f4b476, 2019-12-27 (annotated)

Committer:
mjr
Date:
Fri Dec 27 20:14:23 2019 +0000
Revision:
104:6e06e0f4b476
Parent:
101:755f44622abc
Child:
109:310ac82cbbee
AEAT-6012, TCD1103 updates

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mjr 82:4f6209cb5c33 1 // Plunger Sensor Interface
mjr 82:4f6209cb5c33 2 //
mjr 82:4f6209cb5c33 3 // This module defines the abstract interface to the plunger sensors.
mjr 82:4f6209cb5c33 4 // We support several different physical sensor types, so we need a
mjr 82:4f6209cb5c33 5 // common interface for use in the main code.
mjr 82:4f6209cb5c33 6 //
mjr 82:4f6209cb5c33 7 // In case it's helpful in developing code for new sensor types, I've
mjr 82:4f6209cb5c33 8 // measured the maximum instantaneous speed of a plunger at .175 inches
mjr 82:4f6209cb5c33 9 // per millisecond, or 4.46 mm/ms. (I measured that with an AEDR-8300;
mjr 82:4f6209cb5c33 10 // see that code for more details.)
mjr 82:4f6209cb5c33 11 //
mjr 82:4f6209cb5c33 12
mjr 82:4f6209cb5c33 13 #ifndef PLUNGER_H
mjr 82:4f6209cb5c33 14 #define PLUNGER_H
mjr 82:4f6209cb5c33 15
mjr 87:8d35c74403af 16 #include "config.h"
mjr 87:8d35c74403af 17
mjr 82:4f6209cb5c33 18 // Plunger reading with timestamp
mjr 82:4f6209cb5c33 19 struct PlungerReading
mjr 82:4f6209cb5c33 20 {
mjr 82:4f6209cb5c33 21 // Raw sensor reading, normalied to 0x0000..0xFFFF range
mjr 82:4f6209cb5c33 22 int pos;
mjr 82:4f6209cb5c33 23
mjr 82:4f6209cb5c33 24 // Rimestamp of reading, in microseconds, relative to an arbitrary
mjr 82:4f6209cb5c33 25 // zero point. Note that a 32-bit int can only represent about 71.5
mjr 82:4f6209cb5c33 26 // minutes worth of microseconds, so this value is only meaningful
mjr 82:4f6209cb5c33 27 // to compute a delta from other recent readings. As long as two
mjr 82:4f6209cb5c33 28 // readings are within 71.5 minutes of each other, the time difference
mjr 82:4f6209cb5c33 29 // calculated from the timestamps using 32-bit math will be correct
mjr 82:4f6209cb5c33 30 // *even if a rollover occurs* between the two readings, since the
mjr 82:4f6209cb5c33 31 // calculation is done mod 2^32-1.
mjr 82:4f6209cb5c33 32 uint32_t t;
mjr 82:4f6209cb5c33 33 };
mjr 82:4f6209cb5c33 34
mjr 82:4f6209cb5c33 35 class PlungerSensor
mjr 82:4f6209cb5c33 36 {
mjr 82:4f6209cb5c33 37 public:
mjr 86:e30a1f60f783 38 PlungerSensor(int nativeScale)
mjr 86:e30a1f60f783 39 {
mjr 86:e30a1f60f783 40 // use the joystick scale as our native scale by default
mjr 86:e30a1f60f783 41 this->nativeScale = nativeScale;
mjr 86:e30a1f60f783 42
mjr 86:e30a1f60f783 43 // figure the scaling factor
mjr 86:e30a1f60f783 44 scalingFactor = (65535UL*65536UL) / nativeScale;
mjr 86:e30a1f60f783 45
mjr 86:e30a1f60f783 46 // presume no jitter filter
mjr 86:e30a1f60f783 47 jfWindow = 0;
mjr 86:e30a1f60f783 48
mjr 86:e30a1f60f783 49 // initialize the jitter filter
mjr 86:e30a1f60f783 50 jfLo = jfHi = jfLast = 0;
mjr 91:ae9be42652bf 51
mjr 91:ae9be42652bf 52 // presume normal orientation
mjr 91:ae9be42652bf 53 reverseOrientation = false;
mjr 86:e30a1f60f783 54 }
mjr 82:4f6209cb5c33 55
mjr 82:4f6209cb5c33 56 // ---------- Abstract sensor interface ----------
mjr 82:4f6209cb5c33 57
mjr 82:4f6209cb5c33 58 // Initialize the physical sensor device. This is called at startup
mjr 82:4f6209cb5c33 59 // to set up the device for first use.
mjr 82:4f6209cb5c33 60 virtual void init() { }
mjr 82:4f6209cb5c33 61
mjr 82:4f6209cb5c33 62 // Auto-zero the plunger. Relative sensor types, such as quadrature
mjr 82:4f6209cb5c33 63 // sensors, can lose sync with the absolute position over time if they
mjr 82:4f6209cb5c33 64 // ever miss any motion. We can automatically correct for this by
mjr 82:4f6209cb5c33 65 // resetting to the park position after periods of inactivity. It's
mjr 82:4f6209cb5c33 66 // usually safe to assume that the plunger is at the park position if it
mjr 82:4f6209cb5c33 67 // hasn't moved in a long time, since the spring always returns it to
mjr 82:4f6209cb5c33 68 // that position when it isn't being manipulated. The main loop monitors
mjr 82:4f6209cb5c33 69 // for motion, and calls this after a long enough time goes by without
mjr 82:4f6209cb5c33 70 // seeing any movement. Sensor types that are inherently absolute
mjr 82:4f6209cb5c33 71 // (TSL1410, potentiometers) shouldn't do anything here.
mjr 82:4f6209cb5c33 72 virtual void autoZero() { }
mjr 82:4f6209cb5c33 73
mjr 82:4f6209cb5c33 74 // Is the sensor ready to take a reading? The optical sensor requires
mjr 82:4f6209cb5c33 75 // a fairly long time (2.5ms) to transfer the data for each reading, but
mjr 82:4f6209cb5c33 76 // this is done via DMA, so we can carry on other work while the transfer
mjr 82:4f6209cb5c33 77 // takes place. This lets us poll the sensor to see if it's still busy
mjr 82:4f6209cb5c33 78 // working on the current reading's data transfer.
mjr 82:4f6209cb5c33 79 virtual bool ready() { return true; }
mjr 82:4f6209cb5c33 80
mjr 82:4f6209cb5c33 81 // Read the sensor position, if possible. Returns true on success,
mjr 82:4f6209cb5c33 82 // false if it wasn't possible to take a reading. On success, fills
mjr 86:e30a1f60f783 83 // in 'r' with the current reading and timestamp and returns true.
mjr 86:e30a1f60f783 84 // Returns false if a reading couldn't be taken.
mjr 82:4f6209cb5c33 85 //
mjr 86:e30a1f60f783 86 // r.pos is set to the normalized position reading, and r.t is set to
mjr 86:e30a1f60f783 87 // the timestamp of the reading.
mjr 82:4f6209cb5c33 88 //
mjr 86:e30a1f60f783 89 // The normalized position is the sensor reading, corrected for jitter,
mjr 86:e30a1f60f783 90 // and adjusted to the abstract 0x0000..0xFFFF range.
mjr 86:e30a1f60f783 91 //
mjr 86:e30a1f60f783 92 // The timestamp is the time the sensor reading was taken, relative to
mjr 86:e30a1f60f783 93 // an arbitrary zero point. The arbitrary zero point makes this useful
mjr 86:e30a1f60f783 94 // only for calculating the time between readings. Note that the 32-bit
mjr 86:e30a1f60f783 95 // timestamp rolls over about every 71 minutes, so it should only be
mjr 86:e30a1f60f783 96 // used for time differences between readings taken fairly close together.
mjr 86:e30a1f60f783 97 // In practice, the higher level code only uses this for a few consecutive
mjr 86:e30a1f60f783 98 // readings to calculate (nearly) instantaneous velocities, so the time
mjr 86:e30a1f60f783 99 // spans are only tens of milliseconds.
mjr 82:4f6209cb5c33 100 //
mjr 82:4f6209cb5c33 101 // Timing requirements: for best results, readings should be taken
mjr 86:e30a1f60f783 102 // in well under 5ms. The release motion of the physical plunger
mjr 86:e30a1f60f783 103 // takes from 30ms to 50ms, so we need to collect samples much faster
mjr 86:e30a1f60f783 104 // than that to avoid aliasing during the bounce.
mjr 86:e30a1f60f783 105 bool read(PlungerReading &r)
mjr 86:e30a1f60f783 106 {
mjr 101:755f44622abc 107 // fail if the hardware scan isn't ready
mjr 101:755f44622abc 108 if (!ready())
mjr 101:755f44622abc 109 return false;
mjr 101:755f44622abc 110
mjr 86:e30a1f60f783 111 // get the raw reading
mjr 86:e30a1f60f783 112 if (readRaw(r))
mjr 86:e30a1f60f783 113 {
mjr 91:ae9be42652bf 114 // adjust for orientation
mjr 91:ae9be42652bf 115 r.pos = applyOrientation(r.pos);
mjr 91:ae9be42652bf 116
mjr 86:e30a1f60f783 117 // process it through the jitter filter
mjr 87:8d35c74403af 118 r.pos = jitterFilter(r.pos);
mjr 86:e30a1f60f783 119
mjr 86:e30a1f60f783 120 // adjust to the abstract scale via the scaling factor
mjr 86:e30a1f60f783 121 r.pos = uint16_t(uint32_t((scalingFactor * r.pos) + 32768) >> 16);
mjr 86:e30a1f60f783 122
mjr 86:e30a1f60f783 123 // success
mjr 86:e30a1f60f783 124 return true;
mjr 86:e30a1f60f783 125 }
mjr 86:e30a1f60f783 126 else
mjr 86:e30a1f60f783 127 {
mjr 86:e30a1f60f783 128 // no reading is available
mjr 86:e30a1f60f783 129 return false;
mjr 86:e30a1f60f783 130 }
mjr 86:e30a1f60f783 131 }
mjr 86:e30a1f60f783 132
mjr 86:e30a1f60f783 133 // Get a raw plunger reading. This gets the raw sensor reading with
mjr 86:e30a1f60f783 134 // timestamp, without jitter filtering and without any scale adjustment.
mjr 86:e30a1f60f783 135 virtual bool readRaw(PlungerReading &r) = 0;
mjr 82:4f6209cb5c33 136
mjr 100:1ff35c07217c 137 // Restore the saved calibration data from the configuration. The main
mjr 100:1ff35c07217c 138 // loop calls this at startup to let us initialize internals from the
mjr 100:1ff35c07217c 139 // saved calibration data. This is called even if the plunger isn't
mjr 100:1ff35c07217c 140 // calibrated, which is flagged in the config.
mjr 100:1ff35c07217c 141 virtual void restoreCalibration(Config &) { }
mjr 100:1ff35c07217c 142
mjr 82:4f6209cb5c33 143 // Begin calibration. The main loop calls this when the user activates
mjr 82:4f6209cb5c33 144 // calibration mode. Sensors that work in terms of relative positions,
mjr 82:4f6209cb5c33 145 // such as quadrature-based sensors, can use this to set the reference
mjr 82:4f6209cb5c33 146 // point for the park position internally.
mjr 100:1ff35c07217c 147 virtual void beginCalibration(Config &) { }
mjr 100:1ff35c07217c 148
mjr 100:1ff35c07217c 149 // End calibration. The main loop calls this when calibration mode is
mjr 100:1ff35c07217c 150 // completed.
mjr 100:1ff35c07217c 151 virtual void endCalibration(Config &) { }
mjr 82:4f6209cb5c33 152
mjr 82:4f6209cb5c33 153 // Send a sensor status report to the host, via the joystick interface.
mjr 82:4f6209cb5c33 154 // This provides some common information for all sensor types, and also
mjr 82:4f6209cb5c33 155 // includes a full image snapshot of the current sensor pixels for
mjr 82:4f6209cb5c33 156 // imaging sensor types.
mjr 82:4f6209cb5c33 157 //
mjr 82:4f6209cb5c33 158 // The default implementation here sends the common information
mjr 82:4f6209cb5c33 159 // packet, with the pixel size set to 0.
mjr 82:4f6209cb5c33 160 //
mjr 82:4f6209cb5c33 161 // 'flags' is a combination of bit flags:
mjr 82:4f6209cb5c33 162 // 0x01 -> low-res scan (default is high res scan)
mjr 82:4f6209cb5c33 163 //
mjr 82:4f6209cb5c33 164 // Low-res scan mode means that the sensor should send a scaled-down
mjr 82:4f6209cb5c33 165 // image, at a reduced size determined by the sensor subtype. The
mjr 82:4f6209cb5c33 166 // default if this flag isn't set is to send the full image, at the
mjr 82:4f6209cb5c33 167 // sensor's native pixel size. The low-res version is a reduced size
mjr 82:4f6209cb5c33 168 // image in the normal sense of scaling down a photo image, keeping the
mjr 82:4f6209cb5c33 169 // image intact but at reduced resolution. Note that low-res mode
mjr 82:4f6209cb5c33 170 // doesn't affect the ongoing sensor operation at all. It only applies
mjr 82:4f6209cb5c33 171 // to this single pixel report. The purpose is simply to reduce the USB
mjr 82:4f6209cb5c33 172 // transmission time for the image, to allow for a faster frame rate for
mjr 82:4f6209cb5c33 173 // displaying the sensor image in real time on the PC. For a high-res
mjr 82:4f6209cb5c33 174 // sensor like the TSL1410R, sending the full pixel array by USB takes
mjr 82:4f6209cb5c33 175 // so long that the frame rate is way below regular video rates.
mjr 82:4f6209cb5c33 176 //
mjr 101:755f44622abc 177 virtual void sendStatusReport(class USBJoystick &js, uint8_t flags)
mjr 82:4f6209cb5c33 178 {
mjr 82:4f6209cb5c33 179 // read the current position
mjr 82:4f6209cb5c33 180 int pos = 0xFFFF;
mjr 82:4f6209cb5c33 181 PlungerReading r;
mjr 86:e30a1f60f783 182 if (readRaw(r))
mjr 82:4f6209cb5c33 183 {
mjr 91:ae9be42652bf 184 // adjust for reverse orientation
mjr 91:ae9be42652bf 185 r.pos = applyOrientation(r.pos);
mjr 91:ae9be42652bf 186
mjr 86:e30a1f60f783 187 // success - apply the jitter filter
mjr 86:e30a1f60f783 188 pos = jitterFilter(r.pos);
mjr 82:4f6209cb5c33 189 }
mjr 82:4f6209cb5c33 190
mjr 82:4f6209cb5c33 191 // Send the common status information, indicating 0 pixels, standard
mjr 82:4f6209cb5c33 192 // sensor orientation, and zero processing time. Non-imaging sensors
mjr 86:e30a1f60f783 193 // usually don't have any way to detect the orientation, so assume
mjr 86:e30a1f60f783 194 // normal orientation (flag 0x01). Also assume zero analysis time,
mjr 86:e30a1f60f783 195 // as most non-image sensors don't have to do anything CPU-intensive
mjr 86:e30a1f60f783 196 // with the raw readings (all they usually have to do is scale the
mjr 86:e30a1f60f783 197 // value to the abstract reporting range).
mjr 86:e30a1f60f783 198 js.sendPlungerStatus(0, pos, 0x01, getAvgScanTime(), 0);
mjr 86:e30a1f60f783 199 js.sendPlungerStatus2(nativeScale, jfLo, jfHi, r.pos, 0);
mjr 82:4f6209cb5c33 200 }
mjr 82:4f6209cb5c33 201
mjr 101:755f44622abc 202 // Set extra image integration time, in microseconds. This is only
mjr 101:755f44622abc 203 // meaningful for image-type sensors. This allows the PC client to
mjr 101:755f44622abc 204 // manually adjust the exposure time for testing and debugging
mjr 101:755f44622abc 205 // purposes.
mjr 101:755f44622abc 206 virtual void setExtraIntegrationTime(uint32_t us) { }
mjr 101:755f44622abc 207
mjr 82:4f6209cb5c33 208 // Get the average sensor scan time in microseconds
mjr 82:4f6209cb5c33 209 virtual uint32_t getAvgScanTime() = 0;
mjr 91:ae9be42652bf 210
mjr 91:ae9be42652bf 211 // Apply the orientation filter. The position is in unscaled
mjr 91:ae9be42652bf 212 // native sensor units.
mjr 91:ae9be42652bf 213 int applyOrientation(int pos)
mjr 91:ae9be42652bf 214 {
mjr 91:ae9be42652bf 215 return (reverseOrientation ? nativeScale - pos : pos);
mjr 91:ae9be42652bf 216 }
mjr 82:4f6209cb5c33 217
mjr 91:ae9be42652bf 218 // Apply the jitter filter. The position is in unscaled native
mjr 91:ae9be42652bf 219 // sensor units.
mjr 86:e30a1f60f783 220 int jitterFilter(int pos)
mjr 86:e30a1f60f783 221 {
mjr 86:e30a1f60f783 222 // Check to see where the new reading is relative to the
mjr 86:e30a1f60f783 223 // current window
mjr 86:e30a1f60f783 224 if (pos < jfLo)
mjr 86:e30a1f60f783 225 {
mjr 86:e30a1f60f783 226 // the new position is below the current window, so move
mjr 86:e30a1f60f783 227 // the window down such that the new point is at the bottom
mjr 86:e30a1f60f783 228 // of the window
mjr 86:e30a1f60f783 229 jfLo = pos;
mjr 86:e30a1f60f783 230 jfHi = pos + jfWindow;
mjr 87:8d35c74403af 231
mjr 87:8d35c74403af 232 // figure the new position as the centerpoint of the new window
mjr 87:8d35c74403af 233 jfLast = pos = (jfHi + jfLo)/2;
mjr 86:e30a1f60f783 234 return pos;
mjr 86:e30a1f60f783 235 }
mjr 86:e30a1f60f783 236 else if (pos > jfHi)
mjr 86:e30a1f60f783 237 {
mjr 86:e30a1f60f783 238 // the new position is above the current window, so move
mjr 86:e30a1f60f783 239 // the window up such that the new point is at the top of
mjr 86:e30a1f60f783 240 // the window
mjr 86:e30a1f60f783 241 jfHi = pos;
mjr 86:e30a1f60f783 242 jfLo = pos - jfWindow;
mjr 87:8d35c74403af 243
mjr 87:8d35c74403af 244 // figure the new position as the centerpoint of the new window
mjr 87:8d35c74403af 245 jfLast = pos = (jfHi + jfLo)/2;
mjr 86:e30a1f60f783 246 return pos;
mjr 86:e30a1f60f783 247 }
mjr 86:e30a1f60f783 248 else
mjr 86:e30a1f60f783 249 {
mjr 86:e30a1f60f783 250 // the new position is inside the current window, so repeat
mjr 86:e30a1f60f783 251 // the last reading
mjr 86:e30a1f60f783 252 return jfLast;
mjr 86:e30a1f60f783 253 }
mjr 86:e30a1f60f783 254 }
mjr 86:e30a1f60f783 255
mjr 87:8d35c74403af 256 // Process a configuration variable change. 'varno' is the
mjr 87:8d35c74403af 257 // USB protocol variable number being updated; 'cfg' is the
mjr 87:8d35c74403af 258 // updated configuration.
mjr 87:8d35c74403af 259 virtual void onConfigChange(int varno, Config &cfg)
mjr 87:8d35c74403af 260 {
mjr 87:8d35c74403af 261 switch (varno)
mjr 87:8d35c74403af 262 {
mjr 87:8d35c74403af 263 case 19:
mjr 91:ae9be42652bf 264 // Plunger filters - jitter window and reverse orientation.
mjr 87:8d35c74403af 265 setJitterWindow(cfg.plunger.jitterWindow);
mjr 91:ae9be42652bf 266 setReverseOrientation((cfg.plunger.reverseOrientation & 0x01) != 0);
mjr 87:8d35c74403af 267 break;
mjr 87:8d35c74403af 268 }
mjr 87:8d35c74403af 269 }
mjr 87:8d35c74403af 270
mjr 86:e30a1f60f783 271 // Set the jitter filter window size. This is specified in native
mjr 86:e30a1f60f783 272 // sensor units.
mjr 86:e30a1f60f783 273 void setJitterWindow(int w)
mjr 86:e30a1f60f783 274 {
mjr 86:e30a1f60f783 275 // set the new window size
mjr 86:e30a1f60f783 276 jfWindow = w;
mjr 86:e30a1f60f783 277
mjr 86:e30a1f60f783 278 // reset the running window
mjr 86:e30a1f60f783 279 jfHi = jfLo = jfLast;
mjr 86:e30a1f60f783 280 }
mjr 91:ae9be42652bf 281
mjr 91:ae9be42652bf 282 // Set reverse orientation
mjr 91:ae9be42652bf 283 void setReverseOrientation(bool f) { reverseOrientation = f; }
mjr 86:e30a1f60f783 284
mjr 82:4f6209cb5c33 285 protected:
mjr 86:e30a1f60f783 286 // Native scale of the device. This is the scale used for the position
mjr 86:e30a1f60f783 287 // reading in status reports. This lets us report the position in the
mjr 86:e30a1f60f783 288 // same units the sensor itself uses, to avoid any rounding error
mjr 86:e30a1f60f783 289 // converting to an abstract scale.
mjr 86:e30a1f60f783 290 //
mjr 91:ae9be42652bf 291 // The nativeScale value is the number of units in the range of raw
mjr 91:ae9be42652bf 292 // sensor readings returned from readRaw(). Raw readings thus have a
mjr 91:ae9be42652bf 293 // valid range of 0 to nativeScale-1.
mjr 91:ae9be42652bf 294 //
mjr 86:e30a1f60f783 295 // Image edge detection sensors use the pixel size of the image, since
mjr 86:e30a1f60f783 296 // the position is determined by the pixel position of the shadow in
mjr 86:e30a1f60f783 297 // the image. Quadrature sensors and other sensors that report the
mjr 86:e30a1f60f783 298 // distance in terms of physical distance units should use the number
mjr 86:e30a1f60f783 299 // of quanta in the approximate total plunger travel distance of 3".
mjr 86:e30a1f60f783 300 // For example, the VL6180X uses millimeter quanta, so can report
mjr 86:e30a1f60f783 301 // about 77 quanta over 3"; a quadrature sensor that reports at 1/300"
mjr 86:e30a1f60f783 302 // intervals has about 900 quanta over 3". Absolute encoders (e.g.,
mjr 86:e30a1f60f783 303 // bar code sensors) should use the bar code range.
mjr 86:e30a1f60f783 304 //
mjr 86:e30a1f60f783 305 // Sensors that are inherently analog (e.g., potentiometers, analog
mjr 86:e30a1f60f783 306 // distance sensors) can quantize on any arbitrary scale. In most cases,
mjr 86:e30a1f60f783 307 // it's best to use the same 0..65535 scale used for the regular plunger
mjr 86:e30a1f60f783 308 // reports.
mjr 86:e30a1f60f783 309 uint16_t nativeScale;
mjr 86:e30a1f60f783 310
mjr 86:e30a1f60f783 311 // Scaling factor to convert native readings to abstract units on the
mjr 86:e30a1f60f783 312 // 0x0000..0xFFFF scale used in the higher level sensor-independent
mjr 86:e30a1f60f783 313 // code. Multiply a raw sensor position reading by this value to
mjr 86:e30a1f60f783 314 // get the equivalent value on the abstract scale. This is expressed
mjr 86:e30a1f60f783 315 // as a fixed-point real number with a scale of 65536: calculate it as
mjr 86:e30a1f60f783 316 //
mjr 86:e30a1f60f783 317 // (65535U*65536U) / (nativeScale - 1);
mjr 86:e30a1f60f783 318 uint32_t scalingFactor;
mjr 86:e30a1f60f783 319
mjr 86:e30a1f60f783 320 // Jitter filtering
mjr 86:e30a1f60f783 321 int jfWindow; // window size, in native sensor units
mjr 86:e30a1f60f783 322 int jfLo, jfHi; // bounds of current window
mjr 86:e30a1f60f783 323 int jfLast; // last filtered reading
mjr 91:ae9be42652bf 324
mjr 91:ae9be42652bf 325 // Reverse the raw reading orientation. If set, raw readings will be
mjr 91:ae9be42652bf 326 // switched to the opposite orientation. This allows flipping the sensor
mjr 91:ae9be42652bf 327 // orientation virtually to correct for installing the physical device
mjr 91:ae9be42652bf 328 // backwards.
mjr 91:ae9be42652bf 329 bool reverseOrientation;
mjr 82:4f6209cb5c33 330 };
mjr 82:4f6209cb5c33 331
mjr 87:8d35c74403af 332
mjr 87:8d35c74403af 333 // --------------------------------------------------------------------------
mjr 87:8d35c74403af 334 //
mjr 101:755f44622abc 335 // Generic image sensor interface for image-based plungers.
mjr 101:755f44622abc 336 //
mjr 101:755f44622abc 337 // This interface is designed to allow the underlying sensor code to work
mjr 101:755f44622abc 338 // asynchronously to transfer pixels from the sensor into memory using
mjr 101:755f44622abc 339 // multiple buffers arranged in a circular list. We have a "ready" state,
mjr 101:755f44622abc 340 // which lets the sensor tell us when a buffer is available, and we have
mjr 101:755f44622abc 341 // the notion of "ownership" of the buffer. When the client is done with
mjr 101:755f44622abc 342 // a frame, it must realease the frame back to the sensor so that the sensor
mjr 101:755f44622abc 343 // can use it for a subsequent frame transfer.
mjr 87:8d35c74403af 344 //
mjr 87:8d35c74403af 345 class PlungerSensorImageInterface
mjr 87:8d35c74403af 346 {
mjr 87:8d35c74403af 347 public:
mjr 87:8d35c74403af 348 PlungerSensorImageInterface(int npix)
mjr 87:8d35c74403af 349 {
mjr 87:8d35c74403af 350 native_npix = npix;
mjr 87:8d35c74403af 351 }
mjr 87:8d35c74403af 352
mjr 87:8d35c74403af 353 // initialize the sensor
mjr 87:8d35c74403af 354 virtual void init() = 0;
mjr 87:8d35c74403af 355
mjr 87:8d35c74403af 356 // is the sensor ready?
mjr 87:8d35c74403af 357 virtual bool ready() = 0;
mjr 87:8d35c74403af 358
mjr 101:755f44622abc 359 // Read the image. This retrieves a pointer to the current frame
mjr 101:755f44622abc 360 // buffer, which is in memory space managed by the sensor. This
mjr 101:755f44622abc 361 // MUST only be called when ready() returns true. The buffer is
mjr 101:755f44622abc 362 // locked for the client's use until the client calls releasePix().
mjr 101:755f44622abc 363 // The client MUST call releasePix() when done with the buffer, so
mjr 101:755f44622abc 364 // that the sensor can reuse it for another frame.
mjr 101:755f44622abc 365 virtual void readPix(uint8_t* &pix, uint32_t &t) = 0;
mjr 87:8d35c74403af 366
mjr 101:755f44622abc 367 // Release the current frame buffer back to the sensor.
mjr 101:755f44622abc 368 virtual void releasePix() = 0;
mjr 87:8d35c74403af 369
mjr 87:8d35c74403af 370 // get the average sensor pixel scan time (the time it takes on average
mjr 87:8d35c74403af 371 // to read one image frame from the sensor)
mjr 87:8d35c74403af 372 virtual uint32_t getAvgScanTime() = 0;
mjr 87:8d35c74403af 373
mjr 101:755f44622abc 374 // Set the minimum integration time (microseconds)
mjr 101:755f44622abc 375 virtual void setMinIntTime(uint32_t us) = 0;
mjr 101:755f44622abc 376
mjr 87:8d35c74403af 377 protected:
mjr 87:8d35c74403af 378 // number of pixels on sensor
mjr 87:8d35c74403af 379 int native_npix;
mjr 87:8d35c74403af 380 };
mjr 87:8d35c74403af 381
mjr 87:8d35c74403af 382
mjr 87:8d35c74403af 383 // ----------------------------------------------------------------------------
mjr 87:8d35c74403af 384 //
mjr 87:8d35c74403af 385 // Plunger base class for image-based sensors
mjr 87:8d35c74403af 386 //
mjr 104:6e06e0f4b476 387 template<typename ProcessResult>
mjr 87:8d35c74403af 388 class PlungerSensorImage: public PlungerSensor
mjr 87:8d35c74403af 389 {
mjr 87:8d35c74403af 390 public:
mjr 104:6e06e0f4b476 391 PlungerSensorImage(PlungerSensorImageInterface &sensor,
mjr 104:6e06e0f4b476 392 int npix, int nativeScale, bool negativeImage = false) :
mjr 104:6e06e0f4b476 393 PlungerSensor(nativeScale),
mjr 104:6e06e0f4b476 394 sensor(sensor),
mjr 104:6e06e0f4b476 395 native_npix(npix),
mjr 104:6e06e0f4b476 396 negativeImage(negativeImage),
mjr 104:6e06e0f4b476 397 axcTime(0),
mjr 104:6e06e0f4b476 398 extraIntTime(0)
mjr 87:8d35c74403af 399 {
mjr 87:8d35c74403af 400 }
mjr 87:8d35c74403af 401
mjr 87:8d35c74403af 402 // initialize the sensor
mjr 87:8d35c74403af 403 virtual void init() { sensor.init(); }
mjr 87:8d35c74403af 404
mjr 87:8d35c74403af 405 // is the sensor ready?
mjr 87:8d35c74403af 406 virtual bool ready() { return sensor.ready(); }
mjr 87:8d35c74403af 407
mjr 87:8d35c74403af 408 // get the pixel transfer time
mjr 87:8d35c74403af 409 virtual uint32_t getAvgScanTime() { return sensor.getAvgScanTime(); }
mjr 87:8d35c74403af 410
mjr 101:755f44622abc 411 // set extra integration time
mjr 101:755f44622abc 412 virtual void setExtraIntegrationTime(uint32_t us) { extraIntTime = us; }
mjr 101:755f44622abc 413
mjr 87:8d35c74403af 414 // read the plunger position
mjr 87:8d35c74403af 415 virtual bool readRaw(PlungerReading &r)
mjr 87:8d35c74403af 416 {
mjr 87:8d35c74403af 417 // read pixels from the sensor
mjr 87:8d35c74403af 418 uint8_t *pix;
mjr 87:8d35c74403af 419 uint32_t tpix;
mjr 101:755f44622abc 420 sensor.readPix(pix, tpix);
mjr 87:8d35c74403af 421
mjr 87:8d35c74403af 422 // process the pixels
mjr 87:8d35c74403af 423 int pixpos;
mjr 87:8d35c74403af 424 ProcessResult res;
mjr 101:755f44622abc 425 bool ok = process(pix, native_npix, pixpos, res);
mjr 101:755f44622abc 426
mjr 101:755f44622abc 427 // release the buffer back to the sensor
mjr 101:755f44622abc 428 sensor.releasePix();
mjr 101:755f44622abc 429
mjr 101:755f44622abc 430 // adjust the exposure time
mjr 101:755f44622abc 431 sensor.setMinIntTime(axcTime + extraIntTime);
mjr 101:755f44622abc 432
mjr 101:755f44622abc 433 // if we successfully processed the frame, read the position
mjr 101:755f44622abc 434 if (ok)
mjr 87:8d35c74403af 435 {
mjr 87:8d35c74403af 436 r.pos = pixpos;
mjr 87:8d35c74403af 437 r.t = tpix;
mjr 87:8d35c74403af 438 }
mjr 101:755f44622abc 439
mjr 101:755f44622abc 440 // return the result
mjr 101:755f44622abc 441 return ok;
mjr 87:8d35c74403af 442 }
mjr 87:8d35c74403af 443
mjr 87:8d35c74403af 444 // Send a status report to the joystick interface.
mjr 87:8d35c74403af 445 // See plunger.h for details on the arguments.
mjr 101:755f44622abc 446 virtual void sendStatusReport(USBJoystick &js, uint8_t flags)
mjr 87:8d35c74403af 447 {
mjr 104:6e06e0f4b476 448 // start a timer to measure the processing time
mjr 104:6e06e0f4b476 449 Timer pt;
mjr 104:6e06e0f4b476 450 pt.start();
mjr 104:6e06e0f4b476 451
mjr 87:8d35c74403af 452 // get pixels
mjr 87:8d35c74403af 453 uint8_t *pix;
mjr 87:8d35c74403af 454 uint32_t t;
mjr 101:755f44622abc 455 sensor.readPix(pix, t);
mjr 87:8d35c74403af 456
mjr 87:8d35c74403af 457 // process the pixels and read the position
mjr 87:8d35c74403af 458 int pos, rawPos;
mjr 87:8d35c74403af 459 int n = native_npix;
mjr 87:8d35c74403af 460 ProcessResult res;
mjr 87:8d35c74403af 461 if (process(pix, n, rawPos, res))
mjr 87:8d35c74403af 462 {
mjr 87:8d35c74403af 463 // success - apply the jitter filter
mjr 87:8d35c74403af 464 pos = jitterFilter(rawPos);
mjr 87:8d35c74403af 465 }
mjr 87:8d35c74403af 466 else
mjr 87:8d35c74403af 467 {
mjr 87:8d35c74403af 468 // report 0xFFFF to indicate that the position wasn't read
mjr 87:8d35c74403af 469 pos = 0xFFFF;
mjr 87:8d35c74403af 470 rawPos = 0xFFFF;
mjr 87:8d35c74403af 471 }
mjr 87:8d35c74403af 472
mjr 101:755f44622abc 473 // adjust the exposure time
mjr 101:755f44622abc 474 sensor.setMinIntTime(axcTime + extraIntTime);
mjr 101:755f44622abc 475
mjr 87:8d35c74403af 476 // note the processing time
mjr 87:8d35c74403af 477 uint32_t processTime = pt.read_us();
mjr 87:8d35c74403af 478
mjr 87:8d35c74403af 479 // If a low-res scan is desired, reduce to a subset of pixels. Ignore
mjr 87:8d35c74403af 480 // this for smaller sensors (below 512 pixels)
mjr 87:8d35c74403af 481 if ((flags & 0x01) && n >= 512)
mjr 87:8d35c74403af 482 {
mjr 87:8d35c74403af 483 // figure how many sensor pixels we combine into each low-res pixel
mjr 87:8d35c74403af 484 const int group = 8;
mjr 87:8d35c74403af 485 int lowResPix = n / group;
mjr 87:8d35c74403af 486
mjr 87:8d35c74403af 487 // combine the pixels
mjr 87:8d35c74403af 488 int src, dst;
mjr 87:8d35c74403af 489 for (src = dst = 0 ; dst < lowResPix ; ++dst)
mjr 87:8d35c74403af 490 {
mjr 87:8d35c74403af 491 // average this block of pixels
mjr 87:8d35c74403af 492 int a = 0;
mjr 87:8d35c74403af 493 for (int j = 0 ; j < group ; ++j)
mjr 87:8d35c74403af 494 a += pix[src++];
mjr 87:8d35c74403af 495
mjr 87:8d35c74403af 496 // we have the sum, so get the average
mjr 87:8d35c74403af 497 a /= group;
mjr 87:8d35c74403af 498
mjr 87:8d35c74403af 499 // store the down-res'd pixel in the array
mjr 87:8d35c74403af 500 pix[dst] = uint8_t(a);
mjr 87:8d35c74403af 501 }
mjr 87:8d35c74403af 502
mjr 87:8d35c74403af 503 // update the pixel count to the reduced array size
mjr 87:8d35c74403af 504 n = lowResPix;
mjr 87:8d35c74403af 505 }
mjr 87:8d35c74403af 506
mjr 87:8d35c74403af 507 // figure the report flags
mjr 87:8d35c74403af 508 int jsflags = 0;
mjr 87:8d35c74403af 509
mjr 87:8d35c74403af 510 // add flags for the detected orientation: 0x01 for normal orientation,
mjr 87:8d35c74403af 511 // 0x02 for reversed orientation; no flags if orientation is unknown
mjr 87:8d35c74403af 512 int dir = getOrientation();
mjr 87:8d35c74403af 513 if (dir == 1)
mjr 87:8d35c74403af 514 jsflags |= 0x01;
mjr 87:8d35c74403af 515 else if (dir == -1)
mjr 87:8d35c74403af 516 jsflags |= 0x02;
mjr 87:8d35c74403af 517
mjr 87:8d35c74403af 518 // send the sensor status report headers
mjr 87:8d35c74403af 519 js.sendPlungerStatus(n, pos, jsflags, sensor.getAvgScanTime(), processTime);
mjr 87:8d35c74403af 520 js.sendPlungerStatus2(nativeScale, jfLo, jfHi, rawPos, axcTime);
mjr 104:6e06e0f4b476 521
mjr 87:8d35c74403af 522 // send any extra status headers for subclasses
mjr 87:8d35c74403af 523 extraStatusHeaders(js, res);
mjr 87:8d35c74403af 524
mjr 87:8d35c74403af 525 // If we're not in calibration mode, send the pixels
mjr 87:8d35c74403af 526 extern bool plungerCalMode;
mjr 87:8d35c74403af 527 if (!plungerCalMode)
mjr 87:8d35c74403af 528 {
mjr 104:6e06e0f4b476 529 // If the sensor uses a negative image format (brighter pixels are
mjr 104:6e06e0f4b476 530 // represented by lower numbers in the pixel array), invert the scale
mjr 104:6e06e0f4b476 531 // back to a normal photo-positive scale, so that the client doesn't
mjr 104:6e06e0f4b476 532 // have to know these details.
mjr 104:6e06e0f4b476 533 if (negativeImage)
mjr 104:6e06e0f4b476 534 {
mjr 104:6e06e0f4b476 535 // Invert the photo-negative 255..0 scale to a normal,
mjr 104:6e06e0f4b476 536 // photo-positive 0..255 scale. This is just a matter of
mjr 104:6e06e0f4b476 537 // calculating pos_pixel = 255 - neg_pixel for each pixel.
mjr 104:6e06e0f4b476 538 //
mjr 104:6e06e0f4b476 539 // There's a shortcut we can use here to make this loop go a
mjr 104:6e06e0f4b476 540 // lot faster than the naive approach. Note that 255 decimal
mjr 104:6e06e0f4b476 541 // is 1111111 binary. Subtracting any other binary number
mjr 104:6e06e0f4b476 542 // (in the range 0..255) from 255 will have the effect of
mjr 104:6e06e0f4b476 543 // simply inverting all of the bits in the original number.
mjr 104:6e06e0f4b476 544 // So 255 - X == ~X for any X in 0..255. That might not sound
mjr 104:6e06e0f4b476 545 // like a big deal, but it's actually pretty great, because it
mjr 104:6e06e0f4b476 546 // means that we only have to operate on the bits individually,
mjr 104:6e06e0f4b476 547 // rather than doing arithmetic on the bytes. And if we can
mjr 104:6e06e0f4b476 548 // operate on the bits individually, we can operate on them
mjr 104:6e06e0f4b476 549 // in the largest groups we can with the processor's native
mjr 104:6e06e0f4b476 550 // instruction set, which in the case of ARM is 32-bit DWORDs.
mjr 104:6e06e0f4b476 551 // In other words, we can iterate over the array as a DWORD
mjr 104:6e06e0f4b476 552 // array rather than a BYTE array, which cuts loop iterations
mjr 104:6e06e0f4b476 553 // by a factor of 4.
mjr 104:6e06e0f4b476 554 //
mjr 104:6e06e0f4b476 555 // One other small optimization we can apply is to notice that
mjr 104:6e06e0f4b476 556 // ~X == X ^ ~0, and X ^= ~0 happens to optimize to a single
mjr 104:6e06e0f4b476 557 // ARM instruction. So we can make the ARM C++ compiler
mjr 104:6e06e0f4b476 558 // translate this loop into three assembly instructions (XOR
mjr 104:6e06e0f4b476 559 // with immediate data and auto-increment pointer, decrement
mjr 104:6e06e0f4b476 560 // counter, jump if not zero), which is as fast as we could
mjr 104:6e06e0f4b476 561 // write it in assembly by hand. (This really works in
mjr 104:6e06e0f4b476 562 // practice, too: I clocked this loop at 60us for the
mjr 104:6e06e0f4b476 563 // 1500-pixel TCD1103 array.)
mjr 104:6e06e0f4b476 564 //
mjr 104:6e06e0f4b476 565 uint32_t *pix32 = reinterpret_cast<uint32_t*>(pix);
mjr 104:6e06e0f4b476 566 for (int i = n/4; i != 0; --i)
mjr 104:6e06e0f4b476 567 *pix32++ ^= 0xFFFFFFFF;
mjr 104:6e06e0f4b476 568
mjr 104:6e06e0f4b476 569 // Note! If we ever needed to do this with a sensor where
mjr 104:6e06e0f4b476 570 // the pixel count isn't a multiple of four, we'd have to
mjr 104:6e06e0f4b476 571 // add some code here to deal with the stragglers (the one,
mjr 104:6e06e0f4b476 572 // two, or three extra pixels after the last group of four).
mjr 104:6e06e0f4b476 573 // That's not an issue with any currently supported sensor,
mjr 104:6e06e0f4b476 574 // nor is it likely to be in the future (because any large
mjr 104:6e06e0f4b476 575 // pixel array will be built out of repeated submodules,
mjr 104:6e06e0f4b476 576 // which inherently makes power-of-two bases likely, and
mjr 104:6e06e0f4b476 577 // because engineers tend to have a bias for round numbers
mjr 104:6e06e0f4b476 578 // even when they have to choose arbitrarily). So I'm not
mjr 104:6e06e0f4b476 579 // going to test for this possibility, to save the run-time
mjr 104:6e06e0f4b476 580 // cost. And the worst that happens is we see a couple of
mjr 104:6e06e0f4b476 581 // glitchy-looking pixels at the end of the array in the
mjr 104:6e06e0f4b476 582 // visualizer on the client. But just in case, here's the
mjr 104:6e06e0f4b476 583 // code that would be needed...
mjr 104:6e06e0f4b476 584 //
mjr 104:6e06e0f4b476 585 // int extraPix = n & 3; // remainder of n/4
mjr 104:6e06e0f4b476 586 // for (int i = 0; i < extraPix; ++i)
mjr 104:6e06e0f4b476 587 // reinterpret_cast<uint8_t*>(pix32)[i] ^= 0xFF;
mjr 104:6e06e0f4b476 588 }
mjr 104:6e06e0f4b476 589
mjr 87:8d35c74403af 590 // send the pixels in report-sized chunks until we get them all
mjr 87:8d35c74403af 591 int idx = 0;
mjr 87:8d35c74403af 592 while (idx < n)
mjr 87:8d35c74403af 593 js.sendPlungerPix(idx, n, pix);
mjr 87:8d35c74403af 594 }
mjr 87:8d35c74403af 595
mjr 101:755f44622abc 596 // release the pixel buffer
mjr 101:755f44622abc 597 sensor.releasePix();
mjr 87:8d35c74403af 598 }
mjr 87:8d35c74403af 599
mjr 87:8d35c74403af 600 protected:
mjr 87:8d35c74403af 601 // process an image to read the plunger position
mjr 100:1ff35c07217c 602 virtual bool process(const uint8_t *pix, int npix, int &rawPos, ProcessResult &res) = 0;
mjr 87:8d35c74403af 603
mjr 87:8d35c74403af 604 // send extra status headers, following the standard headers (types 0 and 1)
mjr 87:8d35c74403af 605 virtual void extraStatusHeaders(USBJoystick &js, ProcessResult &res) { }
mjr 87:8d35c74403af 606
mjr 87:8d35c74403af 607 // get the detected orientation
mjr 87:8d35c74403af 608 virtual int getOrientation() const { return 0; }
mjr 87:8d35c74403af 609
mjr 87:8d35c74403af 610 // underlying hardware sensor interface
mjr 87:8d35c74403af 611 PlungerSensorImageInterface &sensor;
mjr 104:6e06e0f4b476 612
mjr 87:8d35c74403af 613 // number of pixels
mjr 87:8d35c74403af 614 int native_npix;
mjr 87:8d35c74403af 615
mjr 104:6e06e0f4b476 616 // Does the sensor report a "negative" image? This is like a photo
mjr 104:6e06e0f4b476 617 // negative, where brighter pixels are represented by lower numbers in
mjr 104:6e06e0f4b476 618 // the pixel array.
mjr 104:6e06e0f4b476 619 bool negativeImage;
mjr 104:6e06e0f4b476 620
mjr 101:755f44622abc 621 // Auto-exposure time. This is for use by process() in the subclass.
mjr 101:755f44622abc 622 // On each frame processing iteration, it can adjust this to optimize
mjr 101:755f44622abc 623 // the image quality.
mjr 87:8d35c74403af 624 uint32_t axcTime;
mjr 101:755f44622abc 625
mjr 101:755f44622abc 626 // Extra exposure time. This is for use by the PC side, mostly for
mjr 101:755f44622abc 627 // debugging use to allow the PC user to manually adjust the exposure
mjr 101:755f44622abc 628 // when inspecting captured frames.
mjr 101:755f44622abc 629 uint32_t extraIntTime;
mjr 87:8d35c74403af 630 };
mjr 87:8d35c74403af 631
mjr 87:8d35c74403af 632
mjr 82:4f6209cb5c33 633 #endif /* PLUNGER_H */