Arnaud VALLEY / Mbed 2 deprecated Pinscape_Controller_V2_arnoz

Mirror with some correction

Dependencies:   mbed FastIO FastPWM USBDevice

Plunger/plunger.h@116:7a67265d7c19, 2021-10-01 (annotated)

Committer:
arnoz
Date:
Fri Oct 01 08:19:46 2021 +0000
Revision:
116:7a67265d7c19
Parent:
113:7330439f2ffc 
- Correct information regarding your last merge

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 113:7330439f2ffc 118 r.pos = postJitterFilter(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 113:7330439f2ffc 188 pos = postJitterFilter(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 113:7330439f2ffc 217
mjr 113:7330439f2ffc 218 // Post-filter a raw reading through the mitter filter. Most plunger
mjr 113:7330439f2ffc 219 // sensor subclasses can use this default implementation, since the
mjr 113:7330439f2ffc 220 // jitter filter is usually applied to the raw position reading.
mjr 113:7330439f2ffc 221 // However, for some sensor types, it might be better to apply the
mjr 113:7330439f2ffc 222 // jitter filtering to the underlying physical sensor reading, before
mjr 113:7330439f2ffc 223 // readRaw() translates the reading into distance units. In that
mjr 113:7330439f2ffc 224 // case, the subclass can override this to simply return the argument
mjr 113:7330439f2ffc 225 // unchanged. This allows subclasses to use jitterFilter() if desired
mjr 113:7330439f2ffc 226 // on their physical sensor readings. It's not either/or, though; a
mjr 113:7330439f2ffc 227 // subclass that overrides jitter post-filtering isn't could use an
mjr 113:7330439f2ffc 228 // entirely different noise filtering algorithm on its sensor data.
mjr 113:7330439f2ffc 229 virtual int postJitterFilter(int pos) { return jitterFilter(pos); }
mjr 82:4f6209cb5c33 230
mjr 91:ae9be42652bf 231 // Apply the jitter filter. The position is in unscaled native
mjr 91:ae9be42652bf 232 // sensor units.
mjr 86:e30a1f60f783 233 int jitterFilter(int pos)
mjr 86:e30a1f60f783 234 {
mjr 86:e30a1f60f783 235 // Check to see where the new reading is relative to the
mjr 86:e30a1f60f783 236 // current window
mjr 86:e30a1f60f783 237 if (pos < jfLo)
mjr 86:e30a1f60f783 238 {
mjr 86:e30a1f60f783 239 // the new position is below the current window, so move
mjr 86:e30a1f60f783 240 // the window down such that the new point is at the bottom
mjr 86:e30a1f60f783 241 // of the window
mjr 86:e30a1f60f783 242 jfLo = pos;
mjr 86:e30a1f60f783 243 jfHi = pos + jfWindow;
mjr 87:8d35c74403af 244
mjr 87:8d35c74403af 245 // figure the new position as the centerpoint of the new window
mjr 87:8d35c74403af 246 jfLast = pos = (jfHi + jfLo)/2;
mjr 86:e30a1f60f783 247 return pos;
mjr 86:e30a1f60f783 248 }
mjr 86:e30a1f60f783 249 else if (pos > jfHi)
mjr 86:e30a1f60f783 250 {
mjr 86:e30a1f60f783 251 // the new position is above the current window, so move
mjr 86:e30a1f60f783 252 // the window up such that the new point is at the top of
mjr 86:e30a1f60f783 253 // the window
mjr 86:e30a1f60f783 254 jfHi = pos;
mjr 86:e30a1f60f783 255 jfLo = pos - jfWindow;
mjr 87:8d35c74403af 256
mjr 87:8d35c74403af 257 // figure the new position as the centerpoint of the new window
mjr 87:8d35c74403af 258 jfLast = pos = (jfHi + jfLo)/2;
mjr 86:e30a1f60f783 259 return pos;
mjr 86:e30a1f60f783 260 }
mjr 86:e30a1f60f783 261 else
mjr 86:e30a1f60f783 262 {
mjr 86:e30a1f60f783 263 // the new position is inside the current window, so repeat
mjr 86:e30a1f60f783 264 // the last reading
mjr 86:e30a1f60f783 265 return jfLast;
mjr 86:e30a1f60f783 266 }
mjr 86:e30a1f60f783 267 }
mjr 86:e30a1f60f783 268
mjr 87:8d35c74403af 269 // Process a configuration variable change. 'varno' is the
mjr 87:8d35c74403af 270 // USB protocol variable number being updated; 'cfg' is the
mjr 87:8d35c74403af 271 // updated configuration.
mjr 87:8d35c74403af 272 virtual void onConfigChange(int varno, Config &cfg)
mjr 87:8d35c74403af 273 {
mjr 87:8d35c74403af 274 switch (varno)
mjr 87:8d35c74403af 275 {
mjr 87:8d35c74403af 276 case 19:
mjr 91:ae9be42652bf 277 // Plunger filters - jitter window and reverse orientation.
mjr 87:8d35c74403af 278 setJitterWindow(cfg.plunger.jitterWindow);
mjr 91:ae9be42652bf 279 setReverseOrientation((cfg.plunger.reverseOrientation & 0x01) != 0);
mjr 87:8d35c74403af 280 break;
mjr 87:8d35c74403af 281 }
mjr 87:8d35c74403af 282 }
mjr 87:8d35c74403af 283
mjr 86:e30a1f60f783 284 // Set the jitter filter window size. This is specified in native
mjr 86:e30a1f60f783 285 // sensor units.
mjr 86:e30a1f60f783 286 void setJitterWindow(int w)
mjr 86:e30a1f60f783 287 {
mjr 86:e30a1f60f783 288 // set the new window size
mjr 86:e30a1f60f783 289 jfWindow = w;
mjr 86:e30a1f60f783 290
mjr 86:e30a1f60f783 291 // reset the running window
mjr 86:e30a1f60f783 292 jfHi = jfLo = jfLast;
mjr 86:e30a1f60f783 293 }
mjr 91:ae9be42652bf 294
mjr 91:ae9be42652bf 295 // Set reverse orientation
mjr 91:ae9be42652bf 296 void setReverseOrientation(bool f) { reverseOrientation = f; }
mjr 86:e30a1f60f783 297
mjr 82:4f6209cb5c33 298 protected:
mjr 86:e30a1f60f783 299 // Native scale of the device. This is the scale used for the position
mjr 86:e30a1f60f783 300 // reading in status reports. This lets us report the position in the
mjr 86:e30a1f60f783 301 // same units the sensor itself uses, to avoid any rounding error
mjr 86:e30a1f60f783 302 // converting to an abstract scale.
mjr 86:e30a1f60f783 303 //
mjr 91:ae9be42652bf 304 // The nativeScale value is the number of units in the range of raw
mjr 91:ae9be42652bf 305 // sensor readings returned from readRaw(). Raw readings thus have a
mjr 91:ae9be42652bf 306 // valid range of 0 to nativeScale-1.
mjr 91:ae9be42652bf 307 //
mjr 86:e30a1f60f783 308 // Image edge detection sensors use the pixel size of the image, since
mjr 86:e30a1f60f783 309 // the position is determined by the pixel position of the shadow in
mjr 86:e30a1f60f783 310 // the image. Quadrature sensors and other sensors that report the
mjr 86:e30a1f60f783 311 // distance in terms of physical distance units should use the number
mjr 86:e30a1f60f783 312 // of quanta in the approximate total plunger travel distance of 3".
mjr 86:e30a1f60f783 313 // For example, the VL6180X uses millimeter quanta, so can report
mjr 86:e30a1f60f783 314 // about 77 quanta over 3"; a quadrature sensor that reports at 1/300"
mjr 86:e30a1f60f783 315 // intervals has about 900 quanta over 3". Absolute encoders (e.g.,
mjr 86:e30a1f60f783 316 // bar code sensors) should use the bar code range.
mjr 86:e30a1f60f783 317 //
mjr 86:e30a1f60f783 318 // Sensors that are inherently analog (e.g., potentiometers, analog
mjr 86:e30a1f60f783 319 // distance sensors) can quantize on any arbitrary scale. In most cases,
mjr 86:e30a1f60f783 320 // it's best to use the same 0..65535 scale used for the regular plunger
mjr 86:e30a1f60f783 321 // reports.
mjr 86:e30a1f60f783 322 uint16_t nativeScale;
mjr 86:e30a1f60f783 323
mjr 86:e30a1f60f783 324 // Scaling factor to convert native readings to abstract units on the
mjr 86:e30a1f60f783 325 // 0x0000..0xFFFF scale used in the higher level sensor-independent
mjr 86:e30a1f60f783 326 // code. Multiply a raw sensor position reading by this value to
mjr 86:e30a1f60f783 327 // get the equivalent value on the abstract scale. This is expressed
mjr 86:e30a1f60f783 328 // as a fixed-point real number with a scale of 65536: calculate it as
mjr 86:e30a1f60f783 329 //
mjr 86:e30a1f60f783 330 // (65535U*65536U) / (nativeScale - 1);
mjr 86:e30a1f60f783 331 uint32_t scalingFactor;
mjr 86:e30a1f60f783 332
mjr 86:e30a1f60f783 333 // Jitter filtering
mjr 86:e30a1f60f783 334 int jfWindow; // window size, in native sensor units
mjr 86:e30a1f60f783 335 int jfLo, jfHi; // bounds of current window
mjr 86:e30a1f60f783 336 int jfLast; // last filtered reading
mjr 91:ae9be42652bf 337
mjr 91:ae9be42652bf 338 // Reverse the raw reading orientation. If set, raw readings will be
mjr 91:ae9be42652bf 339 // switched to the opposite orientation. This allows flipping the sensor
mjr 91:ae9be42652bf 340 // orientation virtually to correct for installing the physical device
mjr 91:ae9be42652bf 341 // backwards.
mjr 91:ae9be42652bf 342 bool reverseOrientation;
mjr 82:4f6209cb5c33 343 };
mjr 82:4f6209cb5c33 344
mjr 87:8d35c74403af 345
mjr 87:8d35c74403af 346 // --------------------------------------------------------------------------
mjr 87:8d35c74403af 347 //
mjr 101:755f44622abc 348 // Generic image sensor interface for image-based plungers.
mjr 101:755f44622abc 349 //
mjr 101:755f44622abc 350 // This interface is designed to allow the underlying sensor code to work
mjr 101:755f44622abc 351 // asynchronously to transfer pixels from the sensor into memory using
mjr 101:755f44622abc 352 // multiple buffers arranged in a circular list. We have a "ready" state,
mjr 101:755f44622abc 353 // which lets the sensor tell us when a buffer is available, and we have
mjr 101:755f44622abc 354 // the notion of "ownership" of the buffer. When the client is done with
mjr 101:755f44622abc 355 // a frame, it must realease the frame back to the sensor so that the sensor
mjr 101:755f44622abc 356 // can use it for a subsequent frame transfer.
mjr 87:8d35c74403af 357 //
mjr 87:8d35c74403af 358 class PlungerSensorImageInterface
mjr 87:8d35c74403af 359 {
mjr 87:8d35c74403af 360 public:
mjr 87:8d35c74403af 361 PlungerSensorImageInterface(int npix)
mjr 87:8d35c74403af 362 {
mjr 87:8d35c74403af 363 native_npix = npix;
mjr 87:8d35c74403af 364 }
mjr 87:8d35c74403af 365
mjr 87:8d35c74403af 366 // initialize the sensor
mjr 87:8d35c74403af 367 virtual void init() = 0;
mjr 87:8d35c74403af 368
mjr 87:8d35c74403af 369 // is the sensor ready?
mjr 87:8d35c74403af 370 virtual bool ready() = 0;
mjr 87:8d35c74403af 371
mjr 101:755f44622abc 372 // Read the image. This retrieves a pointer to the current frame
mjr 101:755f44622abc 373 // buffer, which is in memory space managed by the sensor. This
mjr 101:755f44622abc 374 // MUST only be called when ready() returns true. The buffer is
mjr 101:755f44622abc 375 // locked for the client's use until the client calls releasePix().
mjr 101:755f44622abc 376 // The client MUST call releasePix() when done with the buffer, so
mjr 101:755f44622abc 377 // that the sensor can reuse it for another frame.
mjr 101:755f44622abc 378 virtual void readPix(uint8_t* &pix, uint32_t &t) = 0;
mjr 87:8d35c74403af 379
mjr 101:755f44622abc 380 // Release the current frame buffer back to the sensor.
mjr 101:755f44622abc 381 virtual void releasePix() = 0;
mjr 87:8d35c74403af 382
mjr 87:8d35c74403af 383 // get the average sensor pixel scan time (the time it takes on average
mjr 87:8d35c74403af 384 // to read one image frame from the sensor)
mjr 87:8d35c74403af 385 virtual uint32_t getAvgScanTime() = 0;
mjr 87:8d35c74403af 386
mjr 101:755f44622abc 387 // Set the minimum integration time (microseconds)
mjr 101:755f44622abc 388 virtual void setMinIntTime(uint32_t us) = 0;
mjr 101:755f44622abc 389
mjr 87:8d35c74403af 390 protected:
mjr 87:8d35c74403af 391 // number of pixels on sensor
mjr 87:8d35c74403af 392 int native_npix;
mjr 87:8d35c74403af 393 };
mjr 87:8d35c74403af 394
mjr 87:8d35c74403af 395
mjr 87:8d35c74403af 396 // ----------------------------------------------------------------------------
mjr 87:8d35c74403af 397 //
mjr 87:8d35c74403af 398 // Plunger base class for image-based sensors
mjr 87:8d35c74403af 399 //
mjr 104:6e06e0f4b476 400 template<typename ProcessResult>
mjr 87:8d35c74403af 401 class PlungerSensorImage: public PlungerSensor
mjr 87:8d35c74403af 402 {
mjr 87:8d35c74403af 403 public:
mjr 104:6e06e0f4b476 404 PlungerSensorImage(PlungerSensorImageInterface &sensor,
mjr 104:6e06e0f4b476 405 int npix, int nativeScale, bool negativeImage = false) :
mjr 104:6e06e0f4b476 406 PlungerSensor(nativeScale),
mjr 104:6e06e0f4b476 407 sensor(sensor),
mjr 104:6e06e0f4b476 408 native_npix(npix),
mjr 104:6e06e0f4b476 409 negativeImage(negativeImage),
mjr 104:6e06e0f4b476 410 axcTime(0),
mjr 104:6e06e0f4b476 411 extraIntTime(0)
mjr 87:8d35c74403af 412 {
mjr 87:8d35c74403af 413 }
mjr 87:8d35c74403af 414
mjr 87:8d35c74403af 415 // initialize the sensor
mjr 87:8d35c74403af 416 virtual void init() { sensor.init(); }
mjr 87:8d35c74403af 417
mjr 87:8d35c74403af 418 // is the sensor ready?
mjr 87:8d35c74403af 419 virtual bool ready() { return sensor.ready(); }
mjr 87:8d35c74403af 420
mjr 87:8d35c74403af 421 // get the pixel transfer time
mjr 87:8d35c74403af 422 virtual uint32_t getAvgScanTime() { return sensor.getAvgScanTime(); }
mjr 87:8d35c74403af 423
mjr 101:755f44622abc 424 // set extra integration time
mjr 101:755f44622abc 425 virtual void setExtraIntegrationTime(uint32_t us) { extraIntTime = us; }
mjr 101:755f44622abc 426
mjr 87:8d35c74403af 427 // read the plunger position
mjr 87:8d35c74403af 428 virtual bool readRaw(PlungerReading &r)
mjr 87:8d35c74403af 429 {
mjr 87:8d35c74403af 430 // read pixels from the sensor
mjr 87:8d35c74403af 431 uint8_t *pix;
mjr 87:8d35c74403af 432 uint32_t tpix;
mjr 101:755f44622abc 433 sensor.readPix(pix, tpix);
mjr 87:8d35c74403af 434
mjr 87:8d35c74403af 435 // process the pixels
mjr 87:8d35c74403af 436 int pixpos;
mjr 87:8d35c74403af 437 ProcessResult res;
mjr 101:755f44622abc 438 bool ok = process(pix, native_npix, pixpos, res);
mjr 101:755f44622abc 439
mjr 101:755f44622abc 440 // release the buffer back to the sensor
mjr 101:755f44622abc 441 sensor.releasePix();
mjr 101:755f44622abc 442
mjr 101:755f44622abc 443 // adjust the exposure time
mjr 101:755f44622abc 444 sensor.setMinIntTime(axcTime + extraIntTime);
mjr 101:755f44622abc 445
mjr 101:755f44622abc 446 // if we successfully processed the frame, read the position
mjr 101:755f44622abc 447 if (ok)
mjr 87:8d35c74403af 448 {
mjr 87:8d35c74403af 449 r.pos = pixpos;
mjr 87:8d35c74403af 450 r.t = tpix;
mjr 87:8d35c74403af 451 }
mjr 101:755f44622abc 452
mjr 101:755f44622abc 453 // return the result
mjr 101:755f44622abc 454 return ok;
mjr 87:8d35c74403af 455 }
mjr 87:8d35c74403af 456
mjr 87:8d35c74403af 457 // Send a status report to the joystick interface.
mjr 87:8d35c74403af 458 // See plunger.h for details on the arguments.
mjr 101:755f44622abc 459 virtual void sendStatusReport(USBJoystick &js, uint8_t flags)
mjr 87:8d35c74403af 460 {
mjr 104:6e06e0f4b476 461 // start a timer to measure the processing time
mjr 104:6e06e0f4b476 462 Timer pt;
mjr 104:6e06e0f4b476 463 pt.start();
mjr 104:6e06e0f4b476 464
mjr 87:8d35c74403af 465 // get pixels
mjr 87:8d35c74403af 466 uint8_t *pix;
mjr 87:8d35c74403af 467 uint32_t t;
mjr 101:755f44622abc 468 sensor.readPix(pix, t);
mjr 87:8d35c74403af 469
mjr 87:8d35c74403af 470 // process the pixels and read the position
mjr 87:8d35c74403af 471 int pos, rawPos;
mjr 87:8d35c74403af 472 int n = native_npix;
mjr 87:8d35c74403af 473 ProcessResult res;
mjr 87:8d35c74403af 474 if (process(pix, n, rawPos, res))
mjr 87:8d35c74403af 475 {
mjr 113:7330439f2ffc 476 // success - apply the post jitter filter
mjr 113:7330439f2ffc 477 pos = postJitterFilter(rawPos);
mjr 87:8d35c74403af 478 }
mjr 87:8d35c74403af 479 else
mjr 87:8d35c74403af 480 {
mjr 87:8d35c74403af 481 // report 0xFFFF to indicate that the position wasn't read
mjr 87:8d35c74403af 482 pos = 0xFFFF;
mjr 87:8d35c74403af 483 rawPos = 0xFFFF;
mjr 87:8d35c74403af 484 }
mjr 87:8d35c74403af 485
mjr 101:755f44622abc 486 // adjust the exposure time
mjr 101:755f44622abc 487 sensor.setMinIntTime(axcTime + extraIntTime);
mjr 101:755f44622abc 488
mjr 87:8d35c74403af 489 // note the processing time
mjr 87:8d35c74403af 490 uint32_t processTime = pt.read_us();
mjr 87:8d35c74403af 491
mjr 87:8d35c74403af 492 // If a low-res scan is desired, reduce to a subset of pixels. Ignore
mjr 87:8d35c74403af 493 // this for smaller sensors (below 512 pixels)
mjr 87:8d35c74403af 494 if ((flags & 0x01) && n >= 512)
mjr 87:8d35c74403af 495 {
mjr 87:8d35c74403af 496 // figure how many sensor pixels we combine into each low-res pixel
mjr 87:8d35c74403af 497 const int group = 8;
mjr 87:8d35c74403af 498 int lowResPix = n / group;
mjr 87:8d35c74403af 499
mjr 87:8d35c74403af 500 // combine the pixels
mjr 87:8d35c74403af 501 int src, dst;
mjr 87:8d35c74403af 502 for (src = dst = 0 ; dst < lowResPix ; ++dst)
mjr 87:8d35c74403af 503 {
mjr 87:8d35c74403af 504 // average this block of pixels
mjr 87:8d35c74403af 505 int a = 0;
mjr 87:8d35c74403af 506 for (int j = 0 ; j < group ; ++j)
mjr 87:8d35c74403af 507 a += pix[src++];
mjr 87:8d35c74403af 508
mjr 87:8d35c74403af 509 // we have the sum, so get the average
mjr 87:8d35c74403af 510 a /= group;
mjr 87:8d35c74403af 511
mjr 87:8d35c74403af 512 // store the down-res'd pixel in the array
mjr 87:8d35c74403af 513 pix[dst] = uint8_t(a);
mjr 87:8d35c74403af 514 }
mjr 87:8d35c74403af 515
mjr 87:8d35c74403af 516 // update the pixel count to the reduced array size
mjr 87:8d35c74403af 517 n = lowResPix;
mjr 87:8d35c74403af 518 }
mjr 87:8d35c74403af 519
mjr 87:8d35c74403af 520 // figure the report flags
mjr 87:8d35c74403af 521 int jsflags = 0;
mjr 87:8d35c74403af 522
mjr 87:8d35c74403af 523 // add flags for the detected orientation: 0x01 for normal orientation,
mjr 87:8d35c74403af 524 // 0x02 for reversed orientation; no flags if orientation is unknown
mjr 87:8d35c74403af 525 int dir = getOrientation();
mjr 87:8d35c74403af 526 if (dir == 1)
mjr 87:8d35c74403af 527 jsflags |= 0x01;
mjr 87:8d35c74403af 528 else if (dir == -1)
mjr 87:8d35c74403af 529 jsflags |= 0x02;
mjr 87:8d35c74403af 530
mjr 87:8d35c74403af 531 // send the sensor status report headers
mjr 87:8d35c74403af 532 js.sendPlungerStatus(n, pos, jsflags, sensor.getAvgScanTime(), processTime);
mjr 87:8d35c74403af 533 js.sendPlungerStatus2(nativeScale, jfLo, jfHi, rawPos, axcTime);
mjr 104:6e06e0f4b476 534
mjr 87:8d35c74403af 535 // send any extra status headers for subclasses
mjr 87:8d35c74403af 536 extraStatusHeaders(js, res);
mjr 87:8d35c74403af 537
mjr 87:8d35c74403af 538 // If we're not in calibration mode, send the pixels
mjr 87:8d35c74403af 539 extern bool plungerCalMode;
mjr 87:8d35c74403af 540 if (!plungerCalMode)
mjr 87:8d35c74403af 541 {
mjr 104:6e06e0f4b476 542 // If the sensor uses a negative image format (brighter pixels are
mjr 104:6e06e0f4b476 543 // represented by lower numbers in the pixel array), invert the scale
mjr 104:6e06e0f4b476 544 // back to a normal photo-positive scale, so that the client doesn't
mjr 104:6e06e0f4b476 545 // have to know these details.
mjr 104:6e06e0f4b476 546 if (negativeImage)
mjr 104:6e06e0f4b476 547 {
mjr 104:6e06e0f4b476 548 // Invert the photo-negative 255..0 scale to a normal,
mjr 104:6e06e0f4b476 549 // photo-positive 0..255 scale. This is just a matter of
mjr 104:6e06e0f4b476 550 // calculating pos_pixel = 255 - neg_pixel for each pixel.
mjr 104:6e06e0f4b476 551 //
mjr 104:6e06e0f4b476 552 // There's a shortcut we can use here to make this loop go a
mjr 104:6e06e0f4b476 553 // lot faster than the naive approach. Note that 255 decimal
mjr 109:310ac82cbbee 554 // is 1111111 binary. Subtracting any number in (0..255) from
mjr 109:310ac82cbbee 555 // 255 is the same as inverting the bits in the other number.
mjr 109:310ac82cbbee 556 // That is, 255 - X == ~X for all X in 0..255. That's useful
mjr 109:310ac82cbbee 557 // because it means that we can compute (255-X) as a purely
mjr 109:310ac82cbbee 558 // bitwise operation, which means that we can perform it on
mjr 109:310ac82cbbee 559 // blocks of bytes instead of individual bytes. On ARM, we
mjr 109:310ac82cbbee 560 // can perform bitwise operations four bytes at a time via
mjr 109:310ac82cbbee 561 // DWORD instructions. This lets us compute (255-X) for N
mjr 109:310ac82cbbee 562 // bytes using N/4 loop iterations.
mjr 104:6e06e0f4b476 563 //
mjr 104:6e06e0f4b476 564 // One other small optimization we can apply is to notice that
mjr 109:310ac82cbbee 565 // ~X == X ^ ~0, and that X ^= ~0 can be performed with a
mjr 109:310ac82cbbee 566 // single ARM instruction. So we can make the ARM C++ compiler
mjr 109:310ac82cbbee 567 // translate the loop body into just three instructions: XOR
mjr 109:310ac82cbbee 568 // with immediate data and auto-increment pointer, decrement
mjr 109:310ac82cbbee 569 // the counter, and jump if not zero. That's as fast we could
mjr 109:310ac82cbbee 570 // do it in hand-written assembly. I clocked this loop at
mjr 109:310ac82cbbee 571 // 60us for the 1536-pixel TCD1103 array.
mjr 109:310ac82cbbee 572 //
mjr 109:310ac82cbbee 573 // Note two important constraints:
mjr 109:310ac82cbbee 574 //
mjr 109:310ac82cbbee 575 // - 'pix' must be aligned on a DWORD (4-byte) boundary.
mjr 109:310ac82cbbee 576 // This is REQUIRED, because the XOR in the loop uses a
mjr 109:310ac82cbbee 577 // DWORD memory operand, which will halt the MCU with a
mjr 109:310ac82cbbee 578 // bus error if the pointer isn't DWORD-aligned.
mjr 109:310ac82cbbee 579 //
mjr 109:310ac82cbbee 580 // - 'n' must be a multiple of 4 bytes. This isn't strictly
mjr 109:310ac82cbbee 581 // required, but if it's not observed, the last (N - N/4)
mjr 109:310ac82cbbee 582 // bytes won't be inverted.
mjr 109:310ac82cbbee 583 //
mjr 109:310ac82cbbee 584 // The only sensor that uses a negative image is the TCD1103.
mjr 109:310ac82cbbee 585 // Its buffer is DWORD-aligned because it's allocated via
mjr 109:310ac82cbbee 586 // malloc(), which always does worst-case alignment. Its
mjr 109:310ac82cbbee 587 // buffer is 1546 bytes long, which violates the multiple-of-4
mjr 109:310ac82cbbee 588 // rule, but inconsequentially, as the last 14 bytes represent
mjr 109:310ac82cbbee 589 // dummy pixels that can be ignored (so it's okay that we'll
mjr 109:310ac82cbbee 590 // miss inverting the last two bytes).
mjr 104:6e06e0f4b476 591 //
mjr 104:6e06e0f4b476 592 uint32_t *pix32 = reinterpret_cast<uint32_t*>(pix);
mjr 104:6e06e0f4b476 593 for (int i = n/4; i != 0; --i)
mjr 104:6e06e0f4b476 594 *pix32++ ^= 0xFFFFFFFF;
mjr 104:6e06e0f4b476 595 }
mjr 104:6e06e0f4b476 596
mjr 87:8d35c74403af 597 // send the pixels in report-sized chunks until we get them all
mjr 87:8d35c74403af 598 int idx = 0;
mjr 87:8d35c74403af 599 while (idx < n)
mjr 87:8d35c74403af 600 js.sendPlungerPix(idx, n, pix);
mjr 87:8d35c74403af 601 }
mjr 87:8d35c74403af 602
mjr 101:755f44622abc 603 // release the pixel buffer
mjr 101:755f44622abc 604 sensor.releasePix();
mjr 87:8d35c74403af 605 }
mjr 87:8d35c74403af 606
mjr 87:8d35c74403af 607 protected:
mjr 87:8d35c74403af 608 // process an image to read the plunger position
mjr 100:1ff35c07217c 609 virtual bool process(const uint8_t *pix, int npix, int &rawPos, ProcessResult &res) = 0;
mjr 87:8d35c74403af 610
mjr 87:8d35c74403af 611 // send extra status headers, following the standard headers (types 0 and 1)
mjr 87:8d35c74403af 612 virtual void extraStatusHeaders(USBJoystick &js, ProcessResult &res) { }
mjr 87:8d35c74403af 613
mjr 87:8d35c74403af 614 // get the detected orientation
mjr 87:8d35c74403af 615 virtual int getOrientation() const { return 0; }
mjr 87:8d35c74403af 616
mjr 87:8d35c74403af 617 // underlying hardware sensor interface
mjr 87:8d35c74403af 618 PlungerSensorImageInterface &sensor;
mjr 104:6e06e0f4b476 619
mjr 87:8d35c74403af 620 // number of pixels
mjr 87:8d35c74403af 621 int native_npix;
mjr 87:8d35c74403af 622
mjr 104:6e06e0f4b476 623 // Does the sensor report a "negative" image? This is like a photo
mjr 104:6e06e0f4b476 624 // negative, where brighter pixels are represented by lower numbers in
mjr 104:6e06e0f4b476 625 // the pixel array.
mjr 104:6e06e0f4b476 626 bool negativeImage;
mjr 104:6e06e0f4b476 627
mjr 101:755f44622abc 628 // Auto-exposure time. This is for use by process() in the subclass.
mjr 101:755f44622abc 629 // On each frame processing iteration, it can adjust this to optimize
mjr 101:755f44622abc 630 // the image quality.
mjr 87:8d35c74403af 631 uint32_t axcTime;
mjr 101:755f44622abc 632
mjr 101:755f44622abc 633 // Extra exposure time. This is for use by the PC side, mostly for
mjr 101:755f44622abc 634 // debugging use to allow the PC user to manually adjust the exposure
mjr 101:755f44622abc 635 // when inspecting captured frames.
mjr 101:755f44622abc 636 uint32_t extraIntTime;
mjr 87:8d35c74403af 637 };
mjr 87:8d35c74403af 638
mjr 87:8d35c74403af 639
mjr 82:4f6209cb5c33 640 #endif /* PLUNGER_H */