Gro501-APP2-A26 clean-up and update

README.md

@@ -103,15 +103,15 @@ Joystick inputs: LB is used as a deadman switch and must be always pressed for t
 High-level Mode | Input buttons | Function
 -|-|-
 -1|None| Disabled
-0|`LB`| Closed-loop velocity, open-loop steering
+0|`LB`| Closed-loop velocity, open-loop steering (`LB` + `L3` is the same mode with a fixed 2 m/s ref)
 1|`LB` + `RB`|Fully Open-loop
 2|`LB` + `RT`|Closed-loop position, open-loop steering
-3|`LB` + `A`|Closed-loop velocity, closed-loop steering
+3|`LB` + `A`|Closed-loop velocity, closed-loop steering ( `LB` + `X` is the same mode with a fixed 2 m/s)
 4|`LB` + `B`|Closed-loop position, closed-loop steering
-5|`LB` + `X`|Closed-loop velocity, closed-loop steering
+5|—---------| Empty template 
 6|`LB` + `Y`|Reset encoder command
-7|`LB` + `LY`|Empty Template
-8|`LB` + `Croos key Up/Down`| Empty Template
+7|`LB` + `R3`| Empty Template 
+8|`LB` + `Cross key Up/Down`| Empty Template 
 NaN|`LB` + `LT`|Joystick-based control disabled (no ctl_ref published)
 
 ## Low-level Controller Modes (Arduino modes)
@@ -121,7 +121,7 @@ The low-level mode is the operating mode of the Arduino. The low-level mode is s
 Low-level Mode | Function
 -|-
 0|Disabled
-1|Open-loop PWM control
+1|Open-loop voltage command (converted to PWM)
 2|Closed-loop velocity (based on wheel-encoder feedback)
-2|Closed-loop position (based on wheel-encoder feedback)
+3|Closed-loop position (based on wheel-encoder feedback)
 4|Reset encoder command

racecar_arduino/Controller/platformio.ini

@@ -12,6 +12,10 @@
 platform = atmelavr
 board = megaatmega2560
 framework = arduino
+; Serial TX buffer of 256 bytes instead of 64: the whole 161-char sensor frame is queued at once and sent
+; by the UART interrupt, so loop() does not wait (see sensorsCallback in src/main.cpp). Costs 192 B of RAM.
+; Must be a build flag (not a #define in main.cpp) so that the Arduino core is compiled with it.
+build_flags = -D SERIAL_TX_BUFFER_SIZE=256
 
 lib_deps = 
 	nanopb/Nanopb@^0.4.91

racecar_arduino/Controller/src/PBUtils.cpp

@@ -51,6 +51,62 @@ bool PBUtils::decodePb(char* inputString, int *subMsgId, int &nbsNewMsgs)
   return success;
 }
 
+// pbSend version: 0 = original (String + sprintf), 1 = fast (same bytes on the wire)
+#define PBSEND_FAST 1
+// PBSEND: #define PBSEND_FAST 1 // fast pbSend: same bytes, ~1.8 ms instead of ~9.7 ms (simavr, 16 MHz)
+
+#if PBSEND_FAST
+/*
+ * Send protobufs messages to the serial port with format <id|msg;>
+ * Fast version: same bytes as the String version below, but the frame is built in a fixed
+ * char array (2 hex digits per byte from a table) and sent with one Serial.write.
+ * No String (no malloc/realloc/free) and no sprintf. If an encoding fails, nothing is sent.
+ *
+ * @param nbs: The numbers of id to send
+ * @param ...: List of all the ids to send
+ */
+void PBUtils::pbSend(int nbs, ...)
+{
+  static const char hexDigits[] = "0123456789ABCDEF";
+  char frame[2 * MAX_MSG_LEN + 16]; // "<" + id + "|" + 2 hex chars per byte + ";" + ">" (on the stack)
+  size_t len = 0;
+  frame[len++] = '<';
+
+  va_list idsToSend;
+  va_start(idsToSend, nbs);
+  for (int i = 0; i < nbs; ++i)
+  {
+    int id = va_arg(idsToSend, int);
+    uint8_t bufferOut[MAX_MSG_LEN];
+    pb_ostream_t stream = pb_ostream_from_buffer(bufferOut, sizeof(bufferOut));
+    char idText[12];
+    itoa(id, idText, 10); // same text as String(id)
+    size_t idLen = strlen(idText);
+
+    if (!pb_encode(&stream, idToType[id], idToMsg[id]) ||
+        len + idLen + 2 * stream.bytes_written + 3 > sizeof(frame))
+    {
+      va_end(idsToSend);
+      return; // encoding failed (or frame too long): nothing is sent, as before
+    }
+
+    memcpy(frame + len, idText, idLen);
+    len += idLen;
+    frame[len++] = '|';
+    for (size_t j = 0; j < stream.bytes_written; j++)
+    {
+      frame[len++] = hexDigits[bufferOut[j] >> 4];   // same as sprintf("%02X"): upper case, 2 digits
+      frame[len++] = hexDigits[bufferOut[j] & 0x0F];
+    }
+    frame[len++] = ';';
+  }
+  va_end(idsToSend);
+
+  frame[len++] = '>';
+  Serial.write((const uint8_t *)frame, len); // one call instead of Serial.print(String)
+}
+
+#else // original pbSend
 /*
  * Send protobufs messages to the serial port with format <id|msg;>
  * 
@@ -93,6 +149,7 @@ void PBUtils::pbSend(int nbs, ...)
   if (success)
     Serial.print(toSendBuilder);
 }
+#endif // PBSEND_FAST
 
 /*
  * Convert an input string to a list of PB messages and ids

racecar_arduino/Controller/src/main.cpp

@@ -40,14 +40,9 @@ const int str_pin = 9; // Servo
 const int dri_pwm_pin = 6;  // H bridge drive pwm
 const int dri_dir_pin = 42; //
 
-// debug
-long timer_debug      = 0;
-long time_micros      = 0;
-long time_micros_last = 0;
-
 // Prototype
 void cmdCallback();
-void sensorsCallback(unsigned long dt);
+void sensorsCallback(unsigned long dt_com_us);
 
 ///////////////////////////////////////////////////////////////////
 // Parameters
@@ -65,10 +60,10 @@ const float pos_kd     = 0.0;
 const float pos_ki     = 0.0;
 const float pos_ei_sat = 10000.0;
 
-// Loop period
-const unsigned long time_period_low  = 2;    // 500 Hz for internal PID loop
-const unsigned long time_period_high = 25;   // 50 Hz  for ROS communication
-const unsigned long time_period_com  = 1000; // 1000 ms = max com delay (watchdog)
+// Comparison targets [us]
+const unsigned long period_ctl_us      = 2000;    // 500 Hz internal PID loop
+const unsigned long period_com_us      = 20000;   // 50 Hz ROS communication
+const unsigned long period_watchdog_us = 1000000; // 1 s max without a command
 
 // Hardware min-zero-max range for the steering servo and the drive
 const int min_str_angle = 45;
@@ -78,7 +73,7 @@ const int pwm_min_dri   = -511;
 const int pwm_zer_dri   = 0;
 const int pwm_max_dri   = 511;
 
-const int dri_wakeup_time = 20; // micro second
+const int dri_wakeup_us = 20; // [us] fixed H-bridge wake pulse
 
 // Units Conversion
 const double batteryV             = 8;
@@ -115,13 +110,19 @@ float vel_error_int = 0;
 float pos_error_int = 0;
 
 // Loop timing
-unsigned long time_now       = 0;
-unsigned long time_last_low  = 0;
-unsigned long time_last_high = 0;
-unsigned long time_last_com  = 0; // com watchdog
+// time_ = recorded micros() value, period_ = comparison target, dt_ = measured delta
+unsigned long time_now_us      = 0;
+unsigned long time_last_ctl_us = 0;
+unsigned long time_last_com_us = 0;
+unsigned long time_last_cmd_us = 0; // watchdog: last received command
+unsigned long dt_ctl_us        = 0;
+unsigned long dt_com_us        = 0;
+unsigned long dt_pause_us      = 0; // duration of the last sensorsCallback
+float dt_ctl_ms = 0 ;
+float dt_com_ms = 0 ;
 
 // For odometry
-signed long enc_last_high = 0;
+signed long enc_last_com = 0;
 
 ///////////////////////////////////////////////////////////////////
 // Encoder init/read/reset functions
@@ -259,7 +260,7 @@ void set_pwm(int pwm)
         if (dri_standby == 1)
         {
             digitalWrite(dri_pwm_pin, HIGH);
-            delayMicroseconds(dri_wakeup_time);
+            delayMicroseconds(dri_wakeup_us);
             dri_standby = 0;
         }
 
@@ -317,7 +318,7 @@ const unsigned long baud_rate = 115200;
 ///////////////////////////////////////////////////////////////////
 // Controller One tick
 ///////////////////////////////////////////////////////////////////
-void ctl(int dt_low)
+void ctl(float dt_ctl_ms) // [ms] measured control delta since the last tick
 {
     ///////////////////////////////////////////////
     // STEERING CONTROL
@@ -340,7 +341,7 @@ void ctl(int dt_low)
     // Velocity computation
 
     // TODO: VOUS DEVEZ COMPLETEZ LA DERIVEE FILTRE ICI
-    float vel_raw = (enc_now - enc_old) * tick2m / dt_low * 1000;
+    float vel_raw = (enc_now - enc_old) * tick2m / dt_ctl_ms * 1000.0f;
     float alpha   = 0;       // TODO
     float vel_fil = vel_raw; // Filter TODO
 
@@ -414,6 +415,8 @@ void ctl(int dt_low)
         // Reset encoder counts
 
         clearEncoderCount();
+        enc_now       = readEncoder(); // counter is 0 now: no false speed step next tick
+        enc_last_com = enc_now;       // same for data[9] (distance since last publish)
 
         // reset integral actions
         vel_error_int = 0;
@@ -476,13 +479,13 @@ void setup()
 
 void loop()
 {
-    time_now = millis();
+    time_now_us = micros(); // [us] 4 us resolution; wraps after 71.6 min (unsigned differences stay correct)
 
     /////////////////////////////////////////////////////////////
     // Watchdog: stop the car if no recent communication from ROS
     //////////////////////////////////////////////////////////////
 
-    if ((time_now - time_last_com) > time_period_com)
+    if ((time_now_us - time_last_cmd_us) > period_watchdog_us)
     {
         // All-stop
         dri_ref  = 0; // velocity set-point
@@ -493,15 +496,22 @@ void loop()
     // Low-level controller
     ///////////////////////////////////////
 
-    if ((time_now - time_last_low) > time_period_low)
+    dt_ctl_us = time_now_us - time_last_ctl_us;
+
+    if (dt_ctl_us > period_ctl_us) // 500 Hz control tick
     {
-        ctl(time_now - time_last_low);                // one control tick
-        timer_debug = time_micros - time_micros_last; ///
+        time_last_ctl_us = time_now_us;
+
+        dt_ctl_ms = dt_ctl_us * 0.001f;
 
-        time_last_low    = time_now;
-        time_micros_last = time_micros;
+        // one control tick, dt in [ms]
+        ctl(dt_ctl_ms);
     }
 
+    ////////////////////////////////////////
+    // Receive commands from ROS
+    ///////////////////////////////////////
+
     if (inCmdComplete)
     {
         inCmdComplete = false;
@@ -527,12 +537,20 @@ void loop()
         }
     }
 
-    unsigned long dt = time_now - time_last_high;
-    if (dt > time_period_high)
+    dt_com_us = time_now_us - time_last_com_us;
+
+    ////////////////////////////////////////
+    // Publish sensors data
+    ///////////////////////////////////////
+
+    if (dt_com_us > period_com_us) // 50 Hz communication tick
     {
-        sensorsCallback(dt);
-        time_last_high = time_now;
-        enc_last_high  = enc_now;
+        unsigned long time_pause_us = micros(); // clock at the start of sensorsCallback
+        sensorsCallback(dt_com_us);
+        dt_pause_us      = micros() - time_pause_us;
+        time_last_com_us = time_now_us;
+        enc_last_com     = enc_now;
+        dt_com_ms = dt_com_us * 0.001f;
     }
 }
 
@@ -544,26 +562,27 @@ void cmdCallback()
     dri_ref  = cmdMsg.data[1];  // volt or m/s or m
     ctl_mode = cmdMsg.data[2];  // 1    or 2   or 3*/
 
-    time_last_com = millis();
+    time_last_cmd_us = micros();
 }
 
-void sensorsCallback(unsigned long dt)
+void sensorsCallback(unsigned long dt_com_us)
 {
-    sensorsMsg.data_count = 19;
+    sensorsMsg.data_count = 19; // do not change: pb2roscpp and arduino_sensors expect exactly 19 floats
     sensorsMsg.data[0]    = pos_now; // wheel position in m
     sensorsMsg.data[1]    = vel_old; // wheel velocity in m/sec
 
     // For DEBUG
     sensorsMsg.data[2] = (float)dri_ref; // set point received by arduino
     sensorsMsg.data[3] = (float)dri_cmd; // drive set point in volts
-    // sensorsMsg.data[3] = (float)Serial.available(); // futile: we SHOULD NOT receive anything if Serial is not available.
-    sensorsMsg.data[4] = (float)dri_pwm;            // drive set point in pwm
-    sensorsMsg.data[5] = (float)enc_now;            // raw encoder counts
+    sensorsMsg.data[4] = (float)dt_ctl_ms;
+    // sensorsMsg.data[4] = (float)dri_pwm;            // drive set point in pwm
+    // sensorsMsg.data[5] = (float)enc_now;            // raw encoder counts
+    sensorsMsg.data[5] = (float)dt_pause_us;
     sensorsMsg.data[6] = (float)str_ref;            // steering angle (don't remove/change, used for GRO830)
     sensorsMsg.data[7] = (float)(ctl_mode);         // for com debug
-    sensorsMsg.data[8] = (float)dt;                 // time elapsed since last publish (don't remove/change, used for GRO830)
+    sensorsMsg.data[8] = (float)dt_com_us * 0.001f; // [ms] time elapsed since last publish (don't remove/change, used for GRO830)
     sensorsMsg.data[9] =
-        (enc_now - enc_last_high) * tick2m; // distance travelled since last publish (don't remove/change, used for GRO830)
+        (enc_now - enc_last_com) * tick2m; // distance travelled since last publish (don't remove/change, used for GRO830)
 
 // Read IMU (don't remove/change, used for GRO830)
 #ifdef IMU
@@ -580,7 +599,6 @@ void sensorsCallback(unsigned long dt)
 #endif
 
     pbUtils.pbSend(1, SENSORS);
-    Serial.flush();
 }
 
 // ======================================== SERIAL ========================================

racecar_autopilot/racecar_autopilot/rosbag2csv.py

@@ -34,7 +34,7 @@
 
 
 def get_rosbag_options(path, serialization_format='cdr'):
-    storage_options = rosbag2_py.StorageOptions(uri=path, storage_id='mcap')
+    storage_options = rosbag2_py.StorageOptions(uri=path, storage_id='')  # auto-detect mcap/sqlite3
 
     converter_options = rosbag2_py.ConverterOptions(
         input_serialization_format=serialization_format,
@@ -97,7 +97,7 @@ def dump_bag(bag_path):
         if hasattr(msg, "header"):
             t = msg.header.stamp.sec + 1e-9*msg.header.stamp.nanosec
         else:
-            t = ts
+            t = ts * 1e-9  # bag time [ns] -> [s], same unit as header stamps
         if start_time is None:
             start_time = t
         print(','.join([str(t - start_time)] +