Merge pull request #33 from KSU-MS/feature/launchcontroller

add launchController and implement launch control capability to testing

.github/workflows/main.yml

@@ -3,7 +3,7 @@ name: PlatformIO CI
 on: [push]
 
 jobs:
-  build-and-upload:
+  test-build-upload:
     runs-on: ubuntu-latest
 
     steps:
@@ -20,8 +20,11 @@ jobs:
       - name: Install PlatformIO Core
         run: pip install --upgrade platformio
 
+      - name: run platformio tests
+        run: pio test -e test_env -v
+
       - name: Build PlatformIO Project
-        run: pio run
+        run: pio run -e teensy41 -v
 
       - name: Upload Build Artifact
         uses: actions/upload-artifact@v3

.gitignore

@@ -3,3 +3,4 @@
 .vscode/c_cpp_properties.json
 .vscode/launch.json
 .vscode/ipch
+.vscode/settings.json

README.md

@@ -5,6 +5,13 @@ the vcu CAN out is still TODO mostly, currently it spits out all RX / TX traffic
 below is a loose flow of the data and what handles what
 
 
+## building
+to build, run ```pio run -e teensy41```
+## Running tests
+
+to build and run native tests on windows, follow these instructions: https://code.visualstudio.com/docs/cpp/config-mingw 
+then, run tests by calling ```pio test -e test_env```
+
 
 ```mermaid
     graph LR;
@@ -91,4 +98,39 @@ flowchart TD
 
     P --> Q(RETURN CALCULATED TORQUE)
 
-```  
+```  
+
+## launch control state diagram
+
+```mermaid
+
+stateDiagram-v2
+
+    rtd : READY_TO_DRIVE
+    [*]-->rtd
+    rtd --> checkButtons
+    checkButtons-->toggle_lc:toggle lc if specific buttons are held
+    state toggle_lc{
+        toggle_get_lc_active-->set_lc_active(1):set to 1 if it was 0
+        toggle_get_lc_active-->set_lc_active(0):set to 0 if it was 1
+        set_lc_active(1)-->[*]
+        set_lc_active(0)-->[*]
+    }
+    toggle_lc -->get_lc_active
+    checkButtons-->get_lc_active:if buttons are not held go to check lc state
+    get_lc_active-->launchState:mcu_status.get_launch_ctrl_active returns 1
+    get_lc_active-->command_torque:mcu_status.get_launch_ctrl_active returns 0
+    state launchState{
+        idle-->waiting:gas pressed, button pressed, and no implaus
+        idle-->[*]
+        waiting-->idle: gas released or implaus
+        waiting-->[*]
+        waiting-->launching: gas still pressed and button released 
+        launching-->finished: brake pressed (cancel early)
+        launching-->finished: time completed
+        launching-->[*]:launching in progress
+        finished-->[*]:launch control done, set launch_ctrl_active to 0
+    }
+    launchState-->command_torque:deactivate LC when finished
+    command_torque-->rtd:return to start of RTD loop
+```

include/FlexCAN_util.hpp

@@ -3,6 +3,7 @@
 #include "FlexCAN_T4.h"
 #include "ksu_ev_can.h"
 #include "KS2eCAN.hpp"
+#include "launch_system.h"
 int unpack_flexcan_message(can_obj_ksu_ev_can_h_t *o,CAN_message_t &msg);
 // global wrapper around flexcan_t4 because it is a shit driver that should feel bad
 int WriteToDaqCAN(CAN_message_t &msg);
@@ -13,6 +14,15 @@ int ReadDaqCAN(CAN_message_t &msg);
 int ReadInverterCAN(CAN_message_t &msg);
 int ReadAccumulatorCAN(CAN_message_t &msg);
 
+template<typename T>
+bool sendStructOnCan(T data, uint32_t id)
+{
+    CAN_message_t msg;
+    msg.id = id;
+    static_assert(sizeof(data) <= sizeof(msg.buf), "Data size exceeds message buffer size");
+    memcpy(msg.buf, &data,sizeof(data));
+    return WriteCANToInverter(msg);
+}
 void InitCAN();
 
 #endif

include/KS2eCAN.hpp

@@ -30,12 +30,17 @@
 #define ID_VCU_FW_VERSION                                           0xC8
 #define ID_VCU_BOARD_ANALOG_READS_ONE                               0xC9
 #define ID_VCU_BOARD_ANALOG_READS_TWO                               0xCA
+#define ID_VCU_BASE_LAUNCH_CONTROLLER_INFO                          0xCB
 #define ID_VCU_PEDAL_TRAVEL                                         0xCC
+#define ID_VCU_LAUNCH_CONTROL_COUNTDOWN                             0xCD
 #define ID_DASH_BUTTONS                                             0xEB
 #define ID_DASH_FW_VERSION                                          0xEC
 #define ID_MC_CURRENT_LIMIT_COMMAND                                 0x202
 #define ID_BMS_SOC                                                  0x6B3 //made this real!
-
+#define ID_VCU_DISTANCE_TRACKER_MOTOR                               0xCE
+#define ID_VCU_DISTANCE_TRACKER_WHEELSPEED                          0xCF
+#define ID_VCU_LIFETIME_DATA                                        0xD0
+#define ID_VCU_TRACTION_CONTROLLER_INFO                             0xD1
 
 #define ID_BMS_CURRENT_LIMIT_INFO     0x6B1
 #define ID_BMS_PACK_VOLTAGE_INFO      0x6B2

include/MCU_status.hpp

@@ -18,7 +18,13 @@ enum class MCU_STATE
 // @Parseclass @Custom(parse_mcu_enums)
 class MCU_status {
 public:
-    MCU_status() = default;
+    MCU_status() : 
+        shutdown_states(0xff), 
+        pedal_states(0), 
+        ecu_states(0), 
+        max_torque(0), 
+        torque_mode(0), 
+        distance_travelled(0) {}
     MCU_status(const uint8_t buf[8]) { load(buf); }
 
     inline void load(const uint8_t buf[])  { memcpy(this, buf, sizeof(*this)); }

include/common_structs.h

@@ -0,0 +1,24 @@
+#ifndef COMMON_STRUCTS_H
+#define COMMON_STRUCTS_H
+#include "parameters.hpp"
+typedef struct wheelSpeeds_s {
+    float fl;
+    float fr;
+    float rl;
+    float rr;
+    wheelSpeeds_s(float fl, float fr, float rl, float rr)
+        : fl(fl), fr(fr), rl(rl), rr(rr){}
+} wheelSpeeds_s;
+
+typedef struct lc_countdown_t
+{
+    unsigned long release_countdown;
+    const unsigned long release_delay = LAUNCHCONTROL_RELEASE_DELAY;
+} lc_countdown_t;
+
+typedef struct time_and_distance_tracker_t
+{
+    unsigned long vcu_lifetime_ontime;
+    unsigned long vcu_lifetime_distance;
+} time_and_distance_tracker_t;
+#endif

include/dashboard.hpp

@@ -4,24 +4,24 @@
 #include <stdint.h>
 
 #pragma pack(push,1)
-
+const int LAUNCH_CONTROL_BUTTON = 5;
 class Dashboard 
 {
 
 public:
     Dashboard() = default;
     inline uint8_t get_buttons()  const {return button_states;}
-
+    // Not Connected
     inline bool get_button1()  const {return (button_states&0x01);} 
-    // Button 2 (yellow button) currently used for regen
+    // Green button
     inline bool get_button2()  const {return (button_states&0x02);}
-    // Button 3 is connected to RTD button as of 3-1-2024
+    // Blue button
     inline bool get_button3()  const {return (button_states&0x04);} 
-    // Unused/not connected
+    // Ready to drive
     inline bool get_button4()  const {return (button_states&0x08);}
-    // Unused/not connected
+    // Red button
     inline bool get_button5()  const {return (button_states&0x10);}
-    // Button 6 (green) used for torque mode
+    // Yellow button
     inline bool get_button6()  const {return (button_states&0x20);}
 
     inline void set_buttons(uint8_t inputs)        { button_states = inputs; }
@@ -35,21 +35,32 @@ class Dashboard
      * @return false 
      */
     bool get_button(uint8_t button) {return (button_states & (0x1 << (button - 1)));}
+    /**
+     * @brief Get the button held duration object
+     * 
+     * @param button 
+     * @param duration_ms 
+     * @return true 
+     * @return false 
+     */
+    bool get_button_held_duration(uint8_t button, unsigned long duration_ms);
+    bool get_button_released_duration(uint8_t button, unsigned long duration_ms);
+
     void updateDashCAN();
     /**
      * @brief Get the button last pressed time object
      * 
      * @param buttonNumber 
-     * @return elapsedMillis 
+     * @return unsigned long 
      */
-    elapsedMillis get_button_last_pressed_time(uint8_t buttonNumber);
-    void set_button_last_pressed_time(uint8_t setpoint,uint8_t buttonNumber);
+    unsigned long get_button_last_pressed_time(uint8_t buttonNumber);
+    void set_button_last_pressed_time(unsigned long setpoint,uint8_t buttonNumber);
     void update_dash(uint8_t input);
     bool new_dash_msg_received;
     float last_received_timestamp = 0;
 private:
     uint8_t button_states;
-    elapsedMillis button_last_pressed_time[6] = {0,0,0,0,0,0};
+    elapsedMillis button_last_pressed_time[6] = {0,0,0,0,0,0}; // Maybe this should be a map :skullemoji:
     void reset_all_button_timers();
 };
 

include/distance_tracker.h

@@ -13,30 +13,32 @@ struct old_new_t
 
 struct energy_data_t
 {
-    int16_t energy_wh;
-    int16_t eff_inst;
-    int16_t distance_m;
-    int16_t efficiency_kmkwh;
-    energy_data_t(int16_t wh, int16_t eff, int16_t m, int16_t kmkwh) : energy_wh(wh), eff_inst(eff), distance_m(m), efficiency_kmkwh(kmkwh) {}
+    uint16_t energy_wh;
+    uint16_t eff_inst;
+    uint16_t distance_m;
+    uint16_t efficiency_kmkwh;
+    energy_data_t(uint16_t wh, uint16_t eff, uint16_t m, uint16_t kmkwh) : energy_wh(wh), eff_inst(eff), distance_m(m), efficiency_kmkwh(kmkwh) {}
 };
 
 struct RollingAverage {
     std::deque<float> buffer;
-    float sum = 0.0;
+    float sum = 0;
     int maxSize;
 
     RollingAverage(int maxBufferSize) : maxSize(maxBufferSize) {}
 
     void addValue(float value) {
         buffer.push_back(value);
-        sum += value;
+        sum = sum + value;
+        // printf("Sum: %f\n",sum);
         if (buffer.size() > maxSize) {
             sum -= buffer.front();
             buffer.pop_front();
         }
     }
 
     float getAverage() const {
+        // printf("%d, %f\n",buffer.empty(),buffer.front());
         return buffer.empty() ? 0.0 : sum / buffer.size();
     }
 };
@@ -52,22 +54,26 @@ class distance_tracker_s
     void update(float amps, float volts, float rpm, float circumference, unsigned long newtime)
     {
         // calc elapsed time
-        unsigned long elapsed_time_us = newtime - this->time;
-        float elapsed_time_seconds = static_cast<float>(elapsed_time_us) / 1000000;
+        unsigned long elapsed_time_ms = newtime - this->time;
+        float elapsed_time_seconds = static_cast<float>(elapsed_time_ms) / 1000;
         // update time
         this->time = newtime;
         // calculate distance travelled during the last update time
-        distance_km += elapsed_time_seconds * velocity_ms.oldval;
-        Serial.println(velocity_ms.oldval);
+        distance_m += elapsed_time_seconds * velocity_ms.oldval;
         // calculate power used during the last update time
-        energy_kwh += elapsed_time_seconds / 3600 * power_kw.oldval;
+        float kwh = (elapsed_time_seconds / 3600) * power_kw.oldval;
+        energy_wh += kwh;
 
         // calculate amp hrs used during the last update time
-        capacity_ah += elapsed_time_seconds / 3600 * current_amps.oldval;
+        capacity_ah += (elapsed_time_seconds / 3600) * current_amps.oldval;
 
         // update efficiencies
-        efficiency_kmkwh = distance_km/energy_kwh;
+        efficiency_kmkwh = distance_m/energy_wh;
         efficiency_instantaneous = (elapsed_time_seconds * velocity_ms.oldval) / (elapsed_time_seconds / 3600 * power_kw.oldval);
+        if (power_kw.oldval <= 0.01)
+        {
+            efficiency_instantaneous = 0;
+        }
         avgEff.addValue(efficiency_instantaneous);
         // set old vals to the previous new one
         power_kw.oldval = power_kw.newval;
@@ -79,25 +85,24 @@ class distance_tracker_s
         // Calculate power
         power_kw.newval = amps * volts;
         // Calculate speed
-        velocity_ms.newval = rpm * circumference;
+        velocity_ms.newval = rpm/60 * circumference;
     }
     energy_data_t get_data()
     {
-        energy_data_t data = energy_data_t(static_cast<int16_t>(energy_kwh*1000), static_cast<int16_t>(avgEff.getAverage()*1000), static_cast<int16_t>(distance_km*1000), static_cast<int16_t>(efficiency_kmkwh*1000));
+        energy_data_t data = energy_data_t(static_cast<uint16_t>(energy_wh*10), static_cast<uint16_t>(avgEff.getAverage()*1000), static_cast<uint16_t>(distance_m), static_cast<uint16_t>(efficiency_kmkwh*1000));
         return data;
     }
-
+    float capacity_ah=0;
+    float energy_wh = 0;
+    float distance_m = 0;
+    float efficiency_kmkwh = 0;
+    float efficiency_instantaneous = 0;
 private:
     old_new_t power_kw;
     old_new_t current_amps;
     old_new_t velocity_ms;
     unsigned long time;
-    float capacity_ah;
-    float energy_kwh = 0;
-    float distance_km = 0;
-    float efficiency_kmkwh = 0;
-    float efficiency_instantaneous = 0;
-    RollingAverage avgEff = RollingAverage(200);
+    RollingAverage avgEff = RollingAverage(100);
 };
 
 #endif