diff options
author | David Crocker <dcrocker@eschertech.com> | 2022-07-22 13:21:13 +0300 |
---|---|---|
committer | David Crocker <dcrocker@eschertech.com> | 2022-07-22 13:21:13 +0300 |
commit | b5b5790b303d32286a65192d3790ae246444dc4c (patch) | |
tree | 7fb2d3a6b2ee6daaeee2ccde421c9845372fc1d0 | |
parent | ec52c2e766b31fbf44cf7ccfbd737cf1f702bf55 (diff) |
Removed incomplete non-FP movement calulations
Also implemented some fixes for remote extrusion
-rw-r--r-- | src/Movement/DDA.cpp | 16 | ||||
-rw-r--r-- | src/Movement/DDA.h | 4 | ||||
-rw-r--r-- | src/Movement/DriveMovement.cpp | 389 | ||||
-rw-r--r-- | src/Movement/DriveMovement.h | 21 | ||||
-rw-r--r-- | src/Movement/ExtruderShaper.h | 18 | ||||
-rw-r--r-- | src/Movement/MoveSegment.cpp | 12 | ||||
-rw-r--r-- | src/Movement/MoveSegment.h | 84 |
7 files changed, 38 insertions, 506 deletions
diff --git a/src/Movement/DDA.cpp b/src/Movement/DDA.cpp index fa67e19a..488aac3c 100644 --- a/src/Movement/DDA.cpp +++ b/src/Movement/DDA.cpp @@ -18,10 +18,6 @@ # include <CAN/CanMotion.h> #endif -#if SUPPORT_REMOTE_COMMANDS -# include <Can/CanInterface.h> -#endif - #ifdef DUET_NG # define DDA_MOVE_DEBUG (0) #else @@ -825,10 +821,7 @@ bool DDA::InitFromRemote(const CanMessageMovementLinear& msg) noexcept const int32_t delta = msg.perDrive[drive].steps; if (delta != 0) { - if (shapedSegments == nullptr) - { - EnsureUnshapedSegments(params); - } + EnsureUnshapedSegments(params); // there are no shaped segments, so set up the unshaped ones DriveMovement* const pdm = DriveMovement::Allocate(drive, DMState::idle); pdm->totalSteps = labs(delta); // for now this is the number of net steps, but gets adjusted later if there is a reverse in direction @@ -1349,11 +1342,6 @@ void DDA::Prepare(SimulationMode simMode) noexcept // This code assumes that the previous move in the DDA ring is the previously-executed move, because it fetches the X and Y end coordinates from that move. // Therefore the Move code must not store a new move in that entry until this one has been prepared! (It took me ages to track this down.) // Ideally we would store the initial X and Y coordinates in the DDA, but we need to be economical with memory -# if MS_USE_FPU - // Nothing needed here, use directionVector[Z_AXIS] directly -# else - afterPrepare.cKc = lrintf(directionVector[Z_AXIS] * MoveSegment::KdirectionVector); -# endif params.a2plusb2 = fsquare(directionVector[X_AXIS]) + fsquare(directionVector[Y_AXIS]); params.initialX = prev->GetEndCoordinate(X_AXIS, false); params.initialY = prev->GetEndCoordinate(Y_AXIS, false); @@ -1915,7 +1903,7 @@ pre(state == frozen) } #if SUPPORT_REMOTE_COMMANDS - if (CanInterface::InExpansionMode()) + if (flags.isRemote) { for (const DriveMovement* pdm = activeDMs; pdm != nullptr; pdm = pdm->nextDM) { diff --git a/src/Movement/DDA.h b/src/Movement/DDA.h index f4c2cf8a..fe37aee3 100644 --- a/src/Movement/DDA.h +++ b/src/Movement/DDA.h @@ -345,10 +345,6 @@ private: DriversBitmap drivesMoving; // bitmap of logical drives moving - needed to keep track of whether remote drives are moving static_assert(MaxAxesPlusExtruders <= DriversBitmap::MaxBits()); #endif - // These are used only in delta calculations -#if SUPPORT_LINEAR_DELTA && !MS_USE_FPU - int32_t cKc; // The Z movement fraction multiplied by Kc and converted to integer -#endif } afterPrepare; }; diff --git a/src/Movement/DriveMovement.cpp b/src/Movement/DriveMovement.cpp index fce12e1f..86fc214b 100644 --- a/src/Movement/DriveMovement.cpp +++ b/src/Movement/DriveMovement.cpp @@ -13,16 +13,6 @@ #include <Math/Isqrt.h> #include "Kinematics/LinearDeltaKinematics.h" -#if !MS_USE_FPU - -// The code in this file relies on right shift of a signed operand being arithmetic shift -// Shifting generates fewer instructions than dividing by a constant power of 2 in ARM gcc even though gcc converts the division to other instructions. -// Arithmetic shift rounds towards minus infinity, so it doesn't give quite the same result as dividing when there is a remainder. -static_assert(((int32_t)-21 >> 1) == (int32_t)-11); -static_assert(((int64_t)-10000000001 >> 1) == (int64_t)-5000000001); - -#endif - // Static members DriveMovement *DriveMovement::freeList = nullptr; @@ -68,7 +58,6 @@ void DriveMovement::DebugPrint() const noexcept const char c = (drive < reprap.GetGCodes().GetTotalAxes()) ? reprap.GetGCodes().GetAxisLetters()[drive] : (char)('0' + LogicalDriveToExtruder(drive)); if (state != DMState::idle) { -#if MS_USE_FPU debugPrintf("DM%c%s dir=%c steps=%" PRIu32 " next=%" PRIu32 " rev=%" PRIu32 " interval=%" PRIu32 " ssl=%" PRIu32 " A=%.4e B=%.4e C=%.4e dsf=%.4e tsf=%.1f", c, (state == DMState::stepError) ? " ERR:" : ":", (direction) ? 'F' : 'B', totalSteps, nextStep, reverseStartStep, stepInterval, segmentStepLimit, (double)pA, (double)pB, (double)pC, (double)distanceSoFar, (double)timeSoFar); @@ -85,24 +74,6 @@ void DriveMovement::DebugPrint() const noexcept { debugPrintf("\n"); } -#else - debugPrintf("DM%c%s dir=%c steps=%" PRIu32 " next=%" PRIu32 " rev=%" PRIu32 " interval=%" PRIu32 " ssl=%" PRIu32 " A=%" PRIi64 " B=%" PRIi32 " C=%" PRIi32 " dsf=%" PRIu32 " tsf=%" PRIu32, - c, (state == DMState::stepError) ? " ERR:" : ":", (direction) ? 'F' : 'B', totalSteps, nextStep, reverseStartStep, stepInterval, segmentStepLimit, - iA, iB, iC, iDistanceSoFar, iTimeSoFar); - if (isDelta) - { - debugPrintf(" hmz0sk=%" PRIi32 " minusAaPlusBbTimesS=%" PRIi32 " dSquaredMinusAsquaredMinusBsquared=%" PRIi64 " drev=%" PRIu32 "\n", - mp.delta.hmz0sK, mp.delta.minusAaPlusBbTimesKs, mp.delta.dSquaredMinusAsquaredMinusBsquaredTimesKsquaredSsquared, mp.delta.iReverseStartDistance); - } - else if (isExtruder) - { - debugPrintf(" pa=%" PRIu32 " eed=%" PRIu32 " ebf=%.4e\n", mp.cart.iPressureAdvanceK, mp.cart.iExtraExtrusionDistance, (double)mp.cart.extrusionBroughtForwards); - } - else - { - debugPrintf("\n"); - } -#endif } else { @@ -121,7 +92,6 @@ bool DriveMovement::NewCartesianSegment() noexcept } // Work out the movement limit in steps -#if MS_USE_FPU pC = currentSegment->CalcC(mp.cart.effectiveMmPerStep); if (currentSegment->IsLinear()) { @@ -141,27 +111,6 @@ bool DriveMovement::NewCartesianSegment() noexcept timeSoFar += currentSegment->GetSegmentTime(); segmentStepLimit = (currentSegment->GetNext() == nullptr) ? totalSteps + 1 : (uint32_t)(distanceSoFar * mp.cart.effectiveStepsPerMm) + 1; -#else - iC = currentSegment->CalcC(mp.cart.iEffectiveMmPerStepTimesK); - if (currentSegment->IsLinear()) - { - // Set up pB, pC such that for forward motion, time = pB + pC * stepNumber - iB = currentSegment->CalcLinearB(iDistanceSoFar, iTimeSoFar); - state = DMState::cartLinear; - } - else - { - // Set up pA, pB, pC such that for forward motion, time = pB + sqrt(pA + pC * stepNumber) - iA = currentSegment->CalcNonlinearA(iDistanceSoFar); - iB = currentSegment->CalcNonlinearB(iTimeSoFar); - state = (currentSegment->IsAccelerating()) ? DMState::cartAccel : DMState::cartDecelNoReverse; - } - - iDistanceSoFar += currentSegment->GetSegmentLength(); - iTimeSoFar += currentSegment->GetSegmentTime(); - - segmentStepLimit = (currentSegment->GetNext() == nullptr) ? totalSteps + 1 : (uint32_t)(((iDistanceSoFar * (uint64_t)mp.cart.iEffectiveStepsPerMmTimesK)) >> MoveSegment::SFstepsPerMm) + 1; -#endif if (nextStep < segmentStepLimit) { @@ -185,7 +134,6 @@ bool DriveMovement::NewDeltaSegment(const DDA& dda) noexcept } const float stepsPerMm = reprap.GetPlatform().DriveStepsPerUnit(drive); -#if MS_USE_FPU pC = currentSegment->GetC()/stepsPerMm; //TODO store the reciprocal to avoid the division if (currentSegment->IsLinear()) { @@ -253,56 +201,6 @@ bool DriveMovement::NewDeltaSegment(const DDA& dda) noexcept state = DMState::deltaForwardsReversing; } } -#else - iC = currentSegment->GetC()/stepsPerMm; //TODO store the reciprocal to avoid the division? Use a scaling factor for C - if (currentSegment->IsLinear()) - { - // Set up pB, pC such that for forward motion, time = pB + pC * (distanceMoved * steps/mm) - iB = currentSegment->CalcLinearB(iDistanceSoFar, iTimeSoFar); - } - else - { - // Set up pA, pB, pC such that for forward motion, time = pB + sqrt(pA + pC * (distanceMoved * steps/mm)) - iA = currentSegment->CalcNonlinearA(iDistanceSoFar); - iB = currentSegment->CalcNonlinearB(iTimeSoFar); - } - - const uint32_t startDistance = iDistanceSoFar; - iDistanceSoFar += currentSegment->GetSegmentLength(); - iTimeSoFar += currentSegment->GetSegmentTime(); - - // Work out whether we reverse in this segment and the movement limit in steps - const float sDx = iDistanceSoFar * dda.directionVector[0]; //TODO avoid float maths - const float sDy = iDistanceSoFar * dda.directionVector[1]; //TODO avoid float maths - const int32_t netStepsAtEnd = (int32_t)(isqrt64(mp.delta.dSquaredMinusAsquaredMinusBsquaredTimesKsquaredSsquared - fsquare(stepsPerMm) * (sDx * (sDx + mp.delta.fTwoA) + sDy * (sDy + mp.delta.fTwoB))) - + (iDistanceSoFar * dda.directionVector[2] - mp.delta.h0MinusZ0) * stepsPerMm); //TODO avoid float maths - - if (mp.delta.iReverseStartDistance <= (int32_t)startDistance) - { - // This segment is purely downwards motion and we want the greater of the two quadratic solutions. There may have been upwards motion earlier in the move. - if (direction) - { - direction = false; - directionChanged = true; - } - state = DMState::deltaReverse; - segmentStepLimit = (currentSegment->GetNext() == nullptr) ? totalSteps + 1 - : (reverseStartStep <= totalSteps) ? (uint32_t)((int32_t)(2 * reverseStartStep) - netStepsAtEnd) - : 1 - netStepsAtEnd; - } - else if ((int32_t)iDistanceSoFar <= mp.delta.iReverseStartDistance) - { - // This segment is purely upwards motion of the tower and we want the lower quadratic solution - state = DMState::deltaForwardsNoReverse; - segmentStepLimit = (currentSegment->GetNext() == nullptr) ? totalSteps + 1 : (uint32_t)(netStepsAtEnd + 1); - } - else - { - // This segment ends with reverse motion. We want the lower quadratic solution initially. - segmentStepLimit = (currentSegment->GetNext() == nullptr) ? totalSteps + 1 : (uint32_t)((int32_t)(2 * reverseStartStep) - netStepsAtEnd); - state = DMState::deltaForwardsReversing; - } -#endif if (segmentStepLimit > nextStep) { @@ -325,7 +223,6 @@ bool DriveMovement::NewExtruderSegment() noexcept return false; } -#if MS_USE_FPU const float startDistance = distanceSoFar; const float startTime = timeSoFar; @@ -359,41 +256,6 @@ bool DriveMovement::NewExtruderSegment() noexcept // Work out the movement limit in steps segmentStepLimit = ((currentSegment->GetNext() == nullptr) ? totalSteps : (uint32_t)(distanceSoFar * mp.cart.effectiveStepsPerMm)) + 1; -#else - const uint32_t startDistance = iDistanceSoFar; - const uint32_t startTime = iTimeSoFar; - - // Work out the movement limit in steps - iDistanceSoFar += currentSegment->GetSegmentLength(); - iTimeSoFar += currentSegment->GetSegmentTime(); - - iC = currentSegment->CalcC(mp.cart.iEffectiveMmPerStepTimesK); - if (currentSegment->IsLinear()) - { - // Set up pB, pC such that for forward motion, time = pB + pC * stepNumber - iB = currentSegment->CalcLinearB(startDistance, startTime); - state = DMState::cartLinear; - } - else - { - // Set up pA, pB, pC such that for forward motion, time = pB + sqrt(pA + pC * stepNumber) - iA = currentSegment->CalcNonlinearA(startDistance, mp.cart.iPressureAdvanceK); - iB = currentSegment->CalcNonlinearB(startTime, mp.cart.iPressureAdvanceK); - if (currentSegment->IsAccelerating()) - { - // Extruders have a single acceleration segment. We need to add the extra extrusion distance due to pressure advance to the extrusion distance. - iDistanceSoFar += mp.cart.iExtraExtrusionDistance; - state = DMState::cartAccel; - } - else - { - // This is the single decelerating segment. If it includes pressure advance then it may include reversal. - state = DMState::cartDecelForwardsReversing; // assume that it may reverse - } - } - - segmentStepLimit = ((currentSegment->GetNext() == nullptr) ? totalSteps : (uint32_t)((iDistanceSoFar * (uint64_t)mp.cart.iEffectiveStepsPerMmTimesK)) >> MoveSegment::SFstepsPerMm) + 1; -#endif if (nextStep < segmentStepLimit) { @@ -407,22 +269,12 @@ bool DriveMovement::NewExtruderSegment() noexcept // Prepare this DM for a Cartesian axis move, returning true if there are steps to do bool DriveMovement::PrepareCartesianAxis(const DDA& dda, const PrepParams& params) noexcept { -#if MS_USE_FPU distanceSoFar = 0.0; timeSoFar = 0.0; mp.cart.pressureAdvanceK = 0.0; // We can't use directionVector here because those values relate to Cartesian space, whereas we may be CoreXY etc. mp.cart.effectiveStepsPerMm = (float)totalSteps/dda.totalDistance; mp.cart.effectiveMmPerStep = 1.0/mp.cart.effectiveStepsPerMm; -#else - iDistanceSoFar = 0; - iTimeSoFar = 0; - mp.cart.iPressureAdvanceK = 0; - // We can't use directionVector here because those values relate to Cartesian space, whereas we may be CoreXY etc. - const float stepsTimesK = (float)((uint64_t)totalSteps << MoveSegment::SFstepsPerMm); - mp.cart.iEffectiveStepsPerMmTimesK = stepsTimesK/dda.totalDistance; - mp.cart.iEffectiveMmPerStepTimesK = dda.totalDistance/stepsTimesK; -#endif isDelta = false; isExtruder = false; currentSegment = (dda.shapedSegments != nullptr) ? dda.shapedSegments : dda.unshapedSegments; @@ -453,7 +305,6 @@ bool DriveMovement::PrepareDeltaAxis(const DDA& dda, const PrepParams& params) n const float dSquaredMinusAsquaredMinusBsquared = params.dparams->GetDiagonalSquared(drive) - fsquare(A) - fsquare(B); const float h0MinusZ0 = fastSqrtf(dSquaredMinusAsquaredMinusBsquared); -#if MS_USE_FPU mp.delta.h0MinusZ0 = h0MinusZ0; mp.delta.fTwoA = 2.0 * A; mp.delta.fTwoB = 2.0 * B; @@ -539,72 +390,6 @@ bool DriveMovement::PrepareDeltaAxis(const DDA& dda, const PrepParams& params) n distanceSoFar = 0.0; timeSoFar = 0.0; -#else - mp.delta.h0MinusZ0 = h0MinusZ0; //TODO change to integer - mp.delta.fTwoA = 2.0 * A; //TODO change to integer - mp.delta.fTwoB = 2.0 * B; //TODO change to integer - mp.delta.hmz0sK = lrintf(h0MinusZ0 * stepsPerMm * MoveSegment::Kdelta); - mp.delta.minusAaPlusBbTimesKs = -lrintf(aAplusbB * stepsPerMm * MoveSegment::Kdelta); - mp.delta.dSquaredMinusAsquaredMinusBsquaredTimesKsquaredSsquared = llrintf(dSquaredMinusAsquaredMinusBsquared * fsquare(stepsPerMm * MoveSegment::Kdelta)); - - // Calculate the distance at which we need to reverse direction. - if (params.a2plusb2 <= 0.0) - { - // Pure Z movement. We can't use the main calculation because it divides by a2plusb2. - direction = (dda.directionVector[Z_AXIS] >= 0.0); - const float reverseStartDistance = (direction) ? dda.totalDistance + 1.0 : -1.0; // so that we never reverse and NewDeltaSegment knows which way we are going - mp.delta.iReverseStartDistance = (int32_t)(reverseStartDistance * MoveSegment::Kdistance); - reverseStartStep = totalSteps + 1; - } - else - { - // The distance to reversal is the solution to a quadratic equation. One root corresponds to the carriages being below the bed, - // the other root corresponds to the carriages being above the bed. - const float drev = ((dda.directionVector[Z_AXIS] * fastSqrtf(params.a2plusb2 * params.dparams->GetDiagonalSquared(drive) - fsquare(A * dda.directionVector[Y_AXIS] - B * dda.directionVector[X_AXIS]))) - - aAplusbB)/params.a2plusb2; - mp.delta.iReverseStartDistance = (int32_t)(drev * MoveSegment::Kdistance); - if (drev > 0.0 && drev < dda.totalDistance) // if the reversal point is within range - { - // Calculate how many steps we need to move up before reversing - const float hrev = dda.directionVector[Z_AXIS] * drev + fastSqrtf(dSquaredMinusAsquaredMinusBsquared - 2 * drev * aAplusbB - params.a2plusb2 * fsquare(drev)); - const int32_t numStepsUp = (int32_t)((hrev - mp.delta.h0MinusZ0) * stepsPerMm); - - // We may be almost at the peak height already, in which case we don't really have a reversal. - if (numStepsUp < 1) - { - mp.delta.iReverseStartDistance = -1; // so that we know we have reversed already - reverseStartStep = totalSteps + 1; - direction = false; - } - else - { - reverseStartStep = (uint32_t)numStepsUp + 1; - - // Correct the initial direction and the total number of steps - if (direction) - { - // Net movement is up, so we will go up first and then down by a lesser amount - totalSteps = (2 * numStepsUp) - totalSteps; - } - else - { - // Net movement is down, so we will go up first and then down by a greater amount - direction = true; - totalSteps = (2 * numStepsUp) + totalSteps; - } - } - } - else - { - // No reversal - reverseStartStep = totalSteps + 1; - direction = (drev >= 0.0); - } - } - - iDistanceSoFar = 0; - iTimeSoFar = 0; -#endif isDelta = true; currentSegment = (dda.shapedSegments != nullptr) ? dda.shapedSegments : dda.unshapedSegments; @@ -633,11 +418,19 @@ bool DriveMovement::PrepareExtruder(const DDA& dda, const PrepParams& params) no const float effStepsPerMm = stepsPerMm * fabsf(dda.directionVector[drive]); const float effMmPerStep = 1.0/effStepsPerMm; - ExtruderShaper& shaper = reprap.GetMove().GetExtruderShaper(LogicalDriveToExtruder(drive)); - float forwardDistance = mp.cart.extrusionBroughtForwards = shaper.GetExtrusionPending()/dda.directionVector[drive]; + ExtruderShaper& shaper = reprap.GetMove().GetExtruderShaper( +#if SUPPORT_REMOTE_COMMANDS + (dda.flags.isRemote) ? drive : +#endif + LogicalDriveToExtruder(drive) + ); + float forwardDistance = mp.cart.extrusionBroughtForwards = +#if SUPPORT_REMOTE_COMMANDS + (dda.flags.isRemote) ? 0.0 : +#endif + shaper.GetExtrusionPending()/dda.directionVector[drive]; float reverseDistance; -#if MS_USE_FPU mp.cart.effectiveStepsPerMm = effStepsPerMm; mp.cart.effectiveMmPerStep = effMmPerStep; distanceSoFar = forwardDistance; @@ -741,71 +534,6 @@ bool DriveMovement::PrepareExtruder(const DDA& dda, const PrepParams& params) no forwardDistance += dda.totalDistance; reverseDistance = 0.0; } -#else - mp.cart.iEffectiveStepsPerMmTimesK = lrintf(effStepsPerMm * (float)(1u << MoveSegment::SFstepsPerMm)); - mp.cart.iEffectiveMmPerStepTimesK = lrintf(effMmPerStep * (float)(1u << MoveSegment::SFstepsPerMm)); - iTimeSoFar = 0; - - // Calculate the total forward and reverse movement distances - //TODO distances as integer? - - if (dda.flags.usePressureAdvance && shaper.GetKclocks() > 0.0) - { - // We are using nonzero pressure advance. Movement must be forwards. - mp.cart.iPressureAdvanceK = shaper.GetKclocks(); - const float extraExtrusionDistance = (float)mp.cart.iPressureAdvanceK * (dda.topSpeed - dda.startSpeed); - mp.cart.iExtraExtrusionDistance = lrintf(extraExtrusionDistance * (float)(1u << MoveSegment::SFdistance)); - forwardDistance += extraExtrusionDistance; - - // Check if there is a reversal in the deceleration segment - // There is at most one deceleration segment in the unshaped segments - const MoveSegment *decelSeg = dda.unshapedSegments; - while (decelSeg != nullptr && (decelSeg->IsLinear() || decelSeg->IsAccelerating())) - { - decelSeg = decelSeg->GetNext(); - } - - if (decelSeg == nullptr) - { - forwardDistance += dda.totalDistance; // no deceleration segment - reverseDistance = 0.0; - } - else - { - const float initialDecelSpeed = dda.topSpeed - (float)mp.cart.iPressureAdvanceK * params.unshaped.deceleration; - if (initialDecelSpeed <= 0.0) - { - // The entire deceleration segment is in reverse - forwardDistance += params.unshaped.decelStartDistance; - reverseDistance = ((0.5 * params.unshaped.deceleration * params.unshaped.decelClocks) - initialDecelSpeed) * params.unshaped.decelClocks; - } - else - { - const float timeToReverse = initialDecelSpeed * ((-0.5) * decelSeg->GetC()); // 'c' is -2/deceleration, so -0.5*c is 1/deceleration - if (timeToReverse < params.unshaped.decelClocks) - { - // There is a reversal, although it could be tiny - const float distanceToReverse = fsquare(initialDecelSpeed) * decelSeg->GetC() * (-0.25); // because (v^2-u^2) = 2as, so if v=0 then s=-u^2/2a = u^2/2d = -0.25*u^2*c - forwardDistance += params.unshaped.decelStartDistance + distanceToReverse; - reverseDistance = 0.5 * params.unshaped.deceleration * fsquare(params.unshaped.decelClocks - timeToReverse); // because s = 0.5*a*t^2 - } - else - { - // No reversal - forwardDistance += dda.totalDistance - ((float)mp.cart.iPressureAdvanceK * params.unshaped.deceleration * params.unshaped.decelClocks); - reverseDistance = 0.0; - } - } - } - } - else - { - // No pressure advance. Movement may be backwards but this still counts as forward distance in the calculations. - mp.cart.iPressureAdvanceK = mp.cart.iExtraExtrusionDistance = 0; - forwardDistance += dda.totalDistance; - reverseDistance = 0.0; - } -#endif // Check whether there are any steps at all const float forwardSteps = forwardDistance * effStepsPerMm; @@ -819,7 +547,12 @@ bool DriveMovement::PrepareExtruder(const DDA& dda, const PrepParams& params) no // No movement at all nextStep = totalSteps = 0; reverseStartStep = 1; - shaper.SetExtrusionPending(netDistance * dda.directionVector[drive]); +#if SUPPORT_REMOTE_COMMANDS + if (!dda.flags.isRemote) +#endif + { + shaper.SetExtrusionPending(netDistance * dda.directionVector[drive]); + } return false; } @@ -842,21 +575,36 @@ bool DriveMovement::PrepareExtruder(const DDA& dda, const PrepParams& params) no } totalSteps = (uint32_t)((int32_t)(2 * reverseStartStep) - netSteps - 2); } - shaper.SetExtrusionPending((netDistance - (float)netSteps * effMmPerStep) * dda.directionVector[drive]); +#if SUPPORT_REMOTE_COMMANDS + if (!dda.flags.isRemote) +#endif + { + shaper.SetExtrusionPending((netDistance - (float)netSteps * effMmPerStep) * dda.directionVector[drive]); + } } else { if (forwardSteps >= 1.0) { totalSteps = (uint32_t)forwardSteps; - shaper.SetExtrusionPending((forwardDistance - (float)totalSteps * effMmPerStep) * dda.directionVector[drive]); +#if SUPPORT_REMOTE_COMMANDS + if (!dda.flags.isRemote) +#endif + { + shaper.SetExtrusionPending((forwardDistance - (float)totalSteps * effMmPerStep) * dda.directionVector[drive]); + } } else { // No steps at all, or negative forward steps which I think should be impossible unless the steps/mm is changed nextStep = totalSteps = 0; reverseStartStep = 1; - shaper.SetExtrusionPending(forwardDistance * dda.directionVector[drive]); +#if SUPPORT_REMOTE_COMMANDS + if (!dda.flags.isRemote) +#endif + { + shaper.SetExtrusionPending(forwardDistance * dda.directionVector[drive]); + } return false; } reverseStartStep = totalSteps + 1; // no reverse phase @@ -879,23 +627,12 @@ bool DriveMovement::PrepareExtruder(const DDA& dda, const PrepParams& params) no return CalcNextStepTime(dda); } -#if MS_USE_FPU - // Version of fastSqrtf that allows for slightly negative operands caused by rounding error static inline float fastLimSqrtf(float f) noexcept { return (f > 0.0) ? fastSqrtf(f) : 0.0; } -#else - -static inline uint32_t LimISqrt64(int64_t num) noexcept -{ - return (num <= 0) ? 0 : isqrt64((uint64_t)num); -} - -#endif - // Calculate and store the time since the start of the move when the next step for the specified DriveMovement is due. // We have already incremented nextStep and checked that it does not exceed totalSteps, so at least one more step is due // Return true if all OK, false to abort this move because the calculation has gone wrong @@ -962,39 +699,23 @@ pre(nextStep <= totalSteps; stepsTillRecalc == 0) stepsTillRecalc = (1u << shiftFactor) - 1u; // store number of additional steps to generate -#if MS_USE_FPU float nextCalcStepTime; -#else - uint32_t iNextCalcStepTime; -#endif // Work out the time of the step switch (state) { case DMState::cartLinear: // linear steady speed -#if MS_USE_FPU nextCalcStepTime = pB + (float)(nextStep + stepsTillRecalc) * pC; -#else - iNextCalcStepTime = iB + (nextStep + stepsTillRecalc) * iC; //TODO ??scaling factor for iC ? -#endif break; case DMState::cartAccel: // Cartesian accelerating -#if MS_USE_FPU nextCalcStepTime = pB + fastLimSqrtf(pA + pC * (float)(nextStep + stepsTillRecalc)); -#else - iNextCalcStepTime = iB + LimISqrt64(iA + iC * (nextStep + stepsTillRecalc)); //TODO ??scaling factor for iC ? -#endif break; case DMState::cartDecelForwardsReversing: if (nextStep + stepsTillRecalc < reverseStartStep) { -#if MS_USE_FPU nextCalcStepTime = pB - fastLimSqrtf(pA + pC * (float)(nextStep + stepsTillRecalc)); -#else - iNextCalcStepTime = iB - LimISqrt64(iA + iC * (nextStep + stepsTillRecalc)); //TODO ??scaling factor for iC ? -#endif break; } @@ -1003,19 +724,11 @@ pre(nextStep <= totalSteps; stepsTillRecalc == 0) state = DMState::cartDecelReverse; // no break case DMState::cartDecelReverse: // Cartesian decelerating, reverse motion. Convert the steps to int32_t because the net steps may be negative. -#if MS_USE_FPU nextCalcStepTime = pB + fastLimSqrtf(pA + pC * (float)((2 * (int32_t)(reverseStartStep - 1)) - (int32_t)(nextStep + stepsTillRecalc))); -#else - iNextCalcStepTime = iB + LimISqrt64(iA + iC * ((2 * (int32_t)(reverseStartStep - 1)) - (int32_t)(nextStep + stepsTillRecalc))); //TODO ??scaling factor for iC ? -#endif break; case DMState::cartDecelNoReverse: // Cartesian accelerating with no reversal -#if MS_USE_FPU nextCalcStepTime = pB - fastLimSqrtf(pA + pC * (float)(nextStep + stepsTillRecalc)); -#else - iNextCalcStepTime = iB - LimISqrt64(iA + iC * (nextStep + stepsTillRecalc)); //TODO ??scaling factor for iC ? -#endif break; case DMState::deltaForwardsReversing: // moving forwards @@ -1029,7 +742,6 @@ pre(nextStep <= totalSteps; stepsTillRecalc == 0) case DMState::deltaNormal: // Calculate d*s where d = distance the head has travelled, s = steps/mm for this drive { -#if MS_USE_FPU const float steps = (float)(1u << shiftFactor); if (direction) { @@ -1060,35 +772,6 @@ pre(nextStep <= totalSteps; stepsTillRecalc == 0) : (currentSegment->IsAccelerating()) ? pB + fastLimSqrtf(pA + pCds) : pB - fastLimSqrtf(pA + pCds); //if (currentSegment->IsLinear()) { pA = ds; } //DEBUG -#else - int32_t shiftedK2 = (int32_t)(MoveSegment::Kdelta << shiftFactor); - if (!direction) - { - shiftedK2 = -shiftedK2; - } - mp.delta.hmz0sK += shiftedK2; // get K2 * (new carriage height above Z in steps) - - const int32_t hmz0scK = (int32_t)(((int64_t)mp.delta.hmz0sK * dda.afterPrepare.cKc) >> MoveSegment::SFdelta); - const int32_t t1 = mp.delta.minusAaPlusBbTimesKs + hmz0scK; - const int32_t t2a = (int32_t)(mp.delta.dSquaredMinusAsquaredMinusBsquaredTimesKsquaredSsquared - (int64_t)isquare64(mp.delta.hmz0sK) + (int64_t)isquare64(t1)); - // Due to rounding error we can end up trying to take the square root of a negative number if we do not take precautions here - const uint32_t t2 = LimISqrt64(t2a); - const int32_t dsK = (direction) ? t1 - t2 : t1 + t2; - - // Now feed dsK into the step algorithm for Cartesian motion - if (dsK < 0) - { - state = DMState::stepError; - nextStep += 110000000; // so that we can tell what happened in the debug print - return false; - } - - const int32_t iCds = ((int64_t)iC * dsK) >> (MoveSegment::SFdelta + MoveSegment::SFdistance); - iNextCalcStepTime = (currentSegment->IsLinear()) ? iB + iCds - : (currentSegment->IsAccelerating()) ? iB + LimISqrt64(iA + iCds) - : iB - LimISqrt64(iA + iCds); - //if (currentSegment->IsLinear()) { iA = dsK; } //DEBUG -#endif } break; @@ -1106,9 +789,7 @@ pre(nextStep <= totalSteps; stepsTillRecalc == 0) } #endif -#if MS_USE_FPU uint32_t iNextCalcStepTime = (uint32_t)nextCalcStepTime; -#endif if (iNextCalcStepTime > dda.clocksNeeded) { diff --git a/src/Movement/DriveMovement.h b/src/Movement/DriveMovement.h index 18e2eb38..814fde86 100644 --- a/src/Movement/DriveMovement.h +++ b/src/Movement/DriveMovement.h @@ -103,16 +103,9 @@ private: uint32_t nextStepTime; // how many clocks after the start of this move the next step is due uint32_t stepInterval; // how many clocks between steps -#if MS_USE_FPU float distanceSoFar; float timeSoFar; float pA, pB, pC; -#else - uint32_t iDistanceSoFar; - uint32_t iTimeSoFar; - int64_t iA; - int32_t iB, iC; -#endif // Parameters unique to a style of move (Cartesian, delta or extruder). Currently, extruders and Cartesian moves use the same parameters. union @@ -123,32 +116,18 @@ private: float fTwoA; float fTwoB; float h0MinusZ0; // the height subtended by the rod at the start of the move -#if MS_USE_FPU float fDSquaredMinusAsquaredMinusBsquaredTimesSsquared; float fHmz0s; // the starting height less the starting Z height, multiplied by the Z movement fraction (can go negative) float fMinusAaPlusBbTimesS; float reverseStartDistance; // the overall move distance at which movement reversal occurs -#else - int64_t dSquaredMinusAsquaredMinusBsquaredTimesKsquaredSsquared; - int32_t hmz0sK; // the starting step position less the starting Z height, multiplied by the Z movement fraction and K (can go negative) - int32_t minusAaPlusBbTimesKs; - int32_t iReverseStartDistance; // the overall move distance at which movement reversal occurs -#endif } delta; struct CartesianParameters { -#if MS_USE_FPU float pressureAdvanceK; // how much pressure advance is applied to this move float effectiveStepsPerMm; // the steps/mm multiplied by the movement fraction float effectiveMmPerStep; // reciprocal of [the steps/mm multiplied by the movement fraction] float extraExtrusionDistance; // the extra extrusion distance in the acceleration phase -#else - uint32_t iPressureAdvanceK; // how much pressure advance is applied to this move - uint32_t iEffectiveStepsPerMmTimesK; // the steps/mm multiplied by the movement fraction - uint32_t iEffectiveMmPerStepTimesK; // reciprocal of [the steps/mm multiplied by the movement fraction] - uint32_t iExtraExtrusionDistance; // the extra extrusion distance in the acceleration phase -#endif float extrusionBroughtForwards; // the amount of extrusion brought forwards from previous moves. Only needed for debug output. } cart; } mp; diff --git a/src/Movement/ExtruderShaper.h b/src/Movement/ExtruderShaper.h index 52a0431f..d0cd82cf 100644 --- a/src/Movement/ExtruderShaper.h +++ b/src/Movement/ExtruderShaper.h @@ -21,36 +21,20 @@ class ExtruderShaper { public: ExtruderShaper() -#if MS_USE_FPU : k(0.0), -#else - : ik(0), -#endif extrusionPending(0.0) /*, lastSpeed(0.0)*/ { } // Temporary functions until we support more sophisticated pressure advance -#if MS_USE_FPU float GetKclocks() const noexcept { return k; } // get pressure advance in step clocks float GetKseconds() const noexcept { return k * (1.0/StepClockRate); } void SetKseconds(float val) noexcept { k = val * StepClockRate; } // set pressure advance in seconds -#else - uint32_t GetKclocks() const noexcept { return ik; } // get pressure advance in step clocks - float GetKseconds() const noexcept { return (float)ik * (1.0/StepClockRate); } - void SetKseconds(float val) noexcept { ik = lrintf(val * StepClockRate); } // set pressure advance in seconds -#endif float GetExtrusionPending() const noexcept { return extrusionPending; } void SetExtrusionPending(float ep) noexcept { extrusionPending = ep; } -# -private: -#if MS_USE_FPU +private: float k; // the pressure advance constant in step clocks -#else - uint32_t ik; // the pressure advance constant in step clocks -#endif float extrusionPending; // extrusion we have been asked to do but haven't because it is less than one microstep, in mm -// float lastSpeed; // the speed we were moving at at the end of the last extrusion, needed to implement pressure advance }; #endif /* SRC_MOVEMENT_EXTRUDERSHAPER_H_ */ diff --git a/src/Movement/MoveSegment.cpp b/src/Movement/MoveSegment.cpp index ff8cf83b..11144801 100644 --- a/src/Movement/MoveSegment.cpp +++ b/src/Movement/MoveSegment.cpp @@ -50,7 +50,6 @@ void MoveSegment::AddToTail(MoveSegment *tail) noexcept void MoveSegment::DebugPrint(char ch) const noexcept { -#if MS_USE_FPU debugPrintf("%c d=%.4e t=%.1f ", ch, (double)segLength, (double)segTime); if (IsLinear()) { @@ -60,17 +59,6 @@ void MoveSegment::DebugPrint(char ch) const noexcept { debugPrintf("b=%.4e c=%.4e\n", (double)b, (double)c); } -#else - debugPrintf("%c d=%" PRIu32 " t=%" PRIu32 " ", ch, iSegLength, iSegTime); - if (IsLinear()) - { - debugPrintf("c=%" PRIi32 "\n", ic); - } - else - { - debugPrintf("b=%" PRIi32 " c=%" PRIi32 "\n", ib, ic); - } -#endif } // End diff --git a/src/Movement/MoveSegment.h b/src/Movement/MoveSegment.h index a8ab7928..de089fab 100644 --- a/src/Movement/MoveSegment.h +++ b/src/Movement/MoveSegment.h @@ -134,9 +134,6 @@ #include <RepRapFirmware.h> #include <Platform/Tasks.h> -//#define MS_USE_FPU (__FPU_USED) -#define MS_USE_FPU (1) // Try using floating point maths for processors without FPU too. We have implemented fastSqrtf on those processors. - class MoveSegment { public: @@ -147,7 +144,6 @@ public: MoveSegment(MoveSegment *p_next) noexcept; -#if MS_USE_FPU float GetSegmentLength() const noexcept { return segLength; } float GetSegmentTime() const noexcept { return segTime; } float CalcNonlinearA(float startDistance) const noexcept; @@ -160,20 +156,6 @@ public: void SetLinear(float pSegmentLength, float p_segTime, float p_c) noexcept; void SetNonLinear(float pSegmentLength, float p_segTime, float p_b, float p_c) noexcept; -#else - uint32_t GetSegmentLength() const noexcept { return iSegLength; } - uint32_t GetSegmentTime() const noexcept { return iSegTime; } - int64_t CalcNonlinearA(uint32_t startDistance) const noexcept; - int64_t CalcNonlinearA(uint32_t startDistance, uint32_t pressureAdvanceK) const noexcept; - int32_t CalcNonlinearB(uint32_t startTime) const noexcept; - int32_t CalcNonlinearB(uint32_t startTime, uint32_t pressureAdvanceK) const noexcept; - int32_t CalcLinearB(uint32_t startDistance, uint32_t startTime) const noexcept; - int32_t CalcC(uint32_t mmPerStepTimesK) const noexcept; - int32_t GetC() const noexcept { return ic; } - - void SetLinear(uint32_t pSegmentLength, uint32_t p_segTime, int32_t p_c) noexcept; - void SetNonLinear(uint32_t pSegmentLength, uint32_t p_segTime, int32_t p_b, int32_t p_c) noexcept; -#endif void SetReverse() noexcept; MoveSegment *GetNext() const noexcept; @@ -217,16 +199,9 @@ private: // The 'next' field is a MoveSegment pointer with two flag bits in the bottom two bits uint32_t nextAndFlags; // pointer to the next segment, plus flag bits -#if MS_USE_FPU float segLength; // the length of this segment before applying the movement fraction float segTime; // the time in step clocks at which this move ends float b, c; // the move parameters (b is not needed for linear moves) -#else - uint32_t iSegLength; // the length of this segment before applying the movement fraction - uint32_t iSegTime; // the time in step clocks at which this move ends - int32_t ib, ic; // the move parameters (b is not needed for linear moves) -#endif - }; // Create a new one, leaving the flags clear @@ -257,7 +232,6 @@ inline bool MoveSegment::IsLast() const noexcept return GetNext() == nullptr; } -#if MS_USE_FPU inline float MoveSegment::CalcNonlinearA(float startDistance) const noexcept { @@ -313,64 +287,6 @@ inline bool MoveSegment::IsAccelerating() const noexcept return c > 0.0; } -#else - -inline int64_t MoveSegment::CalcNonlinearA(uint32_t startDistance) const noexcept -{ - return isquare64(ib) - (int64_t)startDistance * ic; -} - -inline int64_t MoveSegment::CalcNonlinearA(uint32_t startDistance, uint32_t pressureAdvanceK) const noexcept -{ - return isquare64(ib - pressureAdvanceK) - (int64_t)startDistance * ic; -} - -inline int32_t MoveSegment::CalcNonlinearB(uint32_t startTime) const noexcept -{ - return ib + (int32_t)startTime; -} - -inline int32_t MoveSegment::CalcNonlinearB(uint32_t startTime, uint32_t pressureAdvanceK) const noexcept -{ - return (ib - (int32_t)pressureAdvanceK) + (int32_t)startTime; -} - -inline int32_t MoveSegment::CalcLinearB(uint32_t startDistance, uint32_t startTime) const noexcept -{ - return (int32_t)startTime - (((int64_t)startDistance * ic) >> SFdistance); -} - -inline int32_t MoveSegment::CalcC(uint32_t mmPerStepTimesK) const noexcept -{ - return (int32_t)((ic * (int64_t)mmPerStepTimesK) >> SFmmPerStep); -} - -inline void MoveSegment::SetLinear(uint32_t pSegmentLength, uint32_t p_segTime, int32_t p_c) noexcept -{ - iSegLength = pSegmentLength; - iSegTime = p_segTime; - ib = 0; - ic = p_c; - nextAndFlags |= LinearFlag; -} - -// Set up an accelerating or decelerating move. We assume that the 'linear' flag is already clear. -inline void MoveSegment::SetNonLinear(uint32_t pSegmentLength, uint32_t p_segTime, int32_t p_b, int32_t p_c) noexcept -{ - iSegLength = pSegmentLength; - iSegTime = p_segTime; - ib = p_b; - ic = p_c; -} - -// Given that this is an accelerating or decelerating move, return true if it is accelerating -inline bool MoveSegment::IsAccelerating() const noexcept -{ - return ic > 0; -} - -#endif - // Release a single MoveSegment. Not thread-safe. inline void MoveSegment::Release(MoveSegment *item) noexcept { |