Corralling all sensor data updates into one place:
I have been struggling with how to manage sensor data updates for Wall-E3. When I originally started working with Wall-E, it had only three sensors – a left and right-side HC-04 ‘ping’ sensor and the front LIDAR distance sensor, so data updates weren’t a significant part of the algorithm. Since then the sensor population as ballooned past the double-digit mark, with seven VL53L0X side/rear distance sensors, the front LIDAR sensor, two high-side current sensors, and the MPU6050 IMU.
Back in August 2020 I decided to change from a ‘request only when needed’ to a TIMER interrupt-based sensor update paradigm. The idea was to update all sensor data X times/sec in an Interrupt Service Routine (ISR). This worked great, but caused other problems that eventually led me to abandon this approach. In addition to not knowing exactly when/where in the program the sensor data changed, it appeared this approach was incompatible with my use of the PID library for motion control. The PID library’s ‘Compute()’ function expects to be called in a loop that runs many times faster than the PID’s internal update period (100mSec by default). PID::Compute() returns without doing anything until its internal 100mSec timer expires, at which point it does one PID computation and then resets the timer. So, there was a conflict, because I wanted to call PID::Compute() each time the TIMER ISR executed (using a ‘global’ boolean flag), but PID::Compute() wants to execute only when it’s internal timer expires. I never could figure out how to make those two requirements work together. Eventually I abandoned both of them. First, I dumped the PID library and rolled my own PIDCalcs() function that computed a new output every time it was called, and I dumped the timer ISR in favor of ‘just in time’ sensor data updates.
Fast-forward to the present, and now I’m still struggling to figure out how to manage sensor data updates. As my latest ‘sand-box’ testing showed, I need to update all the distance sensors even when I’m only tracking one side, so just updating one side or the other doesn’t work. So, I created a ‘UpdateAllDistances()’ function as shown below:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 |
void UpdateAllDistances() { //Purpose: Update all 8 distance measurements glFrontdistval = GetFrontDistCm(); GetRequestedVL53l0xValues(VL53L0X_ALL); UpdateLRDistanceArrays(glLeftCenterCm, glRightCenterCm); } //08/12/20 Extracted inline FRONT_DIST_ARRAY init code so can be called from anywhere void InitFrontDistArray() { //04/01/15 initialize 'stuck detection' arrays //06/17/20 re-wrote for better readability //to ensure var > STUCK_FRONT_VARIANCE_THRESHOLD for first FRONT_DIST_ARRAY_SIZE loops //array is initialized with sawtooth from 0 to MAX_FRONT_DISTANCE_CM myTeePrint.printf("InitFrontDistArray() top: prev_mean = %d, prev_var = %d\n", glFront_Dist_PrevMean, glFront_Dist_PrevVar); int newval = 0; int bumpval = MAX_FRONT_DISTANCE_CM / FRONT_DIST_ARRAY_SIZE; bool bgoingUp = true; for (int i = 0; i < FRONT_DIST_ARRAY_SIZE; i++) { aFrontDist[i] = newval; //DEBUG!! //myTeePrint.printf("i = %d, newval = %d, aFrontdist[%d] = %d\n", i, newval, i, aFrontDist[i]); //DEBUG!! if (bgoingUp) { if (newval < MAX_FRONT_DISTANCE_CM - bumpval) //don't want newval > MAX_FRONT_DISTANCE_CM { newval += bumpval; } else { bgoingUp = false; } } else { if (newval > bumpval) //don't want newval < 0 { newval -= bumpval; } else { bgoingUp = true; } } } //04/19/19 init glFront_Dist_PrevMean & glFront_Dist_PrevVar to mean/var respectively long sum = 0; for (int i = 0; i < FRONT_DIST_ARRAY_SIZE; i++) { sum += aFrontDist[i]; //adds in rest of values } glFront_Dist_PrevMean = (float)sum / (float)FRONT_DIST_ARRAY_SIZE; // Step2: calc new 'brute force' variance float sumsquares = 0; for (int i = 0; i < FRONT_DIST_ARRAY_SIZE; i++) { sumsquares += (aFrontDist[i] - glFront_Dist_PrevMean) * (aFrontDist[i] - glFront_Dist_PrevMean); } glFront_Dist_PrevVar = sumsquares / FRONT_DIST_ARRAY_SIZE; myTeePrint.printf("%lu: aFrontDist Init: glFront_Dist_PrevMean = %3.2f, glFront_Dist_PrevVar = %3.2f\n", millis(), glFront_Dist_PrevMean, glFront_Dist_PrevVar); glFrontvar = glFront_Dist_PrevVar; //added 03/30/21 to prevent 'stuck' at startup //DEBUG!! //myTeePrint.printf("Initial Front Distance Array Contents\n"); //myTeePrint.printf("Idx\tRdist\n"); //for (size_t i = 0; i < REAR_DIST_ARRAY_SIZE; i++) //{ // myTeePrint.printf("%d\t%d\n", i, aFrontDist[i]); //} //DEBUG!! } |
This function also causes the front and rear distance arrays to be updated and new front/rear variances to be calculated. The function is intended to be called from both the left and right wall tracking loops, and anywhere else updated distance and related data updates are required.
Having created this function, the next question becomes – how long does this function take to execute? If it is too long, then tracking performance will suffer. To answer this question, I placed code at the beginning and end of UpdateAllDistances() to toggle a hardware pin so I can measure the elapsed time on a scope, and I placed code at the beginning/end of a small WALL_TRACK_UPDATE_INTERVAL_MSEC test loop in setup(), as follows:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 |
//03/08/22 temporary code to measure UpdateAllEnvironmentParametes() duration myTeePrint.printf("Temporary duration measurement tests...\n"); mSecSinceLastWallTrackUpdate = 0; uint16_t numdots = 0; while (true) { if (mSecSinceLastWallTrackUpdate >= WALL_TRACK_UPDATE_INTERVAL_MSEC) { mSecSinceLastWallTrackUpdate -= WALL_TRACK_UPDATE_INTERVAL_MSEC; digitalToggle(DURATION_MEASUREMENT_PIN2); myTeePrint.printf("%lu\n", millis()); UpdateAllEnvironmentParameters(); } } |
With this test, I got the following output on my HANMATECK DOS1102 digital O’Scope:
As can bee seen in the above plot, UpdateAllEnvironmentParameters() takes around 6-10mSec to update all environmental parameters (basically everything except MPU6050 heading), leaving 190-194mSec to complete the rest of the tracking update loop. This is very good news, as it means I can basically think of UpdateAllEnvironmentParameters() as a one-line ‘do everything’ command with negligible duration.
Next I made the same measurement, but this time with the actual ‘TrackLeftWallOffset() code being executed. As can be seen from the following image, the result is essentially identical to the first test; UpdateAllEnvironmentParameters() takes around 6-10mSec.
Then I did this test one more time, except this time I toggled the blue trace at the beginning and end of wall track processing, to show the time remaining in the 200mSec loop. Here’s what actually happens in the tracking loop:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 |
//09/04/21 back to elapsedMillis timing if (mSecSinceLastWallTrackUpdate > WALL_TRACK_UPDATE_INTERVAL_MSEC) { digitalToggle(DURATION_MEASUREMENT_PIN2); //03/08/22 toggle duration measurement pulse mSecSinceLastWallTrackUpdate -= WALL_TRACK_UPDATE_INTERVAL_MSEC; //03/08/22 abstracted these calls to UpdateAllEnvironmentParameters() UpdateAllEnvironmentParameters();//03/08/22 added to consolidate sensor update calls errcode = CheckForErrorCondx(); //have to weight value by both angle and wall offset WallTrackSteerVal = glLeftSteeringVal + (glLeftCenterCm - WALL_OFFSET_TGTDIST_CM) / 1000.f; //update motor speeds, skipping bad values if (!isnan(WallTrackSteerVal)) { //02/05/22 sampleTime removed from signature WallTrackOutput = PIDCalcs(WallTrackSteerVal, WallTrackSetPoint, lastError, lastInput, lastIval, lastDerror, kp, ki, kd); int glLeftspeednum = MOTOR_SPEED_QTR + WallTrackOutput; int glRightspeednum = MOTOR_SPEED_QTR - WallTrackOutput; glRightspeednum = (glRightspeednum <= MOTOR_SPEED_HALF) ? glRightspeednum : MOTOR_SPEED_HALF; glRightspeednum = (glRightspeednum > 0) ? glRightspeednum : 0; glLeftspeednum = (glLeftspeednum <= MOTOR_SPEED_HALF) ? glLeftspeednum : MOTOR_SPEED_HALF; glLeftspeednum = (glLeftspeednum > 0) ? glLeftspeednum : 0; MoveAhead(glLeftspeednum, glRightspeednum); myTeePrint.printf("%lu\t%d\t%d\t%d\t%d\t\t%d\t%d\t%d\t%d\t\t%2.2f\t%2.2f\t\t%2.2f\t%2.2f\n", millis(), glLeftFrontCm, glLeftCenterCm, glLeftRearCm, glRearCm, glRightFrontCm, glRightCenterCm, glRightRearCm, glFrontdistval, glFrontvar, glRearvar, WallTrackSteerVal, WallTrackSetPoint); } digitalToggle(DURATION_MEASUREMENT_PIN2); //03/08/22 toggle duration measurement pulse } |
And here’s the screen grab from my O’scope showing the actual duration of everything in the above loop.
As can be seen, almost all of the tracking processing time is spent in UpdateAllEnvironmentParameters(), and there is plenty of time to do additional processing (like anomaly handling). The 200 mSec loop is denoted above by adjacent rising edges of the blue trace, and all processing is finished at the trailing edge of the blue trace, so only about 10mSec, or about 5%, of the 200mSec is taken.
So it is clear that consolidating all environmental sensor updates into one function is a big winner. The time taken for sensor data updates is a small percentage of the time available for the entire tracking loop, but it is almost all of the time required in each tracking loop. This is very interesting result. The time required for sensor update probably cannot be reduced, as it depends on the actual hardware sensor response times and the ability to get the sensor data back to the main Teensy 3.5 processor via I2C. However, it now appears that I could easily reduce the overall tracking loop duration from the nominal 200mSec to 100, 50, or even 20mSec with no adverse effects, and presumably a corresponding increase in tracking performance. This is probably the biggest win associated with the change from the Arduino MEGA2560 to the Teensy 3.5 – so much less time required for processing.
Stay tuned,
Frank