lights fsm working

libraries/master/src/master_task.c

@@ -47,9 +47,9 @@ void check_late_cycle(BaseType_t delay) {
 }
 
 TASK(master_task, TASK_STACK_512) {
-  TickType_t xLastWakeTime = xTaskGetTickCount();
   uint32_t counter = 1;
   pre_loop_init();
+  TickType_t xLastWakeTime = xTaskGetTickCount();
   while (true) {
     // LOG_DEBUG("counter: %u\n", counter);
 

libraries/ms-common/inc/soft_timer.h

@@ -25,7 +25,15 @@ typedef void (*SoftTimerCallback)(SoftTimerId id);
 // Adds a software timer. The provided duration is the number of
 // miliseconds before running and the callback is the process to run once
 // the time has expired.
-StatusCode soft_timer_start(uint32_t duration_ms, SoftTimerCallback callback, SoftTimer *timer);
+StatusCode soft_timer_init_and_start(uint32_t duration_ms, SoftTimerCallback callback,
+                                     SoftTimer *timer);
+
+// Creates a new software timer with given params (same as soft_timer_init_and_start) but doesn't
+// start the timer
+StatusCode soft_timer_init(uint32_t duration_ms, SoftTimerCallback callback, SoftTimer *timer);
+
+// Starts the software timer. The timer must already be initialized
+StatusCode soft_timer_start(SoftTimer *timer);
 
 // Cancels the soft timer specified by name. Returns true if successful.
 // the timer is not cancelled immediately,

libraries/ms-common/inc/watchdog.h

@@ -7,7 +7,7 @@ typedef SoftTimer WatchDog;
 
 // start watchdog
 #define watchdog_start(watchdog, timeout_ms, callback) \
-  soft_timer_start(timeout_ms, callback, watchdog)
+  soft_timer_init_and_start(timeout_ms, callback, watchdog)
 
 // :)
 #define watchdog_kick(watchdog) soft_timer_reset(watchdog)

libraries/ms-common/src/persist.c

@@ -46,7 +46,7 @@ static void prv_periodic_commit(SoftTimerId timer_id) {
   }
 
   s_context = persist;
-  soft_timer_start(PERSIST_COMMIT_TIMEOUT_MS, prv_periodic_commit, &persist->timer);
+  soft_timer_init_and_start(PERSIST_COMMIT_TIMEOUT_MS, prv_periodic_commit, &persist->timer);
 }
 
 StatusCode persist_init(PersistStorage *persist, FlashPage page, void *blob, size_t blob_size,
@@ -120,14 +120,15 @@ StatusCode persist_init(PersistStorage *persist, FlashPage page, void *blob, siz
     persist->flash_addr += sizeof(header) + header.size_bytes;
   }
   s_context = persist;
-  return soft_timer_start(PERSIST_COMMIT_TIMEOUT_MS, prv_periodic_commit, &persist->timer);
+  return soft_timer_init_and_start(PERSIST_COMMIT_TIMEOUT_MS, prv_periodic_commit, &persist->timer);
 }
 
 StatusCode persist_ctrl_periodic(PersistStorage *persist, bool enabled) {
   if (id == soft_timer_invalid_timer && enabled) {
     // Enable periodic commit - previously disabled
     s_context = persist;
-    return soft_timer_start(PERSIST_COMMIT_TIMEOUT_MS, prv_periodic_commit, &persist->timer);
+    return soft_timer_init_and_start(PERSIST_COMMIT_TIMEOUT_MS, prv_periodic_commit,
+                                     &persist->timer);
   } else if (id != soft_timer_invalid_timer && !enabled) {
     // Disable periodic commit - previously enabled
     soft_timer_cancel(&persist->timer);

libraries/ms-common/src/soft_timer.c

@@ -1,12 +1,25 @@
 #include "soft_timer.h"
 
-StatusCode soft_timer_start(uint32_t duration_ms, SoftTimerCallback callback, SoftTimer *timer) {
+StatusCode soft_timer_init_and_start(uint32_t duration_ms, SoftTimerCallback callback,
+                                     SoftTimer *timer) {
+  status_ok_or_return(soft_timer_init(duration_ms, callback, timer));
+  return soft_timer_start(timer);
+}
+
+StatusCode soft_timer_init(uint32_t duration_ms, SoftTimerCallback callback, SoftTimer *timer) {
   if (timer->id != NULL) {
     // timer already exist/inuse, delete the old timer
     xTimerDelete(timer->id, 0);
   }
   timer->id = xTimerCreateStatic(NULL, pdMS_TO_TICKS(duration_ms), pdFALSE,  //
                                  NULL, callback, &timer->buffer);
+  return STATUS_CODE_OK;
+}
+
+StatusCode soft_timer_start(SoftTimer *timer) {
+  if (timer->id == NULL) {
+    return STATUS_CODE_UNINITIALIZED;
+  }
   if (xTimerStart(timer->id, 0) != pdPASS) {
     return status_msg(STATUS_CODE_INTERNAL_ERROR, "timer command queue is full");
   }

libraries/ms-common/test/test_soft_timer.c

@@ -31,7 +31,7 @@ void test_soft_timer() {
   for (int i = 0; i < 3; ++i) {
     triggered = false;
     // test soft timer start and cancel
-    soft_timer_start(50, prv_set, &s_timer);
+    soft_timer_init_and_start(50, prv_set, &s_timer);
 
     xTaskDelayUntil(&last_wake, 49);
 
@@ -47,7 +47,7 @@ void test_soft_timer() {
     TEST_ASSERT_FALSE(soft_timer_inuse(&s_timer));
 
     // test start to finish
-    soft_timer_start(200, prv_set, &s_timer);
+    soft_timer_init_and_start(200, prv_set, &s_timer);
 
     xTaskDelayUntil(&last_wake, 99);
 
@@ -70,8 +70,8 @@ void test_multiple_timer() {
   for (int i = 0; i < 3; ++i) {
     triggered = false;
     // test soft timer start and cancel
-    soft_timer_start(100, prv_set, &s_timer);
-    soft_timer_start(50, prv_set, &s_timer_2);
+    soft_timer_init_and_start(100, prv_set, &s_timer);
+    soft_timer_init_and_start(50, prv_set, &s_timer_2);
 
     delay_ms(51);
 

libraries/ms-drivers/inc/pca9555_gpio_expander.h

@@ -54,7 +54,7 @@ typedef struct {
 } Pca9555GpioSettings;
 
 // Initialize PCA9555 GPIO at this I2C port and address.
-StatusCode pca9555_gpio_init(const I2CPort i2c_port, const I2CAddress i2c_address);
+StatusCode pca9555_gpio_init(const I2CPort i2c_port);
 
 // Initialize an PCA9555 GPIO pin by address.
 StatusCode pca9555_gpio_init_pin(const Pca9555GpioAddress *address,

libraries/ms-drivers/src/ads1015.c

@@ -66,7 +66,7 @@ static void prv_timer_callback(SoftTimerId id) {
   current_channel = (Ads1015Channel)(__builtin_ffs(storage->pending_channel_bitset) - 1);
   // Update so that the ADS1015 reads from the next channel.
   prv_set_channel(storage, current_channel);
-  soft_timer_start(ADS1015_CHANNEL_UPDATE_PERIOD_US, prv_timer_callback, &s_timer);
+  soft_timer_init_and_start(ADS1015_CHANNEL_UPDATE_PERIOD_US, prv_timer_callback, &s_timer);
 }
 
 // Inits the storage for ADS1015 and starts the soft timer.
@@ -80,7 +80,7 @@ StatusCode ads1015_init(Ads1015Storage *storage, I2CPort i2c_port, Ads1015Addres
     storage->channel_readings[channel] = ADS1015_DISABLED_CHANNEL_READING;
   }
   s_context = storage;
-  return soft_timer_start(ADS1015_CHANNEL_UPDATE_PERIOD_US, prv_timer_callback, &s_timer);
+  return soft_timer_init_and_start(ADS1015_CHANNEL_UPDATE_PERIOD_US, prv_timer_callback, &s_timer);
 }
 
 // Enable/disables a channel, and sets a callback for the channel.

libraries/ms-drivers/src/pca9555_gpio_expander.c

@@ -20,7 +20,7 @@ static uint8_t prv_pin_bit(const Pca9555PinAddress pin) {
   return prv_is_port_0(pin) ? pin : pin - PCA9555_PIN_IO1_0;
 }
 
-StatusCode pca9555_gpio_init(const I2CPort i2c_port, const I2CAddress i2c_address) {
+StatusCode pca9555_gpio_init(const I2CPort i2c_port) {
   if (s_i2c_port == NUM_I2C_PORTS) {
     s_i2c_port = i2c_port;
   }

libraries/ms-drivers/test/test_pca9555_gpio_expander.c

@@ -30,7 +30,7 @@ void setup_test(void) {
   gpio_it_init();
 
   i2c_init(i2c_port, &i2c_settings);
-  pca9555_gpio_init(i2c_port, MOCK_I2C_ADDRESS);
+  pca9555_gpio_init(i2c_port);
 }
 
 void teardown_test(void) {}

projects/bms_carrier/src/current_sense.c

@@ -45,7 +45,7 @@ static void prv_periodic_ads_read(SoftTimerId id) {
   s_current_storage->ring_idx = (s_current_storage->ring_idx + 1) % NUM_STORED_CURRENT_READINGS;
   ads1259_get_conversion_data(&s_ads1259_storage);
   // Kick new soft timer
-  soft_timer_start(s_current_storage->conv_period_ms, prv_periodic_ads_read, &s_timer);
+  soft_timer_init_and_start(s_current_storage->conv_period_ms, prv_periodic_ads_read, &s_timer);
 
   // update average
   int32_t sum = 0;
@@ -92,6 +92,6 @@ StatusCode current_sense_init(CurrentStorage *storage, SpiSettings *settings,
   s_current_storage->conv_period_ms = conv_period_ms;
   status_ok_or_return(ads1259_init(&s_ads1259_storage, &ads_settings));
   ads1259_get_conversion_data(&s_ads1259_storage);
-  soft_timer_start(s_current_storage->conv_period_ms, prv_periodic_ads_read, &s_timer);
+  soft_timer_init_and_start(s_current_storage->conv_period_ms, prv_periodic_ads_read, &s_timer);
   return STATUS_CODE_OK;
 }

projects/centre_console/src/drive_fsm.c

@@ -236,7 +236,7 @@ StatusCode init_drive_fsm(void) {
   // Add gpio register interrupts
   Pca9555GpioSettings pca_settings = { .direction = PCA9555_GPIO_DIR_OUT,
                                        .state = PCA9555_GPIO_STATE_LOW };
-  pca9555_gpio_init(I2C_PORT_1, 0x20);
+  pca9555_gpio_init(I2C_PORT_1);
   for (int i = 0; i < NUM_DRIVE_FSM_BUTTONS; i++) {
     status_ok_or_return(pca9555_gpio_init_pin(&s_drive_btn_leds[i], &pca_settings));
   }

projects/power_distribution/inc/lights_fsm.h

@@ -10,7 +10,6 @@
 #include "task.h"
 
 #define NUM_LIGHTS_STATES 4
-#define NUM_LIGHTS_TRANSITIONS 10
 #define SIGNAL_BLINK_PERIOD_MS 600  // Signal blink frequency of 1.66Hz
 DECLARE_FSM(lights);
 
@@ -33,7 +32,7 @@ typedef enum EELightType {
 } EELightType;
 
 typedef enum HazardStatus {
-  HAZARD_OFF = NUM_EE_LIGHT_TYPES + 1,
+  HAZARD_OFF = 0,
   HAZARD_ON,
 } HazardStatus;
 

projects/power_distribution/src/lights_fsm.c

@@ -1,44 +1,63 @@
 #include "lights_fsm.h"
 
+#include "log.h"
+#include "outputs.h"
 #include "power_distribution_getters.h"
 
 // Placeholder GPIO Address, will be updated
-GpioAddress RIGHT_LIGHT_ADDR = { .port = GPIO_PORT_A, .pin = 11 };
-GpioAddress LEFT_LIGHT_ADDR = { .port = GPIO_PORT_A, .pin = 12 };
+GpioAddress RIGHT_LIGHT_ADDR = { .port = GPIO_PORT_B, .pin = 5 };
+GpioAddress LEFT_LIGHT_ADDR = { .port = GPIO_PORT_A, .pin = 15 };
 
 // Softtimer module setup for light blinkers
 static SoftTimer s_timer_single;
 static EELightType light_id_callback;
 
 FSM(lights, NUM_LIGHTS_STATES);
+static OutputState left_signal_state = OUTPUT_STATE_OFF;
+static OutputState right_signal_state = OUTPUT_STATE_OFF;
+static LightsStateId fsm_prev_state = INIT_STATE;
 
 static void prv_lights_signal_blinker(SoftTimerId id) {
-  if (light_id_callback == EE_LIGHT_TYPE_SIGNAL_LEFT) {
-    gpio_toggle_state(&LEFT_LIGHT_ADDR);
-    soft_timer_start(SIGNAL_BLINK_PERIOD_MS, prv_lights_signal_blinker, &s_timer_single);
-
-  } else if (light_id_callback == EE_LIGHT_TYPE_SIGNAL_RIGHT) {
-    gpio_toggle_state(&RIGHT_LIGHT_ADDR);
-    soft_timer_start(SIGNAL_BLINK_PERIOD_MS, prv_lights_signal_blinker, &s_timer_single);
-
-  } else if (light_id_callback == EE_LIGHT_TYPE_SIGNAL_HAZARD) {
-    GpioState sync_state = gpio_get_state(&LEFT_LIGHT_ADDR, &sync_state);
-    gpio_set_state(&RIGHT_LIGHT_ADDR, sync_state);
-    gpio_toggle_state(&LEFT_LIGHT_ADDR);
-    gpio_toggle_state(&RIGHT_LIGHT_ADDR);
-    soft_timer_start(SIGNAL_BLINK_PERIOD_MS, prv_lights_signal_blinker, &s_timer_single);
-  } else {
-    // in the initial state light_id_callback == NUM_EE_LIGHT_TYPES
-    gpio_set_state(&RIGHT_LIGHT_ADDR, GPIO_STATE_LOW);
-    gpio_set_state(&LEFT_LIGHT_ADDR, GPIO_STATE_LOW);
-    return;
+  switch (light_id_callback) {
+    case EE_LIGHT_TYPE_SIGNAL_LEFT:
+      left_signal_state ^= 1;
+      pd_set_output_group(OUTPUT_GROUP_LEFT_TURN, left_signal_state);
+      if (right_signal_state == OUTPUT_STATE_ON) {
+        right_signal_state = OUTPUT_STATE_OFF;
+        pd_set_output_group(OUTPUT_GROUP_RIGHT_TURN, right_signal_state);
+      }
+      soft_timer_start(&s_timer_single);
+      break;
+    case EE_LIGHT_TYPE_SIGNAL_RIGHT:
+      right_signal_state ^= 1;
+      pd_set_output_group(OUTPUT_GROUP_RIGHT_TURN, right_signal_state);
+      if (left_signal_state == OUTPUT_STATE_ON) {
+        left_signal_state = OUTPUT_STATE_OFF;
+        pd_set_output_group(OUTPUT_GROUP_LEFT_TURN, left_signal_state);
+      }
+      soft_timer_start(&s_timer_single);
+      break;
+    case EE_LIGHT_TYPE_SIGNAL_HAZARD:
+      if (left_signal_state != right_signal_state) {
+        left_signal_state = OUTPUT_STATE_OFF;
+        right_signal_state = OUTPUT_STATE_OFF;
+      }
+      left_signal_state ^= 1;
+      right_signal_state ^= 1;
+      pd_set_output_group(OUTPUT_GROUP_HAZARD, left_signal_state);
+      soft_timer_start(&s_timer_single);
+      break;
+    default:
+      left_signal_state = OUTPUT_STATE_OFF;
+      right_signal_state = OUTPUT_STATE_OFF;
+      pd_set_output_group(OUTPUT_GROUP_HAZARD, left_signal_state);
   }
 }
 
 static void prv_init_state_input(Fsm *fsm, void *context) {
   // can transition to LEFT, RIGHT, HAZARD
-  EELightType light_event = get_steering_info_input_lights();
-  HazardStatus hazard_status = get_power_info_hazard_state();
+  EELightType light_event = get_steering_info_input_lights();  // can msg id = 682 = 0x2AA
+  HazardStatus hazard_status = get_power_info_hazard_state();  // can msg id = 1026 = 0x5E2
 
   if (hazard_status == HAZARD_ON) {
     fsm_transition(fsm, HAZARD);
@@ -52,6 +71,7 @@ static void prv_init_state_input(Fsm *fsm, void *context) {
 static void prv_init_state_output(void *context) {
   LOG_DEBUG("Transitioned to INIT_STATE\n");
   light_id_callback = NUM_EE_LIGHT_TYPES;
+  fsm_prev_state = INIT_STATE;
 }
 
 static void prv_left_signal_input(Fsm *fsm, void *context) {
@@ -72,7 +92,10 @@ static void prv_left_signal_output(void *context) {
   LOG_DEBUG("Transitioned to LEFT_SIGNAL\n");
   // Toggle Left Signal blinkers at 100 BPM -> 0.6s
   light_id_callback = EE_LIGHT_TYPE_SIGNAL_LEFT;
-  soft_timer_start(SIGNAL_BLINK_PERIOD_MS, prv_lights_signal_blinker, &s_timer_single);
+  if (fsm_prev_state == INIT_STATE) {
+    soft_timer_start(&s_timer_single);
+  }
+  fsm_prev_state = LEFT_SIGNAL;
 }
 
 static void prv_right_signal_input(Fsm *fsm, void *context) {
@@ -93,14 +116,25 @@ static void prv_right_signal_output(void *context) {
   LOG_DEBUG("Transitioned to RIGHT_SIGNAL\n");
   // Toggle Right Signal blinkers at 100 BPM -> 0.6 s
   light_id_callback = EE_LIGHT_TYPE_SIGNAL_RIGHT;
-  soft_timer_start(SIGNAL_BLINK_PERIOD_MS, prv_lights_signal_blinker, &s_timer_single);
+  if (fsm_prev_state == INIT_STATE) {
+    soft_timer_start(&s_timer_single);
+  }
+  fsm_prev_state = RIGHT_SIGNAL;
 }
 
 static void prv_hazard_input(Fsm *fsm, void *context) {
   // can transition to INIT, BPS_FAULT
   EELightType light_event = get_steering_info_input_lights();
+  HazardStatus hazard_status = get_power_info_hazard_state();
 
-  if (light_event == EE_LIGHT_TYPE_OFF) {
+  if (hazard_status == HAZARD_ON) {
+    return;
+  }
+  if (light_event == EE_LIGHT_TYPE_SIGNAL_LEFT) {
+    fsm_transition(fsm, LEFT_SIGNAL);
+  } else if (light_event == EE_LIGHT_TYPE_SIGNAL_RIGHT) {
+    fsm_transition(fsm, RIGHT_SIGNAL);
+  } else {
     fsm_transition(fsm, INIT_STATE);
   }
 }
@@ -109,7 +143,10 @@ static void prv_hazard_output(void *context) {
   LOG_DEBUG("Transitioned to HAZARD\n");
   // Toggle Left and Right Signal blinkers at 100 BPM -> 0.6s
   light_id_callback = EE_LIGHT_TYPE_SIGNAL_HAZARD;
-  soft_timer_start(SIGNAL_BLINK_PERIOD_MS, prv_lights_signal_blinker, &s_timer_single);
+  if (fsm_prev_state == INIT_STATE) {
+    soft_timer_start(&s_timer_single);
+  }
+  fsm_prev_state = HAZARD;
 }
 
 // Lights FSM declaration for states and transitions
@@ -125,13 +162,12 @@ static bool s_PD_transition_list[NUM_LIGHTS_STATES][NUM_LIGHTS_STATES] = {
   TRANSITION(INIT_STATE, HAZARD),        TRANSITION(LEFT_SIGNAL, INIT_STATE),
   TRANSITION(LEFT_SIGNAL, HAZARD),       TRANSITION(LEFT_SIGNAL, RIGHT_SIGNAL),
   TRANSITION(RIGHT_SIGNAL, INIT_STATE),  TRANSITION(RIGHT_SIGNAL, HAZARD),
-  TRANSITION(RIGHT_SIGNAL, LEFT_SIGNAL), TRANSITION(HAZARD, INIT_STATE)
+  TRANSITION(RIGHT_SIGNAL, LEFT_SIGNAL), TRANSITION(HAZARD, INIT_STATE),
+  TRANSITION(HAZARD, LEFT_SIGNAL),       TRANSITION(HAZARD, RIGHT_SIGNAL)
 };
 
 StatusCode init_lights(void) {
-  gpio_init_pin(&LEFT_LIGHT_ADDR, GPIO_OUTPUT_OPEN_DRAIN, GPIO_STATE_LOW);
-  gpio_init_pin(&RIGHT_LIGHT_ADDR, GPIO_OUTPUT_OPEN_DRAIN, GPIO_STATE_LOW);
-
+  soft_timer_init(SIGNAL_BLINK_PERIOD_MS, prv_lights_signal_blinker, &s_timer_single);
   fsm_init(lights, s_PD_lights_list, s_PD_transition_list, INIT_STATE, NULL);
   return STATUS_CODE_OK;
 }