Parse CAN messges

AZURE_RTOS/App/app_azure_rtos.c

@@ -67,6 +67,7 @@ TX_THREAD app_thread;
 TX_THREAD ui_thread;
 TX_THREAD vehicle_thread;
 TX_QUEUE gui_button_queue;
+TX_QUEUE vehicle_update_queue;
 /* USER CODE END PV */
 
 /* Private function prototypes -----------------------------------------------*/
@@ -181,10 +182,23 @@ VOID tx_application_define(VOID *first_unused_memory) {
     Error_Handler();
   }
 
+  void *vehicle_update_queue_start = mem;
+  ULONG vehicle_update_msg_size = sizeof(VehicleUpdate) / sizeof(ULONG);
+  if (sizeof(VehicleUpdate) % sizeof(ULONG) != 0) {
+    vehicle_update_msg_size++;
+  }
+  mem += VEHICLE_UPDATE_QUEUE_SIZE * vehicle_update_msg_size;
+  if (tx_queue_create(&vehicle_update_queue, "Vehicle Update Queue",
+                      vehicle_update_msg_size, vehicle_update_queue_start,
+                      VEHICLE_UPDATE_QUEUE_SIZE * vehicle_update_msg_size) !=
+      TX_SUCCESS) {
+    Error_Handler();
+  }
   void *vehicle_thread_stack = mem;
   mem += THREAD_STACK_SIZE;
+  ULONG hfdcan_addr = (ULONG)&hfdcan1;
   if (tx_thread_create(&vehicle_thread, "Vehicle Thread", vehicle_thread_entry,
-                       &hfdcan1, vehicle_thread_stack, THREAD_STACK_SIZE,
+                       hfdcan_addr, vehicle_thread_stack, THREAD_STACK_SIZE,
                        THREAD_PRIO_VEHICLE, THREAD_PRIO_VEHICLE, 0,
                        TX_AUTO_START) != TX_SUCCESS) {
     Error_Handler();

AZURE_RTOS/App/app_azure_rtos.h

@@ -47,6 +47,7 @@ extern "C" {
 #define THREAD_PRIO_UI 6
 #define THREAD_PRIO_VEHICLE 7
 #define UI_QUEUE_SIZE 10
+#define VEHICLE_UPDATE_QUEUE_SIZE 100
 /* USER CODE END EC */
 
 /* Exported macro ------------------------------------------------------------*/

Core/Inc/main.h

@@ -43,6 +43,7 @@ extern "C" {
 /* USER CODE BEGIN EC */
 extern volatile int ltdc_cb_triggered;
 extern TX_QUEUE gui_button_queue;
+extern TX_QUEUE vehicle_update_queue;
 extern FDCAN_HandleTypeDef hfdcan1;
 /* USER CODE END EC */
 

Core/Inc/vehicle.h

@@ -1,21 +1,120 @@
 #ifndef __INC_VEHICLE_H
 #define __INC_VEHICLE_H
 
+#include "stw_defines.h"
 #include "tx_port.h"
 
 #ifdef __cplusplus
 extern "C" {
 #endif
 
+typedef enum {
+  TS_INACTIVE = 0,
+  TS_ACTIVE = 1,
+  TS_PRECHARGE = 2,
+  TS_DISCHARGE = 3,
+  TS_ERROR = 4,
+  TS_CHARGING_CHECK = 5,
+  TS_CHARGING = 6
+} TSState;
+
+typedef enum {
+  AS_OFF = 0,
+  AS_MANUAL = 1,
+  AS_READY = 2,
+  AS_DRIVING = 3,
+  AS_FINISHED = 4,
+  AS_EMERGENCY = 5
+} ASState;
+
+typedef enum {
+  R2D_NONE = 0,
+  R2D_TSMS = 1,
+  R2D_TSACTIVE = 2,
+  R2D_RESETTING_NODES = 3,
+  R2D_RESETTING_COMMS = 4,
+  R2D_WAITING_INIT = 5,
+  R2D_INIT_STAGE1 = 6,
+  R2D_INIT_STAGE2 = 7,
+  R2D_INIT_SUCCESS = 0xF
+} R2DProgress;
+
+typedef enum {
+  INICHK_START = 0,
+  INICHK_WD_CHECK = 1,
+  INICHK_WD_OK = 2,
+  INICHK_ASB_CHECK_1 = 3,
+  INICHK_ASB_CHECK_2 = 4,
+  INICHK_WAIT_TS = 5,
+  INICHK_EBS_CHECK_A = 6,
+  INICHK_EBS_CHECK_B = 7,
+  INICHK_DONE = 8,
+  INICHK_ERROR = 9
+} IniChkState;
+
 typedef struct {
-  int fl;
+  float fl;
-  int fr;
+  float fr;
-  int rl;
+  float rl;
-  int rr;
+  float rr;
 } TireTemps;
 
+typedef struct {
+  TSState ts_state;
+  ASState as_state;
+  Mission active_mission;
+
+  R2DProgress r2d_progress;
+  struct {
+    int invl_ready : 1;
+    int invr_ready : 1;
+    int sdc_bfl : 1;
+    int sdc_brl : 1;
+    int sdc_acc : 1;
+    int sdc_hvb : 1;
+    int err_sdc : 1;
+    int err_ams : 1;
+    int err_pdu : 1;
+    int err_ini_chk : 1;
+    int err_con_mon : 1;
+    int err_scs : 1;
+    int err_sbspd : 1;
+    int err_appsp : 1;
+    int err_as : 1;
+    int err_ros : 1;
+    int err_res : 1;
+    int err_invl : 1;
+    int err_invr : 1;
+  } errors;
+  IniChkState ini_chk_state;
+
+  unsigned lap_count;
+  TireTemps tire_temps;
+
+  float min_cell_volt;
+  float max_cell_temp;
+  int soc;
+
+  float ts_current;
+  float ts_voltage_bat;
+  float ts_voltage_veh;
+} VehicleState;
+
+extern VehicleState vehicle_state;
+
+typedef enum {
+  VEH_UPD_AMS,
+  VEH_UPD_MISSION,
+  VEH_UPD_STATUS,
+  VEH_UPD_TS_CURRENT,
+  VEH_UPD_TS_VOLTAGE_VEH,
+  VEH_UPD_TS_VOLTAGE_BAT
+} VehicleUpdate;
+
 void vehicle_thread_entry(ULONG hfdcan_addr);
 
+void vehicle_select_mission(Mission mission);
+
 #ifdef __cplusplus
 }
 #endif

Core/Src/vehicle.c

@@ -11,9 +11,26 @@
 #include "stm32h7xx_hal_gpio.h"
 #include "tx_api.h"
 
+#define CAN_ID_AMS_STATUS 0xA
+#define CAN_ID_MISSION_SELECTED 0x400
+#define CAN_ID_AS_MISSION_FB 0x410
+#define CAN_ID_STW_STATUS 0x412
+#define CAN_ID_SHUNT_CURRENT 0x521
+#define CAN_ID_SHUNT_VOLTAGE2 0x523
+#define CAN_ID_SHUNT_VOLTAGE3 0x524
+#define CAN_AMS_STATUS_VOLTAGE_FACTOR (1e-3 * 0x100)
+#define CAN_AMS_STATUS_TEMP_FACTOR (0.0625 * 0x10)
+
+VehicleState vehicle_state = {0};
+
 void vehicle_thread_entry(ULONG hfdcan_addr) {
   ftcan_init((void *)hfdcan_addr);
-  ftcan_add_filter(0x123, 0x7FF);
+  ftcan_add_filter(CAN_ID_AMS_STATUS, 0x7FF);
+  ftcan_add_filter(CAN_ID_AS_MISSION_FB, 0x7FF);
+  ftcan_add_filter(CAN_ID_STW_STATUS, 0x7FF);
+  ftcan_add_filter(CAN_ID_SHUNT_CURRENT, 0x7FF);
+  ftcan_add_filter(CAN_ID_SHUNT_VOLTAGE2, 0x7FF);
+  ftcan_add_filter(CAN_ID_SHUNT_VOLTAGE3, 0x7FF);
 
   while (1) {
     uint8_t data[] = {0xFF, 0xEE};
@@ -22,8 +39,63 @@ void vehicle_thread_entry(ULONG hfdcan_addr) {
   }
 }
 
-void ftcan_msg_received_cb(uint16_t id, size_t datalen, const uint8_t *data) {
+void vehicle_select_mission(Mission mission) {
-  if (id == 0x123) {
+  uint8_t mission_int = mission;
-    HAL_GPIO_TogglePin(STATUS2_GPIO_Port, STATUS2_Pin);
+  ftcan_transmit(CAN_ID_MISSION_SELECTED, &mission_int, 1);
-  }
+}
+
+void ftcan_msg_received_cb(uint16_t id, size_t datalen, const uint8_t *data) {
+  VehicleUpdate msg;
+  switch (id) {
+  case CAN_ID_AMS_STATUS:
+    vehicle_state.ts_state = data[0] & 0x7F;
+    vehicle_state.soc = data[1];
+    vehicle_state.min_cell_volt = data[2] * CAN_AMS_STATUS_VOLTAGE_FACTOR;
+    vehicle_state.max_cell_temp = data[3] * CAN_AMS_STATUS_TEMP_FACTOR;
+    msg = VEH_UPD_AMS;
+    break;
+  case CAN_ID_AS_MISSION_FB:
+    vehicle_state.active_mission = data[0] & 0b111;
+    msg = VEH_UPD_MISSION;
+    break;
+  case CAN_ID_STW_STATUS:
+    vehicle_state.as_state = data[0] & 0b111;
+    vehicle_state.r2d_progress = data[0] >> 4;
+    vehicle_state.errors.invl_ready = (data[1] >> 0) & 1;
+    vehicle_state.errors.invr_ready = (data[1] >> 1) & 1;
+    vehicle_state.errors.sdc_bfl = (data[1] >> 2) & 1;
+    vehicle_state.errors.sdc_brl = (data[1] >> 3) & 1;
+    vehicle_state.errors.sdc_acc = (data[1] >> 4) & 1;
+    vehicle_state.errors.sdc_hvb = (data[1] >> 5) & 1;
+    vehicle_state.lap_count = data[2] & 0b111111;
+    vehicle_state.ini_chk_state = data[3];
+    vehicle_state.errors.err_sdc = (data[4] >> 0) & 1;
+    vehicle_state.errors.err_ams = (data[4] >> 1) & 1;
+    vehicle_state.errors.err_pdu = (data[4] >> 2) & 1;
+    vehicle_state.errors.err_ini_chk = (data[4] >> 3) & 1;
+    vehicle_state.errors.err_con_mon = (data[4] >> 4) & 1;
+    vehicle_state.errors.err_scs = (data[4] >> 5) & 1;
+    vehicle_state.errors.err_sbspd = (data[4] >> 6) & 1;
+    vehicle_state.errors.err_appsp = (data[4] >> 7) & 1;
+    vehicle_state.errors.err_as = (data[5] >> 0) & 1;
+    vehicle_state.errors.err_ros = (data[5] >> 1) & 1;
+    vehicle_state.errors.err_res = (data[5] >> 2) & 1;
+    vehicle_state.errors.err_invl = (data[5] >> 3) & 1;
+    vehicle_state.errors.err_invr = (data[5] >> 4) & 1;
+    msg = VEH_UPD_STATUS;
+    break;
+  case CAN_ID_SHUNT_CURRENT:
+    vehicle_state.ts_current = ftcan_unmarshal_signed(&data, 6) * 1e-3;
+    msg = VEH_UPD_TS_CURRENT;
+    break;
+  case CAN_ID_SHUNT_VOLTAGE2:
+    vehicle_state.ts_voltage_veh = ftcan_unmarshal_signed(&data, 6) * 1e-3;
+    msg = VEH_UPD_TS_VOLTAGE_VEH;
+    break;
+  case CAN_ID_SHUNT_VOLTAGE3:
+    vehicle_state.ts_voltage_bat = ftcan_unmarshal_signed(&data, 6) * 1e-3;
+    msg = VEH_UPD_TS_VOLTAGE_BAT;
+    break;
+  }
+  tx_queue_send(&vehicle_update_queue, &msg, TX_NO_WAIT);
 }

TouchGFX/gui/include/gui/ami_screen/AMIPresenter.hpp

@@ -8,29 +8,31 @@ using namespace touchgfx;
 
 class AMIView;
 
-class AMIPresenter : public touchgfx::Presenter, public ModelListener
+class AMIPresenter : public touchgfx::Presenter, public ModelListener {
-{
 public:
-    AMIPresenter(AMIView& v);
+  AMIPresenter(AMIView &v);
 
-    /**
+  /**
-     * The activate function is called automatically when this screen is "switched in"
+   * The activate function is called automatically when this screen is "switched
-     * (ie. made active). Initialization logic can be placed here.
+   * in" (ie. made active). Initialization logic can be placed here.
-     */
+   */
-    virtual void activate();
+  virtual void activate();
 
-    /**
+  /**
-     * The deactivate function is called automatically when this screen is "switched out"
+   * The deactivate function is called automatically when this screen is
-     * (ie. made inactive). Teardown functionality can be placed here.
+   * "switched out" (ie. made inactive). Teardown functionality can be placed
-     */
+   * here.
-    virtual void deactivate();
+   */
+  virtual void deactivate();
 
-    virtual ~AMIPresenter() {};
+  virtual ~AMIPresenter(){};
+
+  void missionUpdated() override;
 
 private:
-    AMIPresenter();
+  AMIPresenter();
 
-    AMIView& view;
+  AMIView &view;
 };
 
 #endif // AMIPRESENTER_HPP

TouchGFX/gui/include/gui/driverview_screen/DriverViewPresenter.hpp

@@ -27,8 +27,6 @@ public:
 
   virtual ~DriverViewPresenter(){};
 
-  void tireTempsUpdated() override;
-
 private:
   DriverViewPresenter();
 

TouchGFX/gui/include/gui/missionselect_screen/MissionSelectPresenter.hpp

@@ -28,7 +28,7 @@ public:
 
   virtual ~MissionSelectPresenter(){};
 
-  void confirmMission(Mission mission);
+  void missionUpdated() override;
 
 private:
   MissionSelectPresenter();

TouchGFX/gui/include/gui/model/Model.hpp

@@ -13,16 +13,13 @@ public:
 
   void tick();
 
-  void setMission(Mission mission) { this->mission = mission; }
+  Mission getSelectedMission() { return selectedMission; }
-  Mission getMission() { return mission; }
-  TireTemps getTireTemps() { return tireTemps; }
 
 protected:
   ModelListener *modelListener;
 
 private:
-  Mission mission;
+  Mission selectedMission;
-  TireTemps tireTemps;
 };
 
 #endif // MODEL_HPP

TouchGFX/gui/include/gui/model/ModelListener.hpp

@@ -13,7 +13,12 @@ public:
 
   void bind(Model *m) { model = m; }
 
-  virtual void tireTempsUpdated(){};
+  virtual void amsUpdated(){};
+  virtual void missionUpdated(){};
+  virtual void statusUpdated(){};
+  virtual void tsCurrentUpdated(){};
+  virtual void tsVoltageVehicleUpdated(){};
+  virtual void tsVoltageBatteryUpdated(){};
 
 protected:
   Model *model;

TouchGFX/gui/src/ami_screen/AMIPresenter.cpp

@@ -2,9 +2,14 @@
 #include <gui/ami_screen/AMIView.hpp>
 
 #include "main.h"
+#include "vehicle.h"
 
 AMIPresenter::AMIPresenter(AMIView &v) : view(v) {}
 
-void AMIPresenter::activate() { view.setMission(model->getMission()); }
+void AMIPresenter::activate() { missionUpdated(); }
 
 void AMIPresenter::deactivate() {}
+
+void AMIPresenter::missionUpdated() {
+  view.setMission(vehicle_state.active_mission);
+}

TouchGFX/gui/src/ami_screen/AMIView.cpp

@@ -37,4 +37,5 @@ void AMIView::setMission(Mission mission) {
     currentMission.setColor(touchgfx::Color::getColorFromRGB(0xFF, 0, 0));
     break;
   }
+  currentMission.invalidate();
 }

TouchGFX/gui/src/driverview_screen/DriverViewPresenter.cpp

@@ -1,13 +1,10 @@
 #include <gui/driverview_screen/DriverViewPresenter.hpp>
 #include <gui/driverview_screen/DriverViewView.hpp>
 
+#include "vehicle.h"
 
 DriverViewPresenter::DriverViewPresenter(DriverViewView &v) : view(v) {}
 
 void DriverViewPresenter::activate() {}
 
 void DriverViewPresenter::deactivate() {}
-
-void DriverViewPresenter::tireTempsUpdated() {
-  view.setTireTemps(model->getTireTemps());
-}

TouchGFX/gui/src/missionselect_screen/MissionSelectPresenter.cpp

@@ -1,3 +1,7 @@
+#include "gui/common/FrontendApplication.hpp"
+#include "stw_defines.h"
+#include "touchgfx/Application.hpp"
+#include "vehicle.h"
 #include <gui/missionselect_screen/MissionSelectPresenter.hpp>
 #include <gui/missionselect_screen/MissionSelectView.hpp>
 
@@ -8,6 +12,12 @@ void MissionSelectPresenter::activate() {}
 
 void MissionSelectPresenter::deactivate() {}
 
-void MissionSelectPresenter::confirmMission(Mission mission) {
+void MissionSelectPresenter::missionUpdated() {
-  model->setMission(mission);
+  FrontendApplication *app =
+      static_cast<FrontendApplication *>(FrontendApplication::getInstance());
+  if (vehicle_state.active_mission == MISSION_MANUAL) {
+    app->gotoDriverViewScreenNoTransition();
+  } else {
+    app->gotoAMIScreenNoTransition();
+  }
 }

TouchGFX/gui/src/missionselect_screen/MissionSelectView.cpp

@@ -1,6 +1,7 @@
 #include "gui/missionselect_screen/MissionSelectPresenter.hpp"
 #include "stw_defines.h"
 #include "texts/TextKeysAndLanguages.hpp"
+#include "vehicle.h"
 #include <gui/missionselect_screen/MissionSelectView.hpp>
 
 MissionSelectView::MissionSelectView()
@@ -50,12 +51,7 @@ void MissionSelectView::confirmMission() {
     // Can't confirm if we haven't selected a mission yet
     return;
   }
-  presenter->confirmMission(selectedMission);
+  vehicle_select_mission(selectedMission);
-  if (selectedMission == MISSION_MANUAL) {
-    application().gotoDriverViewScreenNoTransition();
-  } else {
-    application().gotoAMIScreenNoTransition();
-  }
 }
 
 void MissionSelectView::setSelectedMission(Mission mission) {

TouchGFX/gui/src/model/Model.cpp

@@ -5,38 +5,35 @@
 #include "tx_api.h"
 #include "ui.h"
 #include "vehicle.h"
+
 #include <gui/model/Model.hpp>
 #include <gui/model/ModelListener.hpp>
 
 Model::Model() : modelListener(0) {}
 
 void Model::tick() {
-  static int incs = 0;
+  VehicleUpdate msg;
-  if ((HAL_GetTick() / 10) % 5 == 0) {
+  while (tx_queue_receive(&vehicle_update_queue, &msg, TX_NO_WAIT) ==
-    tireTemps.fl++;
+         TX_SUCCESS) {
-    if (tireTemps.fl > 90) {
+    switch (msg) {
-      tireTemps.fl = 0;
+    case VEH_UPD_AMS:
+      modelListener->amsUpdated();
+      break;
+    case VEH_UPD_MISSION:
+      modelListener->missionUpdated();
+      break;
+    case VEH_UPD_STATUS:
+      modelListener->statusUpdated();
+      break;
+    case VEH_UPD_TS_CURRENT:
+      modelListener->tsCurrentUpdated();
+      break;
+    case VEH_UPD_TS_VOLTAGE_VEH:
+      modelListener->tsVoltageVehicleUpdated();
+      break;
+    case VEH_UPD_TS_VOLTAGE_BAT:
+      modelListener->tsVoltageBatteryUpdated();
+      break;
     }
-    if (incs % 2 == 0) {
-      tireTemps.fr++;
-      if (tireTemps.fr > 90) {
-        tireTemps.fr = 0;
-      }
-    }
-    if (incs % 3 == 0) {
-      tireTemps.rl++;
-      if (tireTemps.rl > 90) {
-        tireTemps.rl = 0;
-      }
-    }
-    if (incs % 4 == 0) {
-      tireTemps.rr++;
-      if (tireTemps.rr > 90) {
-        tireTemps.rr = 0;
-      }
-    }
-    incs++;
-
-    modelListener->tireTempsUpdated();
   }
 }