added cruise control monitor to centre console (#219)

* added cruise control monitor to centre console

* fixed include headers

* build fixes

* fixed linting error

* addressed review comments

* fix format :D

---------

Co-authored-by: Harsh Patel <[email protected]>
Co-authored-by: Mitchell Ostler <[email protected]>
Co-authored-by: Mitchell Ostler <[email protected]>

libraries/codegen/boards/steering.yaml

@@ -17,6 +17,8 @@ Messages:
         watchdog: 0
       motor_controller:
         watchdog: 0
+      centre_console:
+        watchdog: 0
     signals:
       input_cc:
         length: 8 # (cc_toggle | cc_increade | cc_decrease)

projects/centre_console/inc/cc_monitor.h

@@ -0,0 +1,19 @@
+#pragma once
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "status.h"
+
+#define STEERING_CC_TOGGLE_MASK 0x1
+#define STEERING_CC_INCREASE_SPEED_MASK 0x2
+#define STEERING_CC_DECREASE_SPEED_MASK 0x4
+
+typedef struct LocalState {
+  uint8_t drive_state;
+  bool cc_enabled;
+  bool regen_braking;
+  uint32_t target_velocity;
+} LocalState;
+
+void monitor_cruise_control();

projects/centre_console/src/cc_monitor.c

@@ -0,0 +1,90 @@
+#include "cc_monitor.h"
+
+#include "cc_hw_defs.h"
+#include "centre_console_getters.h"
+#include "centre_console_setters.h"
+#include "drive_fsm.h"
+#include "pca9555_gpio_expander.h"
+
+// Multiplication factor to convert CAN motor velocity (cm/s) into drive output velocity (mm/s)
+#define CONVERT_VELOCITY 5
+
+// Multiplication factor to convert CAN drive output velocity to kph
+#define CONVERT_VELOCITY_TO_KPH 0.0036
+
+#define TEMP_I2C_ADDRESS 0x55
+Pca9555GpioAddress cc_light = { .i2c_address = TEMP_I2C_ADDRESS, .pin = PCA9555_PIN_IO0_0 };
+Pca9555GpioAddress cc_regen_brake_light = { .i2c_address = TEMP_I2C_ADDRESS,
+                                            .pin = PCA9555_PIN_IO0_1 };
+Event TEMP_REGEN_BRAKE_EVENT_PRESSED = 0;
+
+static LocalState state;
+
+void set_local_state() {
+  state.cc_enabled = g_tx_struct.drive_output_cruise_control;
+  state.drive_state = g_tx_struct.drive_output_drive_state;
+  state.target_velocity = g_tx_struct.drive_output_target_velocity;
+  state.regen_braking = g_tx_struct.drive_output_regen_braking;
+}
+
+void update_state() {
+  if (get_pedal_output_brake_output()) {
+    state.cc_enabled = false;
+    return;
+  }
+
+  // Check for regen brake event
+  uint32_t master_task_notif;
+  notify_get(&master_task_notif);
+  if (notify_check_event(&master_task_notif, TEMP_REGEN_BRAKE_EVENT_PRESSED)) {
+    state.regen_braking = !state.regen_braking;
+    if (state.regen_braking) {
+      pca9555_gpio_set_state(&cc_regen_brake_light, PCA9555_GPIO_STATE_LOW);
+    }
+  }
+
+  // Check steering message for cc event (toggle/increase/decrease)
+  uint8_t cc_info = get_steering_info_input_cc();
+
+  if (cc_info & STEERING_CC_TOGGLE_MASK) {
+    if (!state.cc_enabled) {
+      // Store recent speed from MCI as initial cruise control speed
+      unsigned int convert_velocity = CONVERT_VELOCITY;
+      float converted_val =
+          (get_motor_velocity_velocity_l() + get_motor_velocity_velocity_r()) * CONVERT_VELOCITY;
+      state.target_velocity = (unsigned int)converted_val;
+
+      state.cc_enabled = true;
+      pca9555_gpio_set_state(&cc_light, PCA9555_GPIO_STATE_HIGH);
+    } else {
+      state.target_velocity = 0;
+
+      state.cc_enabled = false;
+      pca9555_gpio_set_state(&cc_light, PCA9555_GPIO_STATE_LOW);
+    }
+  } else if (cc_info & STEERING_CC_INCREASE_SPEED_MASK) {
+    state.target_velocity =
+        ((state.target_velocity * CONVERT_VELOCITY_TO_KPH) + 1) / CONVERT_VELOCITY_TO_KPH;
+  } else if (cc_info & STEERING_CC_DECREASE_SPEED_MASK) {
+    state.target_velocity =
+        ((state.target_velocity * CONVERT_VELOCITY_TO_KPH) - 1) / CONVERT_VELOCITY_TO_KPH;
+  }
+}
+
+void update_drive_output() {
+  set_drive_output_drive_state(state.drive_state);
+  set_drive_output_cruise_control(state.cc_enabled);
+  set_drive_output_target_velocity(state.target_velocity);
+  set_drive_output_regen_braking(state.regen_braking);
+}
+
+void monitor_cruise_control() {
+  set_local_state();
+
+  if (state.drive_state != DRIVE) {
+    return;
+  } else {
+    update_state();
+  }
+  update_drive_output();
+}

projects/centre_console/src/main.c

@@ -3,6 +3,7 @@
 #include "can.h"
 #include "can_board_ids.h"
 #include "cc_buttons.h"
+#include "cc_monitor.h"
 #include "delay.h"
 #include "drive_fsm.h"
 #include "fsm.h"
@@ -46,6 +47,7 @@ void run_medium_cycle() {
   wait_tasks(1);
   fsm_run_cycle(drive);
   fsm_run_cycle(power);
+  monitor_cruise_control();
   wait_tasks(2);
   run_can_tx_cycle();
   wait_tasks(1);