diff --git a/FLipWare/FLipWare.ino b/FLipWare/FLipWare.ino index 5c8f3ec..f56b2eb 100644 --- a/FLipWare/FLipWare.ino +++ b/FLipWare/FLipWare.ino @@ -89,12 +89,11 @@ struct SensorData sensorData { }; struct I2CSensorValues sensorValues { - .xRaw=0, .yRaw=0, .pressure=0, + .xRaw=0, .yRaw=0, .pressure=512, .calib_now=CALIBRATION_PERIOD // calibrate sensors after startup ! }; -mutex_t sensorDataMutex; // for synchronizsation of data access between cores struct SlotSettings slotSettings; // contains all slot settings uint8_t workingmem[WORKINGMEM_SIZE]; // working memory (command parser, IR-rec/play) uint8_t actSlot = 0; // number of current slot @@ -110,7 +109,7 @@ uint8_t addonUpgrade = BTMODULE_UPGRADE_IDLE; // if not "idle": we are upgrading void setup() { // prepare synchronizsation of sensor data exchange between cores - mutex_init(&sensorDataMutex); + mutex_init(&(sensorValues.sensorDataMutex)); //load slotSettings memcpy(&slotSettings,&defaultSlotSettings,sizeof(struct SlotSettings)); @@ -181,11 +180,11 @@ void loop() { lastInteractionUpdate = millis(); // get current sensor data from core1 - mutex_enter_blocking(&sensorDataMutex); + mutex_enter_blocking(&(sensorValues.sensorDataMutex)); sensorData.xRaw=sensorValues.xRaw; sensorData.yRaw=sensorValues.yRaw; sensorData.pressure=sensorValues.pressure; - mutex_exit(&sensorDataMutex); + mutex_exit(&(sensorValues.sensorDataMutex)); if (StandAloneMode) { @@ -240,39 +239,40 @@ void setup1() { @return none */ void loop1() { - static unsigned long lastUpdate=0; - + static uint32_t lastMPRLS_ts=0; + // check if there is a message from the other core (sensorboard change, profile ID) if (rp2040.fifo.available()) { setSensorBoard(rp2040.fifo.pop()); } + // if the Data Ready Pin of NAU chip signals new data: get force sensor values + if (digitalRead(DRDY_PIN) == HIGH) { + readForce(&sensorValues); + } - // check if the Data Ready Pin of the NAU chip signals new data, if yes: get sensor values! - if (digitalRead(DRDY_PIN) == HIGH) - { - getSensorValues(); + // if desired sampling period for MPRLS pressure sensor passed: get pressure sensor value + if (millis()-lastMPRLS_ts >= 1000/MPRLS_SAMPLINGRATE) { + lastMPRLS_ts=millis(); + readPressure(&sensorValues); } + // if calibration running: update calibration counter + if (sensorValues.calib_now) { + sensorValues.calib_now--; + // calibrate sensors in the middle of the calibration period + if(sensorValues.calib_now==CALIBRATION_PERIOD/2) { + calibrateSensors(); + } + } + + // reset FlipMouse if sensors don't deliver data for several seconds (interface hangs?) if (!checkSensorWatchdog()) { //Serial.println("WATCHDOG !!"); watchdog_reboot(0, 0, 10); while(1); } - - if (millis() >= lastUpdate + UPDATE_INTERVAL) { - lastUpdate = millis(); - - // get current sensor values - mutex_enter_blocking(&sensorDataMutex); - readPressure(&sensorValues); - readForce(&sensorValues); - mutex_exit(&sensorDataMutex); - - // update calibration counter (if calibration running) - if (sensorValues.calib_now) sensorValues.calib_now--; - - } + delay(1); // core1: sleep a bit ... } diff --git a/FLipWare/FlipWare.h b/FLipWare/FlipWare.h index 2f80fa0..7e58932 100644 --- a/FLipWare/FlipWare.h +++ b/FLipWare/FlipWare.h @@ -39,8 +39,9 @@ #include "bluetooth.h" #include "hid_hal.h" -#define VERSION_STRING "v3.3.1" +#define VERSION_STRING "v3.4" +// V3.4: improved MPRLS pressure sensor processing // V3.3.1: fixed IR-command name bug // V3.3: added Bluetooth Joystick // V3.2: changed pinning to PCB v3.2 @@ -84,7 +85,7 @@ */ #define UPDATE_INTERVAL 5 // update interval for performing HID actions (in milliseconds) #define DEFAULT_CLICK_TIME 8 // time for mouse click (loop iterations from press to release) -#define CALIBRATION_PERIOD 200 // approx. 200*UPDATE_INTERVAL = 1sec calibration time +#define CALIBRATION_PERIOD 1000 // approx. 1000ms calibration time // RAM buffers and memory constraints #define WORKINGMEM_SIZE 300 // reserved RAM for working memory (command parser, IR-rec/play) @@ -152,6 +153,7 @@ struct I2CSensorValues { int xRaw,yRaw; int pressure; uint16_t calib_now; + mutex_t sensorDataMutex; // for synchronization of data access between cores }; /** diff --git a/FLipWare/sensors.cpp b/FLipWare/sensors.cpp index e04baa1..c91e194 100644 --- a/FLipWare/sensors.cpp +++ b/FLipWare/sensors.cpp @@ -15,8 +15,8 @@ #include "utils.h" Adafruit_NAU7802 nau; -LoadcellSensor XS,YS,PS; -int sensorWatchdog=-1; +LoadcellSensor XS, YS, PS; +int sensorWatchdog = -1; #define MPRLS_READ_TIMEOUT (20) ///< millis #define MPRLS_STATUS_POWERED (0x40) ///< Status SPI powered bit @@ -26,26 +26,26 @@ int sensorWatchdog=-1; #define MPRLS_STATUS_MASK (0b01100101) ///< Sensor status mask: only these bits are set /** - * @brief Used pressure sensor type. We can use either the MPXV7007GP - * sensor connected to an analog pin or the MPRLS sensor board with I2C - */ + @brief Used pressure sensor type. We can use either the MPXV7007GP + sensor connected to an analog pin or the MPRLS sensor board with I2C +*/ typedef enum {MPXV, MPRLS, NO_PRESSURE} pressure_type_t; pressure_type_t sensor_pressure = NO_PRESSURE; /** - * @brief Global variables for passing sensor data from the ISR - */ -uint8_t channel, newData=0; -int32_t nau_x=0, nau_y=0; -uint32_t mprls_rawval=512; -uint8_t reportXValues=0,reportYValues=0; + @brief Global variables for passing sensor data from the ISR +*/ +uint8_t channel, newData = 0; +int32_t nau_x = 0, nau_y = 0; +int32_t mprls_rawval = 512; +uint8_t reportXValues = 0, reportYValues = 0; /** - * @brief Used force sensor type. We can use the FSR sensors or possibly - * in a future version the resistor gauge sensors. - */ + @brief Used force sensor type. We can use the FSR sensors or possibly + in a future version the resistor gauge sensors. +*/ typedef enum {NAU7802, NO_FORCE} force_type_t; force_type_t sensor_force = NO_FORCE; @@ -56,94 +56,25 @@ force_type_t sensor_force = NO_FORCE; @return none */ void configureNAU() { - nau.setLDO(NAU7802_3V0); // NAU7802_2V7, NAU7802_2V4 + nau.setLDO(NAU7802_3V0); // NAU7802_2V7, NAU7802_2V4 nau.setGain(NAU7802_GAIN_128); // NAU7802_GAIN_64, NAU7802_GAIN_32 nau.setRate(NAU7802_RATE_320SPS); // NAU7802_RATE_80SPS nau.setPGACap(NAU7802_CAP_OFF); //disable PGA capacitor on channel 2 - // trigger internal calibration + // trigger internal calibration while (! nau.calibrate(NAU7802_CALMOD_INTERNAL)) { - Serial.println("Failed to set internal calibration, retrying!"); + Serial.println("SEN: Failed to set NAU internal calibration, retrying!"); delay(1000); } - + // flush ADC - for (uint8_t i=0; i<10; i++) { + for (uint8_t i = 0; i < 10; i++) { while (! nau.available()) delay(1); nau.read(); - } -} - -/** - @name getValueMPRLS - @brief reads a current value from the MPRLS pressure sensor (polling) - @return nonw -*/ -void getValueMPRLS() { - uint8_t buffer[4] = {0}; - Wire1.requestFrom(MPRLS_ADDR,1); - buffer[0] = Wire1.read(); - //any errors? set pressure value to 512, convert otherwise... - if(buffer[0] & MPRLS_STATUS_BUSY) - { - //sensor is busy, cannot read data - #ifdef DEBUG_OUTPUT_SENSORS - Serial.println("MPRLS: busy"); - #endif - return; - } - if((buffer[0] & MPRLS_STATUS_MATHSAT) || (buffer[0] & MPRLS_STATUS_FAILED)) - { - //sensor failed or saturated - #ifdef DEBUG_OUTPUT_SENSORS - Serial.println("MPRLS:failed or saturated"); - #endif - return; - } else { - //request all 4 bytes - Wire1.requestFrom(MPRLS_ADDR,4); - for(uint8_t i = 0; i<4; i++) buffer[i] = Wire1.read(); - mprls_rawval = (uint32_t(buffer[1]) << 16) | (uint32_t(buffer[2]) << 8) | (uint32_t(buffer[3])); - } - //trigger new conversion - Wire1.beginTransmission(MPRLS_ADDR); - Wire1.write(0xAA); - Wire1.write(0); - Wire1.write(0); - Wire1.endTransmission(); -} - - -/** - @name getSensorValues - @brief called if new data from NAU7802 is available. - Reads NAU data, changes NAU channel and reads MPRLS data, updates global variables. - Expected call frequency is about 64 Hz (-> 32 Hz sampling rate for a new X/Y pair). - We increase the update rate to 64 Hz by interpolating 2 values, see below. - @return none -*/ -void getSensorValues() { - static int32_t xChange=0,yChange=0; - - if (channel==1) { - xChange=(XS.process(nau.read())-nau_x)/2; // interpolate X and Y axis for every new X value - nau.setChannel(NAU7802_CHANNEL2); - nau_x+=xChange; nau_y+=yChange; - channel=2; - getValueMPRLS(); - newData=1; - } - else { - yChange=(YS.process(nau.read())-nau_y)/2; // interpolate X and Y axis for every new Y value - nau.setChannel(NAU7802_CHANNEL1); - nau_x+=xChange; nau_y+=yChange; - channel=1; - getValueMPRLS(); - newData=1; } - sensorWatchdog=0; // we got data, reset watchdog counter! } + /** @name initSensors @brief initialises I2C interface, prepares NAU and MPRLS readouts. [called from core 1] @@ -152,10 +83,11 @@ void getSensorValues() { void initSensors() { //detect if there is an MPRLS sensor connected to I2C (Wire) + Wire1.setClock(400000); // use 400kHz I2C clock Wire1.beginTransmission(MPRLS_ADDR); uint8_t result = Wire1.endTransmission(); //we found the MPRLS sensor, start the initialization - if(result == 0) sensor_pressure = MPRLS; + if (result == 0) sensor_pressure = MPRLS; //NAU7802 init if (!nau.begin(&Wire1)) { @@ -164,9 +96,9 @@ void initSensors() return; } else { sensor_force = NAU7802; - #ifdef DEBUG_OUTPUT_SENSORS - Serial.println("SEN: Found NAU7802"); - #endif +#ifdef DEBUG_OUTPUT_SENSORS + Serial.println("SEN: Found NAU7802"); +#endif pinMode (DRDY_PIN, INPUT); nau.setChannel(NAU7802_CHANNEL1); @@ -174,24 +106,29 @@ void initSensors() nau.setChannel(NAU7802_CHANNEL2); configureNAU(); nau.setChannel(NAU7802_CHANNEL1); - channel=1; + channel = 1; // set signal processing parameters for sip/puff (MPRLS pressure sensor) - PS.setGain(0.1); // low gain necessary - PS.setCompensationFactor(0); - PS.setMovementThreshold(1000); - PS.setIdleDetectionPeriod(1000); - PS.setIdleDetectionThreshold(100); - PS.setBaselineLowpass(0.05); - PS.setNoiseLowpass(1.2); - PS.setActivityLowpass(0.2); + PS.setGain(1.0); // adjust gain for pressure sensor + PS.enableOvershootCompensation(false); + PS.setSampleRate(MPRLS_SAMPLINGRATE); + PS.setMovementThreshold(2500); + PS.setBaselineLowpass(0.4); + PS.setNoiseLowpass(10.0); + + // PS.setStartupTime(2000); + PS.enableAutoCalibration(true); + PS.setActivityLowpass(1); + PS.setIdleDetectionPeriod(500); + PS.setIdleDetectionThreshold(500); + // set default signal processing parameters for X and Y axis (NAU loadcell amplifier) XS.setCompensationFactor(0.20); YS.setCompensationFactor(0.20); // overshoot compensation range (fraction of maximum force) XS.setCompensationDecay(0.95); YS.setCompensationDecay(0.95); // overshoot compensation decrease over time XS.setBaselineLowpass(0.25); YS.setBaselineLowpass(0.25); // low-pass cutoff frequency for the baseline adjustment XS.setNoiseLowpass(3); YS.setNoiseLowpass(3); // low-pass cutoff frequency for valid signal - XS.setActivityLowpass(2); YS.setActivityLowpass(2); // low-pass cutoff frequency for idle detection / autocalibration + XS.setActivityLowpass(2); YS.setActivityLowpass(2); // low-pass cutoff frequency for idle detection / autocalibration XS.setIdleDetectionPeriod(1000); YS.setIdleDetectionPeriod(1000); // 1 second idle detection period (autocalibration) XS.setIdleDetectionThreshold(3000); YS.setIdleDetectionThreshold(3000);// gain-compensated threshold value for idle detection (autocalibration) XS.enableAutoCalibration(1); YS.enableAutoCalibration(1); // enable autocalibration @@ -200,12 +137,87 @@ void initSensors() // attachInterrupt(digitalPinToInterrupt(DRDY_PIN), getSensorValues, RISING); // start processing data ready signals! } - #ifdef DEBUG_OUTPUT_SENSORS - Serial.println("SEN: Calibrated internal offset"); - #endif +#ifdef DEBUG_OUTPUT_SENSORS + Serial.println("SEN: Calibrated internal offset"); +#endif } +/** + @name calibrateSensors + @brief calibrates the offset values for the sensors (pressure & force) + @return none +*/ +void calibrateSensors() +{ + XS.calib(); + YS.calib(); + PS.calib(); +} + + +/** + @name getMPRLSValue + @brief called periodically in order to read out MPRLS pressure data + expected sampling rate ca. 100 Hz + @param newVal: pointer where result will be stored + @return status byte of MPRLS +*/ +int getMPRLSValue(int32_t * newVal) { + uint8_t buffer[4] = {0}; + + // request status byte + // myWire.requestFrom(MPRLS_ADDR,1); + // buffer[0] = myWire.read(); + + + //request all 4 bytes + Wire1.requestFrom(MPRLS_ADDR, 4); + for (uint8_t i = 0; i < 4; i++) buffer[i] = Wire1.read(); + + // update value (ignore status byte errors but return the status byte!) + *newVal = (uint32_t(buffer[1]) << 16) | (uint32_t(buffer[2]) << 8) | (uint32_t(buffer[3])); + + //trigger new conversion + Wire1.beginTransmission(MPRLS_ADDR); + Wire1.write(0xAA); + Wire1.write(0); + Wire1.write(0); + Wire1.endTransmission(); + + return (buffer[0]); +} + + +/** + @name getNAUValues + @brief called if new data from NAU7802 is available. + reads NAU data, changes NAU channel and reads MPRLS data + expected sampling rate ca. 30 Hz per channel (-> 60 Hz interpolated) + @param actX, actY: pointers where results will be stored + @return status byte of MPRLS +*/ +void getNAUValues(int32_t * actX, int32_t * actY) { + static int32_t xChange = 0, yChange = 0; + + if (channel == 1) { + xChange = (XS.process(nau.read()) - *actX) / 2; + nau.setChannel(NAU7802_CHANNEL2); + *actX += xChange; + *actY += yChange; + channel = 2; + } + else { + yChange = (YS.process(nau.read()) - *actY) / 2; + nau.setChannel(NAU7802_CHANNEL1); + *actX += xChange; + *actY += yChange; + channel = 1; + } + sensorWatchdog = 0; // we got data, reset watchdog counter! +} + + /** @name readPressure @brief updates and processes new pressure sensor values form MPRLS. [called from core 1] @@ -214,45 +226,79 @@ void initSensors() */ void readPressure(struct I2CSensorValues *data) { - static int32_t mprls_filtered=512; - - switch(sensor_pressure) + //static uint32_t ts=0; + //static int32_t raw_mid=0; + //int sr=0; + int actPressure = 512; + + switch (sensor_pressure) { - case MPRLS: - //only proceed if value != 0 - if(mprls_rawval) + case MPRLS: { - //calibrate offset in the middle of the calibration period - if(data->calib_now==CALIBRATION_PERIOD/2) - { - #ifdef DEBUG_OUTPUT_SENSORS - Serial.print("MPRLS: calib: "); - Serial.println(mprls_rawval); - #endif - PS.calib(); + + // get new value from MPRLS chip + int mprlsStatus = getMPRLSValue(&mprls_rawval); + + // any errors? - just indicate them via orange LED! + if (mprlsStatus & MPRLS_STATUS_BUSY) { + //Serial.println("MPRLS: busy"); + digitalWrite (6, HIGH); } - + else if (mprlsStatus & MPRLS_STATUS_FAILED) { + //Serial.println("MPRLS:failed"); + digitalWrite (6, HIGH); + } + else if (mprlsStatus & MPRLS_STATUS_MATHSAT) { + //Serial.println("MPRLS:saturated"); + digitalWrite (6, HIGH); + } + else digitalWrite (6, LOW); + + int med = calculateMedian(mprls_rawval); + if (abs(med - mprls_rawval) > SPIKE_DETECTION_THRESHOLD) { + mprls_rawval = med; + } + // calculate filtered pressure value, apply signal conditioning - mprls_filtered=PS.process(mprls_rawval); - data->pressure = 512 + mprls_filtered / MPRLS_DIVIDER; + int mprls_filtered = PS.process(mprls_rawval); + if (mprls_filtered > 0) mprls_filtered = sqrt(mprls_filtered); + if (mprls_filtered < 0) mprls_filtered = -sqrt(-mprls_filtered); + + /* + // raw_mid=mprls_rawval; + Serial.print (mprls_rawval-raw_mid); Serial.print(" "); + Serial.print (mprls_filtered); Serial.print(" "); + sr= 1000000 / (micros()-ts); + ts=micros(); + Serial.print (sr); Serial.print(" "); + Serial.println(" "); + */ + + actPressure = 512 + mprls_filtered / MPRLS_DIVIDER; // clamp to 1/1022 (allows disabling strong sip/puff) - if(data->pressure < 1) data->pressure = 1; - if(data->pressure > 1022) data->pressure = 1022; - - // during calibration period: bypass activities - if(data->calib_now) data->pressure=512; + if (actPressure < 1) actPressure = 1; + if (actPressure > 1022) actPressure = 1022; + // during calibration period: set pressure to center (bypass) + if (data->calib_now) actPressure = 512; } break; + case NO_PRESSURE: - data->pressure = 512; + actPressure = 512; break; + case MPXV: default: - data->pressure = analogRead(PRESSURE_SENSOR_PIN); + actPressure = analogRead(PRESSURE_SENSOR_PIN); break; } + + // here we provide new pressure values for further processing by core 0 ! + mutex_enter_blocking(&(data->sensorDataMutex)); + data->pressure = actPressure; + mutex_exit(&(data->sensorDataMutex)); } /** @@ -263,54 +309,49 @@ void readPressure(struct I2CSensorValues *data) */ void readForce(struct I2CSensorValues *data) { - static int32_t currentX=0,currentY=0; + static uint8_t printCount = 0; + int32_t currentX = 0, currentY = 0; - switch(sensor_force) + switch (sensor_force) { case NAU7802: - //calibrate offset in the middle of the calibration period - if(data->calib_now==CALIBRATION_PERIOD/2) { - XS.calib(); - YS.calib(); - } + // get new values from NAU chip + getNAUValues (&nau_x, &nau_y); - // update x/y-values - if (newData) { - static uint8_t printCount=0; - newData=0; - currentX=nau_x / NAU_DIVIDER; - currentY=nau_y / NAU_DIVIDER; - - // prevent unintended baseline correction if other axis is moving - YS.lockBaseline(XS.isMoving()); - XS.lockBaseline(YS.isMoving()); - - // report values if enabled (only every second iteration as we interpolate 2 values) - if (printCount++ > 1) { - printCount=0; - if (reportXValues) XS.printValues(0x07, 40000); - if (reportYValues) YS.printValues(0x07, 40000); - if (reportXValues || reportYValues) Serial.println(""); - } - } + // prevent unintended baseline correction if other axis is moving + YS.lockBaseline(XS.isMoving()); + XS.lockBaseline(YS.isMoving()); + + // report values if enabled (only every second iteration as we interpolate 2 values) + if (printCount++ > 1) { + printCount = 0; + if (reportXValues) XS.printValues(0x07, 40000); + if (reportYValues) YS.printValues(0x07, 40000); + if (reportXValues || reportYValues) Serial.println(""); + } + + if (data->calib_now) { + // during calibration period: set X/Y value to zero + currentX = 0; + currentY = 0; + } + else { + currentX = nau_x / NAU_DIVIDER; + currentY = nau_y / NAU_DIVIDER; + } + break; - if(data->calib_now) { - // during calibration period: set X/Y value to zero - data->xRaw=data->yRaw=0; - } - else { - // here we provide new X/Y values for further processing by core 0 ! - data->xRaw = currentX; - data->yRaw = currentY; - } - break; case NO_FORCE: default: - data->xRaw=0; - data->yRaw=0; break; } + + // here we provide new X/Y values for further processing by core 0 ! + mutex_enter_blocking(&(data->sensorDataMutex)); + data->xRaw = currentX; + data->yRaw = currentY; + mutex_exit(&(data->sensorDataMutex)); } @@ -323,12 +364,12 @@ void readForce(struct I2CSensorValues *data) void calculateDirection(struct SensorData * sensorData) { sensorData->forceRaw = __ieee754_sqrtf(sensorData->xRaw * sensorData->xRaw + sensorData->yRaw * sensorData->yRaw); - if (sensorData->forceRaw !=0) { + if (sensorData->forceRaw != 0) { sensorData->angle = atan2f ((float)sensorData->yRaw / sensorData->forceRaw, (float)sensorData->xRaw / sensorData->forceRaw ); - + // get 8 directions - sensorData->dir=(180+22+(int)(sensorData->angle*57.29578))/45+1; // translate rad to deg and make 8 sections - if (sensorData->dir>8) sensorData->dir=1; + sensorData->dir = (180 + 22 + (int)(sensorData->angle * 57.29578)) / 45 + 1; // translate rad to deg and make 8 sections + if (sensorData->dir > 8) sensorData->dir = 1; } } @@ -344,9 +385,9 @@ void applyDeadzone(struct SensorData * sensorData, struct SlotSettings * slotSet if (slotSettings->stickMode == STICKMODE_ALTERNATIVE) { // rectangular deadzone for alternative modes - if (sensorData->xRaw < -slotSettings->dx) + if (sensorData->xRaw < -slotSettings->dx) sensorData->x = sensorData->xRaw + slotSettings->dx; // apply deadzone values x direction - else if (sensorData->xRaw > slotSettings->dx) + else if (sensorData->xRaw > slotSettings->dx) sensorData->x = sensorData->xRaw - slotSettings->dx; else sensorData->x = 0; @@ -359,12 +400,12 @@ void applyDeadzone(struct SensorData * sensorData, struct SlotSettings * slotSet } else { // circular deadzone for mouse control - if (sensorData->forceRaw != 0) { - float a= slotSettings->dx>0 ? slotSettings->dx : 1 ; - float b= slotSettings->dy>0 ? slotSettings->dy : 1 ; - float s=sinf(sensorData->angle); - float c=cosf(sensorData->angle); - sensorData->deadZone = a*b / __ieee754_sqrtf(a*a*s*s + b*b*c*c); // ellipse equation, polar form + if (sensorData->forceRaw != 0) { + float a = slotSettings->dx > 0 ? slotSettings->dx : 1 ; + float b = slotSettings->dy > 0 ? slotSettings->dy : 1 ; + float s = sinf(sensorData->angle); + float c = cosf(sensorData->angle); + sensorData->deadZone = a * b / __ieee754_sqrtf(a * a * s * s + b * b * c * c); // ellipse equation, polar form } else sensorData->deadZone = slotSettings->dx; @@ -378,68 +419,68 @@ void applyDeadzone(struct SensorData * sensorData, struct SlotSettings * slotSet /** @name setSensorBoard @brief activates a certain parameters profile for signal processing, depending on the selected senosorboard ID - @param sensorBoardID: the ID of the sensorboard signal processing profile (e.g. SENSOR_BOARD_STRAINGAUGE) + @param sensorBoardID: the ID of the sensorboard signal processing profile (e.g. SENSOR_BOARD_STRAINGAUGE) @return none */ -void setSensorBoard(int sensorBoardID) +void setSensorBoard(int sensorBoardID) { switch (sensorBoardID) { - case SENSORBOARD_SENSITIVITY_HIGH: + case SENSORBOARD_SENSITIVITY_HIGH: XS.setGain(0.5); YS.setGain(0.5); XS.setMovementThreshold(2000); YS.setMovementThreshold(2000); XS.setIdleDetectionPeriod(200); YS.setIdleDetectionPeriod(200); XS.setIdleDetectionThreshold(500); YS.setIdleDetectionThreshold(500); XS.enableOvershootCompensation(false); YS.enableOvershootCompensation(false); - break; + break; case SENSORBOARD_SENSITIVITY_MEDIUM: XS.setGain(0.2); YS.setGain(0.2); XS.setMovementThreshold(1000); YS.setMovementThreshold(1000); XS.setIdleDetectionPeriod(200); YS.setIdleDetectionPeriod(200); XS.setIdleDetectionThreshold(250); YS.setIdleDetectionThreshold(250); XS.enableOvershootCompensation(false); YS.enableOvershootCompensation(false); - break; + break; case SENSORBOARD_SENSITIVITY_LOW: XS.setGain(0.1); YS.setGain(0.1); XS.setMovementThreshold(500); YS.setMovementThreshold(500); XS.setIdleDetectionPeriod(200); YS.setIdleDetectionPeriod(200); XS.setIdleDetectionThreshold(120); YS.setIdleDetectionThreshold(120); XS.enableOvershootCompensation(false); YS.enableOvershootCompensation(false); - break; + break; case SENSORBOARD_SENSITIVITY_VERY_LOW: XS.setGain(0.05); YS.setGain(0.05); XS.setMovementThreshold(300); YS.setMovementThreshold(300); XS.setIdleDetectionPeriod(200); YS.setIdleDetectionPeriod(200); XS.setIdleDetectionThreshold(80); YS.setIdleDetectionThreshold(80); XS.enableOvershootCompensation(false); YS.enableOvershootCompensation(false); - break; - case SENSORBOARD_NODMS_HIGH: + break; + case SENSORBOARD_NODMS_HIGH: XS.setGain(2.5); YS.setGain(3.0); XS.setMovementThreshold(1500); YS.setMovementThreshold(1500); XS.setCompensationFactor(0.10); YS.setCompensationFactor(0.10); XS.enableOvershootCompensation(true); YS.enableOvershootCompensation(true); - break; + break; case SENSORBOARD_NODMS_MEDIUM: XS.setGain(1.8); YS.setGain(2.0); XS.setMovementThreshold(1800); YS.setMovementThreshold(1800); XS.setCompensationFactor(0.15); YS.setCompensationFactor(0.15); XS.enableOvershootCompensation(true); YS.enableOvershootCompensation(true); - break; + break; case SENSORBOARD_NODMS_LOW: XS.setGain(1.3); YS.setGain(1.6); - XS.setMovementThreshold(2000); YS.setMovementThreshold(2000); + XS.setMovementThreshold(2000); YS.setMovementThreshold(2000); XS.setCompensationFactor(0.20); YS.setCompensationFactor(0.20); XS.enableOvershootCompensation(true); YS.enableOvershootCompensation(true); - break; + break; // signal reporting settings case SENSORBOARD_REPORT_X: - reportXValues=!reportXValues; - break; + reportXValues = !reportXValues; + break; case SENSORBOARD_REPORT_Y: - reportYValues=!reportYValues; - break; - } + reportYValues = !reportYValues; + break; + } } /** @@ -449,10 +490,77 @@ void setSensorBoard(int sensorBoardID) */ uint8_t checkSensorWatchdog() { //if we never received any valid values, proceed - if(sensorWatchdog == -1) return(true); + if (sensorWatchdog == -1) return (true); //if we received at least one time values, but then it stops, //check the value and reset after ~1s (SENSOR_WATCHDOG_TIMEOUT) if (sensorWatchdog++ > SENSOR_WATCHDOG_TIMEOUT) - return(false); - return(true); + return (false); + return (true); +} + +/** + @name swap + @brief swaps two integers, helper function for median +*/ +void swap(int *a, int *b) { + int temp = *a; + *a = *b; + *b = temp; +} + + +/** + @name quick sort + @brief sorts array of integers, helper function for median +*/ +void quickSort(int values[], int left, int right) { + if (left >= right) { + return; + } + + int pivot = values[left]; + int i = left + 1; + int j = right; + + while (1) { + while (i <= j && values[i] <= pivot) i++; + while (j >= i && values[j] >= pivot) j--; + if (i > j) + break; + + swap(&values[i], &values[j]); + } + swap(&values[left], &values[j]); + + quickSort(values, left, j - 1); + quickSort(values, j + 1, right); +} + + +/** + @name calculateMedian + @brief calculates median value (attention: uses static buffer, useable for just 1 signal!) + @param value: next sample + @return median value +*/ +int calculateMedian(int value) { + static int medianBuf[MEDIAN_VALUES] = {0}; + static int medianBufPos = 0; + int values[MEDIAN_VALUES]; + + medianBuf[medianBufPos] = value; + medianBufPos++; + if (medianBufPos >= MEDIAN_VALUES) medianBufPos = 0; + memcpy(values, medianBuf, sizeof(int)*MEDIAN_VALUES); + + quickSort(values, 0, MEDIAN_VALUES - 1); + + if (MEDIAN_VALUES % 2 == 0) { + int midIndex1 = MEDIAN_VALUES / 2 - 1; + int midIndex2 = MEDIAN_VALUES / 2; + return (values[midIndex1] + values[midIndex2]) / 2; + } else { + int midIndex = MEDIAN_VALUES / 2; + return values[midIndex]; + } } diff --git a/FLipWare/sensors.h b/FLipWare/sensors.h index 9e4ee7e..e66205c 100644 --- a/FLipWare/sensors.h +++ b/FLipWare/sensors.h @@ -18,37 +18,23 @@ #include // for signal conditioning #include //NAU7802 library (Benjamin Aigner's fork with channel change feature) -/** Data ready pin of NAU chip */ -#define DRDY_PIN 21 -/** Analog sensor input if a analog pressure sensor is used */ -#define PRESSURE_SENSOR_PIN A3 +#define DRDY_PIN 21 // Data ready pin of NAU chip +#define PRESSURE_SENSOR_PIN A3 // Analog input pin if a analog pressure sensor is used +#define MPRLS_ADDR 0x18 // I2C address of the MPRLS pressure sensor -/** - * @brief I2C address of the MPRLS pressure sensor - */ -#define MPRLS_ADDR 0x18 +#define MPRLS_DIVIDER 2 // divider for the MPRLS raw value. +#define MPRLS_SAMPLINGRATE 100 // sampling frequency of MPRLS sensor +#define MEDIAN_VALUES 5 // number of values used for median-based spike filter (for MPRLS sensor) +#define SPIKE_DETECTION_THRESHOLD 1000 // distance from median value which classifies a spike -/** - * @brief Fixed divider for the MPRLS raw value. - */ -#define MPRLS_DIVIDER 50 +#define NAU_DIVIDER 120 // divider for the NAU raw values +#define SENSOR_WATCHDOG_TIMEOUT 3000 // watchdog reset time (no NAU sensor data for x millsec. resets device) -/** - * @brief Fixed divider for the NAU raw values - */ -#define NAU_DIVIDER 120 /** - * @brief Sensor Watchdog value (approx. milliseconds until device resets) - */ -#define SENSOR_WATCHDOG_TIMEOUT 3000 - - - -/** - * @brief Sensorboard IDs for different signal processing parameters - */ + @brief Sensorboard IDs for different signal processing parameters +*/ #define SENSORBOARD_SENSITIVITY_HIGH 0 #define SENSORBOARD_SENSITIVITY_MEDIUM 1 #define SENSORBOARD_SENSITIVITY_LOW 2 @@ -66,6 +52,13 @@ */ void initSensors(); +/** + @name calibrateSensors + @brief calibrates the offset values for the sensors (pressure & force) + @return none +*/ +void calibrateSensors(); + /** @name readPressure @brief read current pressure sensor (either MPXV7007GP or MPRS) @@ -99,7 +92,7 @@ void applyDeadzone(struct SensorData * sensorData, struct SlotSettings * slotSet /** @name setSensorBoard @brief activates a certain parameters profile for signal processing, depending on the selected senosorboard ID - @param sensorBoardID: the ID of the sensorboard signal processing profile (e.g. SENSOR_BOARD_STRAINGAUGE) + @param sensorBoardID: the ID of the sensorboard signal processing profile (e.g. SENSOR_BOARD_STRAINGAUGE) @return none */ void setSensorBoard(int sensorBoardID); @@ -113,12 +106,32 @@ uint8_t checkSensorWatchdog(); /** - @name getSensorValues - @brief called if new data from NAU7802 is available. - Reads NAU data, changes NAU channel and reads MPRLS data, updates global variables. - Expected sampling rate ca. 64 Hz - @return none + @name getMPRLSValue + @brief called periodically in order to read out MPRLS pressure data + expected sampling rate ca. 100 Hz + @param newVal: pointer where result will be stored + @return status byte of MPRLS +*/ +int getMPRLSValue(int32_t * newVal); + +/** + @name getNAUValues + @brief called if new data from NAU7802 is available. + reads NAU data, changes NAU channel and reads MPRLS data + expected sampling rate ca. 30 Hz per channel (-> 60 Hz interpolated) + @param actX, actY: pointers where results will be stored + @return status byte of MPRLS */ -void getSensorValues(); +void getNAUValues(int32_t * actX, int32_t * actY); + + +/** + @name calculateMedian + @brief calculates median value (attention: uses static buffer, useable for just 1 signal!) + @param value: next sample + @return median value +*/ +int calculateMedian(int value); + #endif /* _SENSORS_H_ */