optimize 64bit fixed point usage

diff --git a/src/libs/StepTicker.h b/src/libs/StepTicker.h
index 7aa5f29..6b9cf62 100644 (file)
--- a/src/libs/StepTicker.h
+++ b/src/libs/StepTicker.h
@@ -23,7 +23,6 @@ class Block;
 
 // handle 2.62 Fixed point
 #define STEPTICKER_FPSCALE (1LL<<62)
-
 #define STEPTICKER_FROMFP(x) ((float)(x)/STEPTICKER_FPSCALE)
 
 class StepTicker{

diff --git a/src/modules/robot/Block.cpp b/src/modules/robot/Block.cpp
index c0ec39b..31f93ee 100644 (file)
--- a/src/modules/robot/Block.cpp
+++ b/src/modules/robot/Block.cpp
@@ -26,6 +26,7 @@ using std::string;
 #define STEP_TICKER_FREQUENCY THEKERNEL->step_ticker->get_frequency()
 
 uint8_t Block::n_actuators= 0;
+double Block::fp_scale= 0;
 
 // A block represents a movement, it's length for each stepper motor, and the corresponding acceleration curves.
 // It's stacked on a queue, and that queue is then executed in order, to move the motors.
@@ -37,6 +38,12 @@ Block::Block()
     clear();
 }
 
+void Block::init(uint8_t n)
+{
+    n_actuators= n;
+    fp_scale= (double)STEPTICKER_FPSCALE / pow((double)STEP_TICKER_FREQUENCY, 2.0); // we scale up by fixed point offset first to avoid tiny values
+}
+
 void Block::clear()
 {
     is_ready            = false;
@@ -201,8 +208,6 @@ void Block::calculate_trapezoid( float entryspeed, float exitspeed )
     // Theorically, if accel is done per tick, the speed curve should be perfect.
     this->total_move_ticks = total_move_ticks;
 
-    //puts "accelerate_until: #{this->accelerate_until}, decelerate_after: #{this->decelerate_after}, g: #{this->acceleration_per_tick}, total_move_ticks: #{this->total_move_ticks}"
-
     this->initial_rate = initial_rate;
     this->exit_speed = exitspeed;
 
@@ -293,11 +298,16 @@ float Block::max_exit_speed()
 // 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
-    double acceleration_per_tick = acceleration_in_steps / std::pow(STEP_TICKER_FREQUENCY, 2.0);
-    double deceleration_per_tick = deceleration_in_steps / std::pow(STEP_TICKER_FREQUENCY, 2.0);
+    // 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];
@@ -325,10 +335,10 @@ void Block::prepare(float acceleration_in_steps, float deceleration_in_steps)
             this->tick_info[m].next_accel_event = this->decelerate_after;
         }
 
-        // convert to fixed point after scaling
+        // 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) * STEPTICKER_FPSCALE);
-        this->tick_info[m].deceleration_change= -(int64_t)round((deceleration_per_tick * aratio) * 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

diff --git a/src/modules/robot/Block.h b/src/modules/robot/Block.h
index f538204..3f53dd6 100644 (file)
--- a/src/modules/robot/Block.h
+++ b/src/modules/robot/Block.h
@@ -14,6 +14,9 @@
 class Block {
     public:
         Block();
+
+        static void init(uint8_t);
+
         void calculate_trapezoid( float entry_speed, float exit_speed );
 
         float reverse_pass(float exit_speed);
@@ -28,6 +31,8 @@ class Block {
         float max_allowable_speed( float acceleration, float target_velocity, float distance);
         void prepare(float acceleration_in_steps, float deceleration_in_steps);
 
+        static double fp_scale; // optimize to store this as it does not change
+
     public:
         std::array<uint32_t, k_max_actuators> steps; // Number of steps for each axis for this block
         uint32_t steps_event_count;  // Steps for the longest axis
@@ -63,6 +68,7 @@ class Block {
         // need info for each active motor
         //std::array<tickinfo_t, k_max_actuators> tick_info;
         std::vector<tickinfo_t> tick_info;
+
         static uint8_t n_actuators;
 
         struct {

diff --git a/src/modules/robot/Conveyor.cpp b/src/modules/robot/Conveyor.cpp
index 2d0ce2e..682e7ef 100644 (file)
--- a/src/modules/robot/Conveyor.cpp
+++ b/src/modules/robot/Conveyor.cpp
@@ -82,7 +82,7 @@ void Conveyor::on_module_loaded()
 // we allocate the queue here after config is completed so we do not run out of memory during config
 void Conveyor::start(uint8_t n)
 {
-    Block::n_actuators= n; // set the number of motors which determines how big the tick info vector is
+    Block::init(n); // set the number of motors which determines how big the tick info vector is
     queue.resize(queue_size);
     running = true;
 }