Skip to content

Commit 1086d49

Browse files
authored
Read the tof-sensors in parallel (#224)
- Read the tof-sensors in parallel - primary sensor gives pace - max 35 readings per minute in csv - nerd display shows number of echo triggers of primary sensor in the middle - do not report timeout readings - inner loop runs more often (no wait() till tof results are available) - SENSOR_QUIET_PERIOD_AFTER_START_MICRO_SEC to 30ms (was 35) - relates #221 - call display only every 20ms max. - Introduce SENSOR_QUIET_PERIOD_AFTER_OPPOSITE_START_MICRO_SEC - remove unused code, from alternating reading - Show number of no signal readings error on about pages
1 parent 9ab8f4d commit 1086d49

File tree

5 files changed

+165
-208
lines changed

5 files changed

+165
-208
lines changed

src/OpenBikeSensorFirmware.cpp

Lines changed: 47 additions & 42 deletions
Original file line numberDiff line numberDiff line change
@@ -61,8 +61,11 @@ static BluetoothManager* bluetoothManager;
6161

6262
Gps gps;
6363

64-
static const uint32_t BLUETOOTH_INTERVAL_MILLIS = 100;
65-
static uint32_t lastBluetoothInterval = 0;
64+
static const long BLUETOOTH_INTERVAL_MILLIS = 150;
65+
static long lastBluetoothInterval = 0;
66+
67+
static const long DISPLAY_INTERVAL_MILLIS = 20;
68+
static long lastDisplayInterval = 0;
6669

6770
float TemperatureValue = -1;
6871

@@ -328,7 +331,7 @@ void setup() {
328331
while (!gps.hasFix(displayTest)) {
329332
currentTimeMillis = millis();
330333
gps.handle();
331-
sensorManager->getDistances();
334+
sensorManager->pollDistancesParallel();
332335
reportBluetooth();
333336
gps.showWaitStatus(displayTest);
334337
buttonState = digitalRead(PushButton_PIN);
@@ -353,7 +356,7 @@ void setup() {
353356
void serverLoop() {
354357
gps.handle();
355358
configServerHandle();
356-
sensorManager->getDistancesNoWait();
359+
sensorManager->pollDistancesParallel();
357360
handleButtonInServerMode();
358361
}
359362

@@ -415,10 +418,9 @@ void loop() {
415418
currentSet->isInsidePrivacyArea = gps.isInsidePrivacyArea();
416419
currentSet->gpsRecord = gps.getCurrentGpsRecord();
417420

421+
lastMeasurements = sensorManager->m_sensors[confirmationSensorID].numberOfTriggers;
418422
sensorManager->reset();
419423

420-
int measurements = 0;
421-
422424
// if the detected minimum was measured more than 5s ago, it is discarded and cannot be confirmed
423425
int timeDelta = (int) (currentTimeMillis - timeOfMinimum);
424426
if (datasetToConfirm != nullptr &&
@@ -428,24 +430,44 @@ void loop() {
428430
datasetToConfirm = nullptr;
429431
}
430432

433+
int loops = 0;
431434
// do this for the time specified by measureInterval, e.g. 1s
432435
while ((currentTimeMillis - startTimeMillis) < measureInterval) {
436+
loops++;
433437

434438
currentTimeMillis = millis();
435-
sensorManager->getDistances();
439+
if (sensorManager->pollDistancesParallel()) {
440+
// if a new minimum on the selected sensor is detected, the value and the time of detection will be stored
441+
const uint16_t reading = sensorManager->sensorValues[confirmationSensorID];
442+
if (reading > 0 && reading < minDistanceToConfirm) {
443+
minDistanceToConfirm = reading;
444+
minDistanceToConfirmIndex = sensorManager->getCurrentMeasureIndex();
445+
// if there was no measurement of this sensor for this index, it is the
446+
// one before. This happens with fast confirmations.
447+
while (minDistanceToConfirmIndex > 0
448+
&& sensorManager->m_sensors[confirmationSensorID].echoDurationMicroseconds[minDistanceToConfirmIndex] <= 0) {
449+
minDistanceToConfirmIndex--;
450+
}
451+
datasetToConfirm = currentSet;
452+
timeOfMinimum = currentTimeMillis;
453+
}
454+
}
436455
gps.handle();
437456

438-
displayTest->showValues(
439-
sensorManager->m_sensors[LEFT_SENSOR_ID],
440-
sensorManager->m_sensors[RIGHT_SENSOR_ID],
441-
minDistanceToConfirm,
442-
voltageMeter->readPercentage(),
443-
(int16_t) TemperatureValue,
444-
lastMeasurements,
445-
currentSet->isInsidePrivacyArea,
446-
gps.getSpeed(),
447-
gps.getValidSatellites()
448-
);
457+
if (lastDisplayInterval != (currentTimeMillis / DISPLAY_INTERVAL_MILLIS)) {
458+
lastDisplayInterval = currentTimeMillis / DISPLAY_INTERVAL_MILLIS;
459+
displayTest->showValues(
460+
sensorManager->m_sensors[LEFT_SENSOR_ID],
461+
sensorManager->m_sensors[RIGHT_SENSOR_ID],
462+
minDistanceToConfirm,
463+
voltageMeter->readPercentage(),
464+
(int16_t) TemperatureValue,
465+
lastMeasurements,
466+
currentSet->isInsidePrivacyArea,
467+
gps.getSpeed(),
468+
gps.getValidSatellites()
469+
);
470+
}
449471

450472
reportBluetooth();
451473
buttonState = digitalRead(PushButton_PIN);
@@ -478,23 +500,9 @@ void loop() {
478500
lastButtonState = buttonState;
479501
}
480502

481-
// if a new minimum on the selected sensor is detected, the value and the time of detection will be stored
482-
const uint16_t reading = sensorManager->sensorValues[confirmationSensorID];
483-
if (reading > 0 && reading < minDistanceToConfirm) {
484-
minDistanceToConfirm = reading;
485-
minDistanceToConfirmIndex = sensorManager->getCurrentMeasureIndex();
486-
// if there was no measurement of this sensor for this index, it is the
487-
// one before. This happens with fast confirmations.
488-
while (minDistanceToConfirmIndex > 0
489-
&& sensorManager->m_sensors[confirmationSensorID].echoDurationMicroseconds[minDistanceToConfirmIndex] <= 0) {
490-
minDistanceToConfirmIndex--;
491-
}
492-
datasetToConfirm = currentSet;
493-
timeOfMinimum = currentTimeMillis;
494-
}
495-
measurements++;
503+
if(BMP280_active == true) TemperatureValue = bmp280.readTemperature();
496504

497-
if(BMP280_active == true) TemperatureValue = bmp280.readTemperature();
505+
yield(); //
498506
} // end measureInterval while
499507

500508
// Write the minimum values of the while-loop to a set
@@ -510,15 +518,12 @@ void loop() {
510518
&(sensorManager->startOffsetMilliseconds), currentSet->measurements * sizeof(uint16_t));
511519

512520
#ifdef DEVELOP
513-
Serial.write("min. distance: ");
514-
Serial.print(currentSet->sensorValues[confirmationSensorID]) ;
515-
Serial.write(" cm,");
516-
Serial.print(measurements);
517-
Serial.write(" measurements \n");
521+
log_i("min. distance: %dcm, loops %d",
522+
currentSet->sensorValues[confirmationSensorID],
523+
loops);
518524
#endif
519525

520526
dataBuffer.push(currentSet);
521-
lastMeasurements = measurements;
522527
// convert all data that does not wait for confirmation.
523528
while ((!dataBuffer.isEmpty() && dataBuffer.first() != datasetToConfirm) || dataBuffer.isFull()) {
524529
DataSet* dataset = dataBuffer.shift();
@@ -552,8 +557,8 @@ void loop() {
552557
displayTest->normalDisplay();
553558
}
554559
}
555-
556-
log_i("Time elapsed in loop: %lu milliseconds", currentTimeMillis - startTimeMillis);
560+
log_i("Time in loop: %lums %d inner loops, %d measures",
561+
currentTimeMillis - startTimeMillis, loops, lastMeasurements);
557562
// synchronize to full measureIntervals
558563
startTimeMillis = (currentTimeMillis / measureInterval) * measureInterval;
559564
}

src/configServer.cpp

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -903,10 +903,12 @@ static void handleAbout(HTTPRequest *req, HTTPResponse * res) {
903903
page += keyValue("Left Sensor max duration", sensorManager->getMaxDurationUs(LEFT_SENSOR_ID), "&#xB5;s");
904904
page += keyValue("Left Sensor min duration", sensorManager->getMinDurationUs(LEFT_SENSOR_ID), "&#xB5;s");
905905
page += keyValue("Left Sensor last start delay", sensorManager->getLastDelayTillStartUs(LEFT_SENSOR_ID), "&#xB5;s");
906+
page += keyValue("Left Sensor signal errors", sensorManager->getNoSignalReadings(LEFT_SENSOR_ID));
906907
page += keyValue("Right Sensor raw", sensorManager->getRawMedianDistance(RIGHT_SENSOR_ID), "cm");
907908
page += keyValue("Right Sensor max duration", sensorManager->getMaxDurationUs(RIGHT_SENSOR_ID), "&#xB5;s");
908909
page += keyValue("Right Sensor min duration", sensorManager->getMinDurationUs(RIGHT_SENSOR_ID), "&#xB5;s");
909910
page += keyValue("Right Sensor last start delay", sensorManager->getLastDelayTillStartUs(RIGHT_SENSOR_ID), "&#xB5;s");
911+
page += keyValue("Right Sensor signal errors", sensorManager->getNoSignalReadings(RIGHT_SENSOR_ID));
910912

911913
res->print(page);
912914
page.clear();

0 commit comments

Comments
 (0)