diff options
Diffstat (limited to 'src/Movement/Grid.cpp')
-rw-r--r-- | src/Movement/Grid.cpp | 222 |
1 files changed, 177 insertions, 45 deletions
diff --git a/src/Movement/Grid.cpp b/src/Movement/Grid.cpp index ba194537..3cd91e49 100644 --- a/src/Movement/Grid.cpp +++ b/src/Movement/Grid.cpp @@ -9,12 +9,11 @@ #include "RepRapFirmware.h" #include <cmath> -// Increase the version number in the following string whenever we change the format of the height map file. -const char *HeightMapComment = "RepRapFirmware height map file v1"; +const char *GridDefinition::HeightMapLabelLine = "xmin,xmax,ymin,ymax,radius,spacing,xnum,ynum"; // Initialise the grid to be invalid GridDefinition::GridDefinition() - : xMin(0.0), xMax(-1.0), yMin(0.0), yMax(-1.0), radius(-1.0), spacing(DefaultGridSpacing), gridHeights(nullptr), + : xMin(0.0), xMax(-1.0), yMin(0.0), yMax(-1.0), radius(-1.0), spacing(DefaultGridSpacing), numX(0), numY(0), recipSpacing(1.0/spacing), isValid(false) { } @@ -24,13 +23,13 @@ GridDefinition::GridDefinition(const float xRange[2], const float yRange[2], flo { numX = (xMax - xMin >= MinRange && spacing >= MinSpacing) ? (uint32_t)((xMax - xMin) * recipSpacing) + 1 : 0; numY = (yMax - yMin >= MinRange && spacing >= MinSpacing) ? (uint32_t)((yMax - yMin) * recipSpacing) + 1 : 0; - isValid = NumPoints() != 0 && NumPoints() <= MaxGridProbePoints && (radius < 0.0 || radius >= 1.0); + CheckValidity(); + } -void GridDefinition::SetStorage(const float *heightStorage, const uint32_t *heightSetStorage) +void GridDefinition::CheckValidity() { - gridHeights = heightStorage; - gridHeightSet = heightSetStorage; + isValid = NumPoints() != 0 && NumPoints() <= MaxGridProbePoints && (radius < 0.0 || radius >= 1.0); } float GridDefinition::GetXCoordinate(unsigned int xIndex) const @@ -49,27 +48,54 @@ bool GridDefinition::IsInRadius(float x, float y) const } // Append the grid parameters to the end of a string -void GridDefinition::PrintParameters(StringRef& r) const +void GridDefinition::PrintParameters(StringRef& s) const +{ + s.catf("X%.1f:%.1f, Y%.1f:%.1f, radius %.1f, spacing %.1f, %d points", xMin, xMax, yMin, yMax, radius, spacing, NumPoints()); +} + +// Write the parameter label line to a string +void GridDefinition::WriteHeadingAndParameters(StringRef& s) const +{ + s.printf("%s\n%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%u,%u\n", HeightMapLabelLine, xMin, xMax, yMin, yMax, radius, spacing, numX, numY); +} + +// Check the parameter label line returning true if correct +/*static*/ bool GridDefinition::CheckHeading(const StringRef& s) { - r.catf("X%.1f:%.1f, Y%.1f:%.1f, radius %.1f, spacing %.1f, %d points", xMin, xMax, yMin, yMax, radius, spacing, NumPoints()); + return StringStartsWith(s.Pointer(), HeightMapLabelLine); } -// Print what is wrong with the grid +// Read the grid parameters from a string returning true if success +bool GridDefinition::ReadParameters(const StringRef& s) +{ + bool ok = (sscanf(s.Pointer(), "%f,%f,%f,%f,%f,%f,%lu,%lu", &xMin, &xMax, &yMin, &yMax, &radius, &spacing, &numX, &numY) == 8); + if (ok) + { + CheckValidity(); + } + else + { + isValid = false; + } + return ok; +} + +// Print what is wrong with the grid, appending it to the existing string void GridDefinition::PrintError(StringRef& r) const { if (spacing < MinSpacing) { r.cat("Spacing too small"); } - else if (NumXpoints() == 0) + else if (numX == 0) { r.cat("X range too small"); } - else if (NumYpoints() == 0) + else if (numY == 0) { r.cat("Y range too small"); } - else if (NumPoints() > MaxGridProbePoints) + else if (numX > MaxGridProbePoints || numY > MaxGridProbePoints || NumPoints() > MaxGridProbePoints) // check X and Y individually in case X*Y overflows { r.catf("Too many grid points (maximum %d, needed %d)", MaxGridProbePoints, NumPoints()); } @@ -80,8 +106,44 @@ void GridDefinition::PrintError(StringRef& r) const } } +// Increase the version number in the following string whenever we change the format of the height map file. +const char *HeightMap::HeightMapComment = "RepRapFirmware height map file v1"; + +HeightMap::HeightMap(float *heightStorage) : gridHeights(heightStorage) { } + +void HeightMap::SetGrid(const GridDefinition& gd) +{ + def = gd; + ClearGridHeights(); +} + +void HeightMap::ClearGridHeights() +{ + for (size_t i = 0; i < MaxGridProbePoints/32; ++i) + { + gridHeightSet[i] = 0; + } +} + +// Set the height of a grid point +void HeightMap::SetGridHeight(size_t xIndex, size_t yIndex, float height) +{ + size_t index = yIndex * def.numX + xIndex; + if (index < MaxGridProbePoints) + { + gridHeights[index] = height; + gridHeightSet[index/32] |= 1u << (index & 31u); + } +} + +// Return the minimum number of segments for a move by this X or Y amount +unsigned int HeightMap::GetMinimumSegments(float distance) const +{ + return (distance > 0.0) ? (unsigned int)(distance * def.recipSpacing + 0.4) : 1; +} + // Save the grid to file returning true if an error occurred -bool GridDefinition::SaveToFile(FileStore *f) const +bool HeightMap::SaveToFile(FileStore *f) const { char bufferSpace[500]; StringRef buf(bufferSpace, ARRAY_SIZE(bufferSpace)); @@ -93,7 +155,7 @@ bool GridDefinition::SaveToFile(FileStore *f) const time_t timeNow = reprap.GetPlatform()->GetDateTime(); const struct tm * const timeInfo = gmtime(&timeNow); buf.catf(" generated at %04u-%02u-%02u %02u:%02u", - timeInfo->tm_year, timeInfo->tm_mon, timeInfo->tm_mday, timeInfo->tm_hour, timeInfo->tm_min); + timeInfo->tm_year + 1900, timeInfo->tm_mon, timeInfo->tm_mday, timeInfo->tm_hour, timeInfo->tm_min); } buf.cat('\n'); if (!f->Write(buf.Pointer())) @@ -102,10 +164,7 @@ bool GridDefinition::SaveToFile(FileStore *f) const } // Write the grid parameters - buf.printf("xmin,xmax,ymin,ymax,radius,spacing,xnum,ynum\n" - "%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%u,%u\n", - xMin, xMax, yMin, yMax, radius, spacing, numX, numY - ); + def.WriteHeadingAndParameters(buf); if (!f->Write(buf.Pointer())) { return true; @@ -113,10 +172,10 @@ bool GridDefinition::SaveToFile(FileStore *f) const // Write the grid heights uint32_t index = 0; - for (uint32_t i = 0; i < numY; ++i) + for (uint32_t i = 0; i < def.numY; ++i) { buf.Clear(); - for (uint32_t j = 0; j < numX; ++j) + for (uint32_t j = 0; j < def.numX; ++j) { if (j != 0) { @@ -142,20 +201,93 @@ bool GridDefinition::SaveToFile(FileStore *f) const return false; } -// Load the grid from file returning true if an error occurred -bool GridDefinition::LoadFromFile(FileStore *f) +// Load the grid from file, returning true if an error occurred with the error reason appended to the buffer +bool HeightMap::LoadFromFile(FileStore *f, StringRef& r) { - //TODO - return true; + const size_t MaxLineLength = 200; // maximum length of a line in the height map file + const char* const readFailureText = "failed to read line from file"; + char buffer[MaxLineLength + 1]; + StringRef s(buffer, ARRAY_SIZE(buffer)); + + ClearGridHeights(); + GridDefinition newGrid; + + if (f->ReadLine(buffer, sizeof(buffer)) <= 0) + { + r.cat(readFailureText); + } + else if (!StringStartsWith(buffer, HeightMapComment)) // check the version line is as expected + { + r.cat("bad header line or wrong version header"); + } + else if (f->ReadLine(buffer, sizeof(buffer)) <= 0) + { + r.cat(readFailureText); + } + else if (!GridDefinition::CheckHeading(s)) // check the label line is as expected + { + r.cat("bad label line"); + } + else if (f->ReadLine(buffer, sizeof(buffer)) <= 0) // read the height map parameters + { + r.cat(readFailureText); + } + else if (!newGrid.ReadParameters(s)) + { + r.cat("failed to parse grid parameters"); + } + else if (!newGrid.IsValid()) + { + r.cat("invalid grid"); + } + else + { + SetGrid(newGrid); + for (uint32_t row = 0; row < def.numY; ++row) // read the grid a row at a time + { + if (f->ReadLine(buffer, sizeof(buffer)) <= 0) + { + r.cat(readFailureText); + return true; // failed to read a line + } + const char *p = buffer; + for (uint32_t col = 0; col < def.numX; ++col) + { + if (*p == '0' && (p[1] == ',' || p[1] == 0)) + { + // Values of 0 with no decimal places in un-probed values, so leave the point set as not valid + ++p; + } + else + { + char* np = nullptr; + const float f = strtod(p, &np); + if (np == p) + { + r.catf("number expected at line %u column %d", row + 3, (p - buffer) + 1); + return true; // failed to read a number + } + SetGridHeight(col, row, f); + p = np; + } + if (*p == ',') + { + ++p; + } + } + } + return false; // success! + } + return true; // an error occurred } // Compute the height error at the specified point -float GridDefinition::ComputeHeightError(float x, float y) const +float HeightMap::ComputeHeightError(float x, float y) const { - const float xf = (x - xMin) * recipSpacing; + const float xf = (x - def.xMin) * def.recipSpacing; const float xFloor = floor(xf); const int32_t xIndex = (int32_t)xFloor; - const float yf = (y - yMin) * recipSpacing; + const float yf = (y - def.yMin) * def.recipSpacing; const float yFloor = floor(yf); const int32_t yIndex = (int32_t)yFloor; @@ -166,29 +298,29 @@ float GridDefinition::ComputeHeightError(float x, float y) const // We are off the bottom left corner of the grid return GetHeightError(0, 0); } - else if (yIndex >= (int)NumYpoints()) + else if (yIndex >= (int)def.numY) { - return GetHeightError(0, NumYpoints()); + return GetHeightError(0, def.numY); } else { return InterpolateY(0, yIndex, yf - yFloor); } } - else if (xIndex >= (int)NumXpoints()) + else if (xIndex >= (int)def.numX) { if (yIndex < 0) { // We are off the bottom left corner of the grid - return GetHeightError(NumXpoints(), 0); + return GetHeightError(def.numX, 0); } - else if (yIndex >= (int)NumYpoints()) + else if (yIndex >= (int)def.numY) { - return GetHeightError(NumXpoints(), NumYpoints()); + return GetHeightError(def.numX, def.numY); } else { - return InterpolateY(NumXpoints(), yIndex, yf - yFloor); + return InterpolateY(def.numX, yIndex, yf - yFloor); } } else @@ -198,9 +330,9 @@ float GridDefinition::ComputeHeightError(float x, float y) const // We are off the bottom left corner of the grid return InterpolateX(xIndex, 0, xf - xFloor); } - else if (yIndex >= (int)NumYpoints()) + else if (yIndex >= (int)def.numY) { - return InterpolateX(xIndex, NumYpoints(), xf - xFloor); + return InterpolateX(xIndex, def.numY, xf - xFloor); } else { @@ -209,25 +341,25 @@ float GridDefinition::ComputeHeightError(float x, float y) const } } -float GridDefinition::GetHeightError(uint32_t xIndex, uint32_t yIndex) const +float HeightMap::GetHeightError(uint32_t xIndex, uint32_t yIndex) const { const uint32_t index = GetMapIndex(xIndex, yIndex); return (IsHeightSet(index)) ? gridHeights[index] : 0.0; } -float GridDefinition::InterpolateX(uint32_t xIndex, uint32_t yIndex, float xFrac) const +float HeightMap::InterpolateX(uint32_t xIndex, uint32_t yIndex, float xFrac) const { const uint32_t index1 = GetMapIndex(xIndex, yIndex); return Interpolate2(index1, index1 + 1, xFrac); } -float GridDefinition::InterpolateY(uint32_t xIndex, uint32_t yIndex, float yFrac) const +float HeightMap::InterpolateY(uint32_t xIndex, uint32_t yIndex, float yFrac) const { const uint32_t index1 = GetMapIndex(xIndex, yIndex); - return Interpolate2(index1, index1 + numX, yFrac); + return Interpolate2(index1, index1 + def.numX, yFrac); } -float GridDefinition::Interpolate2(uint32_t index1, uint32_t index2, float frac) const +float HeightMap::Interpolate2(uint32_t index1, uint32_t index2, float frac) const { const bool b1 = IsHeightSet(index1); const bool b2 = IsHeightSet(index2); @@ -237,11 +369,11 @@ float GridDefinition::Interpolate2(uint32_t index1, uint32_t index2, float frac) : 0.0; } -float GridDefinition::InterpolateXY(uint32_t xIndex, uint32_t yIndex, float xFrac, float yFrac) const +float HeightMap::InterpolateXY(uint32_t xIndex, uint32_t yIndex, float xFrac, float yFrac) const { const uint32_t indexX0Y0 = GetMapIndex(xIndex, yIndex); // (X0,Y0) const uint32_t indexX1Y0 = indexX0Y0 + 1; // (X1,Y0) - const uint32_t indexX0Y1 = indexX0Y0 + numX; // (X0 Y1) + const uint32_t indexX0Y1 = indexX0Y0 + def.numX; // (X0 Y1) const uint32_t indexX1Y1 = indexX0Y1 + 1; // (X1,Y1) const unsigned int cc = ((unsigned int)IsHeightSet(indexX0Y0) << 0) + ((unsigned int)IsHeightSet(indexX1Y0) << 1) @@ -291,7 +423,7 @@ float GridDefinition::InterpolateXY(uint32_t xIndex, uint32_t yIndex, float xFra } } -float GridDefinition::InterpolateCorner(uint32_t cornerIndex, uint32_t indexX, uint32_t indexY, float xFrac, float yFrac) const +float HeightMap::InterpolateCorner(uint32_t cornerIndex, uint32_t indexX, uint32_t indexY, float xFrac, float yFrac) const { return ((xFrac * gridHeights[indexX]) + (yFrac * gridHeights[indexY]) + ((2.0 - xFrac - yFrac) * gridHeights[cornerIndex]))/2; } |