From 348d9dfdd1727e691390def576e0f1309ee86393 Mon Sep 17 00:00:00 2001 From: Mark Meyer Date: Sat, 12 Aug 2023 14:32:06 +0200 Subject: [PATCH 01/11] fix: change isr handling --- src/sensor.cpp | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/src/sensor.cpp b/src/sensor.cpp index faadaa1b..51152a2f 100644 --- a/src/sensor.cpp +++ b/src/sensor.cpp @@ -128,11 +128,7 @@ static void IRAM_ATTR isr(HCSR04SensorInfo* const sensor) { // mechanism we should see a similar delay. if (sensor->end == MEASUREMENT_IN_PROGRESS) { const uint32_t now = micros(); - if (HIGH == digitalRead(sensor->echoPin)) { - sensor->start = now; - } else { // LOW - sensor->end = now; - } + sensor->end = now; } } @@ -147,9 +143,9 @@ static void IRAM_ATTR isr1() { void HCSR04SensorManager::attachSensorInterrupt(uint8_t idx) { // bad bad bad .... if (idx == 0) { - attachInterrupt(TOF_SENSOR[idx]->echoPin, isr0, CHANGE); + attachInterrupt(TOF_SENSOR[idx]->echoPin, isr0, FALLING); } else { - attachInterrupt(TOF_SENSOR[idx]->echoPin, isr1, CHANGE); + attachInterrupt(TOF_SENSOR[idx]->echoPin, isr1, FALLING); } // a solution like below leads to crashes with: @@ -334,6 +330,7 @@ bool HCSR04SensorManager::collectSensorResult(uint8_t sensorId) { sensor->echoDurationMicroseconds[lastReadingCount] = -1; } else { duration = microsBetween(start, end); + sensor->echoDurationMicroseconds[lastReadingCount] = duration; } uint16_t dist; From a73b02882ae7d3937e12285c9ca91f84039da88b Mon Sep 17 00:00:00 2001 From: Mark Meyer Date: Tue, 15 Aug 2023 23:57:24 +0200 Subject: [PATCH 02/11] chore: refactor sensor code and move it to core0 --- src/OpenBikeSensorFirmware.cpp | 85 +++----- src/configServer.cpp | 6 - src/displays.cpp | 14 +- src/sensor.cpp | 370 +++++++++------------------------ src/sensor.h | 56 +++-- src/tofmanager.cpp | 301 +++++++++++++++++++++++++++ src/tofmanager.h | 83 ++++++++ 7 files changed, 552 insertions(+), 363 deletions(-) create mode 100644 src/tofmanager.cpp create mode 100644 src/tofmanager.h diff --git a/src/OpenBikeSensorFirmware.cpp b/src/OpenBikeSensorFirmware.cpp index a454563a..553b4f22 100644 --- a/src/OpenBikeSensorFirmware.cpp +++ b/src/OpenBikeSensorFirmware.cpp @@ -59,12 +59,16 @@ Config config; SSD1306DisplayDevice* obsDisplay; HCSR04SensorManager* sensorManager; +#ifdef OBS_BLUETOOTH static BluetoothManager* bluetoothManager; +#endif Gps gps; +#ifdef OBS_BLUETOOTH static const long BLUETOOTH_INTERVAL_MILLIS = 100; static long lastBluetoothInterval = 0; +#endif static const long DISPLAY_INTERVAL_MILLIS = 25; static long lastDisplayInterval = 0; @@ -104,7 +108,6 @@ uint8_t batteryPercentage(); void serverLoop(); void handleButtonInServerMode(); bool loadConfig(ObsConfig &cfg); -void copyCollectedSensorData(DataSet *set); // The BMP280 can keep up to 3.4MHz I2C speed, so no need for an individual slower speed void switch_wire_speed_to_VL53(){ @@ -114,26 +117,13 @@ void switch_wire_speed_to_SSD1306(){ Wire.setClock(500000); } +// FIXME void setupSensors() { sensorManager = new HCSR04SensorManager; - - HCSR04SensorInfo sensorManaged1; - sensorManaged1.triggerPin = (config.displayConfig & DisplaySwapSensors) ? 25 : 15; - sensorManaged1.echoPin = (config.displayConfig & DisplaySwapSensors) ? 26 : 4; - sensorManaged1.sensorLocation = (char*) "Right"; // TODO - sensorManager->registerSensor(sensorManaged1, 0); - - HCSR04SensorInfo sensorManaged2; - sensorManaged2.triggerPin = (config.displayConfig & DisplaySwapSensors) ? 15 : 25; - sensorManaged2.echoPin = (config.displayConfig & DisplaySwapSensors) ? 4 : 26; - sensorManaged2.sensorLocation = (char*) "Left"; // TODO - sensorManager->registerSensor(sensorManaged2, 1); - - sensorManager->setOffsets(config.sensorOffsets); - - sensorManager->setPrimarySensor(LEFT_SENSOR_ID); + sensorManager->initSensors(); } +#ifdef OBS_BLUETOOTH static void setupBluetooth(const ObsConfig &cfg, const String &trackUniqueIdentifier) { if (cfg.getProperty(ObsConfig::PROPERTY_BLUETOOTH)) { obsDisplay->showTextOnGrid(2, obsDisplay->newLine(), "Bluetooth .."); @@ -193,6 +183,7 @@ static void buttonBluetooth(const DataSet *dataSet, uint16_t measureIndex) { left, right); } } +#endif void setup() { Serial.begin(115200); @@ -261,12 +252,6 @@ void setup() { } delay(333); // Added for user experience - //############################################################## - // Init HCSR04 - //############################################################## - - setupSensors(); - //############################################################## // Check, if the button is pressed // Enter configuration mode and enable OTA @@ -283,8 +268,10 @@ void setup() { if (button.read() == HIGH || triggerServerMode) { obsDisplay->showTextOnGrid(2, obsDisplay->newLine(), "Start Server"); +#ifdef OBS_BLUETOOTH ESP_ERROR_CHECK_WITHOUT_ABORT( esp_bt_mem_release(ESP_BT_MODE_BTDM)); // no bluetooth at all here. +#endif delay(200); startServer(&cfg); @@ -333,7 +320,9 @@ void setup() { gps.handle(); +#ifdef OBS_BLUETOOTH setupBluetooth(cfg, trackUniqueIdentifier); +#endif obsDisplay->showTextOnGrid(2, obsDisplay->newLine(), "Wait for GPS"); obsDisplay->newLine(); @@ -343,8 +332,6 @@ void setup() { while (!gps.hasFix(obsDisplay)) { currentTimeMillis = millis(); gps.handle(); - sensorManager->pollDistancesAlternating(); - reportBluetooth(); gps.showWaitStatus(obsDisplay); if (button.read() == HIGH) { log_d("Skipped get GPS..."); @@ -360,12 +347,16 @@ void setup() { gps.handle(1100); // Added for user experience gps.pollStatistics(); obsDisplay->clear(); + + //############################################################## + // Init HCSR04 + //############################################################## + setupSensors(); } void serverLoop() { gps.handle(); configServerHandle(); - sensorManager->pollDistancesAlternating(); handleButtonInServerMode(); } @@ -426,7 +417,7 @@ void loop() { currentSet->millis = startTimeMillis; currentSet->batteryLevel = voltageMeter->read(); - lastMeasurements = sensorManager->m_sensors[confirmationSensorID].numberOfTriggers; + lastMeasurements = sensorManager->getSensor(confirmationSensorID)->numberOfTriggers; sensorManager->reset(startTimeMillis); // if the detected minimum was measured more than 5s ago, it is discarded and cannot be confirmed @@ -451,16 +442,9 @@ void loop() { gps.handle(); if (sensorManager->pollDistancesAlternating()) { // if a new minimum on the selected sensor is detected, the value and the time of detection will be stored - const uint16_t reading = sensorManager->sensorValues[confirmationSensorID]; + const uint16_t reading = sensorManager->getLastValidValue(confirmationSensorID); if (reading > 0 && reading < minDistanceToConfirm) { minDistanceToConfirm = reading; - minDistanceToConfirmIndex = sensorManager->getCurrentMeasureIndex(); - // if there was no measurement of this sensor for this index, it is the - // one before. This happens with fast confirmations. - while (minDistanceToConfirmIndex > 0 - && sensorManager->m_sensors[confirmationSensorID].echoDurationMicroseconds[minDistanceToConfirmIndex] <= 0) { - minDistanceToConfirmIndex--; - } datasetToConfirm = currentSet; timeOfMinimum = currentTimeMillis; } @@ -469,8 +453,8 @@ void loop() { if (lastDisplayInterval != (currentTimeMillis / DISPLAY_INTERVAL_MILLIS)) { lastDisplayInterval = currentTimeMillis / DISPLAY_INTERVAL_MILLIS; obsDisplay->showValues( - sensorManager->m_sensors[LEFT_SENSOR_ID], - sensorManager->m_sensors[RIGHT_SENSOR_ID], + sensorManager->getSensor(LEFT_SENSOR_ID), + sensorManager->getSensor(RIGHT_SENSOR_ID), minDistanceToConfirm, voltageMeter->readPercentage(), (int16_t) TemperatureValue, @@ -481,7 +465,9 @@ void loop() { ); } gps.handle(); +#ifdef OBS_BLUETOOTH reportBluetooth(); +#endif if (button.gotPressed()) { // after button was released, detect long press here // immediate user feedback - we start the action obsDisplay->highlight(); @@ -491,13 +477,17 @@ void loop() { if (datasetToConfirm != nullptr) { datasetToConfirm->confirmedDistances.push_back(minDistanceToConfirm); datasetToConfirm->confirmedDistancesIndex.push_back(minDistanceToConfirmIndex); +#ifdef OBS_BLUETOOTH buttonBluetooth(datasetToConfirm, minDistanceToConfirmIndex); +#endif datasetToConfirm = nullptr; } else { // confirming an overtake without left measure - currentSet->confirmedDistances.push_back(MAX_SENSOR_VALUE); - currentSet->confirmedDistancesIndex.push_back( - sensorManager->getCurrentMeasureIndex()); + if (sensorManager->hasReadings(LEFT_SENSOR_ID)) { + currentSet->confirmedDistances.push_back(MAX_SENSOR_VALUE); + } +#ifdef OBS_BLUETOOTH buttonBluetooth(currentSet, sensorManager->getCurrentMeasureIndex()); +#endif } minDistanceToConfirm = MAX_SENSOR_VALUE; // ready for next confirmation } @@ -510,7 +500,7 @@ void loop() { currentSet->gpsRecord = gps.getCurrentGpsRecord(); currentSet->isInsidePrivacyArea = gps.isInsidePrivacyArea(); if (currentSet->gpsRecord.getTow() == thisLoopTow) { - copyCollectedSensorData(currentSet); + sensorManager->copyData(currentSet); log_d("NEW SET: TOW: %u GPSms: %u, SETms: %u, GPS Time: %s, SET Time: %s, innerLoops: %d, buffer: %d, write time %3ums, loop time: %4ums, measurements: %2d", currentSet->gpsRecord.getTow(), currentSet->gpsRecord.getCreatedAtMillisTicks(), currentSet->millis, @@ -561,19 +551,6 @@ void loop() { TimeUtils::timeToString().c_str(), thisLoopTow); } -void copyCollectedSensorData(DataSet *set) {// Write the minimum values of the while-loop to a set - for (auto & m_sensor : sensorManager->m_sensors) { - set->sensorValues.push_back(m_sensor.minDistance); - } - set->measurements = sensorManager->lastReadingCount; - memcpy(&(set->readDurationsRightInMicroseconds), - &(sensorManager->m_sensors[0].echoDurationMicroseconds), set->measurements * sizeof(int32_t)); - memcpy(&(set->readDurationsLeftInMicroseconds), - &(sensorManager->m_sensors[1].echoDurationMicroseconds), set->measurements * sizeof(int32_t)); - memcpy(&(set->startOffsetMilliseconds), - &(sensorManager->startOffsetMilliseconds), set->measurements * sizeof(uint16_t)); -} - uint8_t batteryPercentage() { uint8_t result = 0; if (voltageMeter) { diff --git a/src/configServer.cpp b/src/configServer.cpp index 2c982048..870f6af4 100644 --- a/src/configServer.cpp +++ b/src/configServer.cpp @@ -1058,7 +1058,6 @@ static void handleBackupDownload(HTTPRequest *, HTTPResponse * res) { static void handleBackupRestore(HTTPRequest * req, HTTPResponse * res) { HTTPMultipartBodyParser parser(req); - sensorManager->detachInterrupts(); while(parser.nextField()) { @@ -1093,7 +1092,6 @@ static void handleBackupRestore(HTTPRequest * req, HTTPResponse * res) { res->print("ERROR"); } } - sensorManager->attachInterrupts(); } @@ -1533,7 +1531,6 @@ static void handleFlashUpdateUrlAction(HTTPRequest * req, HTTPResponse * res) { log_i("Flash App Url is '%s'", url.c_str()); Firmware f(String("OBS/") + String(OBSVersion)); - sensorManager->detachInterrupts(); if (f.downloadToFlash(url, updateProgress)) { obsDisplay->showTextOnGrid(0, 3, "Success!"); sendRedirect(res, "/updatesd"); @@ -1542,13 +1539,11 @@ static void handleFlashUpdateUrlAction(HTTPRequest * req, HTTPResponse * res) { obsDisplay->showTextOnGrid(0, 4, f.getLastMessage()); sendRedirect(res, "/about"); } - sensorManager->attachInterrupts(); } static void handleFlashFileUpdateAction(HTTPRequest *req, HTTPResponse *res) { // TODO: Add some assertions, cleanup with handleFlashUpdateUrlAction HTTPMultipartBodyParser parser(req); - sensorManager->detachInterrupts(); Update.begin(); Update.onProgress([](size_t pos, size_t all) { obsDisplay->drawProgressBar(4, pos, all); @@ -1585,7 +1580,6 @@ static void handleFlashFileUpdateAction(HTTPRequest *req, HTTPResponse *res) { res->print(errorMsg); } } - sensorManager->attachInterrupts(); } static void handleDeleteFiles(HTTPRequest *req, HTTPResponse * res) { diff --git a/src/displays.cpp b/src/displays.cpp index c5a9db13..fe72a6d5 100644 --- a/src/displays.cpp +++ b/src/displays.cpp @@ -55,13 +55,13 @@ void SSD1306DisplayDevice::displaySimple(uint16_t value) { } void SSD1306DisplayDevice::showValues( - HCSR04SensorInfo sensor1, HCSR04SensorInfo sensor2, uint16_t minDistanceToConfirm, int16_t batteryPercentage, + HCSR04SensorInfo* sensor1, HCSR04SensorInfo* sensor2, uint16_t minDistanceToConfirm, int16_t batteryPercentage, int16_t TemperaturValue, int lastMeasurements, boolean insidePrivacyArea, double speed, uint8_t satellites) { handleHighlight(); - uint16_t value1 = sensor1.minDistance; + uint16_t value1 = sensor1->minDistance; if (minDistanceToConfirm != MAX_SENSOR_VALUE) { value1 = minDistanceToConfirm; } @@ -69,7 +69,7 @@ void SSD1306DisplayDevice::showValues( displaySimple(value1); } else { if (config.displayConfig & DisplayLeft) { - String loc1 = sensor1.sensorLocation; + String loc1 = sensor1->sensorLocation; if (insidePrivacyArea) { loc1 = "(" + loc1 + ")"; } @@ -83,8 +83,8 @@ void SSD1306DisplayDevice::showValues( } // Show sensor2, when DisplayRight is configured if (config.displayConfig & DisplayRight) { - uint16_t value2 = sensor2.distance; - String loc2 = sensor2.sensorLocation; + uint16_t value2 = sensor2->distance; + String loc2 = sensor2->sensorLocation; this->prepareTextOnGrid(3, 0, loc2); if (value2 == MAX_SENSOR_VALUE || value2 == 0) { this->prepareTextOnGrid(2, 1, "---", LARGE_FONT, 5, 0); @@ -98,8 +98,8 @@ void SSD1306DisplayDevice::showValues( const int bufSize = 64; char buffer[bufSize]; // #ifdef NERD_SENSOR_DISTANCE - snprintf(buffer, bufSize - 1, "%03d|%02d|%03d", sensor1.rawDistance, - lastMeasurements, sensor2.rawDistance); + snprintf(buffer, bufSize - 1, "%03d|%02d|%03d", sensor1->rawDistance, + lastMeasurements, sensor2->rawDistance); // #endif #ifdef NERD_HEAP snprintf(buffer, bufSize - 1, "%03d|%02d|%uk", sensor1.rawDistance, diff --git a/src/sensor.cpp b/src/sensor.cpp index 51152a2f..f07e2df8 100644 --- a/src/sensor.cpp +++ b/src/sensor.cpp @@ -22,6 +22,8 @@ */ #include "sensor.h" + +#include "writer.h" #include "FunctionalInterrupt.h" /* * Sensor types: @@ -46,135 +48,43 @@ static const uint16_t MAX_DISTANCE_MEASURED_CM = 320; // candidate to check I co static const uint32_t MIN_DURATION_MICRO_SEC = MIN_DISTANCE_MEASURED_CM * MICRO_SEC_TO_CM_DIVIDER; const uint32_t MAX_DURATION_MICRO_SEC = MAX_DISTANCE_MEASURED_CM * MICRO_SEC_TO_CM_DIVIDER; -/* This time is maximum echo pin high time if the sensor gets no response - * HC-SR04: observed 71ms, docu 60ms - * JSN-SR04T: observed 58ms - */ -static const uint32_t MAX_TIMEOUT_MICRO_SEC = 75000; - -/* To avoid that we receive a echo from a former measurement, we do not - * start a new measurement within the given time from the start of the - * former measurement of the opposite sensor. - * High values can lead to the situation that we only poll the - * primary sensor for a while!? - */ -static const uint32_t SENSOR_QUIET_PERIOD_AFTER_OPPOSITE_START_MICRO_SEC = 30 * 1000; - -/* The last end (echo goes to low) of a measurement must be this far - * away before a new measurement is started. - */ -static const uint32_t SENSOR_QUIET_PERIOD_AFTER_END_MICRO_SEC = 10 * 1000; +void HCSR04SensorManager::initSensors() { + registerSensor(m_sensors[0], 0); + m_sensors[0].sensorLocation = "Right"; -/* The last start of a new measurement must be as long ago as given here - away, until we start a new measurement. - - With 35ms I could get stable readings down to 19cm (new sensor) - - With 30ms I could get stable readings down to 25/35cm only (new sensor) - - It looked fine with the old sensor for all values - */ -static const uint32_t SENSOR_QUIET_PERIOD_AFTER_START_MICRO_SEC = 35 * 1000; + registerSensor(m_sensors[1], 1); + m_sensors[1].sensorLocation = "Left"; -/* Value of HCSR04SensorInfo::end during an ongoing measurement. */ -static const uint32_t MEASUREMENT_IN_PROGRESS = 0; + // queue_buffer = new uint8_t[sizeof(LLMessage) * 40]; + // static_queue = new StaticQueue_t; -/* Some calculations: - * - * Assumption: - * Small car length: 4m (average 2017 4,4m, really smallest is 2.3m ) - * Speed difference: 100km/h = 27.8m/s - * Max interesting distance: MAX_DISTANCE_MEASURED_CM = 250cm - * - * Measured: - * MAX_TIMEOUT_MICRO_SEC == ~ 75_000 micro seconds, if the sensor gets no echo - * - * Time of the overtake process: - * 4m / 27.8m/s = 0.143 sec - * - * We need to have time for 3 measures to get the car - * - * 0.148 sec / 3 measures = 48 milli seconds pre measure = 48_000 micro_sec - * - * -> If we wait till MAX_TIMEOUT_MICRO_SEC (75ms) because the right - * sensor has no measure we are to slow. (3x75 = 225ms > 143ms) - * - * If we only waif tor the primary (left) sensor we are good again, - * and so trigger one sensor while the other is still in "timeout-state" - * we save some time. Assuming 1st measure is just before the car appears - * besides the sensor: - * - * 75ms open sensor + 2x SENSOR_QUIET_PERIOD_AFTER_START_MICRO_SEC (35ms) = 145ms which is - * close to the needed 148ms - */ + sensor_queue = xQueueCreate( 40, sizeof( LLMessage ) /*, queue_buffer, static_queue */); + if (sensor_queue == 0) { + log_e("Oh wow. Sensor queue was not created successfully :("); + } else { + log_i("Sensor queue created"); + } -// Hack to get the pointers to the isr -static HCSR04SensorInfo * TOF_SENSOR[NUMBER_OF_TOF_SENSORS]; + TOFManager::startupManager(sensor_queue); +} void HCSR04SensorManager::registerSensor(const HCSR04SensorInfo& sensorInfo, uint8_t idx) { if (idx >= NUMBER_OF_TOF_SENSORS) { log_e("Can not register sensor for index %d, only %d tof sensors supported", idx, NUMBER_OF_TOF_SENSORS); return; } - m_sensors[idx] = sensorInfo; - pinMode(sensorInfo.triggerPin, OUTPUT); - pinMode(sensorInfo.echoPin, INPUT_PULLUP); // hint from https://youtu.be/xwsT-e1D9OY?t=354 sensorValues[idx] = MAX_SENSOR_VALUE; m_sensors[idx].median = new Median(5, MAX_SENSOR_VALUE); - - TOF_SENSOR[idx] = &m_sensors[idx]; - attachSensorInterrupt(idx); -} - -static void IRAM_ATTR isr(HCSR04SensorInfo* const sensor) { - // since the measurement of start and stop use the same interrupt - // mechanism we should see a similar delay. - if (sensor->end == MEASUREMENT_IN_PROGRESS) { - const uint32_t now = micros(); - sensor->end = now; - } -} - -static void IRAM_ATTR isr0() { - isr(TOF_SENSOR[0]); -} - -static void IRAM_ATTR isr1() { - isr(TOF_SENSOR[1]); -} - -void HCSR04SensorManager::attachSensorInterrupt(uint8_t idx) { - // bad bad bad .... - if (idx == 0) { - attachInterrupt(TOF_SENSOR[idx]->echoPin, isr0, FALLING); - } else { - attachInterrupt(TOF_SENSOR[idx]->echoPin, isr1, FALLING); - } - - // a solution like below leads to crashes with: - // Guru Meditation Error: Core 1 panic'ed (Cache disabled but cached memory region accessed) - // Core 1 was running in ISR context: - // attachInterrupt(sensorInfo.echoPin, - // std::bind(&isr, sensorInfo.echoPin, &sensorInfo.start, &sensorInfo.end), CHANGE); -} - -void HCSR04SensorManager::detachInterrupts() { - for (auto & sensor : m_sensors) { - detachInterrupt(sensor.echoPin); - } -} - -void HCSR04SensorManager::attachInterrupts() { - for (size_t idx = 0; idx < NUMBER_OF_TOF_SENSORS; ++idx) { - attachSensorInterrupt(idx); - } } void HCSR04SensorManager::reset(uint32_t startMillisTicks) { + log_d("Reset"); for (auto & sensor : m_sensors) { sensor.minDistance = MAX_SENSOR_VALUE; memset(&(sensor.echoDurationMicroseconds), 0, sizeof(sensor.echoDurationMicroseconds)); sensor.numberOfTriggers = 0; } lastReadingCount = 0; - lastSensor = 1 - primarySensor; memset(&(startOffsetMilliseconds), 0, sizeof(startOffsetMilliseconds)); startReadingMilliseconds = startMillisTicks; } @@ -189,171 +99,96 @@ void HCSR04SensorManager::setOffsets(std::vector offsets) { } } -/* The primary sensor defines the measurement interval, we trigger a measurement if this - * sensor is ready. - */ -void HCSR04SensorManager::setPrimarySensor(uint8_t idx) { - primarySensor = idx; -} - /* Polls for new readings, if sensors are not ready, the * method returns false. */ bool HCSR04SensorManager::pollDistancesAlternating() { bool newMeasurements = false; - if (lastSensor == primarySensor && isReadyForStart(1 - primarySensor)) { - setSensorTriggersToLow(); - lastSensor = 1 - primarySensor; - sendTriggerToSensor(1 - primarySensor); - } else if (isReadyForStart(primarySensor)) { - newMeasurements = collectSensorResults(); - setSensorTriggersToLow(); - lastSensor = primarySensor; - sendTriggerToSensor(primarySensor); + LLMessage message = {0}; + + while(xQueueReceive(sensor_queue, &message, 5) == pdPASS) { + // // woohoo! we received a new measurement + newMeasurements = true; + processSensorMessage(&message); } + return newMeasurements; } -/* Polls for new readings, if sensors are not ready, the - * method returns false. If sensors are ready and readings - * are available, data is read and updated. If the primary - * sensor is ready for new measurement, a fresh measurement - * is triggered. Method returns true if new data (no timeout - * measurement) was collected. - */ -bool HCSR04SensorManager::pollDistancesParallel() { - bool newMeasurements = false; - if (isReadyForStart(primarySensor)) { - setSensorTriggersToLow(); - newMeasurements = collectSensorResults(); - const bool secondSensorIsReady = isReadyForStart(1 - primarySensor); - sendTriggerToSensor(primarySensor); - if (secondSensorIsReady) { - sendTriggerToSensor(1 - primarySensor); - } +// FIXME +/* Returns true if there was a no timeout reading. */ +bool HCSR04SensorManager::processSensorMessage(LLMessage* message) { + log_d("Processing sensor message from sensor = %i", message->sensor_index); + HCSR04SensorInfo* const sensor = &m_sensors[message->sensor_index]; + bool reading_valid = false; + + uint32_t duration = microsBetween(message->start, message->end); + sensor->echoDurationMicroseconds[lastReadingCount] = duration; + + uint16_t distance; + + if (duration < MIN_DURATION_MICRO_SEC || duration >= MAX_DURATION_MICRO_SEC) { + distance = MAX_SENSOR_VALUE; + } else { + reading_valid = true; + distance = static_cast(duration / MICRO_SEC_TO_CM_DIVIDER); } - return newMeasurements; -} -uint16_t HCSR04SensorManager::getCurrentMeasureIndex() { - return lastReadingCount - 1; -} + sensor->rawDistance = distance; + sensor->median->addValue(distance); + sensorValues[message->sensor_index] = + sensor->distance = correctSensorOffset(medianMeasure(sensor, distance), sensor->offset); -/* Send echo trigger to the selected sensor and prepare measurement data - * structures. - */ -void HCSR04SensorManager::sendTriggerToSensor(uint8_t sensorId) { - HCSR04SensorInfo * const sensor = &(m_sensors[sensorId]); - updateStatistics(sensor); - sensor->end = MEASUREMENT_IN_PROGRESS; // will be updated with LOW signal - sensor->numberOfTriggers++; - sensor->measurementRead = false; - const uint32_t now = micros(); - sensor->trigger = now; // will be updated with HIGH signal - sensor->start = now; - digitalWrite(sensor->triggerPin, HIGH); - // 10us are specified but some sensors are more stable with 20us according - // to internet reports - delayMicroseconds(20); - digitalWrite(sensor->triggerPin, LOW); -} + if (sensor->distance > 0 && sensor->distance < sensor->minDistance) { + sensor->minDistance = sensor->distance; + } + sensor->measurementRead = true; -/* Checks if the given sensor is ready for a new measurement cycle. - */ -boolean HCSR04SensorManager::isReadyForStart(uint8_t sensorId) { - HCSR04SensorInfo * const sensor = &m_sensors[sensorId]; - boolean ready = false; - const uint32_t now = micros(); - const uint32_t start = sensor->start; - const uint32_t end = sensor->end; - if (end != MEASUREMENT_IN_PROGRESS) { // no measurement in flight or just finished - const uint32_t startOther = m_sensors[1 - sensorId].start; - if ( (microsBetween(now, end) > SENSOR_QUIET_PERIOD_AFTER_END_MICRO_SEC) - && (microsBetween(now, start) > SENSOR_QUIET_PERIOD_AFTER_START_MICRO_SEC) - && (microsBetween(now, startOther) > SENSOR_QUIET_PERIOD_AFTER_OPPOSITE_START_MICRO_SEC)) { - ready = true; - } - if (digitalRead(sensor->echoPin) != LOW) { - log_e("Measurement done, but echo pin is high for %s sensor", sensor->sensorLocation); - sensor->numberOfLowAfterMeasurement++; + startOffsetMilliseconds[lastReadingCount] = millisSince(startReadingMilliseconds); + + if(message->sensor_index == 1) { + if (lastReadingCount < MAX_NUMBER_MEASUREMENTS_PER_INTERVAL - 1) { + lastReadingCount++; } - } else if (microsBetween(now, start) > MAX_TIMEOUT_MICRO_SEC) { - sensor->numberOfToLongMeasurement++; - // signal or interrupt was lost altogether this is an error, - // should we raise it?? Now pretend the sensor is ready, hope it helps to give it a trigger. - ready = true; - // do not log to much there might be devices out there with one sensor only - log_d("Timeout trigger for %s duration %u us - echo pin state: %d trigger: %u " - "start: %u end: %u now: %u", - sensor->sensorLocation, now - start, digitalRead(sensor->echoPin), sensor->trigger, - start, end, now); } - return ready; + + return reading_valid; } -bool HCSR04SensorManager::collectSensorResults() { - bool validReading = false; - for (size_t idx = 0; idx < NUMBER_OF_TOF_SENSORS; ++idx) { - if (collectSensorResult(idx)) { - validReading = true; - } - } - if (validReading) { - registerReadings(); - } - return validReading; +uint16_t HCSR04SensorManager::getSensorValue(uint8_t sensorIndex) { + return sensorValues[sensorIndex]; } -void HCSR04SensorManager::registerReadings() { - startOffsetMilliseconds[lastReadingCount] = millisSince(startReadingMilliseconds); - if (lastReadingCount < MAX_NUMBER_MEASUREMENTS_PER_INTERVAL - 1) { - lastReadingCount++; - } +HCSR04SensorInfo* HCSR04SensorManager::getSensor(uint8_t sensorIndex) { + return &m_sensors[sensorIndex]; } -/* Returns true if there was a no timeout reading. */ -bool HCSR04SensorManager::collectSensorResult(uint8_t sensorId) { - HCSR04SensorInfo* const sensor = &m_sensors[sensorId]; - if (sensor->measurementRead) { - return false; // already read - } - bool validReading = false; - const uint32_t end = sensor->end; - const uint32_t start = getFixedStart(sensorId, sensor); - uint32_t duration; - if (end == MEASUREMENT_IN_PROGRESS) { - duration = microsSince(start); - if (duration < MAX_DURATION_MICRO_SEC) { - return false; // still measuring - } - // measurement is still in flight! But the time we want to wait is up (> MAX_DURATION_MICRO_SEC) - sensor->echoDurationMicroseconds[lastReadingCount] = -1; - } else { - duration = microsBetween(start, end); +boolean HCSR04SensorManager::hasReadings(uint8_t sensorIndex) { + // FIXME + return false; +} - sensor->echoDurationMicroseconds[lastReadingCount] = duration; - } - uint16_t dist; - if (duration < MIN_DURATION_MICRO_SEC || duration >= MAX_DURATION_MICRO_SEC) { - dist = MAX_SENSOR_VALUE; - } else { - validReading = true; - dist = static_cast(duration / MICRO_SEC_TO_CM_DIVIDER); +void HCSR04SensorManager::copyData(DataSet* set) { + log_d("Let's copy some data."); + for (int i = 0; i < NUMBER_OF_TOF_SENSORS; i++){ + set->sensorValues.push_back(m_sensors[i].minDistance); } - sensor->rawDistance = dist; - sensor->median->addValue(dist); - sensorValues[sensorId] = - sensor->distance = correctSensorOffset(medianMeasure(sensor, dist), sensor->offset); - log_v("Raw sensor[%d] distance read %03u / %03u (%03u, %03u, %03u) -> *%03ucm*, duration: %zu us - echo pin state: %d", - sensorId, sensor->rawDistance, dist, sensor->distances[0], sensor->distances[1], - sensor->distances[2], sensorValues[sensorId], duration, digitalRead(sensor->echoPin)); + set->measurements = sensorManager->lastReadingCount; + memcpy(&(set->readDurationsRightInMicroseconds), + &(m_sensors[0].echoDurationMicroseconds), set->measurements * sizeof(int32_t)); + memcpy(&(set->readDurationsLeftInMicroseconds), + &(m_sensors[1].echoDurationMicroseconds), set->measurements * sizeof(int32_t)); + memcpy(&(set->startOffsetMilliseconds), + &(startOffsetMilliseconds), set->measurements * sizeof(uint16_t)); +} - if (sensor->distance > 0 && sensor->distance < sensor->minDistance) { - sensor->minDistance = sensor->distance; - } - sensor->measurementRead = true; - return validReading; +uint16_t HCSR04SensorManager::getLastValidValue(uint8_t sensorIndex) { + // if there was no measurement of this sensor for the current index, it is the + // one before. This happens with fast confirmations. + uint16_t val = 0; + // FIXME + return val; } uint16_t HCSR04SensorManager::getRawMedianDistance(uint8_t sensorId) { @@ -393,9 +228,11 @@ uint32_t HCSR04SensorManager::getNumberOfInterruptAdjustments(const uint8_t sens * sensor this adds 300 microseconds or 5 centimeters to the measured result. * After research, is a bug in the ESP!? See https://esp32.com/viewtopic.php?t=10124 */ -uint32_t HCSR04SensorManager::getFixedStart( +// FIXME +/* uint32_t HCSR04SensorManager::getFixedStart( + uint32_t start = 0; size_t idx, HCSR04SensorInfo * const sensor) { - uint32_t start = sensor->start; + uint32_t start = sensor->start; // the error appears if both sensors trigger the interrupt at the exact same // time, if this happens, trigger time == start time if (start == sensor->trigger && sensor->end != MEASUREMENT_IN_PROGRESS) { @@ -409,6 +246,7 @@ uint32_t HCSR04SensorManager::getFixedStart( } return start; } + */ uint16_t HCSR04SensorManager::correctSensorOffset(uint16_t dist, uint16_t offset) { uint16_t result; @@ -422,28 +260,25 @@ uint16_t HCSR04SensorManager::correctSensorOffset(uint16_t dist, uint16_t offset return result; } -void HCSR04SensorManager::setSensorTriggersToLow() { - for (auto & m_sensor : m_sensors) { - digitalWrite(m_sensor.triggerPin, LOW); - } -} +void HCSR04SensorManager::updateStatistics(LLMessage* message) { +#if 0 + HCSR04SensorInfo* sensor = m_sensors[message->sensor_index]; + + const uint32_t start_delay = message->start - message->trigger; + if (start_delay > 0) { + sensor->lastDelayTillStartUs = start_delay; -void HCSR04SensorManager::updateStatistics(HCSR04SensorInfo * const sensor) { - if (sensor->end != MEASUREMENT_IN_PROGRESS) { - const uint32_t startDelay = sensor->start - sensor->trigger; - if (startDelay != 0) { - const uint32_t duration = sensor->end - sensor->start; - if (duration > sensor->maxDurationUs) { - sensor->maxDurationUs = duration; - } - if (duration < sensor->minDurationUs && duration > 1) { - sensor->minDurationUs = duration; - } - sensor->lastDelayTillStartUs = startDelay; + const uint32_t duration = message->end - message->start; + if (duration > sensor->maxDurationUs) { + sensor->maxDurationUs = duration; + } + if (duration < sensor->minDurationUs && duration > 1) { + sensor->minDurationUs = duration; } } else { sensor->numberOfNoSignals++; } +#endif } /* Determines the microseconds between the 2 time counters given. @@ -511,3 +346,4 @@ uint16_t HCSR04SensorManager::median(uint16_t a, uint16_t b, uint16_t c) { } return c; } + diff --git a/src/sensor.h b/src/sensor.h index 443c684b..906a265e 100644 --- a/src/sensor.h +++ b/src/sensor.h @@ -30,6 +30,10 @@ #include "globals.h" #include "utils/median.h" +#include "tofmanager.h" + +struct DataSet; + /* About the speed of sound: See also http://www.sengpielaudio.com/Rechner-schallgeschw.htm (german) - speed of sound depends on ambient temperature @@ -67,12 +71,7 @@ struct HCSR04SensorInfo { uint16_t nextMedianDistance = 0; uint16_t minDistance = MAX_SENSOR_VALUE; uint16_t distance = MAX_SENSOR_VALUE; - char* sensorLocation; - // timestamp when the trigger signal was sent in us micros() - uint32_t trigger = 0; - volatile uint32_t start = 0; - /* if end == 0 - a measurement is in progress */ - volatile uint32_t end = 1; + char const* sensorLocation; int32_t echoDurationMicroseconds[MAX_NUMBER_MEASUREMENTS_PER_INTERVAL + 1]; Median*median = nullptr; @@ -97,15 +96,12 @@ class HCSR04SensorManager { void reset(uint32_t startMillisTicks); void registerSensor(const HCSR04SensorInfo &, uint8_t idx); void setOffsets(std::vector); - void setPrimarySensor(uint8_t idx); - void detachInterrupts(); - void attachInterrupts(); + void initSensors(); /* Returns the current raw median distance in cm for the * given sensor. */ uint16_t getRawMedianDistance(uint8_t sensorId); /* Index for CSV. */ - uint16_t getCurrentMeasureIndex(); uint32_t getMaxDurationUs(uint8_t sensorId); uint32_t getMinDurationUs(uint8_t sensorId); uint32_t getLastDelayTillStartUs(uint8_t sensorId); @@ -114,34 +110,36 @@ class HCSR04SensorManager { uint32_t getNumberOfToLongMeasurement(const uint8_t sensorId); uint32_t getNumberOfInterruptAdjustments(const uint8_t sensorId); - HCSR04SensorInfo m_sensors[NUMBER_OF_TOF_SENSORS]; - uint16_t sensorValues[NUMBER_OF_TOF_SENSORS]; - uint16_t lastReadingCount = 0; - uint16_t startOffsetMilliseconds[MAX_NUMBER_MEASUREMENTS_PER_INTERVAL + 1]; - bool pollDistancesParallel(); - bool pollDistancesAlternating(); + HCSR04SensorInfo* getSensor(uint8_t sensorIndex); + uint16_t getSensorValue(uint8_t sensorIndex); + uint16_t getLastValidValue(uint8_t sensorIndex); + boolean hasReadings(uint8_t sensorIndex); + void copyData(DataSet* set); - protected: + bool pollDistancesAlternating(); - private: - void sendTriggerToSensor(uint8_t sensorId); - bool collectSensorResult(uint8_t sensorId); - void setSensorTriggersToLow(); - bool collectSensorResults(); - void attachSensorInterrupt(uint8_t idx); - uint32_t getFixedStart(size_t idx, HCSR04SensorInfo * const sensor); - boolean isReadyForStart(uint8_t sensorId); - void registerReadings(); static uint16_t medianMeasure(HCSR04SensorInfo* const sensor, uint16_t value); static uint16_t median(uint16_t a, uint16_t b, uint16_t c); static uint16_t correctSensorOffset(uint16_t dist, uint16_t offset); static uint32_t microsBetween(uint32_t a, uint32_t b); static uint32_t microsSince(uint32_t a); static uint16_t millisSince(uint16_t milliseconds); - static void updateStatistics(HCSR04SensorInfo * const sensor); + + protected: + HCSR04SensorInfo m_sensors[NUMBER_OF_TOF_SENSORS]; + uint16_t sensorValues[NUMBER_OF_TOF_SENSORS]; + static void updateStatistics(LLMessage* message); + uint16_t lastReadingCount = 0; + uint16_t startOffsetMilliseconds[MAX_NUMBER_MEASUREMENTS_PER_INTERVAL + 1]; + + private: + bool processSensorMessage(LLMessage* message); + uint32_t getFixedStart(size_t idx, HCSR04SensorInfo * const sensor); uint32_t startReadingMilliseconds = 0; - uint8_t primarySensor = 1; - uint8_t lastSensor; + + QueueHandle_t sensor_queue; + uint8_t* queue_buffer; + StaticQueue_t *static_queue; }; #endif diff --git a/src/tofmanager.cpp b/src/tofmanager.cpp new file mode 100644 index 00000000..da39dac0 --- /dev/null +++ b/src/tofmanager.cpp @@ -0,0 +1,301 @@ +#include "tofmanager.h" +#include "sensor.h" + +/* Value of HCSR04SensorInfo::end during an ongoing measurement. */ +#define MEASUREMENT_IN_PROGRESS 0 + +/* To avoid that we receive a echo from a former measurement, we do not + * start a new measurement within the given time from the start of the + * former measurement of the opposite sensor. + * High values can lead to the situation that we only poll the + * primary sensor for a while!? + */ +#define SENSOR_QUIET_PERIOD_AFTER_OPPOSITE_START_MICRO_SEC (30 * 1000) + +/* The last end (echo goes to low) of a measurement must be this far + * away before a new measurement is started. + */ +#define SENSOR_QUIET_PERIOD_AFTER_END_MICRO_SEC (10 * 1000) + +/* The last start of a new measurement must be as long ago as given here + away, until we start a new measurement. + - With 35ms I could get stable readings down to 19cm (new sensor) + - With 30ms I could get stable readings down to 25/35cm only (new sensor) + - It looked fine with the old sensor for all values + */ +#define SENSOR_QUIET_PERIOD_AFTER_START_MICRO_SEC (35 * 1000) + +/* This time is maximum echo pin high time if the sensor gets no response + * HC-SR04: observed 71ms, docu 60ms + * JSN-SR04T: observed 58ms + */ +#define MAX_TIMEOUT_MICRO_SEC 75000 + +/* The core to run the RT component on */ +#define RT_CORE 0 + +/* Delay to call when we're waiting for work */ +#define TICK_DELAY 1 + +/* Delay in ticks for when we run out of memory */ +#define OOM_TICK_DELAY 10000 + +/* Enable init code for running alone */ +#define ALONE_ON_CORE 1 + +/* default interrupt flags */ +#define OBS_INTR_FLAG 0 + +/* Some calculations: + * + * Assumption: + * Small car length: 4m (average 2017 4,4m, really smallest is 2.3m ) + * Speed difference: 100km/h = 27.8m/s + * Max interesting distance: MAX_DISTANCE_MEASURED_CM = 250cm + * + * Measured: + * MAX_TIMEOUT_MICRO_SEC == ~ 75_000 micro seconds, if the sensor gets no echo + * + * Time of the overtake process: + * 4m / 27.8m/s = 0.143 sec + * + * We need to have time for 3 measures to get the car + * + * 0.148 sec / 3 measures = 48 milli seconds pre measure = 48_000 micro_sec + * + * -> If we wait till MAX_TIMEOUT_MICRO_SEC (75ms) because the right + * sensor has no measure we are to slow. (3x75 = 225ms > 143ms) + * + * If we only waif tor the primary (left) sensor we are good again, + * and so trigger one sensor while the other is still in "timeout-state" + * we save some time. Assuming 1st measure is just before the car appears + * besides the sensor: + * + * 75ms open sensor + 2x SENSOR_QUIET_PERIOD_AFTER_START_MICRO_SEC (35ms) = 145ms which is + * close to the needed 148ms + */ + +/* Send echo trigger to the selected sensor and prepare measurement data + * structures. + */ +void TOFManager::startMeasurement(uint8_t sensorId) { + LLSensor* sensor = sensors(sensorId); + + sensor->started_measurements += 1; + + sensor->end = MEASUREMENT_IN_PROGRESS; // will be updated with LOW signal + unsigned long now = esp_timer_get_time(); + + sensor->trigger = 0; // will be updated with HIGH signal + sensor->start = now; + + gpio_set_level(sensor->triggerPin, 1); + // 10us are specified but some sensors are more stable with 20us according + // to internet reports + usleep(20); + gpio_set_level(sensor->triggerPin, 0); + + attachSensorInterrupt(sensorId); +} + +/* Checks if the given sensor is ready for a new measurement cycle. + */ +boolean TOFManager::isSensorReady(uint8_t sensorId) { + LLSensor* sensor = sensors(sensorId); + boolean ready = false; + const uint32_t now = micros(); + const uint32_t start = sensor->start; + const uint32_t end = sensor->end; + if (end != MEASUREMENT_IN_PROGRESS) { // no measurement in flight or just finished + const uint32_t startOther = sensors(1 - sensorId)->start; + if ( (HCSR04SensorManager::microsBetween(now, end) > SENSOR_QUIET_PERIOD_AFTER_END_MICRO_SEC) + && (HCSR04SensorManager::microsBetween(now, start) > SENSOR_QUIET_PERIOD_AFTER_START_MICRO_SEC) + && (HCSR04SensorManager::microsBetween(now, startOther) > SENSOR_QUIET_PERIOD_AFTER_OPPOSITE_START_MICRO_SEC)) { + ready = true; + sensors(sensorId)->successful_measurements += 1; + } + } else if (HCSR04SensorManager::microsBetween(now, start) > MAX_TIMEOUT_MICRO_SEC) { + // signal or interrupt was lost altogether this is an error, + // should we raise a it?? Now pretend the sensor is ready, hope it helps to give it a trigger. + ready = true; + sensors(sensorId)->successful_measurements += 1; + } + return ready; +} + +boolean TOFManager::isMeasurementComplete(uint8_t sensor_idx) { + if (sensors(sensor_idx)->end != MEASUREMENT_IN_PROGRESS) { + return true; + } + uint32_t now = esp_timer_get_time(); + if (sensors(sensor_idx)->start + MAX_TIMEOUT_MICRO_SEC < now) { + return true; + } + + return false; +} + +void TOFManager::startupManager(QueueHandle_t queue) { + esp_log_level_set("tofmanager", ESP_LOG_INFO); + ESP_LOGI("tofmanager","Currently on core %i", xPortGetCoreID()); + ESP_LOGI("tofmanager","Switching to core %i", RT_CORE); + + xTaskCreatePinnedToCore( + TOFManager::sensorTaskFunction, /* Task function. */ + "TOFManager", /* String with name of task. */ + 16 * 1024, /* Stack size in bytes. */ + queue, /* Parameter passed as input of the task */ + 1, /* Priority of the task. */ + NULL, /* Task handle. */ + RT_CORE /* the core to run on */ + ); +} + +void TOFManager::sensorTaskFunction(void *parameter) { + esp_log_level_set("tofmanager", ESP_LOG_INFO); + isr_log_i("tofmanager", "Hello from core %i!", xPortGetCoreID()); + + LLSensor* sensors = new LLSensor[2]; + + TOFManager* sensorManager = new TOFManager((QueueHandle_t)parameter, sensors); + + if (sensors == 0 || sensorManager == 0) { + ESP_LOGE("tofmanager","Error, apparently we've run out of memory (sensors=%x, sensorManager=%x).", + sensors, sensorManager); + } + + sensorManager->init(); + sensorManager->loop(); +} + +void TOFManager::init() { + LLSensor* sensorManaged1 = sensors(0); + sensorManaged1->triggerPin = GPIO_NUM_25; + sensorManaged1->echoPin = GPIO_NUM_26; + setupSensor(sensorManaged1, 0); + + LLSensor* sensorManaged2 = sensors(1); + sensorManaged2->triggerPin = GPIO_NUM_15; + sensorManaged2->echoPin = GPIO_NUM_4; + setupSensor(sensorManaged2, 1); + +#if ALONE_ON_CORE + esp_timer_init(); + gpio_install_isr_service(OBS_INTR_FLAG); +#endif + ESP_LOGI("tofmanager", "TOFManager init complete"); +} + +void TOFManager::sendData(uint8_t sensor_idx) { + LLSensor* sensor = sensors(sensor_idx); + + LLMessage message = {0}; + + message.sensor_index = sensor_idx; + message.trigger = sensor->trigger; + message.start = sensor->start; + message.end = sensor->end; + xQueueSendToBack(sensor_queue, &message, portMAX_DELAY); + //delete message; +} + +void TOFManager::loop() { + ESP_LOGI("tofmanager","Entering TOFManager::loop"); + currentSensor = primarySensor; + startMeasurement(primarySensor); + + int statLog = 0; + + while ( true ) { + if (statLog > 2048) { + ESP_LOGI("tofmanager", "statlog: sensor1->irq_cnt=%u, sensor2->irq_cnt=%u", + sensors(0)->interrupt_count, sensors(1)->interrupt_count); + ESP_LOGI("tofmanager", "statlog: success=(%u, %u), started=(%u,%u)", + sensors(0)->successful_measurements, + sensors(1)->successful_measurements, + sensors(0)->started_measurements, + sensors(1)->started_measurements); + statLog = 0; + } + statLog += 1; + + // wait for sensor + if (!isMeasurementComplete(currentSensor)) { + vTaskDelay(TICK_DELAY); + } else { + sendData(currentSensor); + + // switch to next sensor + if (currentSensor == primarySensor && isSensorReady(1 - primarySensor)) { + disableMeasurements(); + currentSensor = 1 - primarySensor; + startMeasurement(currentSensor); + } else if (isSensorReady(primarySensor)) { + disableMeasurements(); + currentSensor = primarySensor; + startMeasurement(primarySensor); + } + } + } +} + +static void IRAM_ATTR sensorInterrupt(void *isrParam) { + LLSensor* sensor = (LLSensor*) isrParam; + // since the measurement of start and stop use the same interrupt + // mechanism we should see a similar delay. + if (sensor->end == MEASUREMENT_IN_PROGRESS) { + sensor->interrupt_count += 1; + + if (sensor->trigger == 0) { + sensor->trigger = esp_timer_get_time(); + } else { + sensor->end = esp_timer_get_time(); + } + } +} + +void TOFManager::setupSensor(LLSensor* sensor, uint8_t idx) { + gpio_config_t io_conf_trigger = {}; + io_conf_trigger.intr_type = GPIO_INTR_DISABLE; + io_conf_trigger.mode = GPIO_MODE_OUTPUT; + io_conf_trigger.pin_bit_mask = 1ULL << sensor->triggerPin; + io_conf_trigger.pull_down_en = GPIO_PULLDOWN_DISABLE; + io_conf_trigger.pull_up_en = GPIO_PULLUP_DISABLE; + gpio_config(&io_conf_trigger); + // gpio_pad_select_gpio(sensor->triggerPin); + // gpio_set_direction(sensor->triggerPin, GPIO_MODE_OUTPUT); + // gpio_pulldown_dis(sensor->triggerPin); + // gpio_pullup_dis(sensor->triggerPin); + + gpio_config_t io_conf_echo = {}; + io_conf_echo.intr_type = GPIO_INTR_ANYEDGE; + io_conf_echo.mode = GPIO_MODE_INPUT; + io_conf_echo.pin_bit_mask = 1ULL << sensor->echoPin; + io_conf_echo.pull_down_en = GPIO_PULLDOWN_DISABLE; + io_conf_echo.pull_up_en = GPIO_PULLUP_ENABLE; + gpio_config(&io_conf_echo); + + // gpio_pad_select_gpio(sensor->echoPin); + // gpio_set_direction(sensor->echoPin, GPIO_MODE_INPUT); + // gpio_pulldown_dis(sensor->echoPin); + // gpio_pullup_en(sensor->echoPin); // hint from https://youtu.be/xwsT-e1D9OY?t=354 + // gpio_intr_enable(sensor->echoPin); + + // gpio_set_intr_type(sensor->echoPin, GPIO_INTR_NEGEDGE); +} + +void TOFManager::attachSensorInterrupt(uint8_t idx) { + gpio_isr_handler_add(sensors(idx)->echoPin, sensorInterrupt, sensors(idx)); +} + +void TOFManager::detachInterrupts() { + for (int i = 0; i < NUMBER_OF_TOF_SENSORS; i++) { + gpio_isr_handler_remove(sensors(i)->echoPin); + } +} + +void TOFManager::disableMeasurements() { + for (int i = 0; i < NUMBER_OF_TOF_SENSORS; i ++) { + gpio_set_level(sensors(i)->triggerPin, 0); + } +} diff --git a/src/tofmanager.h b/src/tofmanager.h new file mode 100644 index 00000000..bc0b9524 --- /dev/null +++ b/src/tofmanager.h @@ -0,0 +1,83 @@ +#ifndef TOFMANAGER_H +#define TOFMANAGER_H + +#include + +#include "utils/median.h" + +typedef struct { + uint8_t sensor_index; // the global index of the sensor this event occured on + uint32_t trigger; // the microseconds since the sensor was triggered + uint32_t start; // the microseconds since the sensor started the measurement + uint32_t end; // the microseconds since the sensor started the measurement +} LLMessage; + +/* low-level TOF sensor state (private interface) + * + * This structs holds the low level state of each sensor channel. + */ +typedef struct { + gpio_num_t triggerPin; + gpio_num_t echoPin; + + /* timestamp when the trigger signal was sent in microseconds */ + volatile uint32_t trigger = 0; + /* timestamp when the reading was started in microseconds */ + volatile uint32_t start = 0; + /* timestamp when the reading was complete in microseconds + * if end == 0 - a measurement is in progress */ + volatile uint32_t end = 0; + + volatile uint32_t interrupt_count = 0; + uint32_t started_measurements = 0; + uint32_t successful_measurements = 0; +} LLSensor; + +/* low-level TOF sensor manager + * + * This class is responsible for all the low level plumbing: interrupts, timings, pins, etc + * This class deals exclusively with microseconds. The companion high level interface is + * HCSR04SensorManager. + */ +class TOFManager { + public: + void setupSensor(LLSensor*, uint8_t idx); + void detachInterrupts(); + void attachInterrupts(); + + void loop(); + void init(); + + /* + * startup() + * + * The only thing you ever need to call. This will spawn another task on a different CPU + * and you won't have to interact with this class at all. + * + * The LLManager will send you LLMessages on the sensor_queue you pass in. + */ + static void startupManager(QueueHandle_t sensor_queue); + private: + TOFManager(QueueHandle_t q, LLSensor* sensors) : m_sensors(sensors), sensor_queue(q) {} + ~TOFManager(); + + void startMeasurement(uint8_t sensorId); + void disableMeasurements(); + void attachSensorInterrupt(uint8_t idx); + boolean isSensorReady(uint8_t sensorId); + boolean isMeasurementComplete(uint8_t sensor_idx); + + uint8_t primarySensor = 1; + uint8_t currentSensor = 0; + + void sendData(uint8_t sensor_idx); + static void sensorTaskFunction(void *parameter); + + LLSensor* sensors(int i) { + return &m_sensors[i]; + } + LLSensor* m_sensors; + QueueHandle_t sensor_queue; +}; + +#endif // TOFMANAGER_H From 401df1aebed5cadf4f4268818c40d744c689d2c8 Mon Sep 17 00:00:00 2001 From: Mark Meyer Date: Wed, 16 Aug 2023 13:48:32 +0200 Subject: [PATCH 03/11] chore: further refactoring and move everything to C++20 --- src/OpenBikeSensorFirmware.cpp | 8 +- src/config.h | 4 + src/configServer.cpp | 40 ++++----- src/displays.cpp | 2 +- src/displays.h | 4 +- src/globals.h | 4 +- src/obsimprov.h | 28 ++++--- src/sensor.cpp | 145 ++++++++++++++++----------------- src/sensor.h | 59 ++++++++------ src/tofmanager.cpp | 89 +++++++++++--------- src/tofmanager.h | 13 +-- 11 files changed, 212 insertions(+), 184 deletions(-) diff --git a/src/OpenBikeSensorFirmware.cpp b/src/OpenBikeSensorFirmware.cpp index 553b4f22..a7cd8e4e 100644 --- a/src/OpenBikeSensorFirmware.cpp +++ b/src/OpenBikeSensorFirmware.cpp @@ -58,7 +58,7 @@ Button button(PUSHBUTTON_PIN); Config config; SSD1306DisplayDevice* obsDisplay; -HCSR04SensorManager* sensorManager; +DistanceSensorManager* sensorManager; #ifdef OBS_BLUETOOTH static BluetoothManager* bluetoothManager; #endif @@ -119,7 +119,7 @@ void switch_wire_speed_to_SSD1306(){ // FIXME void setupSensors() { - sensorManager = new HCSR04SensorManager; + sensorManager = new DistanceSensorManager; sensorManager->initSensors(); } @@ -440,9 +440,9 @@ void loop() { } button.handle(currentTimeMillis); gps.handle(); - if (sensorManager->pollDistancesAlternating()) { + if (sensorManager->pollDistances()) { // if a new minimum on the selected sensor is detected, the value and the time of detection will be stored - const uint16_t reading = sensorManager->getLastValidValue(confirmationSensorID); + const uint16_t reading = sensorManager->getLastValidRawDistance(confirmationSensorID); if (reading > 0 && reading < minDistanceToConfirm) { minDistanceToConfirm = reading; datasetToConfirm = currentSet; diff --git a/src/config.h b/src/config.h index da970b9c..537e1b73 100644 --- a/src/config.h +++ b/src/config.h @@ -29,6 +29,10 @@ #include #include +#include + +typedef std::basic_string obs_string; + enum DisplayOptions { DisplaySatellites = 0x01, // 1 DisplayVelocity = 0x02, // 2 diff --git a/src/configServer.cpp b/src/configServer.cpp index 870f6af4..8cd49db0 100644 --- a/src/configServer.cpp +++ b/src/configServer.cpp @@ -40,6 +40,8 @@ #include "utils/timeutils.h" #include "obsimprov.h" +typedef std::basic_string obs_string; + using namespace httpsserver; static const char *const HTML_ENTITY_FAILED_CROSS = "❌"; @@ -419,7 +421,7 @@ static String getParameter(const std::vector> ¶ms, static String getParameter(HTTPRequest *req, const String& name, const String& def = "") { - std::string value; + obs_string value; if (req->getParams()->getQueryParameter(name.c_str(), value)) { return String(value.c_str()); } @@ -557,7 +559,7 @@ void registerPages(HTTPServer * httpServer) { httpServer->registerNode(new ResourceNode("/settings/security", HTTP_POST, handleSettingSecurityAction)); httpServer->addMiddleware(&accessFilter); - httpServer->setDefaultHeader("Server", std::string("OBS/") + OBSVersion); + httpServer->setDefaultHeader("Server", obs_string("OBS/") + OBSVersion); } void beginPages() { @@ -566,7 +568,7 @@ void beginPages() { insecureServer->registerNode(new ResourceNode("/cert", HTTP_GET, handleDownloadCert)); insecureServer->registerNode(new ResourceNode("/http", HTTP_POST, handleHttpAction)); insecureServer->setDefaultNode(new ResourceNode("", HTTP_GET, handleHttpsRedirect)); - insecureServer->setDefaultHeader("Server", std::string("OBS/") + OBSVersion); + insecureServer->setDefaultHeader("Server", obs_string("OBS/") + OBSVersion); } static int ticks; @@ -702,7 +704,7 @@ static void wifiConectedActions() { } /* callback function called if wifi data is received via improv */ -bool initWifi(const std::string & ssid, const std::string & password) { +bool initWifi(const obs_string & ssid, const obs_string & password) { log_i("Received WiFi credentials for SSID '%s'", ssid.c_str()); theObsConfig->setProperty(0, ObsConfig::PROPERTY_WIFI_SSID, ssid); theObsConfig->setProperty(0, ObsConfig::PROPERTY_WIFI_PASSWORD, password); @@ -735,8 +737,8 @@ static ObsImprov::State improvCallbackGetWifiStatus() { return result; } -static std::string improvCallbackGetDeviceUrl() { - std::string url = ""; +static obs_string improvCallbackGetDeviceUrl() { + obs_string url = ""; if (WiFiClass::status() == WL_CONNECTED) { theObsConfig->saveConfig(); url += "http://"; @@ -881,7 +883,7 @@ static void handleAbout(HTTPRequest *req, HTTPResponse * res) { res->print("

ESP32

"); res->print(keyValue("Chip Model", ESP.getChipModel())); - res->print(keyValue("Chip Revision", ESP.getChipRevision())); + res->print(keyValue("Chip Revision", (uint32_t) ESP.getChipRevision())); res->print(keyValue("Heap size", ObsUtils::toScaledByteString(ESP.getHeapSize()))); res->print(keyValue("Free heap", ObsUtils::toScaledByteString(ESP.getFreeHeap()))); res->print(keyValue("Min. free heap", ObsUtils::toScaledByteString(ESP.getMinFreeHeap()))); @@ -909,8 +911,8 @@ static void handleAbout(HTTPRequest *req, HTTPResponse * res) { res->print(page); page.clear(); - page += keyValue("Cores", ESP.getChipCores()); - page += keyValue("CPU frequency", ESP.getCpuFreqMHz(), "MHz"); + page += keyValue("Cores", (const uint32_t) ESP.getChipCores()); + page += keyValue("CPU frequency", (const uint32_t) ESP.getCpuFreqMHz(), "MHz"); page += keyValue("SPIFFS size", ObsUtils::toScaledByteString(SPIFFS.totalBytes())); page += keyValue("SPIFFS used", ObsUtils::toScaledByteString(SPIFFS.usedBytes())); @@ -973,12 +975,12 @@ static void handleAbout(HTTPRequest *req, HTTPResponse * res) { page += keyValue("SD fs used", ObsUtils::toScaledByteString(SD.usedBytes())); page += "

TOF Sensors

"; - page += keyValue("Left Sensor raw", sensorManager->getRawMedianDistance(LEFT_SENSOR_ID), "cm"); + page += keyValue("Left Sensor raw", (const uint32_t) sensorManager->getMedianRawDistance(LEFT_SENSOR_ID), "cm"); page += keyValue("Left Sensor max duration", sensorManager->getMaxDurationUs(LEFT_SENSOR_ID), "µs"); page += keyValue("Left Sensor min duration", sensorManager->getMinDurationUs(LEFT_SENSOR_ID), "µs"); page += keyValue("Left Sensor last start delay", sensorManager->getLastDelayTillStartUs(LEFT_SENSOR_ID), "µs"); page += keyValue("Left Sensor signal errors", sensorManager->getNoSignalReadings(LEFT_SENSOR_ID)); - page += keyValue("Right Sensor raw", sensorManager->getRawMedianDistance(RIGHT_SENSOR_ID), "cm"); + page += keyValue("Right Sensor raw", (const uint32_t) sensorManager->getMedianRawDistance(RIGHT_SENSOR_ID), "cm"); page += keyValue("Right Sensor max duration", sensorManager->getMaxDurationUs(RIGHT_SENSOR_ID), "µs"); page += keyValue("Right Sensor min duration", sensorManager->getMinDurationUs(RIGHT_SENSOR_ID), "µs"); page += keyValue("Right Sensor last start delay", sensorManager->getLastDelayTillStartUs(RIGHT_SENSOR_ID), "µs"); @@ -993,18 +995,18 @@ static void handleAbout(HTTPRequest *req, HTTPResponse * res) { page += keyValue("GPS hdop", gps.getCurrentGpsRecord().getHdopString()); page += keyValue("GPS fix", String(gps.getCurrentGpsRecord().getFixStatus(), 16)); page += keyValue("GPS fix flags", String(gps.getCurrentGpsRecord().getFixStatusFlags(), 16)); - page += keyValue("GPS satellites", gps.getValidSatellites()); + page += keyValue("GPS satellites",(const uint32_t) gps.getValidSatellites()); page += keyValue("GPS uptime", gps.getUptime(), "ms"); - page += keyValue("GPS noise level", gps.getLastNoiseLevel()); + page += keyValue("GPS noise level", (const uint32_t) gps.getLastNoiseLevel()); page += keyValue("GPS baud rate", gps.getBaudRate()); page += keyValue("GPS ALP bytes", gps.getNumberOfAlpBytesSent()); page += keyValue("GPS messages", gps.getMessagesHtml()); page += "

Display / Button

"; - page += keyValue("Button State", digitalRead(PUSHBUTTON_PIN)); - page += keyValue("Display i2c last error", Wire.getWriteError()); + page += keyValue("Button State",(const int32_t) digitalRead(PUSHBUTTON_PIN)); + page += keyValue("Display i2c last error", (const uint32_t) Wire.getWriteError()); page += keyValue("Display i2c speed", Wire.getClock() / 1000, "KHz"); - page += keyValue("Display i2c timeout", Wire.getTimeOut(), "ms"); + page += keyValue("Display i2c timeout",(const uint32_t) Wire.getTimeOut(), "ms"); page += "

WiFi

"; page += keyValue("Local IP", WiFi.localIP().toString()); @@ -1340,8 +1342,8 @@ void uploadTracks(HTTPResponse *res) { obsDisplay->showTextOnGrid(3, 5, String(failedCount)); if (res) { html += "

...all files done

"; - html += keyValue("OK", okCount); - html += keyValue("Failed", failedCount); + html += keyValue("OK",(const uint32_t) okCount); + html += keyValue("Failed", (const uint32_t) failedCount); html += ""; html += footer; res->print(html); @@ -1850,7 +1852,7 @@ static void accessFilter(HTTPRequest * req, HTTPResponse * res, std::functionsetStatusCode(401); res->setStatusText("Unauthorized"); res->setHeader("Content-Type", "text/plain"); - res->setHeader("WWW-Authenticate", std::string("Basic realm=\"") + OBS_ID_SHORT.c_str() + "\""); + res->setHeader("WWW-Authenticate", obs_string("Basic realm=\"") + OBS_ID_SHORT.c_str() + "\""); res->println("401: See OBS display"); obsDisplay->clearProgressBar(5); diff --git a/src/displays.cpp b/src/displays.cpp index fe72a6d5..7d41fa58 100644 --- a/src/displays.cpp +++ b/src/displays.cpp @@ -55,7 +55,7 @@ void SSD1306DisplayDevice::displaySimple(uint16_t value) { } void SSD1306DisplayDevice::showValues( - HCSR04SensorInfo* sensor1, HCSR04SensorInfo* sensor2, uint16_t minDistanceToConfirm, int16_t batteryPercentage, + DistanceSensor* sensor1, DistanceSensor* sensor2, uint16_t minDistanceToConfirm, int16_t batteryPercentage, int16_t TemperaturValue, int lastMeasurements, boolean insidePrivacyArea, double speed, uint8_t satellites) { diff --git a/src/displays.h b/src/displays.h index 60aadb8d..f81f02d0 100644 --- a/src/displays.h +++ b/src/displays.h @@ -51,7 +51,7 @@ extern const uint8_t TempLogo[]; #define HUGE_FONT Open_Sans_Regular_Plain_50 // Forward declare classes to build (because there is a cyclic dependency between sensor.h and displays.h) -class HCSR04SensorInfo; +class DistanceSensor; const int CLK = 33; //Set the CLK pin connection to the display const int DIO = 25; //Set the DIO pin connection to the display @@ -317,7 +317,7 @@ class SSD1306DisplayDevice : public DisplayDevice { void showNumButtonPressed(); void showValues( - HCSR04SensorInfo sensor1, HCSR04SensorInfo sensor2, + DistanceSensor* sensor1, DistanceSensor* sensor2, uint16_t minDistanceToConfirm, int16_t batteryPercentage, int16_t TemperaturValue, int lastMeasurements, boolean insidePrivacyArea, double speed, uint8_t satellites); diff --git a/src/globals.h b/src/globals.h index d430ed62..e7e64dfd 100644 --- a/src/globals.h +++ b/src/globals.h @@ -37,6 +37,8 @@ class HCSR04SensorManager; #include "sensor.h" #include "VoltageMeter.h" +#include + // This file should contain declarations of all variables that will be used globally. // The variables don't have to be set here, but need to be declared. @@ -54,7 +56,7 @@ extern Config config; extern SSD1306DisplayDevice* obsDisplay; -extern HCSR04SensorManager* sensorManager; +extern DistanceSensorManager* sensorManager; extern VoltageMeter* voltageMeter; diff --git a/src/obsimprov.h b/src/obsimprov.h index 0c857095..f5e5206a 100644 --- a/src/obsimprov.h +++ b/src/obsimprov.h @@ -33,6 +33,8 @@ #include #include +#include "config.h" + /** * Utility class to implement the improv protocol. * See protocol documentation at https://www.improv-wifi.com/serial/ for @@ -57,9 +59,9 @@ class ObsImprov { * @param getDeviceUrl if the device wifi is up the url needed to reach * the device should be returned, a empty string otherwise. */ - ObsImprov(std::function initWifi, + ObsImprov(std::function initWifi, std::function getWifiStatus, - std::function getDeviceUrl, + std::function getDeviceUrl, HardwareSerial* serial = &Serial) : mSerial(serial), mInitWifi(initWifi), @@ -83,10 +85,10 @@ class ObsImprov { * Should be called as soon as possible after creating a ObsImprov instance. * Empty data is reported upstream if needed earlier. */ - void setDeviceInfo(const std::string & firmwareName, - const std::string & firmwareVersion, - const std::string & hardwareVariant, - const std::string & deviceName); + void setDeviceInfo(const obs_string & firmwareName, + const obs_string & firmwareVersion, + const obs_string & hardwareVariant, + const obs_string & deviceName); /** * Returns true if any improv message was received. @@ -124,21 +126,21 @@ class ObsImprov { HardwareSerial* mSerial; std::vector mBuffer; uint8_t mHeaderPos = 0; - std::string mFirmwareName; - std::string mFirmwareVersion; - std::string mHardwareVariant; - std::string mDeviceName; + obs_string mFirmwareName = ""; + obs_string mFirmwareVersion = ""; + obs_string mHardwareVariant = ""; + obs_string mDeviceName = ""; static const char *HEADER; static const uint8_t HEADER_LENGTH; - const std::function mInitWifi; + const std::function mInitWifi; const std::function mWifiStatus; - const std::function mDeviceUrl; + const std::function mDeviceUrl; bool mImprovActive = false; void sendWifiSuccess(Command cmd = Command::WIFI_SETTINGS) const; void sendCurrentState(State state) const; void sendErrorState(Error error) const; void handleRpcGetDeviceInfo() const; - void appendStringAndLength(std::vector &response, std::string data) const; + void appendStringAndLength(std::vector &response, obs_string data) const; void sendPayload(Stream *stream, std::vector payload) const; bool isCompleteImprovMessage(std::vector buffer) const; bool isValidImprovMessage(std::vector buffer) const; diff --git a/src/sensor.cpp b/src/sensor.cpp index f07e2df8..3d2690dc 100644 --- a/src/sensor.cpp +++ b/src/sensor.cpp @@ -48,17 +48,11 @@ static const uint16_t MAX_DISTANCE_MEASURED_CM = 320; // candidate to check I co static const uint32_t MIN_DURATION_MICRO_SEC = MIN_DISTANCE_MEASURED_CM * MICRO_SEC_TO_CM_DIVIDER; const uint32_t MAX_DURATION_MICRO_SEC = MAX_DISTANCE_MEASURED_CM * MICRO_SEC_TO_CM_DIVIDER; -void HCSR04SensorManager::initSensors() { - registerSensor(m_sensors[0], 0); +void DistanceSensorManager::initSensors() { m_sensors[0].sensorLocation = "Right"; - - registerSensor(m_sensors[1], 1); m_sensors[1].sensorLocation = "Left"; - // queue_buffer = new uint8_t[sizeof(LLMessage) * 40]; - // static_queue = new StaticQueue_t; - - sensor_queue = xQueueCreate( 40, sizeof( LLMessage ) /*, queue_buffer, static_queue */); + sensor_queue = xQueueCreate( 40, sizeof( LLMessage )); if (sensor_queue == 0) { log_e("Oh wow. Sensor queue was not created successfully :("); } else { @@ -68,33 +62,29 @@ void HCSR04SensorManager::initSensors() { TOFManager::startupManager(sensor_queue); } -void HCSR04SensorManager::registerSensor(const HCSR04SensorInfo& sensorInfo, uint8_t idx) { - if (idx >= NUMBER_OF_TOF_SENSORS) { - log_e("Can not register sensor for index %d, only %d tof sensors supported", idx, NUMBER_OF_TOF_SENSORS); - return; - } - sensorValues[idx] = MAX_SENSOR_VALUE; - m_sensors[idx].median = new Median(5, MAX_SENSOR_VALUE); -} - -void HCSR04SensorManager::reset(uint32_t startMillisTicks) { +void DistanceSensorManager::reset(uint32_t startMillisTicks) { log_d("Reset"); + for (auto & sensor : m_sensors) { sensor.minDistance = MAX_SENSOR_VALUE; memset(&(sensor.echoDurationMicroseconds), 0, sizeof(sensor.echoDurationMicroseconds)); sensor.numberOfTriggers = 0; + sensor.median = new Median(5, MAX_SENSOR_VALUE); + sensor.rawDistance = MAX_SENSOR_VALUE; } + + memset(&(captureOffsetMS), 0, sizeof(captureOffsetMS)); + lastReadingCount = 0; - memset(&(startOffsetMilliseconds), 0, sizeof(startOffsetMilliseconds)); - startReadingMilliseconds = startMillisTicks; + sensorStartTimestampMS = startMillisTicks; } -void HCSR04SensorManager::setOffsets(std::vector offsets) { +void DistanceSensorManager::setOffsets(std::vector offsets) { for (size_t idx = 0; idx < NUMBER_OF_TOF_SENSORS; ++idx) { if (idx < offsets.size()) { - m_sensors[idx].offset = offsets[idx]; + getSensor(idx)->offset = offsets[idx]; } else { - m_sensors[idx].offset = 0; + getSensor(idx)->offset = 0; } } } @@ -102,7 +92,7 @@ void HCSR04SensorManager::setOffsets(std::vector offsets) { /* Polls for new readings, if sensors are not ready, the * method returns false. */ -bool HCSR04SensorManager::pollDistancesAlternating() { +bool DistanceSensorManager::pollDistances() { bool newMeasurements = false; LLMessage message = {0}; @@ -115,36 +105,40 @@ bool HCSR04SensorManager::pollDistancesAlternating() { return newMeasurements; } +uint16_t DistanceSensorManager::computeDistance(uint32_t travel_time) { + uint16_t distance = MAX_SENSOR_VALUE; + if (travel_time > MIN_DURATION_MICRO_SEC || travel_time <= MAX_DURATION_MICRO_SEC) { + distance = static_cast(travel_time / MICRO_SEC_TO_CM_DIVIDER); + } + return distance; +} + +void DistanceSensorManager::proposeMinDistance(DistanceSensor* const sensor, uint16_t distance) { + if (distance > 0 && distance < sensor->minDistance) { + sensor->minDistance = distance; + } +} + // FIXME /* Returns true if there was a no timeout reading. */ -bool HCSR04SensorManager::processSensorMessage(LLMessage* message) { +bool DistanceSensorManager::processSensorMessage(LLMessage* message) { log_d("Processing sensor message from sensor = %i", message->sensor_index); - HCSR04SensorInfo* const sensor = &m_sensors[message->sensor_index]; + DistanceSensor* const sensor = &m_sensors[message->sensor_index]; bool reading_valid = false; uint32_t duration = microsBetween(message->start, message->end); - sensor->echoDurationMicroseconds[lastReadingCount] = duration; - uint16_t distance; + uint16_t rawDistance = computeDistance(duration); - if (duration < MIN_DURATION_MICRO_SEC || duration >= MAX_DURATION_MICRO_SEC) { - distance = MAX_SENSOR_VALUE; - } else { - reading_valid = true; - distance = static_cast(duration / MICRO_SEC_TO_CM_DIVIDER); - } - - sensor->rawDistance = distance; - sensor->median->addValue(distance); - sensorValues[message->sensor_index] = - sensor->distance = correctSensorOffset(medianMeasure(sensor, distance), sensor->offset); + sensor->rawDistance = rawDistance; + sensor->median->addValue(rawDistance); + sensor->distance = computeOffsetDistance(medianMeasure(sensor, rawDistance), sensor->offset); + sensor->echoDurationMicroseconds[lastReadingCount] = duration; + sensor->rawDistance = rawDistance; - if (sensor->distance > 0 && sensor->distance < sensor->minDistance) { - sensor->minDistance = sensor->distance; - } - sensor->measurementRead = true; + captureOffsetMS[lastReadingCount] = millisSince(sensorStartTimestampMS); - startOffsetMilliseconds[lastReadingCount] = millisSince(startReadingMilliseconds); + proposeMinDistance(sensor, rawDistance); if(message->sensor_index == 1) { if (lastReadingCount < MAX_NUMBER_MEASUREMENTS_PER_INTERVAL - 1) { @@ -155,20 +149,19 @@ bool HCSR04SensorManager::processSensorMessage(LLMessage* message) { return reading_valid; } -uint16_t HCSR04SensorManager::getSensorValue(uint8_t sensorIndex) { - return sensorValues[sensorIndex]; +uint16_t DistanceSensorManager::getSensorRawDistance(uint8_t sensorIndex) { + return getSensor(sensorIndex)->rawDistance; } -HCSR04SensorInfo* HCSR04SensorManager::getSensor(uint8_t sensorIndex) { +DistanceSensor* DistanceSensorManager::getSensor(uint8_t sensorIndex) { return &m_sensors[sensorIndex]; } -boolean HCSR04SensorManager::hasReadings(uint8_t sensorIndex) { - // FIXME - return false; +boolean DistanceSensorManager::hasReadings(uint8_t sensorIndex) { + return lastReadingCount > 0; } -void HCSR04SensorManager::copyData(DataSet* set) { +void DistanceSensorManager::copyData(DataSet* set) { log_d("Let's copy some data."); for (int i = 0; i < NUMBER_OF_TOF_SENSORS; i++){ set->sensorValues.push_back(m_sensors[i].minDistance); @@ -180,46 +173,52 @@ void HCSR04SensorManager::copyData(DataSet* set) { memcpy(&(set->readDurationsLeftInMicroseconds), &(m_sensors[1].echoDurationMicroseconds), set->measurements * sizeof(int32_t)); memcpy(&(set->startOffsetMilliseconds), - &(startOffsetMilliseconds), set->measurements * sizeof(uint16_t)); + &(captureOffsetMS), set->measurements * sizeof(uint16_t)); } -uint16_t HCSR04SensorManager::getLastValidValue(uint8_t sensorIndex) { +uint16_t DistanceSensorManager::getLastValidRawDistance(uint8_t sensorIndex) { // if there was no measurement of this sensor for the current index, it is the // one before. This happens with fast confirmations. uint16_t val = 0; - // FIXME + for (int i = lastReadingCount - 1; i >= 0; i--) { + uint16_t maybeVal = getSensor(sensorIndex)->echoDurationMicroseconds[i]; + if (maybeVal != 0) { + val = maybeVal; + break; + } + } return val; } -uint16_t HCSR04SensorManager::getRawMedianDistance(uint8_t sensorId) { +uint16_t DistanceSensorManager::getMedianRawDistance(uint8_t sensorId) { return m_sensors[sensorId].median->median(); } -uint32_t HCSR04SensorManager::getMaxDurationUs(uint8_t sensorId) { +uint32_t DistanceSensorManager::getMaxDurationUs(uint8_t sensorId) { return m_sensors[sensorId].maxDurationUs; } -uint32_t HCSR04SensorManager::getMinDurationUs(uint8_t sensorId) { +uint32_t DistanceSensorManager::getMinDurationUs(uint8_t sensorId) { return m_sensors[sensorId].minDurationUs; } -uint32_t HCSR04SensorManager::getLastDelayTillStartUs(uint8_t sensorId) { +uint32_t DistanceSensorManager::getLastDelayTillStartUs(uint8_t sensorId) { return m_sensors[sensorId].lastDelayTillStartUs; } -uint32_t HCSR04SensorManager::getNoSignalReadings(const uint8_t sensorId) { +uint32_t DistanceSensorManager::getNoSignalReadings(const uint8_t sensorId) { return m_sensors[sensorId].numberOfNoSignals; } -uint32_t HCSR04SensorManager::getNumberOfLowAfterMeasurement(const uint8_t sensorId) { +uint32_t DistanceSensorManager::getNumberOfLowAfterMeasurement(const uint8_t sensorId) { return m_sensors[sensorId].numberOfLowAfterMeasurement; } -uint32_t HCSR04SensorManager::getNumberOfToLongMeasurement(const uint8_t sensorId) { +uint32_t DistanceSensorManager::getNumberOfToLongMeasurement(const uint8_t sensorId) { return m_sensors[sensorId].numberOfToLongMeasurement; } -uint32_t HCSR04SensorManager::getNumberOfInterruptAdjustments(const uint8_t sensorId) { +uint32_t DistanceSensorManager::getNumberOfInterruptAdjustments(const uint8_t sensorId) { return m_sensors[sensorId].numberOfInterruptAdjustments; } @@ -248,7 +247,7 @@ uint32_t HCSR04SensorManager::getNumberOfInterruptAdjustments(const uint8_t sens } */ -uint16_t HCSR04SensorManager::correctSensorOffset(uint16_t dist, uint16_t offset) { +uint16_t DistanceSensorManager::computeOffsetDistance(uint16_t dist, uint16_t offset) { uint16_t result; if (dist == MAX_SENSOR_VALUE) { result = MAX_SENSOR_VALUE; @@ -260,15 +259,15 @@ uint16_t HCSR04SensorManager::correctSensorOffset(uint16_t dist, uint16_t offset return result; } -void HCSR04SensorManager::updateStatistics(LLMessage* message) { -#if 0 - HCSR04SensorInfo* sensor = m_sensors[message->sensor_index]; +void DistanceSensorManager::updateStatistics(LLMessage* message) { +#if 1 + DistanceSensor* sensor = getSensor(message->sensor_index); - const uint32_t start_delay = message->start - message->trigger; + auto start_delay = message->start - message->trigger; if (start_delay > 0) { sensor->lastDelayTillStartUs = start_delay; - const uint32_t duration = message->end - message->start; + auto duration = message->end - message->start; if (duration > sensor->maxDurationUs) { sensor->maxDurationUs = duration; } @@ -288,7 +287,7 @@ void HCSR04SensorManager::updateStatistics(LLMessage* message) { * We should not use 64bit variables for "start" and "end" because access * to 64bit vars is not atomic for our 32bit cpu. */ -uint32_t HCSR04SensorManager::microsBetween(uint32_t a, uint32_t b) { +uint32_t DistanceSensorManager::microsBetween(uint32_t a, uint32_t b) { uint32_t result = a - b; if (result & 0x80000000) { result = -result; @@ -301,11 +300,11 @@ uint32_t HCSR04SensorManager::microsBetween(uint32_t a, uint32_t b) { * so we take care for the overflow. Times we measure are way below the * possible maximum. */ -uint32_t HCSR04SensorManager::microsSince(uint32_t a) { +uint32_t DistanceSensorManager::microsSince(uint32_t a) { return microsBetween(micros(), a); } -uint16_t HCSR04SensorManager::millisSince(uint16_t milliseconds) { +uint16_t DistanceSensorManager::millisSince(uint16_t milliseconds) { uint16_t result = ((uint16_t) millis()) - milliseconds; if (result & 0x8000) { result = -result; @@ -313,7 +312,7 @@ uint16_t HCSR04SensorManager::millisSince(uint16_t milliseconds) { return result; } -uint16_t HCSR04SensorManager::medianMeasure(HCSR04SensorInfo *const sensor, uint16_t value) { +uint16_t DistanceSensorManager::medianMeasure(DistanceSensor *const sensor, uint16_t value) { sensor->distances[sensor->nextMedianDistance] = value; sensor->nextMedianDistance++; @@ -330,7 +329,7 @@ uint16_t HCSR04SensorManager::medianMeasure(HCSR04SensorInfo *const sensor, uint return median(sensor->distances[0], sensor->distances[1], sensor->distances[2]); } -uint16_t HCSR04SensorManager::median(uint16_t a, uint16_t b, uint16_t c) { +uint16_t DistanceSensorManager::median(uint16_t a, uint16_t b, uint16_t c) { if (a < b) { if (a >= c) { return a; diff --git a/src/sensor.h b/src/sensor.h index 906a265e..7fd12c32 100644 --- a/src/sensor.h +++ b/src/sensor.h @@ -24,10 +24,9 @@ #ifndef OBS_SENSOR_H #define OBS_SENSOR_H -#include #include +#include -#include "globals.h" #include "utils/median.h" #include "tofmanager.h" @@ -62,23 +61,25 @@ extern const uint16_t MAX_SENSOR_VALUE; const uint8_t NUMBER_OF_TOF_SENSORS = 2; -struct HCSR04SensorInfo { - uint8_t triggerPin = 15; - uint8_t echoPin = 4; +struct DistanceSensor { uint16_t offset = 0; uint16_t rawDistance = 0; uint16_t distances[MEDIAN_DISTANCE_MEASURES] = { MAX_SENSOR_VALUE, MAX_SENSOR_VALUE, MAX_SENSOR_VALUE }; uint16_t nextMedianDistance = 0; uint16_t minDistance = MAX_SENSOR_VALUE; uint16_t distance = MAX_SENSOR_VALUE; + char const* sensorLocation; int32_t echoDurationMicroseconds[MAX_NUMBER_MEASUREMENTS_PER_INTERVAL + 1]; + Median*median = nullptr; + // statistics uint32_t maxDurationUs = 0; uint32_t minDurationUs = UINT32_MAX; uint32_t lastDelayTillStartUs = 0; + // counts how often no echo and also no timeout signal was received // should only happen with defect or missing sensors uint32_t numberOfNoSignals = 0; @@ -86,21 +87,20 @@ struct HCSR04SensorInfo { uint32_t numberOfToLongMeasurement = 0; uint32_t numberOfInterruptAdjustments = 0; uint16_t numberOfTriggers = 0; - bool measurementRead; }; -class HCSR04SensorManager { +class DistanceSensorManager { public: - HCSR04SensorManager() {} - virtual ~HCSR04SensorManager() {} + DistanceSensorManager() {} + virtual ~DistanceSensorManager() {} void reset(uint32_t startMillisTicks); - void registerSensor(const HCSR04SensorInfo &, uint8_t idx); void setOffsets(std::vector); void initSensors(); + /* Returns the current raw median distance in cm for the * given sensor. */ - uint16_t getRawMedianDistance(uint8_t sensorId); + uint16_t getMedianRawDistance(uint8_t sensorId); /* Index for CSV. */ uint32_t getMaxDurationUs(uint8_t sensorId); uint32_t getMinDurationUs(uint8_t sensorId); @@ -110,32 +110,37 @@ class HCSR04SensorManager { uint32_t getNumberOfToLongMeasurement(const uint8_t sensorId); uint32_t getNumberOfInterruptAdjustments(const uint8_t sensorId); - HCSR04SensorInfo* getSensor(uint8_t sensorIndex); - uint16_t getSensorValue(uint8_t sensorIndex); - uint16_t getLastValidValue(uint8_t sensorIndex); + DistanceSensor* getSensor(uint8_t sensorIndex); + uint16_t getSensorRawDistance(uint8_t sensorIndex); + uint16_t getLastValidRawDistance(uint8_t sensorIndex); boolean hasReadings(uint8_t sensorIndex); void copyData(DataSet* set); - bool pollDistancesAlternating(); + bool pollDistances(); - static uint16_t medianMeasure(HCSR04SensorInfo* const sensor, uint16_t value); + protected: + uint16_t captureOffsetMS[MAX_NUMBER_MEASUREMENTS_PER_INTERVAL + 1]; + DistanceSensor m_sensors[NUMBER_OF_TOF_SENSORS]; + void updateStatistics(LLMessage* message); + uint16_t lastReadingCount = 0; + + /* offer a new value for the minimum distance */ + void proposeMinDistance(DistanceSensor* const sensor, uint16_t distance); + + /* compute the distance to an object considering `travel_time` */ + uint16_t computeDistance(uint32_t travel_time); + + private: + static uint16_t medianMeasure(DistanceSensor* const sensor, uint16_t value); static uint16_t median(uint16_t a, uint16_t b, uint16_t c); - static uint16_t correctSensorOffset(uint16_t dist, uint16_t offset); + static uint16_t computeOffsetDistance(uint16_t dist, uint16_t offset); static uint32_t microsBetween(uint32_t a, uint32_t b); static uint32_t microsSince(uint32_t a); static uint16_t millisSince(uint16_t milliseconds); - protected: - HCSR04SensorInfo m_sensors[NUMBER_OF_TOF_SENSORS]; - uint16_t sensorValues[NUMBER_OF_TOF_SENSORS]; - static void updateStatistics(LLMessage* message); - uint16_t lastReadingCount = 0; - uint16_t startOffsetMilliseconds[MAX_NUMBER_MEASUREMENTS_PER_INTERVAL + 1]; - - private: bool processSensorMessage(LLMessage* message); - uint32_t getFixedStart(size_t idx, HCSR04SensorInfo * const sensor); - uint32_t startReadingMilliseconds = 0; + uint32_t getFixedStart(size_t idx, DistanceSensor * const sensor); + uint32_t sensorStartTimestampMS = 0; QueueHandle_t sensor_queue; uint8_t* queue_buffer; diff --git a/src/tofmanager.cpp b/src/tofmanager.cpp index da39dac0..dabdfec5 100644 --- a/src/tofmanager.cpp +++ b/src/tofmanager.cpp @@ -1,6 +1,8 @@ #include "tofmanager.h" #include "sensor.h" +#include + /* Value of HCSR04SensorInfo::end during an ongoing measurement. */ #define MEASUREMENT_IN_PROGRESS 0 @@ -29,7 +31,7 @@ * HC-SR04: observed 71ms, docu 60ms * JSN-SR04T: observed 58ms */ -#define MAX_TIMEOUT_MICRO_SEC 75000 +#define MAX_TIMEOUT_MICRO_SEC 50000 /* The core to run the RT component on */ #define RT_CORE 0 @@ -86,16 +88,14 @@ void TOFManager::startMeasurement(uint8_t sensorId) { sensor->end = MEASUREMENT_IN_PROGRESS; // will be updated with LOW signal unsigned long now = esp_timer_get_time(); - sensor->trigger = 0; // will be updated with HIGH signal - sensor->start = now; + sensor->trigger = now; + sensor->start = 0; // will be updated with HIGH signal gpio_set_level(sensor->triggerPin, 1); // 10us are specified but some sensors are more stable with 20us according // to internet reports usleep(20); gpio_set_level(sensor->triggerPin, 0); - - attachSensorInterrupt(sensorId); } /* Checks if the given sensor is ready for a new measurement cycle. @@ -103,22 +103,23 @@ void TOFManager::startMeasurement(uint8_t sensorId) { boolean TOFManager::isSensorReady(uint8_t sensorId) { LLSensor* sensor = sensors(sensorId); boolean ready = false; - const uint32_t now = micros(); - const uint32_t start = sensor->start; - const uint32_t end = sensor->end; + auto now = esp_timer_get_time(); + auto start = sensor->start.load(); + auto end = sensor->end.load(); + if (end != MEASUREMENT_IN_PROGRESS) { // no measurement in flight or just finished - const uint32_t startOther = sensors(1 - sensorId)->start; - if ( (HCSR04SensorManager::microsBetween(now, end) > SENSOR_QUIET_PERIOD_AFTER_END_MICRO_SEC) - && (HCSR04SensorManager::microsBetween(now, start) > SENSOR_QUIET_PERIOD_AFTER_START_MICRO_SEC) - && (HCSR04SensorManager::microsBetween(now, startOther) > SENSOR_QUIET_PERIOD_AFTER_OPPOSITE_START_MICRO_SEC)) { + auto startOther = sensors(1 - sensorId)->start.load(); + if ((now - end) > SENSOR_QUIET_PERIOD_AFTER_END_MICRO_SEC + && (now - start) > SENSOR_QUIET_PERIOD_AFTER_START_MICRO_SEC + && (now - startOther) > SENSOR_QUIET_PERIOD_AFTER_OPPOSITE_START_MICRO_SEC) { ready = true; - sensors(sensorId)->successful_measurements += 1; + sensors(sensorId)->successful_measurements += 1; } - } else if (HCSR04SensorManager::microsBetween(now, start) > MAX_TIMEOUT_MICRO_SEC) { + } else if (now - sensor->trigger > MAX_TIMEOUT_MICRO_SEC) { // signal or interrupt was lost altogether this is an error, // should we raise a it?? Now pretend the sensor is ready, hope it helps to give it a trigger. ready = true; - sensors(sensorId)->successful_measurements += 1; + sensors(sensorId)->lost_signals += 1; } return ready; } @@ -128,7 +129,7 @@ boolean TOFManager::isMeasurementComplete(uint8_t sensor_idx) { return true; } uint32_t now = esp_timer_get_time(); - if (sensors(sensor_idx)->start + MAX_TIMEOUT_MICRO_SEC < now) { + if (sensors(sensor_idx)->trigger + MAX_TIMEOUT_MICRO_SEC < now) { return true; } @@ -169,6 +170,11 @@ void TOFManager::sensorTaskFunction(void *parameter) { } void TOFManager::init() { +#if ALONE_ON_CORE + esp_timer_init(); + gpio_install_isr_service(OBS_INTR_FLAG); +#endif + LLSensor* sensorManaged1 = sensors(0); sensorManaged1->triggerPin = GPIO_NUM_25; sensorManaged1->echoPin = GPIO_NUM_26; @@ -179,10 +185,6 @@ void TOFManager::init() { sensorManaged2->echoPin = GPIO_NUM_4; setupSensor(sensorManaged2, 1); -#if ALONE_ON_CORE - esp_timer_init(); - gpio_install_isr_service(OBS_INTR_FLAG); -#endif ESP_LOGI("tofmanager", "TOFManager init complete"); } @@ -196,7 +198,6 @@ void TOFManager::sendData(uint8_t sensor_idx) { message.start = sensor->start; message.end = sensor->end; xQueueSendToBack(sensor_queue, &message, portMAX_DELAY); - //delete message; } void TOFManager::loop() { @@ -204,10 +205,12 @@ void TOFManager::loop() { currentSensor = primarySensor; startMeasurement(primarySensor); - int statLog = 0; + uint32_t statLog = 0; + float startLoop = esp_timer_get_time(); while ( true ) { - if (statLog > 2048) { + if (statLog > 1024) { + float spent = ((float)esp_timer_get_time() - startLoop) / 1000000.0; ESP_LOGI("tofmanager", "statlog: sensor1->irq_cnt=%u, sensor2->irq_cnt=%u", sensors(0)->interrupt_count, sensors(1)->interrupt_count); ESP_LOGI("tofmanager", "statlog: success=(%u, %u), started=(%u,%u)", @@ -215,23 +218,31 @@ void TOFManager::loop() { sensors(1)->successful_measurements, sensors(0)->started_measurements, sensors(1)->started_measurements); + ESP_LOGI("tofmanager", "statlog: (s0) trigger-start-end = %x-%x-%x", + sensors(0)->trigger.load(), sensors(0)->start.load(), sensors(0)->end.load()); + if (spent > 0) { + ESP_LOGI("tofmanager", "statlog: (%f,%f) started/s", + (float)sensors(0)->started_measurements / spent, + (float)sensors(0)->started_measurements / spent); + } statLog = 0; } statLog += 1; // wait for sensor if (!isMeasurementComplete(currentSensor)) { - vTaskDelay(TICK_DELAY); + vTaskDelay(1); + //taskYIELD(); } else { - sendData(currentSensor); - - // switch to next sensor + // switch to next sensor & send out data if (currentSensor == primarySensor && isSensorReady(1 - primarySensor)) { - disableMeasurements(); + sendData(currentSensor); + disableMeasurement(currentSensor); currentSensor = 1 - primarySensor; startMeasurement(currentSensor); } else if (isSensorReady(primarySensor)) { - disableMeasurements(); + sendData(currentSensor); + disableMeasurement(currentSensor); currentSensor = primarySensor; startMeasurement(primarySensor); } @@ -240,14 +251,14 @@ void TOFManager::loop() { } static void IRAM_ATTR sensorInterrupt(void *isrParam) { - LLSensor* sensor = (LLSensor*) isrParam; // since the measurement of start and stop use the same interrupt // mechanism we should see a similar delay. - if (sensor->end == MEASUREMENT_IN_PROGRESS) { - sensor->interrupt_count += 1; + LLSensor* sensor = (LLSensor*) isrParam; - if (sensor->trigger == 0) { - sensor->trigger = esp_timer_get_time(); + sensor->interrupt_count += 1; + if (sensor->end == MEASUREMENT_IN_PROGRESS) { + if (sensor->start == 0) { + sensor->start = esp_timer_get_time(); } else { sensor->end = esp_timer_get_time(); } @@ -280,8 +291,10 @@ void TOFManager::setupSensor(LLSensor* sensor, uint8_t idx) { // gpio_pulldown_dis(sensor->echoPin); // gpio_pullup_en(sensor->echoPin); // hint from https://youtu.be/xwsT-e1D9OY?t=354 // gpio_intr_enable(sensor->echoPin); - +; // gpio_set_intr_type(sensor->echoPin, GPIO_INTR_NEGEDGE); + + attachSensorInterrupt(idx); } void TOFManager::attachSensorInterrupt(uint8_t idx) { @@ -294,8 +307,6 @@ void TOFManager::detachInterrupts() { } } -void TOFManager::disableMeasurements() { - for (int i = 0; i < NUMBER_OF_TOF_SENSORS; i ++) { - gpio_set_level(sensors(i)->triggerPin, 0); - } +void TOFManager::disableMeasurement(uint8_t idx) { + gpio_set_level(sensors(idx)->triggerPin, 0); } diff --git a/src/tofmanager.h b/src/tofmanager.h index bc0b9524..5c78f019 100644 --- a/src/tofmanager.h +++ b/src/tofmanager.h @@ -2,6 +2,8 @@ #define TOFMANAGER_H #include +#include +#include #include "utils/median.h" @@ -21,16 +23,17 @@ typedef struct { gpio_num_t echoPin; /* timestamp when the trigger signal was sent in microseconds */ - volatile uint32_t trigger = 0; + std::atomic_uint_least64_t trigger{0}; /* timestamp when the reading was started in microseconds */ - volatile uint32_t start = 0; + std::atomic_uint_least64_t start{0}; /* timestamp when the reading was complete in microseconds * if end == 0 - a measurement is in progress */ - volatile uint32_t end = 0; + std::atomic_uint_least64_t end{0}; volatile uint32_t interrupt_count = 0; uint32_t started_measurements = 0; uint32_t successful_measurements = 0; + uint32_t lost_signals = 0; } LLSensor; /* low-level TOF sensor manager @@ -62,13 +65,13 @@ class TOFManager { ~TOFManager(); void startMeasurement(uint8_t sensorId); - void disableMeasurements(); + void disableMeasurement(uint8_t sensor_idx); void attachSensorInterrupt(uint8_t idx); boolean isSensorReady(uint8_t sensorId); boolean isMeasurementComplete(uint8_t sensor_idx); uint8_t primarySensor = 1; - uint8_t currentSensor = 0; + volatile uint8_t currentSensor = 0; void sendData(uint8_t sensor_idx); static void sensorTaskFunction(void *parameter); From c8ee9541a75c5a5432408edeb740c8a7367ce434 Mon Sep 17 00:00:00 2001 From: Mark Meyer Date: Thu, 17 Aug 2023 17:15:19 +0200 Subject: [PATCH 04/11] chore: assorted fixes --- src/configServer.cpp | 2 -- src/globals.h | 1 - src/sensor.cpp | 2 +- 3 files changed, 1 insertion(+), 4 deletions(-) diff --git a/src/configServer.cpp b/src/configServer.cpp index 8cd49db0..ab494792 100644 --- a/src/configServer.cpp +++ b/src/configServer.cpp @@ -40,8 +40,6 @@ #include "utils/timeutils.h" #include "obsimprov.h" -typedef std::basic_string obs_string; - using namespace httpsserver; static const char *const HTML_ENTITY_FAILED_CROSS = "❌"; diff --git a/src/globals.h b/src/globals.h index e7e64dfd..6eadccc8 100644 --- a/src/globals.h +++ b/src/globals.h @@ -37,7 +37,6 @@ class HCSR04SensorManager; #include "sensor.h" #include "VoltageMeter.h" -#include // This file should contain declarations of all variables that will be used globally. // The variables don't have to be set here, but need to be declared. diff --git a/src/sensor.cpp b/src/sensor.cpp index 3d2690dc..57049c74 100644 --- a/src/sensor.cpp +++ b/src/sensor.cpp @@ -167,7 +167,7 @@ void DistanceSensorManager::copyData(DataSet* set) { set->sensorValues.push_back(m_sensors[i].minDistance); } - set->measurements = sensorManager->lastReadingCount; + set->measurements = lastReadingCount; memcpy(&(set->readDurationsRightInMicroseconds), &(m_sensors[0].echoDurationMicroseconds), set->measurements * sizeof(int32_t)); memcpy(&(set->readDurationsLeftInMicroseconds), From 63b68b80d55ba57fbe3de3b4632d24a56d633d4d Mon Sep 17 00:00:00 2001 From: Mark Meyer Date: Thu, 17 Aug 2023 17:15:50 +0200 Subject: [PATCH 05/11] fix: fix a null ptr dereference --- src/OpenBikeSensorFirmware.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/OpenBikeSensorFirmware.cpp b/src/OpenBikeSensorFirmware.cpp index a7cd8e4e..17330a4d 100644 --- a/src/OpenBikeSensorFirmware.cpp +++ b/src/OpenBikeSensorFirmware.cpp @@ -484,6 +484,7 @@ void loop() { } else { // confirming an overtake without left measure if (sensorManager->hasReadings(LEFT_SENSOR_ID)) { currentSet->confirmedDistances.push_back(MAX_SENSOR_VALUE); + currentSet->confirmedDistancesIndex.push_back(0); } #ifdef OBS_BLUETOOTH buttonBluetooth(currentSet, sensorManager->getCurrentMeasureIndex()); From 53f9ef12ba328ce887869416f9ea23844455d4b0 Mon Sep 17 00:00:00 2001 From: Mark Meyer Date: Thu, 17 Aug 2023 17:50:12 +0200 Subject: [PATCH 06/11] fix: revert to old sensor behaviour - change MAX_TIMEOUT_MICRO_SEC back to old value - switch sensors - make logging less noisy --- src/tofmanager.cpp | 28 ++++++++++++++++------------ 1 file changed, 16 insertions(+), 12 deletions(-) diff --git a/src/tofmanager.cpp b/src/tofmanager.cpp index dabdfec5..5d9aed25 100644 --- a/src/tofmanager.cpp +++ b/src/tofmanager.cpp @@ -31,7 +31,7 @@ * HC-SR04: observed 71ms, docu 60ms * JSN-SR04T: observed 58ms */ -#define MAX_TIMEOUT_MICRO_SEC 50000 +#define MAX_TIMEOUT_MICRO_SEC 75000 /* The core to run the RT component on */ #define RT_CORE 0 @@ -176,13 +176,13 @@ void TOFManager::init() { #endif LLSensor* sensorManaged1 = sensors(0); - sensorManaged1->triggerPin = GPIO_NUM_25; - sensorManaged1->echoPin = GPIO_NUM_26; + sensorManaged1->triggerPin = GPIO_NUM_15; + sensorManaged1->echoPin = GPIO_NUM_4; setupSensor(sensorManaged1, 0); LLSensor* sensorManaged2 = sensors(1); - sensorManaged2->triggerPin = GPIO_NUM_15; - sensorManaged2->echoPin = GPIO_NUM_4; + sensorManaged2->triggerPin = GPIO_NUM_25; + sensorManaged2->echoPin = GPIO_NUM_26; setupSensor(sensorManaged2, 1); ESP_LOGI("tofmanager", "TOFManager init complete"); @@ -205,29 +205,33 @@ void TOFManager::loop() { currentSensor = primarySensor; startMeasurement(primarySensor); - uint32_t statLog = 0; float startLoop = esp_timer_get_time(); + // this toggle switches once approx every 8 seconds + uint8_t logToggle = 0; + while ( true ) { - if (statLog > 1024) { + uint8_t newToggle = (esp_timer_get_time() >> 23) & 0x1; + if (newToggle != logToggle) { float spent = ((float)esp_timer_get_time() - startLoop) / 1000000.0; - ESP_LOGI("tofmanager", "statlog: sensor1->irq_cnt=%u, sensor2->irq_cnt=%u", + ESP_LOGI("tofmanager", "stats: sensor1->irq_cnt=%u, sensor2->irq_cnt=%u", sensors(0)->interrupt_count, sensors(1)->interrupt_count); - ESP_LOGI("tofmanager", "statlog: success=(%u, %u), started=(%u,%u)", + ESP_LOGI("tofmanager", "stats: success=(%u, %u), started=(%u,%u)", sensors(0)->successful_measurements, sensors(1)->successful_measurements, sensors(0)->started_measurements, sensors(1)->started_measurements); - ESP_LOGI("tofmanager", "statlog: (s0) trigger-start-end = %x-%x-%x", + ESP_LOGI("tofmanager", "stats: (s0) trigger-start-end = %x-%x-%x", sensors(0)->trigger.load(), sensors(0)->start.load(), sensors(0)->end.load()); + ESP_LOGI("tofmanager", "stats: (s1) trigger-start-end = %x-%x-%x", + sensors(1)->trigger.load(), sensors(1)->start.load(), sensors(1)->end.load()); if (spent > 0) { ESP_LOGI("tofmanager", "statlog: (%f,%f) started/s", (float)sensors(0)->started_measurements / spent, (float)sensors(0)->started_measurements / spent); } - statLog = 0; + logToggle = newToggle; } - statLog += 1; // wait for sensor if (!isMeasurementComplete(currentSensor)) { From 364794af3e2bf28f500aa77f9efa736ca1f8cdcc Mon Sep 17 00:00:00 2001 From: Mark Meyer Date: Thu, 17 Aug 2023 19:53:11 +0200 Subject: [PATCH 07/11] fix: offset based computation and plug a memory leak --- src/OpenBikeSensorFirmware.cpp | 1 + src/sensor.cpp | 8 +++++--- src/utils/median.h | 1 - 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/OpenBikeSensorFirmware.cpp b/src/OpenBikeSensorFirmware.cpp index 17330a4d..20d108d0 100644 --- a/src/OpenBikeSensorFirmware.cpp +++ b/src/OpenBikeSensorFirmware.cpp @@ -120,6 +120,7 @@ void switch_wire_speed_to_SSD1306(){ // FIXME void setupSensors() { sensorManager = new DistanceSensorManager; + sensorManager->setOffsets(config.sensorOffsets); sensorManager->initSensors(); } diff --git a/src/sensor.cpp b/src/sensor.cpp index 57049c74..69c93a9a 100644 --- a/src/sensor.cpp +++ b/src/sensor.cpp @@ -69,6 +69,7 @@ void DistanceSensorManager::reset(uint32_t startMillisTicks) { sensor.minDistance = MAX_SENSOR_VALUE; memset(&(sensor.echoDurationMicroseconds), 0, sizeof(sensor.echoDurationMicroseconds)); sensor.numberOfTriggers = 0; + delete sensor.median; sensor.median = new Median(5, MAX_SENSOR_VALUE); sensor.rawDistance = MAX_SENSOR_VALUE; } @@ -114,7 +115,7 @@ uint16_t DistanceSensorManager::computeDistance(uint32_t travel_time) { } void DistanceSensorManager::proposeMinDistance(DistanceSensor* const sensor, uint16_t distance) { - if (distance > 0 && distance < sensor->minDistance) { + if ((distance > 0) && (distance < sensor->minDistance)) { sensor->minDistance = distance; } } @@ -132,13 +133,14 @@ bool DistanceSensorManager::processSensorMessage(LLMessage* message) { sensor->rawDistance = rawDistance; sensor->median->addValue(rawDistance); - sensor->distance = computeOffsetDistance(medianMeasure(sensor, rawDistance), sensor->offset); + uint16_t offsetDistance = computeOffsetDistance(medianMeasure(sensor, rawDistance), sensor->offset); + sensor->distance = offsetDistance; sensor->echoDurationMicroseconds[lastReadingCount] = duration; sensor->rawDistance = rawDistance; captureOffsetMS[lastReadingCount] = millisSince(sensorStartTimestampMS); - proposeMinDistance(sensor, rawDistance); + proposeMinDistance(sensor, offsetDistance); if(message->sensor_index == 1) { if (lastReadingCount < MAX_NUMBER_MEASUREMENTS_PER_INTERVAL - 1) { diff --git a/src/utils/median.h b/src/utils/median.h index 95e06446..e36fa475 100644 --- a/src/utils/median.h +++ b/src/utils/median.h @@ -41,7 +41,6 @@ template class Median { } }; ~Median() { - log_i("Will free Median(%d/%d) -> 0x%lx", size, mid, data); delete[] data; delete[] temp; }; From e1ecdc32763dd33921da1494cfe0b614edc05edb Mon Sep 17 00:00:00 2001 From: Mark Meyer Date: Thu, 17 Aug 2023 21:31:32 +0200 Subject: [PATCH 08/11] fix: repair broken handling of MAX_SENSOR_VALUE --- src/sensor.cpp | 25 ++++++++++++++++--------- 1 file changed, 16 insertions(+), 9 deletions(-) diff --git a/src/sensor.cpp b/src/sensor.cpp index 69c93a9a..93235d33 100644 --- a/src/sensor.cpp +++ b/src/sensor.cpp @@ -129,18 +129,25 @@ bool DistanceSensorManager::processSensorMessage(LLMessage* message) { uint32_t duration = microsBetween(message->start, message->end); - uint16_t rawDistance = computeDistance(duration); - - sensor->rawDistance = rawDistance; - sensor->median->addValue(rawDistance); - uint16_t offsetDistance = computeOffsetDistance(medianMeasure(sensor, rawDistance), sensor->offset); - sensor->distance = offsetDistance; sensor->echoDurationMicroseconds[lastReadingCount] = duration; - sensor->rawDistance = rawDistance; - captureOffsetMS[lastReadingCount] = millisSince(sensorStartTimestampMS); + if (duration > MIN_DURATION_MICRO_SEC && duration < MAX_DURATION_MICRO_SEC) { + // these values should only be set if we have correct data + uint16_t rawDistance = computeDistance(duration); + sensor->median->addValue(rawDistance); - proposeMinDistance(sensor, offsetDistance); + sensor->rawDistance = rawDistance; + + uint16_t offsetDistance = computeOffsetDistance(medianMeasure(sensor, rawDistance), sensor->offset); + sensor->distance = offsetDistance; + proposeMinDistance(sensor, offsetDistance); + reading_valid = true; + } else { + sensor->rawDistance = MAX_SENSOR_VALUE; + sensor->distance = MAX_SENSOR_VALUE; + } + + captureOffsetMS[lastReadingCount] = millisSince(sensorStartTimestampMS); if(message->sensor_index == 1) { if (lastReadingCount < MAX_NUMBER_MEASUREMENTS_PER_INTERVAL - 1) { From 45914d7d8efd9a66b25352030ae3452c733880ac Mon Sep 17 00:00:00 2001 From: Mark Meyer Date: Thu, 17 Aug 2023 22:45:23 +0200 Subject: [PATCH 09/11] chore: add more documentation to DistanceSensor --- src/sensor.h | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/sensor.h b/src/sensor.h index 7fd12c32..93a11a0b 100644 --- a/src/sensor.h +++ b/src/sensor.h @@ -62,24 +62,38 @@ extern const uint16_t MAX_SENSOR_VALUE; const uint8_t NUMBER_OF_TOF_SENSORS = 2; struct DistanceSensor { + // offset from the sensor to the end of the handlebar (cm) uint16_t offset = 0; + // the distance from the sensor to some object (cm) uint16_t rawDistance = 0; + // median of the offset distance (cm) uint16_t distances[MEDIAN_DISTANCE_MEASURES] = { MAX_SENSOR_VALUE, MAX_SENSOR_VALUE, MAX_SENSOR_VALUE }; + // index into the previous member uint16_t nextMedianDistance = 0; + // minimum offset distance the sensor noticed between resets (cm) uint16_t minDistance = MAX_SENSOR_VALUE; + // current offset distance of the sensor (cm) uint16_t distance = MAX_SENSOR_VALUE; + // UI string that denotes the sensor position char const* sensorLocation; + // an array with all of the time-of-flight measurements since the last reset int32_t echoDurationMicroseconds[MAX_NUMBER_MEASUREMENTS_PER_INTERVAL + 1]; + // a data structure for computing the median offset distance since the last reset (cm) Median*median = nullptr; // statistics + + // maximum time-of-flight the sensor measured since the last reset (microseconds) uint32_t maxDurationUs = 0; + // minimum time-of-flight the sensor measured since the last reset (microseconds) uint32_t minDurationUs = UINT32_MAX; + // internal delay to start metric uint32_t lastDelayTillStartUs = 0; + // FIXME: internal statistics // counts how often no echo and also no timeout signal was received // should only happen with defect or missing sensors uint32_t numberOfNoSignals = 0; From 5163dd6f3b8eeda971d2b3f81caf443181bfd0c7 Mon Sep 17 00:00:00 2001 From: Mark Meyer Date: Thu, 17 Aug 2023 22:58:20 +0200 Subject: [PATCH 10/11] chore: modernize toolchain --- platformio.ini | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/platformio.ini b/platformio.ini index 3bc685a0..053fa859 100644 --- a/platformio.ini +++ b/platformio.ini @@ -38,7 +38,11 @@ src_dir = src extra_configs = custom_config.ini [env:esp32dev] -platform = espressif32 @ 5.3.0 +build_type = release +platform = espressif32@5.4.0 +platform_packages = + toolchain-xtensa-esp32@12.2.0+20230208 + framework-arduinoespressif32@https://github.com/espressif/arduino-esp32#2.0.11 board = esp32dev framework = arduino monitor_speed = 115200 @@ -66,3 +70,12 @@ build_flags = -DHTTPS_CONNECTION_DATA_CHUNK_SIZE=1024 ; build number "-dev" will be replaced in github action -DBUILD_NUMBER=\"-dev\" + -Wall + -std=gnu++20 + -fno-rtti +build_unflags = + -std=gnu++11 +debug_build_flags = + -Os +debug_build_unflags = + -Og From 087389b517a22450a8de44cbc78227e6b9322a26 Mon Sep 17 00:00:00 2001 From: Mark Meyer Date: Thu, 17 Aug 2023 23:21:40 +0200 Subject: [PATCH 11/11] chore: add documentation and delete some unused code --- src/sensor.cpp | 1 - src/tofmanager.cpp | 48 +++++++++++++++++++++++++--------------------- src/tofmanager.h | 10 +++++----- 3 files changed, 31 insertions(+), 28 deletions(-) diff --git a/src/sensor.cpp b/src/sensor.cpp index 93235d33..7cda1d46 100644 --- a/src/sensor.cpp +++ b/src/sensor.cpp @@ -354,4 +354,3 @@ uint16_t DistanceSensorManager::median(uint16_t a, uint16_t b, uint16_t c) { } return c; } - diff --git a/src/tofmanager.cpp b/src/tofmanager.cpp index 5d9aed25..4c609324 100644 --- a/src/tofmanager.cpp +++ b/src/tofmanager.cpp @@ -39,9 +39,6 @@ /* Delay to call when we're waiting for work */ #define TICK_DELAY 1 -/* Delay in ticks for when we run out of memory */ -#define OOM_TICK_DELAY 10000 - /* Enable init code for running alone */ #define ALONE_ON_CORE 1 @@ -77,7 +74,7 @@ * close to the needed 148ms */ -/* Send echo trigger to the selected sensor and prepare measurement data +/* Send echo trigger to the sensor `sensorId` and prepare measurement data * structures. */ void TOFManager::startMeasurement(uint8_t sensorId) { @@ -98,7 +95,7 @@ void TOFManager::startMeasurement(uint8_t sensorId) { gpio_set_level(sensor->triggerPin, 0); } -/* Checks if the given sensor is ready for a new measurement cycle. +/* Check if the sensor `sensorId` is ready for a new measurement cycle. */ boolean TOFManager::isSensorReady(uint8_t sensorId) { LLSensor* sensor = sensors(sensorId); @@ -124,6 +121,7 @@ boolean TOFManager::isSensorReady(uint8_t sensorId) { return ready; } +/* Check if the measurement on the given sensor `sensor_idx` is complete. */ boolean TOFManager::isMeasurementComplete(uint8_t sensor_idx) { if (sensors(sensor_idx)->end != MEASUREMENT_IN_PROGRESS) { return true; @@ -136,6 +134,8 @@ boolean TOFManager::isMeasurementComplete(uint8_t sensor_idx) { return false; } +/* Initialize the sensor manager and start it up. Use this to start the sensor. + * This function will start a new FreeRTOS task. */ void TOFManager::startupManager(QueueHandle_t queue) { esp_log_level_set("tofmanager", ESP_LOG_INFO); ESP_LOGI("tofmanager","Currently on core %i", xPortGetCoreID()); @@ -144,14 +144,17 @@ void TOFManager::startupManager(QueueHandle_t queue) { xTaskCreatePinnedToCore( TOFManager::sensorTaskFunction, /* Task function. */ "TOFManager", /* String with name of task. */ - 16 * 1024, /* Stack size in bytes. */ - queue, /* Parameter passed as input of the task */ - 1, /* Priority of the task. */ - NULL, /* Task handle. */ - RT_CORE /* the core to run on */ + 16 * 1024, /* Stack size in bytes. */ + queue, /* Parameter passed as input of the task */ + 1, /* Priority of the task. */ + NULL, /* Task handle. */ + RT_CORE /* the core to run on */ ); } +/* Main function that will be started by startupManager(). + * This function will never return. This function is supposed to run in its own + * FreeRTOS task. */ void TOFManager::sensorTaskFunction(void *parameter) { esp_log_level_set("tofmanager", ESP_LOG_INFO); isr_log_i("tofmanager", "Hello from core %i!", xPortGetCoreID()); @@ -169,6 +172,7 @@ void TOFManager::sensorTaskFunction(void *parameter) { sensorManager->loop(); } +/* Initialize the TOFManager: setup sensor pins and interrupts. */ void TOFManager::init() { #if ALONE_ON_CORE esp_timer_init(); @@ -188,6 +192,8 @@ void TOFManager::init() { ESP_LOGI("tofmanager", "TOFManager init complete"); } +/* Looking at a single sensor `sensor_idx`, take its data and + * send it off to the high level sensor component. */ void TOFManager::sendData(uint8_t sensor_idx) { LLSensor* sensor = sensors(sensor_idx); @@ -200,6 +206,9 @@ void TOFManager::sendData(uint8_t sensor_idx) { xQueueSendToBack(sensor_queue, &message, portMAX_DELAY); } +/* The main loop of the TOFManager. This function will never return. + * It sequences all measurements and sends their data to the high leve component + * of the sensor code. */ void TOFManager::loop() { ESP_LOGI("tofmanager","Entering TOFManager::loop"); currentSensor = primarySensor; @@ -254,6 +263,8 @@ void TOFManager::loop() { } } +/* Sensor interrupt function. Use the `isrParam` to pass in a LLSensor* + * Fetch the current phase of that sensor and set it's values. */ static void IRAM_ATTR sensorInterrupt(void *isrParam) { // since the measurement of start and stop use the same interrupt // mechanism we should see a similar delay. @@ -269,6 +280,7 @@ static void IRAM_ATTR sensorInterrupt(void *isrParam) { } } +/* Set up a single `sensor` including its pins and interrupts. */ void TOFManager::setupSensor(LLSensor* sensor, uint8_t idx) { gpio_config_t io_conf_trigger = {}; io_conf_trigger.intr_type = GPIO_INTR_DISABLE; @@ -277,10 +289,6 @@ void TOFManager::setupSensor(LLSensor* sensor, uint8_t idx) { io_conf_trigger.pull_down_en = GPIO_PULLDOWN_DISABLE; io_conf_trigger.pull_up_en = GPIO_PULLUP_DISABLE; gpio_config(&io_conf_trigger); - // gpio_pad_select_gpio(sensor->triggerPin); - // gpio_set_direction(sensor->triggerPin, GPIO_MODE_OUTPUT); - // gpio_pulldown_dis(sensor->triggerPin); - // gpio_pullup_dis(sensor->triggerPin); gpio_config_t io_conf_echo = {}; io_conf_echo.intr_type = GPIO_INTR_ANYEDGE; @@ -290,27 +298,23 @@ void TOFManager::setupSensor(LLSensor* sensor, uint8_t idx) { io_conf_echo.pull_up_en = GPIO_PULLUP_ENABLE; gpio_config(&io_conf_echo); - // gpio_pad_select_gpio(sensor->echoPin); - // gpio_set_direction(sensor->echoPin, GPIO_MODE_INPUT); - // gpio_pulldown_dis(sensor->echoPin); - // gpio_pullup_en(sensor->echoPin); // hint from https://youtu.be/xwsT-e1D9OY?t=354 - // gpio_intr_enable(sensor->echoPin); -; - // gpio_set_intr_type(sensor->echoPin, GPIO_INTR_NEGEDGE); - attachSensorInterrupt(idx); } +/* Attach the interrupt handler to the pin that belongs to the sensor + * designated by `idx`. */ void TOFManager::attachSensorInterrupt(uint8_t idx) { gpio_isr_handler_add(sensors(idx)->echoPin, sensorInterrupt, sensors(idx)); } +/* Detach all interrupts for our sensors. */ void TOFManager::detachInterrupts() { for (int i = 0; i < NUMBER_OF_TOF_SENSORS; i++) { gpio_isr_handler_remove(sensors(i)->echoPin); } } +/* Disable the measurement on the sensor designated by `idx`. */ void TOFManager::disableMeasurement(uint8_t idx) { gpio_set_level(sensors(idx)->triggerPin, 0); } diff --git a/src/tofmanager.h b/src/tofmanager.h index 5c78f019..66dea26f 100644 --- a/src/tofmanager.h +++ b/src/tofmanager.h @@ -5,8 +5,6 @@ #include #include -#include "utils/median.h" - typedef struct { uint8_t sensor_index; // the global index of the sensor this event occured on uint32_t trigger; // the microseconds since the sensor was triggered @@ -36,11 +34,13 @@ typedef struct { uint32_t lost_signals = 0; } LLSensor; +// FIXME unify index parameter names + /* low-level TOF sensor manager * * This class is responsible for all the low level plumbing: interrupts, timings, pins, etc * This class deals exclusively with microseconds. The companion high level interface is - * HCSR04SensorManager. + * DistanceSensorManager. These two parts of the code communicate via a FreeRTOS Queue. */ class TOFManager { public: @@ -52,7 +52,7 @@ class TOFManager { void init(); /* - * startup() + * startupManager() * * The only thing you ever need to call. This will spawn another task on a different CPU * and you won't have to interact with this class at all. @@ -62,7 +62,7 @@ class TOFManager { static void startupManager(QueueHandle_t sensor_queue); private: TOFManager(QueueHandle_t q, LLSensor* sensors) : m_sensors(sensors), sensor_queue(q) {} - ~TOFManager(); + ~TOFManager(); // FIXME void startMeasurement(uint8_t sensorId); void disableMeasurement(uint8_t sensor_idx);