Display steering angle, speed & cones in AMI

Core/Inc/vehicle_state.h

@@ -8,6 +8,8 @@ extern "C" {
 #include "stw_defines.h"
 #include <stdint.h>
 
+#define NUM_CONES 12
+
 typedef enum {
   TS_INACTIVE = 0,
   TS_ACTIVE = 1,
@@ -88,6 +90,11 @@ typedef struct {
   float bat_r;
 } Temperatures;
 
+typedef struct {
+  uint16_t x;
+  uint16_t y;
+} ConePosition;
+
 typedef struct {
   TSState ts_state;
   ASState as_state;
@@ -151,6 +158,14 @@ typedef struct {
   float brake_press_r;
 
   float distance_total;
+
+  uint32_t last_jetson_msg;
+  uint32_t last_epsc_msg;
+  float desired_angle;
+  float measured_angle;
+  float desired_speed;
+
+  ConePosition cone_pos[NUM_CONES];
 } VehicleState;
 
 extern VehicleState vehicle_state;