Launch control + Internal speed controller

can_spec_my18.yml

@@ -26,6 +26,28 @@ boards:
         - MCCommand
         - VCUControlsParams
         - VCUControlsMonitoring
+        - VCUSpeedCntrlKpTimes1000
+        - VCUSpeedCntrlKiTimes1000
+        - VCUSpeedCntrlKdTimes1000
+        - VCUSpeedCntrlIWindupMax
+        - VCUSpeedCntrlIWindupMin
+        - VCUSpeedCntrlMinOutputValue
+        - VCUSpeedCntrlMaxOutputValue
+        - VCUSpeedCntrlMinInputValue
+        - VCUSpeedCntrlMaxInputValue
+        - VCUSpeedCntrlErrorUpdateTimeout
+        - VCUSpeedCntrlDt
+        - VCUSpeedCntrlEnabled
+        - VCUSpeedCntrlOutOfInputRangeThrottled
+        - VCUSpeedCntrlOutOfOutputRangeThrottled
+        - VCUSpeedCntrlErrorUpdateTimedOut
+        - VCUSpeedCntrlRPMSetpoint
+        - VCUSpeedCntrlCommandedTorque
+        - VCUSpeedCntrlRPMError
+        - VCUSpeedCntrlLastRPMError
+        - VCUSpeedCntrlDerivRPMError
+        - VCUSpeedCntrlRPMErrorAccumulated
+        - VCUSpeedCntrlLastErrorUpdateTimestamp
     subscribe:
       can0:
         - FrontCanNodeBrakeThrottle
@@ -869,6 +891,204 @@ buses:
             position: 48
             length: 16
             c_type: uint16_t
+      VCUSpeedCntrlKpTimes1000:
+        can_id:  0x223
+        period: 200ms
+        is_big_endian: true
+        segments:
+          kp_times_1000:
+            position: 0
+            length: 32
+            c_type: int32_t
+      VCUSpeedCntrlKiTimes1000:
+        can_id:  0x225
+        period: 200ms
+        is_big_endian: true
+        segments:
+          ki_times_1000:
+            position: 0
+            length: 32
+            c_type: int32_t
+      VCUSpeedCntrlKdTimes1000:
+        can_id:  0x227
+        period: 200ms
+        is_big_endian: true
+        segments:
+          kd_times_1000:
+            position: 0
+            length: 32
+            c_type: int32_t
+      VCUSpeedCntrlIWindupMax:
+        can_id:  0x229
+        period: 200ms
+        is_big_endian: true
+        segments:
+          i_windup_max:
+            position: 0
+            length: 32
+            c_type: int32_t
+      VCUSpeedCntrlIWindupMin:
+        can_id:  0x22B
+        period: 200ms
+        is_big_endian: true
+        segments:
+          i_windup_min:
+            position: 0
+            length: 32
+            c_type: int32_t
+      VCUSpeedCntrlMinOutputValue:
+        can_id:  0x22D
+        period: 200ms
+        is_big_endian: true
+        segments:
+          min_output_value:
+            position: 0
+            length: 32
+            c_type: int32_t
+      VCUSpeedCntrlMaxOutputValue:
+        can_id:  0x22F
+        period: 200ms
+        is_big_endian: true
+        segments:
+          max_output_value:
+            position: 0
+            length: 32
+            c_type: int32_t
+      VCUSpeedCntrlMinInputValue:
+        can_id:  0x231
+        period: 200ms
+        is_big_endian: true
+        segments:
+          min_input_value:
+            position: 0
+            length: 32
+            c_type: int32_t
+      VCUSpeedCntrlMaxInputValue:
+        can_id:  0x233
+        period: 200ms
+        is_big_endian: true
+        segments:
+          max_input_value:
+            position: 0
+            length: 32
+            c_type: int32_t
+      VCUSpeedCntrlErrorUpdateTimeout:
+        can_id:  0x235
+        period: 200ms
+        is_big_endian: true
+        segments:
+          error_update_timeout:
+            position: 0
+            length: 32
+            c_type: int32_t
+      VCUSpeedCntrlDt:
+        can_id:  0x237
+        period: 200ms
+        is_big_endian: true
+        segments:
+          dt:
+            position: 0
+            length: 32
+            c_type: int32_t
+      VCUSpeedCntrlEnabled:
+        can_id: 0x239
+        period: 200ms
+        is_big_endian: true
+        segments:
+          enabled:
+            position: 0
+            length: 1
+            c_type: bool
+      VCUSpeedCntrlOutOfInputRangeThrottled:
+        can_id: 0x23B
+        period: 200ms
+        is_big_endian: true
+        segments:
+          out_of_input_range_throttled:
+            position: 0
+            length: 1
+            c_type: bool
+      VCUSpeedCntrlOutOfOutputRangeThrottled:
+        can_id: 0x23D
+        period: 200ms
+        is_big_endian: true
+        segments:
+          out_of_output_range_throttled:
+            position: 0
+            length: 1
+            c_type: bool
+      VCUSpeedCntrlErrorUpdateTimedOut:
+        can_id: 0x23F
+        period: 200ms
+        is_big_endian: true
+        segments:
+          error_update_timed_out:
+            position: 0
+            length: 1
+            c_type: bool
+      VCUSpeedCntrlRPMSetpoint:
+        can_id: 0x241
+        period: 200ms
+        is_big_endian: true
+        segments:
+          rpm_setpoint:
+            position: 0
+            length: 32
+            c_type: int32_t
+      VCUSpeedCntrlCommandedTorque:
+        can_id: 0x243
+        period: 200ms
+        is_big_endian: true
+        segments:
+          commanded_torque:
+            position: 0
+            length: 32
+            c_type: int32_t
+      VCUSpeedCntrlRPMError:
+        can_id: 0x245
+        period: 200ms
+        is_big_endian: true
+        segments:
+          rpm_error:
+            position: 0
+            length: 32
+            c_type: int32_t
+      VCUSpeedCntrlLastRPMError:
+        can_id: 0x247
+        period: 200ms
+        is_big_endian: true
+        segments:
+          last_rpm_error:
+            position: 0
+            length: 32
+            c_type: int32_t
+      VCUSpeedCntrlDerivRPMError:
+        can_id: 0x249
+        period: 200ms
+        is_big_endian: true
+        segments:
+          deriv_rpm_error:
+            position: 0
+            length: 32
+            c_type: int32_t
+      VCUSpeedCntrlRPMErrorAccumulated:
+        can_id: 0x24B
+        period: 200ms
+        is_big_endian: true
+        segments:
+          rpm_error_accumulated:
+            position: 0
+            length: 32
+            c_type: int32_t
+      VCUSpeedCntrlLastErrorUpdateTimestamp:
+        can_id: 0x24D
+        period: 200ms
+        is_big_endian: true
+        segments:
+          last_error_update_timestamp:
+            position: 0
+            length: 32
+            c_type: uint32_t
       MCCommand:
         can_id: 0x0C0 # From manual
         period: 9ms  # MC needs a message at 2 Hz or higher

src/vcu/inc/can_handles.h

@@ -4,6 +4,8 @@
 #include "stdio.h"
 #include "stm32f2xx_hal.h"
 #include "CANlib.h"
+#include <stdbool.h>
+#include <stdint.h>
 
 #include "state.h"
 #include "fault.h"
@@ -41,4 +43,27 @@ void sendSpeedCmdMsg(int16_t speed, int16_t torque_limit);
 void sendMotorOffCmdMsg(void);
 void send_mc_fault_clear(void);
 
+void sendVCUSpeedCntrlKpTimes1000Msg(void);
+void sendVCUSpeedCntrlKiTimes1000Msg(void);
+void sendVCUSpeedCntrlKdTimes1000Msg(void);
+void sendVCUSpeedCntrlIWindupMaxMsg(void);
+void sendVCUSpeedCntrlIWindupMinMsg(void);
+void sendVCUSpeedCntrlMinOutputValueMsg(void);
+void sendVCUSpeedCntrlMaxOutputValueMsg(void);
+void sendVCUSpeedCntrlMinInputValueMsg(void);
+void sendVCUSpeedCntrlMaxInputValueMsg(void);
+void sendVCUSpeedCntrlErrorUpdateTimeoutMsg(void);
+void sendVCUSpeedCntrlDtMsg(void);
+void sendVCUSpeedCntrlEnabledMsg(void);
+void sendVCUSpeedCntrlOutOfInputRangeThrottledMsg(void);
+void sendVCUSpeedCntrlOutOfOutputRangeThrottledMsg(void);
+void sendVCUSpeedCntrlErrorUpdateTimedOutMsg(void);
+void sendVCUSpeedCntrlRPMSetpointMsg(void);
+void sendVCUSpeedCntrlCommandedTorqueMsg(void);
+void sendVCUSpeedCntrlRPMErrorMsg(void);
+void sendVCUSpeedCntrlLastRPMErrorMsg(void);
+void sendVCUSpeedCntrlDerivRPMErrorMsg(void);
+void sendVCUSpeedCntrlRPMErrorAccumulatedMsg(void);
+void sendVCUSpeedCntrlLastErrorUpdateTimestampMsg(void);
+
 #endif // ifndef __CAN_HANDLES_H

src/vcu/inc/controls.h

@@ -9,6 +9,10 @@
 #include "fault_pedalbox.h"
 #include "fault_brakes.h"
 
+// Internal Speed Controller settings for launch control
+#define MAX_INPUT_SPEED 3000
+#define SPEED_CONTROLLER_UPDATE_PERIOD_MS  1
+
 // TODO: Extract MAX_TORQUE from RMS EEPROM (over CAN?)
 #define MAX_TORQUE    2400 // unit: dNm
 #define MAX_ACCEL_VAL 1000

src/vcu/inc/speed_controller.h

@@ -0,0 +1,73 @@
+#ifndef __SPEED_CONTROLLER
+#define __SPEED_CONTROLLER
+
+#include "stm32f2xx_hal.h"
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "can_handles.h"
+
+extern can0_VCUSpeedCntrlKpTimes1000_T kp_times_1000_frame;
+extern can0_VCUSpeedCntrlKiTimes1000_T ki_times_1000_frame;
+extern can0_VCUSpeedCntrlKdTimes1000_T kd_times_1000_frame;
+extern can0_VCUSpeedCntrlIWindupMax_T i_windup_max_frame;
+extern can0_VCUSpeedCntrlIWindupMin_T i_windup_min_frame;
+extern can0_VCUSpeedCntrlMinOutputValue_T min_output_value_frame;
+extern can0_VCUSpeedCntrlMaxOutputValue_T max_output_value_frame;
+extern can0_VCUSpeedCntrlMinInputValue_T min_input_value_frame;
+extern can0_VCUSpeedCntrlMaxInputValue_T max_input_value_frame;
+extern can0_VCUSpeedCntrlErrorUpdateTimeout_T error_update_timeout_frame;
+extern can0_VCUSpeedCntrlDt_T dt_frame;
+extern can0_VCUSpeedCntrlEnabled_T enabled_frame;
+extern can0_VCUSpeedCntrlOutOfInputRangeThrottled_T out_of_input_range_throttled_frame;
+extern can0_VCUSpeedCntrlOutOfOutputRangeThrottled_T out_of_output_range_throttled_frame;
+extern can0_VCUSpeedCntrlErrorUpdateTimedOut_T error_update_timed_out_frame;
+extern can0_VCUSpeedCntrlRPMSetpoint_T rpm_setpoint_frame;
+extern can0_VCUSpeedCntrlCommandedTorque_T commanded_torque_frame;
+extern can0_VCUSpeedCntrlRPMError_T rpm_error_frame;
+extern can0_VCUSpeedCntrlLastRPMError_T last_rpm_error_frame;
+extern can0_VCUSpeedCntrlDerivRPMError_T deriv_rpm_error_frame;
+extern can0_VCUSpeedCntrlRPMErrorAccumulated_T rpm_error_accumulated_frame;
+extern can0_VCUSpeedCntrlLastErrorUpdateTimestamp_T last_error_update_timestamp_frame;
+
+// PUBLIC STRUCTS
+typedef struct {
+	int32_t kp_times_1000;
+    int32_t ki_times_1000;
+    int32_t kd_times_1000;
+    int32_t i_windup_max;
+    int32_t i_windup_min;
+    int32_t min_output_value;
+    int32_t max_output_value;
+    int32_t min_input_value;
+    int32_t max_input_value;
+    int32_t dt;
+   	uint32_t error_update_timeout;
+} SpeedControllerParams;
+
+typedef struct {
+    bool enabled;
+    bool out_of_input_range_throttled;
+    bool out_of_output_range_throttled;
+    bool error_update_timed_out;
+    int32_t rpm_setpoint;
+    int32_t commanded_torque;
+    int32_t rpm_error;
+    int32_t last_rpm_error;
+    int32_t deriv_rpm_error;
+    int32_t rpm_error_accumulated;
+    uint32_t last_error_update_timestamp;
+} SpeedControllerInternalVars;
+
+// PUBLIC FUNCTIONS
+void init_speed_controller_defaults(int32_t max_input_speed, 
+    int32_t max_output_torque, uint32_t update_period_millis);
+void enable_speed_controller(void);
+void disable_speed_controller(void);
+void set_speed_controller_setpoint(int32_t rpm);
+void update_speed_controller_error(int32_t actual_rpm, 
+	uint32_t actual_rpm_msg_timestamp);
+int32_t get_speed_controller_torque_command(void);
+bool get_speed_controller_enabled(void);
+
+#endif // ifndef __SPEED_CONTROLLER

src/vcu/inc/state_vcu_driving.h

@@ -6,6 +6,7 @@
 #include "state_vcu.h"
 #include "can_handles.h"
 #include "controls.h"
+#include "speed_controller.h"
 
 #define AA_SETUP 10
 #define AA_HOLD 10