initial attempt to use pendsv for processing block changes

diff --git a/src/libs/Kernel.cpp b/src/libs/Kernel.cpp
index 3c28210..2d6c41a 100644 (file)
--- a/src/libs/Kernel.cpp
+++ b/src/libs/Kernel.cpp
@@ -100,23 +100,24 @@ Kernel::Kernel(){
     NVIC_SetPriorityGrouping(0);
     NVIC_SetPriority(TIMER0_IRQn, 2);
     NVIC_SetPriority(TIMER1_IRQn, 1);
-    NVIC_SetPriority(TIMER2_IRQn, 3);
+    NVIC_SetPriority(TIMER2_IRQn, 4);
+    NVIC_SetPriority(PendSV_IRQn, 3);
 
     // Set other priorities lower than the timers
-    NVIC_SetPriority(ADC_IRQn, 4);
-    NVIC_SetPriority(USB_IRQn, 4);
+    NVIC_SetPriority(ADC_IRQn, 5);
+    NVIC_SetPriority(USB_IRQn, 5);
 
     // If MRI is enabled
     if( MRI_ENABLE ){
-        if( NVIC_GetPriority(UART0_IRQn) > 0 ){ NVIC_SetPriority(UART0_IRQn, 4); }
-        if( NVIC_GetPriority(UART1_IRQn) > 0 ){ NVIC_SetPriority(UART1_IRQn, 4); }
-        if( NVIC_GetPriority(UART2_IRQn) > 0 ){ NVIC_SetPriority(UART2_IRQn, 4); }
-        if( NVIC_GetPriority(UART3_IRQn) > 0 ){ NVIC_SetPriority(UART3_IRQn, 4); }
+        if( NVIC_GetPriority(UART0_IRQn) > 0 ){ NVIC_SetPriority(UART0_IRQn, 5); }
+        if( NVIC_GetPriority(UART1_IRQn) > 0 ){ NVIC_SetPriority(UART1_IRQn, 5); }
+        if( NVIC_GetPriority(UART2_IRQn) > 0 ){ NVIC_SetPriority(UART2_IRQn, 5); }
+        if( NVIC_GetPriority(UART3_IRQn) > 0 ){ NVIC_SetPriority(UART3_IRQn, 5); }
     }else{
-        NVIC_SetPriority(UART0_IRQn, 4);
-        NVIC_SetPriority(UART1_IRQn, 4);
-        NVIC_SetPriority(UART2_IRQn, 4);
-        NVIC_SetPriority(UART3_IRQn, 4);
+        NVIC_SetPriority(UART0_IRQn, 5);
+        NVIC_SetPriority(UART1_IRQn, 5);
+        NVIC_SetPriority(UART2_IRQn, 5);
+        NVIC_SetPriority(UART3_IRQn, 5);
     }
 
     // Configure the step ticker

diff --git a/src/libs/StepTicker.cpp b/src/libs/StepTicker.cpp
index 1fdb00d..28cae15 100644 (file)
--- a/src/libs/StepTicker.cpp
+++ b/src/libs/StepTicker.cpp
@@ -146,9 +146,27 @@ extern "C" void TIMER0_IRQHandler (void){
     StepTicker::global_step_ticker->TIMER0_IRQHandler();
 }
 
+extern "C" void PendSV_Handler(void) {
+    StepTicker::global_step_ticker->PendSV_IRQHandler();
+}
+
+// slightly lower priority than TIMER0, the whole end of block/start of block is done here allowing the timer to continue ticking
+void StepTicker::PendSV_IRQHandler (void){
+    #ifdef STEPTICKER_DEBUG_PIN
+    stepticker_debug_pin= 1;
+    #endif
+
+    this->signal_a_move_finished();
+
+    #ifdef STEPTICKER_DEBUG_PIN
+    stepticker_debug_pin= 0;
+    #endif
+}
+
 void StepTicker::TIMER0_IRQHandler (void){
     // Reset interrupt register
     LPC_TIM0->IR |= 1 << 0;
+    LPC_TIM0->MR0 = this->period;
 
     // Step pins
     uint16_t bitmask = 1;
@@ -164,79 +182,15 @@ void StepTicker::TIMER0_IRQHandler (void){
         LPC_TIM1->TCR = 1;
         this->reset_step_pins = false;
     }else{
-        // Nothing happened, nothing after this really matters
-        // TODO : This could be a problem when we use Actuators instead of StepperMotors, because this flag is specific to step generation
-        LPC_TIM0->MR0 = this->period;
         return;
     }
 
     // If a move finished in this tick, we have to tell the actuator to act accordingly
     if( this->a_move_finished ){
-
-        #ifdef STEPTICKER_DEBUG_PIN
-        stepticker_debug_pin= 1;
-        #endif
-
-        // Do not get out of here before everything is nice and tidy
-        LPC_TIM0->MR0 = 20000000;
-
-        this->signal_a_move_finished();
-
-        // If we went over the duration an interrupt is supposed to last, we have a problem
-        // That can happen typically when we change blocks, where more than usual computation is done
-        // This can be OK, if we take notice of it, which we do now
-        if(LPC_TIM0->TC > this->period ){ // TODO: remove the size condition
-            //overruns++;
-            uint32_t start_tc = LPC_TIM0->TC;
-
-            // How many ticks we want to skip ( this does not include the current tick, but we add the time we spent doing this computation last time )
-            uint32_t ticks_to_skip = (  ( LPC_TIM0->TC + this->last_duration ) / this->period );
-
-            // Next step is now to reduce this to how many steps we can *actually* skip, as we do not want to cause an actual step
-            uint32_t ticks_we_actually_can_skip = ticks_to_skip;
-
-            int i;
-            uint32_t bm;
-            for (i = 0, bm = 1; i < num_motors; i++, bm <<= 1)
-            {
-                if((this->active_motor_bm & bm) != 0) {
-                    if(this->active_motors[i]->fx_ticks_per_step > this->active_motors[i]->fx_counter) {
-                        ticks_we_actually_can_skip =
-                            min(ticks_we_actually_can_skip, (uint32_t)((active_motors[i]->fx_ticks_per_step - active_motors[i]->fx_counter) >> active_motors[i]->fx_shift));
-                    }else{
-                        ticks_we_actually_can_skip= 0;
-                    }
-                }
-            }
-
-            // Adding to MR0 for this time is not enough, we must also increment the counters ourself artificially
-            for (i = 0, bm = 1; i < num_motors; i++, bm <<= 1)
-            {
-                if (this->active_motor_bm & bm)
-                    this->active_motors[i]->fx_counter += ((uint64_t)ticks_we_actually_can_skip << active_motors[i]->fx_shift);
-            }
-
-            // When must we have our next MR0 ? ( +1 is here to account that we are actually doing a legit MR0 match here too, not only overtime )
-            LPC_TIM0->MR0 = ( ticks_to_skip + 1 ) * this->period;
-
-            // This is so that we know how long this computation takes, and we can take it into account next time
-            int difference = (int)(LPC_TIM0->TC) - (int)(start_tc);
-            if( difference > 0 ){ this->last_duration = (uint32_t)difference; }
-
-
-        }else{
-            LPC_TIM0->MR0 = this->period;
-        }
-
-        while( LPC_TIM0->TC > LPC_TIM0->MR0 ){
-            LPC_TIM0->MR0 += this->period;
-        }
-
-        #ifdef STEPTICKER_DEBUG_PIN
-        stepticker_debug_pin= 0;
-        #endif
+        // we delegate the slow stuff to the pendsv handler
+        //NVIC_SetPendingIRQ(PendSV_IRQn); this doesn't work
+        SCB->ICSR = 0x10000000; // SCB_ICSR_PENDSVSET_Msk;
     }
-
 }
 
 

diff --git a/src/libs/StepTicker.h b/src/libs/StepTicker.h
index 4f78370..a0d9c07 100644 (file)
--- a/src/libs/StepTicker.h
+++ b/src/libs/StepTicker.h
@@ -29,7 +29,7 @@ class StepTicker{
         void add_motor_to_active_list(StepperMotor* motor);
         void remove_motor_from_active_list(StepperMotor* motor);
         void TIMER0_IRQHandler (void);
-        //uint32_t overruns;
+        void PendSV_IRQHandler (void);
 
     private:
         float frequency;

diff --git a/src/modules/robot/Stepper.cpp b/src/modules/robot/Stepper.cpp
index b0074d3..252d8a0 100644 (file)
--- a/src/modules/robot/Stepper.cpp
+++ b/src/modules/robot/Stepper.cpp
@@ -321,7 +321,6 @@ void Stepper::set_step_events_per_second( float steps_per_second )
 // All we do is reset the other timer so that it does what we want
 uint32_t Stepper::synchronize_acceleration(uint32_t dummy)
 {
-
     // No move was done, this is called from on_block_begin
     // This means we setup the accel timer in a way where it gets called right after
     // we exit this step interrupt, and so that it is then in synch with
@@ -341,7 +340,7 @@ uint32_t Stepper::synchronize_acceleration(uint32_t dummy)
             this->main_stepper->attach_signal_step(this->current_block->decelerate_after, this, &Stepper::synchronize_acceleration);
         }
     } else {
-        // If we are called not at the first steps, this means we are beginning deceleratingration
+        // If we are called not at the first steps, this means we are beginning deceleration
         NVIC_SetPendingIRQ(TIMER2_IRQn);
         // Synchronize both counters
         LPC_TIM2->TC = LPC_TIM0->TC;