Welcome to mirror list, hosted at ThFree Co, Russian Federation.

github.com/Duet3D/RepRapFirmware.git - Unnamed repository; edit this file 'description' to name the repository.
summaryrefslogtreecommitdiff
path: root/src/Movement/Grid.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/Movement/Grid.cpp')
-rw-r--r--src/Movement/Grid.cpp222
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;
}
generated by cgit v1.2.3 (git 2.25.1) at 2024-08-20 14:40:47 +0300