extended init wait time

lib/interfaces/include/AMSInterface.h

@@ -19,6 +19,7 @@ const float DEFAULT_INIT_VOLTAGE    = 3.5;
 const float DEFAULT_TEMP_ALPHA      = 0.8;
 const float DEFAULT_VOLTAGE_ALPHA   = 0.8;
 const uint16_t MAX_PACK_CHARGE      = 48600;
+const unsigned long DEFAULT_INITIALIZATION_WAIT_INTERVAL = 5000;
 
 
 /// @brief this class is for interfacing with the AMS (accumulator management system) 
@@ -40,7 +41,9 @@ class AMSInterface
         use_em_for_soc_(true),
         charge_(0.0f),
         SoC_(0.0f),
-        has_initialized_charge_(false) {};
+        has_initialized_charge_(false),
+        has_received_bms_voltage_(false),
+        initialization_startup_interval_(DEFAULT_INITIALIZATION_WAIT_INTERVAL) {};
 
     /* Overloaded constructor that only takes in software OK pin and uses default voltages and temp*/
     AMSInterface(CANBufferType *msg_output_queue, int sw_ok_pin):
@@ -240,6 +243,12 @@ class AMSInterface
     */
    bool has_initialized_charge_;
 
+   bool has_received_bms_voltage_;
+
+   unsigned long initialization_startup_interval_;
+
+   unsigned long timestamp_start_;
+
 
     // Check if lowest cell temperature is below threshold
     bool is_below_pack_charge_critical_low_thresh();

lib/interfaces/src/AMSInterface.cpp

@@ -20,6 +20,8 @@ void AMSInterface::init(SysTick_s &initial_tick) {
 
     last_tick_ = initial_tick;
 
+    timestamp_start_ = last_tick_.millis;
+
     // Initializes the bms_voltages_ member variable to an invalid state. This will
     // get overridden once retrieve_voltage_CAN() has been called at least once.
     bms_voltages_.low_voltage_ro = 0xFFFFU;
@@ -116,7 +118,7 @@ void AMSInterface::tick(const SysTick_s &tick) {
 
     // If AMSInterface has a valid reading in bms_voltages_ and the charge is not
     // yet initialized, then call initialize_charge.
-    if (!has_initialized_charge_) {
+    if ((!has_initialized_charge_) && (has_received_bms_voltage_) && ((tick.millis - timestamp_start_) > initialization_startup_interval_)) {
 
         bool bms_voltages_is_invalid = bms_voltages_.low_voltage_ro == 0xFFFFU && bms_voltages_.high_voltage_ro == 0x1111U;
 
@@ -160,6 +162,11 @@ void AMSInterface::retrieve_temp_CAN(CAN_message_t &recvd_msg) {
 
 void AMSInterface::retrieve_voltage_CAN(CAN_message_t &can_msg) {
     Unpack_BMS_VOLTAGES_hytech(&bms_voltages_, can_msg.buf, can_msg.len);
+    if (!has_received_bms_voltage_)
+    {
+        has_received_bms_voltage_ = true;
+    }
+
 }
 
 void AMSInterface::retrieve_em_measurement_CAN(CAN_message_t &can_msg) {