Improved Standstill Hold functionality

- Standstill Hold should now engage sooner if Brake pedal is pressed to prevent continuous rolling in a Downhill situation.

Inc/util.h

@@ -70,7 +70,7 @@ void adcCalibLim(void);
 void updateCurSpdLim(void);
 void saveConfig(void);
 int  addDeadBand(int16_t u, int16_t deadBand, int16_t in_min, int16_t in_max, int16_t out_min, int16_t out_max);
-void standstillHold(int16_t *speedCmd);
+void standstillHold(void);
 void electricBrake(uint16_t speedBlend, uint8_t reverseDir);
 void cruiseControl(uint8_t button);
 

Src/main.c

@@ -218,6 +218,10 @@ int main(void) {
         speedBlend = (uint16_t)(((CLAMP(speedAvgAbs,10,60) - 10) << 15) / 50); // speedBlend [0,1] is within [10 rpm, 60rpm]
       #endif
 
+      #ifdef STANDSTILL_HOLD_ENABLE
+        standstillHold();                                           // Apply Standstill Hold functionality. Only available and makes sense for VOLTAGE or TORQUE Mode
+      #endif
+
       #ifdef VARIANT_HOVERCAR        
         if (speedAvgAbs < 60) {                                     // Check if Hovercar is physically close to standstill to enable Double tap detection on Brake pedal for Reverse functionality
           multipleTapDet(cmd1, HAL_GetTick(), &MultipleTapBrake);   // Brake pedal in this case is "cmd1" variable
@@ -259,10 +263,6 @@ int main(void) {
       steer = (int16_t)(steerFixdt >> 16);  // convert fixed-point to integer
       speed = (int16_t)(speedFixdt >> 16);  // convert fixed-point to integer
 
-      #ifdef STANDSTILL_HOLD_ENABLE
-        standstillHold(&speed);                 // Apply Standstill Hold functionality. Only available and makes sense for VOLTAGE or TORQUE Mode
-      #endif
-
       // ####### VARIANT_HOVERCAR #######
       #ifdef VARIANT_HOVERCAR        
         if (!MultipleTapBrake.b_multipleTap) {  // Check driving direction

Src/util.c

@@ -195,6 +195,10 @@ static uint8_t button2;                 // Green
 static uint8_t brakePressed;
 #endif
 
+#if defined(CRUISE_CONTROL_SUPPORT) || (defined(STANDSTILL_HOLD_ENABLE) && (CTRL_TYP_SEL == FOC_CTRL) && (CTRL_MOD_REQ != SPD_MODE))
+static uint8_t cruiseCtrlAcv = 0;
+static uint8_t standstillAcv = 0;
+#endif
 
 /* =========================== Initialization Functions =========================== */
 
@@ -583,23 +587,30 @@ int addDeadBand(int16_t u, int16_t deadBand, int16_t in_min, int16_t in_max, int
 
  /*
  * Standstill Hold Function
- * This function will switch to SPEED mode at standstill to provide an anti-roll functionality.
+ * This function uses Cruise Control to provide an anti-roll functionality at standstill.
- * Only available and makes sense for VOLTAGE or TORQUE mode.
+ * Only available and makes sense for FOC VOLTAGE or FOC TORQUE mode.
  * 
- * Input: pointer *speedCmd
+ * Input:  none
- * Output: modified Control Mode Request
+ * Output: standstillAcv
  */
-void standstillHold(int16_t *speedCmd) {
+void standstillHold(void) {
   #if defined(STANDSTILL_HOLD_ENABLE) && (CTRL_TYP_SEL == FOC_CTRL) && (CTRL_MOD_REQ != SPD_MODE)
-    if (*speedCmd > -20 && *speedCmd < 20) {                          // If speedCmd (Throttle) is small
+    if (!rtP_Left.b_cruiseCtrlEna) {                          // If Stanstill in NOT Active -> try Activation
-      if (ctrlModReqRaw != SPD_MODE && speedAvgAbs < 3) {             // and If measured speed is small (meaning we are at standstill)
+      if (((cmd1 > 50 || cmd2 < -50) && speedAvgAbs < 30)     // Check if Brake is pressed AND measured speed is small
-        ctrlModReqRaw = SPD_MODE;                                     // Switch to Speed mode
+          || (cmd2 < 20 && speedAvgAbs < 5)) {                // OR Throttle is small AND measured speed is very small
+        rtP_Left.n_cruiseMotTgt   = 0;
+        rtP_Right.n_cruiseMotTgt  = 0;
+        rtP_Left.b_cruiseCtrlEna  = 1;
+        rtP_Right.b_cruiseCtrlEna = 1;
+        standstillAcv = 1;
+      } 
+    }
+    else {                                                    // If Stanstill is Active -> try Deactivation
+      if (cmd1 < 20 && cmd2 > 50 && !cruiseCtrlAcv) {         // Check if Brake is released AND Throttle is pressed AND no Cruise Control
+        rtP_Left.b_cruiseCtrlEna  = 0;
+        rtP_Right.b_cruiseCtrlEna = 0;
+        standstillAcv = 0;
       }
-      if (ctrlModReqRaw == SPD_MODE) {                                // If we are in Speed mode
-        *speedCmd = 0;                                                // Request standstill (0 rpm)
-      }
-    } else if (ctrlModReqRaw != CTRL_MOD_REQ && (*speedCmd < -50 || *speedCmd > 50)) { // Else if speedCmd (Throttle) becomes significant
-      ctrlModReqRaw = CTRL_MOD_REQ;                                   // Follow the Mode request
     }
   #endif
 }
@@ -646,19 +657,21 @@ void electricBrake(uint16_t speedBlend, uint8_t reverseDir) {
  * This function activates/deactivates cruise control.
  * 
  * Input: button (as a pulse)
- * Output: none
+ * Output: cruiseCtrlAcv
  */
 void cruiseControl(uint8_t button) {
   #ifdef CRUISE_CONTROL_SUPPORT
-    if (button && !rtP_Left.b_cruiseCtrlEna) {                       // Cruise control activated
+    if (button && !rtP_Left.b_cruiseCtrlEna) {                          // Cruise control activated
       rtP_Left.n_cruiseMotTgt   = rtY_Left.n_mot;
       rtP_Right.n_cruiseMotTgt  = rtY_Right.n_mot;
       rtP_Left.b_cruiseCtrlEna  = 1;
       rtP_Right.b_cruiseCtrlEna = 1;
-      shortBeepMany(2, 1);                                           // 200 ms beep delay. Acts as a debounce also.
+      cruiseCtrlAcv = 1;
-    } else if (button && rtP_Left.b_cruiseCtrlEna) {                 // Cruise control deactivated
+      shortBeepMany(2, 1);                                              // 200 ms beep delay. Acts as a debounce also.
+    } else if (button && rtP_Left.b_cruiseCtrlEna && !standstillAcv) {  // Cruise control deactivated if no Standstill Hold is active
       rtP_Left.b_cruiseCtrlEna  = 0;
       rtP_Right.b_cruiseCtrlEna = 0;
+      cruiseCtrlAcv = 0;
       shortBeepMany(2, -1);
     }
   #endif