+// prepare block for the step ticker, called everytime the block changes
+// this is done during planning so does not delay tick generation and step ticker can simply grab the next block during the interrupt
+void Block::prepare(float acceleration_in_steps, float deceleration_in_steps)
+{
+
+ float inv = 1.0F / this->steps_event_count;
+
+ // Now figure out the acceleration PER TICK, this should ideally be held as a double as it's very critical to the block timing
+ // steps/tick^2
+ // was....
+ // float acceleration_per_tick = acceleration_in_steps / STEP_TICKER_FREQUENCY_2; // that is 100,000² too big for a float
+ // float deceleration_per_tick = deceleration_in_steps / STEP_TICKER_FREQUENCY_2;
+ double acceleration_per_tick = acceleration_in_steps * fp_scale; // this is now scaled to fit a 2.30 fixed point number
+ double deceleration_per_tick = deceleration_in_steps * fp_scale;
+
+ for (uint8_t m = 0; m < n_actuators; m++) {
+ uint32_t steps = this->steps[m];
+ this->tick_info[m].steps_to_move = steps;
+ if(steps == 0) continue;
+
+ float aratio = inv * steps;
+
+ this->tick_info[m].steps_per_tick = (int64_t)round((((double)this->initial_rate * aratio) / STEP_TICKER_FREQUENCY) * STEPTICKER_FPSCALE); // steps/sec / tick frequency to get steps per tick in 2.62 fixed point
+ this->tick_info[m].counter = 0; // 2.62 fixed point
+ this->tick_info[m].step_count = 0;
+ this->tick_info[m].next_accel_event = this->total_move_ticks + 1;
+
+ double acceleration_change = 0;
+ if(this->accelerate_until != 0) { // If the next accel event is the end of accel
+ this->tick_info[m].next_accel_event = this->accelerate_until;
+ acceleration_change = acceleration_per_tick;
+
+ } else if(this->decelerate_after == 0 /*&& this->accelerate_until == 0*/) {
+ // we start off decelerating
+ acceleration_change = -deceleration_per_tick;
+
+ } else if(this->decelerate_after != this->total_move_ticks /*&& this->accelerate_until == 0*/) {
+ // If the next event is the start of decel ( don't set this if the next accel event is accel end )
+ this->tick_info[m].next_accel_event = this->decelerate_after;
+ }
+
+ // already converted to fixed point just needs scaling by ratio
+ //#define STEPTICKER_TOFP(x) ((int64_t)round((double)(x)*STEPTICKER_FPSCALE))
+ this->tick_info[m].acceleration_change= (int64_t)round(acceleration_change * aratio);
+ this->tick_info[m].deceleration_change= -(int64_t)round(deceleration_per_tick * aratio);
+ this->tick_info[m].plateau_rate= (int64_t)round(((this->maximum_rate * aratio) / STEP_TICKER_FREQUENCY) * STEPTICKER_FPSCALE);
+
+ #if 0
+ THEKERNEL->streams->printf("spt: %08lX %08lX, ac: %08lX %08lX, dc: %08lX %08lX, pr: %08lX %08lX\n",
+ (uint32_t)(this->tick_info[m].steps_per_tick>>32), // 2.62 fixed point
+ (uint32_t)(this->tick_info[m].steps_per_tick&0xFFFFFFFF), // 2.62 fixed point
+ (uint32_t)(this->tick_info[m].acceleration_change>>32), // 2.62 fixed point signed
+ (uint32_t)(this->tick_info[m].acceleration_change&0xFFFFFFFF), // 2.62 fixed point signed
+ (uint32_t)(this->tick_info[m].deceleration_change>>32), // 2.62 fixed point
+ (uint32_t)(this->tick_info[m].deceleration_change&0xFFFFFFFF), // 2.62 fixed point
+ (uint32_t)(this->tick_info[m].plateau_rate>>32), // 2.62 fixed point
+ (uint32_t)(this->tick_info[m].plateau_rate&0xFFFFFFFF) // 2.62 fixed point
+ );
+ #endif
+ }
+}
+
+// returns current rate (steps/sec) for the given actuator
+float Block::get_trapezoid_rate(int i) const
+{
+ // convert steps per tick from fixed point to float and convert to steps/sec
+ // FIXME steps_per_tick can change at any time, potential race condition if it changes while being read here
+ return STEPTICKER_FROMFP(tick_info[i].steps_per_tick) * STEP_TICKER_FREQUENCY;
+}