Freezes during float calculation procedure

I’m having an issue where my program runs a loop that performs calculations. Around the 20th loop the calculations freeze (uncertain exactly which loop due to delay in serial out for my debugging).

The error manifests between line 110 and line 168 (although it could be caused by a call outside that range). When I comment out this region of the code it runs fine.

My shotCounter ranges between 0 and 3200. I used excel to see the values of all my float variables. The float variables have up to 7 characters (including decimal point) and have a max value of 719.775.

// define variables to read into from SF30 laser distance
float sfDistance;
int Byte_L, Byte_H;
unsigned long myYawSteps = 400*4;  // This is a 400 step motor with two Eighth Steps = quarter step
unsigned long myPitchSteps = 400*4;  // This is a 400 step motor with two Eighth Steps = quarter step

/*----------------------------------------------------------------
* scanRoom function - what we've been waiting for...actually scan the room
*/
void scanRoom() {
  DEBUG_PRINTLN(F("Start room scan"));
  
  //Define Variables
  float range,azimuth,elevation;
  float degPerYawStep = float(360)/myYawSteps;
  float degPerPitchStep = float(360)/myPitchSteps;
  unsigned long shotCounter = 0;
  unsigned int bufferCounter = 0;
  unsigned long calculatePosition = 0;

  char readingBuffer [9]; //Need 8 + 1 for trailing null from dtostrf
  char lineOutputBuffer [31];
  float measurementBuffer [128];  //each float takes up 4 bytes

  //test varialbes to delete later
  //float testrange = 39.999;
  //float testazimuth = 99.9999;
  //float testelevation = 100.99;
  float testintensity = 256.0;

  //Warmup the SF30
  warmupSF30();

  //Make sure the motors are not sleeping
  digitalWrite(slpPin,HIGH);

  //Open the SD card
  DEBUG_PRINTLN(F("Start SD card"));
  delay(10);

  //For now, overwrite old file...need file increment function
  if (SD.exists("OL1.CSV")) {
   //DEBUG_PRINTLN("Can access SD card, but can't open new file");
   //delay(1); //let bluetooth catch up
   SD.remove("OL1.CSV");
  }

  delay(100);

  myFile = SD.open("OL1.CSV", FILE_WRITE);
  myFile.println("Range,Azimuth,Elevation");
  myFile.close();

  //Begin scanning procedure
  //Turn the pitch motor 1 full turn while taking measurements
  //Write to SD card every 512 Bytes
  //Turn azimuth motor 1 'step' for each pitch motor turn
  
  //Turn on the SF30
  sf30_serial.print("#Y");
  delay(10);
  
  myFile = SD.open("OL1.CSV", FILE_WRITE);
  //myFile.println("Range,Azimuth,Elevation,Intensity");
  //myFile.close();
  DEBUG_PRINTLN(F("Scanning"));
  delay(100); //let bluetooth catch up
  
  //Turn Azimuth
  //for(int aloop = 0; aloop < myYawSteps; aloop++) {
  for(int aloop = 0; aloop < 2; aloop++) {
    // Calculate the azimuth for this loop
    //azimuth = float(aloop)*degPerYawStep;

    //Turn Pitch
    for(int ploop = 0; ploop < myPitchSteps; ploop++) {
    //for(int ploop = 0; ploop < 16; ploop++) {
      // Take a measurement
      //If there are more than 1 reading (2 bytes) then we have gotten ahead of ourselves and need to clear old data
      while (sf30_serial.available() > 2) { //If there are more than 1 measurement to be read
        Byte_H = sf30_serial.read();
        while (!sf30_serial.available()); //Don't get off order, wait for second bit.
        Byte_L = sf30_serial.read();
        //do nothing with this data, but throw it away as old data
      }
      
      while (!sf30_serial.available()); //Wait until data is available
      Byte_H = sf30_serial.read();
      while (!sf30_serial.available()); //Don't get off order, wait for second bit.
      Byte_L = sf30_serial.read();
      range = (float(Byte_L))/256 + Byte_H;
	  
      //if the range is over max distance of 50m try again one time
      if(range > 50.0) {
        while (!sf30_serial.available()); //Wait until data is available
        Byte_H = sf30_serial.read();
        while (!sf30_serial.available()); //Don't get off order, wait for second bit.
        Byte_L = sf30_serial.read();
        range = (float(Byte_L))/256 + Byte_H;
      }
      measurementBuffer[shotCounter % 128] = range;

      //add readings to sdBuffer in 32 byte increments
      //8 range 1 ',' 8 azimuth 1 ',' 8 elevation 1 ',' 3 intensity 1 'lf' 1'cr'
     
      //The (number of cycles)(bytes output per cycle) needs to have a factor of 512
      //readingBuffer is 128 floats which calculates 128 rounds of 32 bytes total
      //If we are at the end of the buffer shotcounter % 128 = 127
      //Send the buffer to the sd card
      if (shotCounter % 128 == 127) {
        //DEBUG_PRINTLN("Write Loop");
        //delay(100);
        
        for (int bb = 0; bb < 128; bb++) {
          //DEBUG_PRINT(" ");
          //DEBUG_PRINTLN(freeMemory());
          calculatePosition = shotCounter - 127 + bb;
          DEBUG_PRINT(" ");
          DEBUG_PRINT(calculatePosition);
          delay(10);
          if ((calculatePosition) < myPitchSteps) { //if calculatePosition < 1600
            elevation = float(calculatePosition)*degPerPitchStep;
            azimuth = float(0); //this is the first pitch turn...so azimuth is still 0
          }
          else {
            calculatePosition = calculatePosition % myPitchSteps;
            DEBUG_PRINT(" ");
            DEBUG_PRINT(calculatePosition);
            delay(10);
            elevation = float((calculatePosition))*degPerPitchStep;
            calculatePosition = shotCounter - 127 + bb;
            calculatePosition = calculatePosition/myPitchSteps;
            DEBUG_PRINT(" ");
            DEBUG_PRINT(calculatePosition);
            delay(10);
            azimuth = float((calculatePosition))*degPerYawStep;
          }

          calculatePosition = (shotCounter % 127) + bb;
          DEBUG_PRINT(" ");
          DEBUG_PRINT(calculatePosition);
          delay(10);
          dtostrf(measurementBuffer[calculatePosition],8,4,readingBuffer);
          //dtostrf(float(2),8,4,readingBuffer);
          for (int br = 0; br < 8; br++) {
            lineOutputBuffer[br] = readingBuffer[br];
          }
          lineOutputBuffer[8] = ',';
          dtostrf(azimuth,8,4,readingBuffer);
          for (int ba = 0; ba < 8; ba++) {
            lineOutputBuffer[ba+9] = readingBuffer[ba];
          }
          lineOutputBuffer[17] = ',';
          dtostrf(elevation,8,4,readingBuffer);
          for (int be = 0; be < 8; be++) {
            lineOutputBuffer[be+18] = readingBuffer[be];
          }
          lineOutputBuffer[26] = ',';
          dtostrf(testintensity,5,0,readingBuffer);
          for (int ba = 0; ba < 3; ba++) {
            lineOutputBuffer[ba+27] = readingBuffer[ba+2];
          }
          lineOutputBuffer[30] = ' ';
          lineOutputBuffer[31] = 10;
          //write one line at a time to save memory
          myFile.write(lineOutputBuffer,32);
        }
      }

      //Step the pitch motor forward 1 step
      //For now, run forward 2 eighth steps to get quarter step...need improvement
      StepMotorForward(1);
      StepMotorForward(1);

      //Increment the shot counter every time we take a shot
      shotCounter += 1;
    } //end pitch loop

    //DEBUG_PRINTLN("Yaw Loop");
    //Once every yaw cycle save the file in case of crash
    myFile.flush();
    }

    // Run the Yaw motor forward 1 step since the pitch has run a full rotation
    //For now, run forward 2 eighth steps to get quarter step...need improvement
    StepMotorForward(2);
    StepMotorForward(2);
    
  }  //end azimuth loop

  //Make sure the motors are sleeping
  digitalWrite(slpPin,LOW);
  myFile.close();
  delay(100);
  DEBUG_PRINTLN(F("Write Complete"));
}

I’m grasping at straws trying to figure out why this is freezing. Also, I feel like there should be a more efficient way to calculate this. I am rotating my pitch motor 1600 steps. For every step I capture a distance. Then I write the distance, the pitch angle, the azimuth angle (1 azimuth step per 1600 pitch steps), and intensity (if available). In order to save precious memory I am trying to store 128 distance measurements. Then once those are collected I’m trying to calculate the azimuth and pitch that associate with that reading. I take one line of those and send to SD card and repeat.

No idea where the error is but it sounds you're running out of memory.

But, impossible to tell because you didn't post all the code. If you think that is to much (which it looks like it), try to make a simplified sketch with the same error. At least make it compile.

And yeah, you do a whole lot of float math. You can probably save a lot (of memory and speed) by sticking to int math and do it fixed point. You don't need a float because you want a decimal place :wink: For example, instead of saving degrees you save everything in milli-degrees :slight_smile:

Remainder of the code is: https://github.com/caveradam/OpenLidar/blob/master/OpenLidarMainSpeedSD/OpenLidarMainSpeedSD.ino

Also, I used freeMemory to try to debug the project and had 200 bytes of memory free when the program freezes. Unless I messed up on where I put the freeMemory function.

      while (sf30_serial.available() > 2) { //If there are more than 1 measurement to be read
        Byte_H = sf30_serial.read();
        while (!sf30_serial.available()); //Don't get off order, wait for second bit.
        Byte_L = sf30_serial.read();
        //do nothing with this data, but throw it away as old data
      }

Why do you need there to be 3 or more bytes to read, when you only read 2?

Why are there ANY delay()s in the scanRoom(), especially in the scanning loops?

Good questions.

Iff there are more than 2 bytes in the sensor buffer I only want the most recent reading. So I loop until there are only 2 or less bytes in the buffer. My gimbal turns and has some delay. There is a chance that while it was turning the sensor took multiple readings. Especially while waiting for the SD card.

The delays are ONLY for debugging purposes. If I don't have a delay then my Serial.Print doesn't have enough time to send information to me when the arduino freezes. The delay lets the serial information get to the computer before moving on. All delays will be removed when the problem is resolved.

Could the problem be here?

calculatePosition = (shotCounter % 127) + bb;

(Note the presence of the constant 127.)

By the way, why are you using floating-point at all in your code?

CaverAdam:
All delays will be removed when the problem is resolved.

Reminds me of "the beatings will continue until morale improves".