Rushiraj Jawale / Mbed OS firmware_framework1

uart

Dependencies:   MAX32620FTHR USBDevice

gesture.c@0:c62425b2f286, 18 months ago (annotated)

Committer:
rsjawale24
Date:
Sun Oct 16 15:35:01 2022 +0000
Revision:
0:c62425b2f286
ok

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rsjawale24 0:c62425b2f286 1 /*******************************************************************************
rsjawale24 0:c62425b2f286 2 * Copyright (C) Maxim Integrated Products, Inc., All rights Reserved.
rsjawale24 0:c62425b2f286 3 *
rsjawale24 0:c62425b2f286 4 * This software is protected by copyright laws of the United States and
rsjawale24 0:c62425b2f286 5 * of foreign countries. This material may also be protected by patent laws
rsjawale24 0:c62425b2f286 6 * and technology transfer regulations of the United States and of foreign
rsjawale24 0:c62425b2f286 7 * countries. This software is furnished under a license agreement and/or a
rsjawale24 0:c62425b2f286 8 * nondisclosure agreement and may only be used or reproduced in accordance
rsjawale24 0:c62425b2f286 9 * with the terms of those agreements. Dissemination of this information to
rsjawale24 0:c62425b2f286 10 * any party or parties not specified in the license agreement and/or
rsjawale24 0:c62425b2f286 11 * nondisclosure agreement is expressly prohibited.
rsjawale24 0:c62425b2f286 12 *
rsjawale24 0:c62425b2f286 13 * The above copyright notice and this permission notice shall be included
rsjawale24 0:c62425b2f286 14 * in all copies or substantial portions of the Software.
rsjawale24 0:c62425b2f286 15 *
rsjawale24 0:c62425b2f286 16 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
rsjawale24 0:c62425b2f286 17 * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
rsjawale24 0:c62425b2f286 18 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
rsjawale24 0:c62425b2f286 19 * IN NO EVENT SHALL MAXIM INTEGRATED BE LIABLE FOR ANY CLAIM, DAMAGES
rsjawale24 0:c62425b2f286 20 * OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
rsjawale24 0:c62425b2f286 21 * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
rsjawale24 0:c62425b2f286 22 * OTHER DEALINGS IN THE SOFTWARE.
rsjawale24 0:c62425b2f286 23 *
rsjawale24 0:c62425b2f286 24 * Except as contained in this notice, the name of Maxim Integrated
rsjawale24 0:c62425b2f286 25 * Products, Inc. shall not be used except as stated in the Maxim Integrated
rsjawale24 0:c62425b2f286 26 * Products, Inc. Branding Policy.
rsjawale24 0:c62425b2f286 27 *
rsjawale24 0:c62425b2f286 28 * The mere transfer of this software does not imply any licenses
rsjawale24 0:c62425b2f286 29 * of trade secrets, proprietary technology, copyrights, patents,
rsjawale24 0:c62425b2f286 30 * trademarks, maskwork rights, or any other form of intellectual
rsjawale24 0:c62425b2f286 31 * property whatsoever. Maxim Integrated Products, Inc. retains all
rsjawale24 0:c62425b2f286 32 * ownership rights.
rsjawale24 0:c62425b2f286 33 *******************************************************************************
rsjawale24 0:c62425b2f286 34 */
rsjawale24 0:c62425b2f286 35
rsjawale24 0:c62425b2f286 36 #include "gesture_common.h"
rsjawale24 0:c62425b2f286 37
rsjawale24 0:c62425b2f286 38 // Gesture states
rsjawale24 0:c62425b2f286 39 typedef enum {STATE_INACTIVE, GESTURE_IN_PROGRESS} GestureState;
rsjawale24 0:c62425b2f286 40
rsjawale24 0:c62425b2f286 41 static uint32_t reset_flag = TRUE;
rsjawale24 0:c62425b2f286 42
rsjawale24 0:c62425b2f286 43 // Create a static instance of the configuration to maintain current config parameters
rsjawale24 0:c62425b2f286 44 static GestureConfig gestCfg;
rsjawale24 0:c62425b2f286 45
rsjawale24 0:c62425b2f286 46 static void runDynamicGesture(const GestureConfig *cfg, int _pixels[], DynamicGestureResult *gesResult);
rsjawale24 0:c62425b2f286 47 void noiseWindow3Filter(int pixels[], const float alpha, const uint32_t reset_flag);
rsjawale24 0:c62425b2f286 48
rsjawale24 0:c62425b2f286 49 void resetGesture()
rsjawale24 0:c62425b2f286 50 {
rsjawale24 0:c62425b2f286 51 reset_flag = TRUE;
rsjawale24 0:c62425b2f286 52 // Reset submodules
rsjawale24 0:c62425b2f286 53 resetTracking();
rsjawale24 0:c62425b2f286 54 }
rsjawale24 0:c62425b2f286 55
rsjawale24 0:c62425b2f286 56 // Get a copy of the config struct
rsjawale24 0:c62425b2f286 57 void getGestureConfig(GestureConfig *_cfg)
rsjawale24 0:c62425b2f286 58 {
rsjawale24 0:c62425b2f286 59 *_cfg = gestCfg;
rsjawale24 0:c62425b2f286 60 }
rsjawale24 0:c62425b2f286 61
rsjawale24 0:c62425b2f286 62 // Get a pointer to the config struct
rsjawale24 0:c62425b2f286 63 GestureConfig * getGestureConfigPtr()
rsjawale24 0:c62425b2f286 64 {
rsjawale24 0:c62425b2f286 65 return &gestCfg;
rsjawale24 0:c62425b2f286 66 }
rsjawale24 0:c62425b2f286 67
rsjawale24 0:c62425b2f286 68 // Copy the user's config struct to the local struct, and initialize calculated values
rsjawale24 0:c62425b2f286 69 void configGesture(const GestureConfig *_cfg)
rsjawale24 0:c62425b2f286 70 {
rsjawale24 0:c62425b2f286 71 if (!_cfg) {
rsjawale24 0:c62425b2f286 72 initConfigStructToDefaults(&gestCfg); // Use default configuration if pointer is NULL
rsjawale24 0:c62425b2f286 73 }
rsjawale24 0:c62425b2f286 74 else {
rsjawale24 0:c62425b2f286 75 gestCfg = *_cfg; // make a copy of the struct
rsjawale24 0:c62425b2f286 76 }
rsjawale24 0:c62425b2f286 77
rsjawale24 0:c62425b2f286 78 configTracking(gestCfg.sample_period_ms, gestCfg.adc_full_scale, &gestCfg.trackingConfig);
rsjawale24 0:c62425b2f286 79
rsjawale24 0:c62425b2f286 80 resetGesture();
rsjawale24 0:c62425b2f286 81 }
rsjawale24 0:c62425b2f286 82
rsjawale24 0:c62425b2f286 83 void runGesture(int pixels[], GestureResult *gesResult)
rsjawale24 0:c62425b2f286 84 {
rsjawale24 0:c62425b2f286 85 // Initialize result structure
rsjawale24 0:c62425b2f286 86 memset(gesResult, 0, sizeof(GestureResult));
rsjawale24 0:c62425b2f286 87 gesResult->state = STATE_INACTIVE;
rsjawale24 0:c62425b2f286 88 gesResult->gesture = GEST_NONE;
rsjawale24 0:c62425b2f286 89
rsjawale24 0:c62425b2f286 90 // Noise filter
rsjawale24 0:c62425b2f286 91 if (gestCfg.enable_window_filter) {
rsjawale24 0:c62425b2f286 92 noiseWindow3Filter(pixels, gestCfg.window_filter_alpha, reset_flag);
rsjawale24 0:c62425b2f286 93 }
rsjawale24 0:c62425b2f286 94
rsjawale24 0:c62425b2f286 95 // Process pixels for dynamic gesture
rsjawale24 0:c62425b2f286 96 if (1) {
rsjawale24 0:c62425b2f286 97 DynamicGestureResult dynamicResult;
rsjawale24 0:c62425b2f286 98 runDynamicGesture(&gestCfg, pixels, &dynamicResult);
rsjawale24 0:c62425b2f286 99 gesResult->state = dynamicResult.state;
rsjawale24 0:c62425b2f286 100 gesResult->n_sample = dynamicResult.n_sample;
rsjawale24 0:c62425b2f286 101 gesResult->maxpixel = dynamicResult.maxpixel;
rsjawale24 0:c62425b2f286 102 gesResult->x = dynamicResult.x;
rsjawale24 0:c62425b2f286 103 gesResult->y = dynamicResult.y;
rsjawale24 0:c62425b2f286 104 }
rsjawale24 0:c62425b2f286 105 // Process pixels for tracking
rsjawale24 0:c62425b2f286 106 if (0) {
rsjawale24 0:c62425b2f286 107 TrackingResult trackResult;
rsjawale24 0:c62425b2f286 108 runTracking(&gestCfg.trackingConfig, pixels, &trackResult);
rsjawale24 0:c62425b2f286 109 gesResult->state = trackResult.state;
rsjawale24 0:c62425b2f286 110 gesResult->maxpixel = trackResult.maxpixel; // Will override dynamic result if any
rsjawale24 0:c62425b2f286 111 gesResult->x = trackResult.x; // Will override dynamic result if any
rsjawale24 0:c62425b2f286 112 gesResult->y = trackResult.y; // Will override dynamic result if any
rsjawale24 0:c62425b2f286 113 }
rsjawale24 0:c62425b2f286 114 }
rsjawale24 0:c62425b2f286 115
rsjawale24 0:c62425b2f286 116 static void runDynamicGesture(const GestureConfig *cfg, int pixels[], DynamicGestureResult *gesResult)
rsjawale24 0:c62425b2f286 117 {
rsjawale24 0:c62425b2f286 118 memset(gesResult, 0, sizeof(DynamicGestureResult));
rsjawale24 0:c62425b2f286 119
rsjawale24 0:c62425b2f286 120 static GestureState state = STATE_INACTIVE;
rsjawale24 0:c62425b2f286 121 GestureEvent gest_event = GEST_NONE;
rsjawale24 0:c62425b2f286 122 static uint32_t n_sample = 0, n_frame =0;
rsjawale24 0:c62425b2f286 123
rsjawale24 0:c62425b2f286 124 if (reset_flag) {
rsjawale24 0:c62425b2f286 125 state = STATE_INACTIVE;
rsjawale24 0:c62425b2f286 126 }
rsjawale24 0:c62425b2f286 127
rsjawale24 0:c62425b2f286 128 int rawmaxpixel = getMaxPixelValue(pixels, NUM_SENSOR_PIXELS);
rsjawale24 0:c62425b2f286 129
rsjawale24 0:c62425b2f286 130 // Static background subtraction
rsjawale24 0:c62425b2f286 131 {
rsjawale24 0:c62425b2f286 132 static float foreground_pixels[NUM_SENSOR_PIXELS], background_pixels[NUM_SENSOR_PIXELS];
rsjawale24 0:c62425b2f286 133 if (reset_flag) {
rsjawale24 0:c62425b2f286 134 for(uint32_t i=0; i<NUM_SENSOR_PIXELS; i++) {
rsjawale24 0:c62425b2f286 135 foreground_pixels[i] = pixels[i]; // clear the filter
rsjawale24 0:c62425b2f286 136 background_pixels[i] = pixels[i]; // clear the filter
rsjawale24 0:c62425b2f286 137 }
rsjawale24 0:c62425b2f286 138 }
rsjawale24 0:c62425b2f286 139
rsjawale24 0:c62425b2f286 140 float background_alpha = cfg->background_filter_alpha;
rsjawale24 0:c62425b2f286 141
rsjawale24 0:c62425b2f286 142 subtractBackground(pixels, foreground_pixels, background_pixels, NUM_SENSOR_PIXELS, cfg->low_pass_filter_alpha, background_alpha);
rsjawale24 0:c62425b2f286 143 }
rsjawale24 0:c62425b2f286 144
rsjawale24 0:c62425b2f286 145 // Clear the reset flag. All reset activity should be done by now
rsjawale24 0:c62425b2f286 146 if (reset_flag) {
rsjawale24 0:c62425b2f286 147 reset_flag = FALSE;
rsjawale24 0:c62425b2f286 148 }
rsjawale24 0:c62425b2f286 149
rsjawale24 0:c62425b2f286 150 // Find post-filter max pixel
rsjawale24 0:c62425b2f286 151 int maxpixel=getMaxPixelValue(pixels, NUM_SENSOR_PIXELS);
rsjawale24 0:c62425b2f286 152
rsjawale24 0:c62425b2f286 153 float cmx,cmy;
rsjawale24 0:c62425b2f286 154 {
rsjawale24 0:c62425b2f286 155 #if INTERP_FACTOR == 1
rsjawale24 0:c62425b2f286 156 int *interp_pixels;
rsjawale24 0:c62425b2f286 157 interp_pixels = pixels;
rsjawale24 0:c62425b2f286 158 #else
rsjawale24 0:c62425b2f286 159 static int interp_pixels[NUM_INTERP_PIXELS];
rsjawale24 0:c62425b2f286 160 interpn(pixels, interp_pixels, SENSOR_XRES, SENSOR_YRES, INTERP_FACTOR);
rsjawale24 0:c62425b2f286 161 #endif
rsjawale24 0:c62425b2f286 162
rsjawale24 0:c62425b2f286 163 // Thresholding
rsjawale24 0:c62425b2f286 164 zeroPixelsBelowThreshold(interp_pixels,NUM_INTERP_PIXELS,(int)(maxpixel/cfg->zero_clamp_threshold_factor)); // zero out pixels below some percent of peak
rsjawale24 0:c62425b2f286 165 zeroPixelsBelowThreshold(interp_pixels,NUM_INTERP_PIXELS,cfg->zero_clamp_threshold); // and also zero out below fixed threshold
rsjawale24 0:c62425b2f286 166
rsjawale24 0:c62425b2f286 167 // Center of mass
rsjawale24 0:c62425b2f286 168 int totalmass=0;
rsjawale24 0:c62425b2f286 169 if (maxpixel >= cfg->end_detection_threshold) {
rsjawale24 0:c62425b2f286 170 calcCenterOfMass(interp_pixels, INTERP_XRES, INTERP_YRES, &cmx, &cmy, &totalmass); // Only calculate COM if there is a pixel above the noise (avoid divide-by-zero)
rsjawale24 0:c62425b2f286 171 cmx = cmx/INTERP_FACTOR;
rsjawale24 0:c62425b2f286 172 cmy = cmy/INTERP_FACTOR * DY_PIXEL_SCALE; // scale y so it has same unit dimension as x
rsjawale24 0:c62425b2f286 173 }
rsjawale24 0:c62425b2f286 174 }
rsjawale24 0:c62425b2f286 175
rsjawale24 0:c62425b2f286 176 n_frame++;
rsjawale24 0:c62425b2f286 177
rsjawale24 0:c62425b2f286 178 gesResult->n_sample = n_sample;
rsjawale24 0:c62425b2f286 179 gesResult->maxpixel = maxpixel; //rawmaxpixel;
rsjawale24 0:c62425b2f286 180 gesResult->state = maxpixel >= cfg->end_detection_threshold ? GESTURE_IN_PROGRESS : STATE_INACTIVE;
rsjawale24 0:c62425b2f286 181 gesResult->x = maxpixel >= cfg->end_detection_threshold ? cmx : -1.00;
rsjawale24 0:c62425b2f286 182 gesResult->y = maxpixel >= cfg->end_detection_threshold ? cmy : -1.00;
rsjawale24 0:c62425b2f286 183 }
rsjawale24 0:c62425b2f286 184
rsjawale24 0:c62425b2f286 185 void noiseWindow3Filter(int pixels[], const float alpha, const uint32_t reset_flag)
rsjawale24 0:c62425b2f286 186 {
rsjawale24 0:c62425b2f286 187 static int nwin[3][NUM_SENSOR_PIXELS];
rsjawale24 0:c62425b2f286 188 if (reset_flag) {
rsjawale24 0:c62425b2f286 189 for(uint32_t i=0; i<NUM_SENSOR_PIXELS; i++) {
rsjawale24 0:c62425b2f286 190 nwin[0][i] = pixels[i]; // clear the filter
rsjawale24 0:c62425b2f286 191 nwin[1][i] = pixels[i]; // clear the filter
rsjawale24 0:c62425b2f286 192 nwin[2][i] = pixels[i]; // clear the filter
rsjawale24 0:c62425b2f286 193 }
rsjawale24 0:c62425b2f286 194 }
rsjawale24 0:c62425b2f286 195 else {
rsjawale24 0:c62425b2f286 196 for (uint32_t i=0; i<NUM_SENSOR_PIXELS; i++) {
rsjawale24 0:c62425b2f286 197 nwin[0][i] = nwin[1][i];
rsjawale24 0:c62425b2f286 198 nwin[1][i] = nwin[2][i];
rsjawale24 0:c62425b2f286 199 nwin[2][i] = pixels[i];
rsjawale24 0:c62425b2f286 200 pixels[i] = alpha * nwin[1][i] + (1-alpha)*(nwin[0][i] + nwin[2][i])/2;
rsjawale24 0:c62425b2f286 201 }
rsjawale24 0:c62425b2f286 202 }
rsjawale24 0:c62425b2f286 203 }