* Improve message processing in fpga task

* Improve management of the emergency stop gpio
goldobot · Apr 1, 2024 · dadf0ff · dadf0ff
1 parent 8fed36d
commit dadf0ff
Show file tree

Hide file tree

Showing 3 changed files with 16 additions and 5 deletions.
diff --git a/goldobot/src/tasks/fpga.cpp b/goldobot/src/tasks/fpga.cpp
@@ -39,7 +39,7 @@ void FpgaTask::taskFunction() {
 #endif
 
   while (1) {
-    while (m_message_queue.message_ready()) {
+    if ((m_cnt%5!=0) && m_message_queue.message_ready()) {
       process_message();
     }
 

diff --git a/goldobot/src/tasks/propulsion.cpp b/goldobot/src/tasks/propulsion.cpp
@@ -60,8 +60,10 @@ void PropulsionTask::doStep() {
   m_statistics.max_interval = std::max(interval_cycles, m_statistics.max_interval);
 
   // Process emergency gpio
-  auto emergency_stop = hal::gpio_get(7) ? true : false;
-  if (emergency_stop) {
+  auto gpio_emergency_stop = hal::gpio_get(7) ? true : false;
+  if (gpio_emergency_stop &&
+      ((m_controller.state() == PropulsionController::State::FollowTrajectory) ||
+       (m_controller.state() == PropulsionController::State::Rotate))) {
     m_controller.emergencyStop();
   }
 
@@ -604,6 +606,12 @@ void PropulsionTask::onCommandEnd() {
 
   m_is_executing_command = false;
   if (m_controller.state() == PropulsionController::State::Error) {
+#if 0 /* FIXME : TODO : try to improve the emergency stop sequence */
+    if ((m_controller.error() == PropulsionController::Error::EmergencyStop)) {
+      /* FIXME : TODO : is this OK? */
+      m_controller.clearError();
+    }
+#endif
   }
 }
 

diff --git a/libs/goldo_propulsion/src/controller.cpp b/libs/goldo_propulsion/src/controller.cpp
@@ -57,15 +57,18 @@ void PropulsionController::clearError() {
 
 bool PropulsionController::commandFinished() { return m_command_finished; }
 
+/* FIXME : DEBUG */
+float debug_emergency_accel = 2.0f;
+
 void PropulsionController::emergencyStop() {
   switch (m_state) {
     case State::FollowTrajectory:
-      m_speed_controller.setAccelerationLimits(2, 2);
+      m_speed_controller.setAccelerationLimits(debug_emergency_accel, debug_emergency_accel);
       m_speed_controller.setRequestedSpeed(0);
       m_emergency_stop = true;
       return;
     case State::Rotate:
-      m_speed_controller.setAccelerationLimits(2, 2);
+      m_speed_controller.setAccelerationLimits(debug_emergency_accel, debug_emergency_accel);
       m_speed_controller.setRequestedSpeed(0);
       m_emergency_stop = true;
       return;