Feat/current steering system first version is done

include/VCF_Constants.h

@@ -8,7 +8,7 @@ namespace VCFInterfaceConstants {
     /* Teensy 4.1 GPIO pins */
 
     /* Not on Schematic
-    // constexpr int BTN_DIM_READ = 28;
+    // constexpr int BTN_DIM_READ = 28; // Currently used for steering system recalibration TODO: change pin
     // constexpr int BTN_PRESET_READ = 31;
     // constexpr int BTN_MODE_READ = 27; // USED TO BE 26.
     */
@@ -93,9 +93,9 @@ namespace VCFInterfaceConstants {
     constexpr float FL_LOADCELL_OFFSET = 0.0;
     constexpr float FR_LOADCELL_SCALE =  1.0; //Values 
     constexpr float FR_LOADCELL_OFFSET = 0.0;
-    
+
     constexpr float FR_SUS_POT_SCALE = 0.01396; // Calibrated for mm between mounting bolts
-    constexpr float FR_SUS_POT_OFFSET = 150.8; // Can change offset once car is on ground to center at zero
+    constexpr float FR_SUS_POT_OFFSET = 150.8;
     constexpr float FL_SUS_POT_SCALE = 0.01396; // Same as FR
     constexpr float FL_SUS_POT_OFFSET = 150.8;
 
@@ -125,7 +125,7 @@ namespace VCFSystemConstants {
     constexpr uint32_t ACCEL_MAX_SENSOR_PEDAL_2 = 4000;
     constexpr float ACCEL_DEADZONE_MARGIN = 0.03f;
     constexpr float ACCEL_MECHANICAL_ACTIVATION_PERCENTAGE = 0.05f;
-    
+
     constexpr uint32_t BRAKE_1_MIN_ADDR = 16;
     constexpr uint32_t BRAKE_2_MIN_ADDR = 20;
     constexpr uint32_t BRAKE_1_MAX_ADDR = 24;
@@ -137,6 +137,30 @@ namespace VCFSystemConstants {
     constexpr uint32_t BRAKE_MAX_SENSOR_PEDAL_2 = 4000;
     constexpr float BRAKE_DEADZONE_MARGIN = 0.04f;
     constexpr float BRAKE_MECHANICAL_ACTIVATION_PERCENTAGE = 0.5f;
+
+    // Steering System Constants
+    constexpr uint32_t MIN_STEERING_SIGNAL_ANALOG_ADDR = 56; //Raw ADC value from analog sensor at minimum (left) steering angle (calibration)
+    constexpr uint32_t MAX_STEERING_SIGNAL_ANALOG_ADDR = 60; //Raw ADC value from analog sensor at maximum (right) steering angle
+    constexpr uint32_t MIN_STEERING_SIGNAL_DIGITAL_ADDR = 32; //Raw ADC value from digital sensor at minimum (left) steering angle
+    constexpr uint32_t MAX_STEERING_SIGNAL_DIGITAL_ADDR = 36; //Raw ADC value from digital sensor at maximum (right) steering angle
+
+    constexpr int32_t ANALOG_MIN_WITH_MARGINS_ADDR = 40;
+    constexpr int32_t ANALOG_MAX_WITH_MARGINS_ADDR = 44;
+    constexpr int32_t DIGITAL_MIN_WITH_MARGINS_ADDR = 48;
+    constexpr int32_t DIGITAL_MAX_WITH_MARGINS_ADDR = 52;
+
+
+    // implausibility values
+    constexpr float ANALOG_TOLERANCE = 0.05f; //+- 0.5% error (analog sensor tolerance according to datasheet)
+    constexpr float DIGITAL_TOLERANCE = 0.05f; // +- 0.2 degrees error
+    constexpr float ERROR_BETWEEN_SENSORS_TOLERANCE = 5.0f;
+
+    // rate of angle change
+    constexpr float MAX_DTHETA_THRESHOLD = 50.0f; //maximum change in angle since last reading to consider the reading valid
+
+    // degrees per bit
+    constexpr float DEG_PER_COUNT_DIGITAL = 360.0f / 16384.0f;
+    constexpr float DEG_PER_COUNT_ANALOG = 360.0f / 3686.4f;
 }
 
 // software configuration constants
@@ -150,45 +174,52 @@ namespace VCFTaskConstants {
     constexpr unsigned long CAN_SEND_PRIORITY = 10;
     constexpr unsigned long CAN_SEND_PERIOD = 2000;               // 2 000 us = 500 Hz
 
-    constexpr unsigned long PEDALS_PRIORITY = 5;
-    constexpr unsigned long PEDALS_SEND_PERIOD = 4000;            // 4 000 us = 250 Hz
-    constexpr unsigned long PEDALS_SAMPLE_PERIOD = 500;           // 500 us = 2 kHz
-
-    constexpr unsigned long PEDALS_RECALIBRATION_PRIORITY = 150;
-    constexpr unsigned long PEDALS_RECALIBRATION_PERIOD = 100000; // 100 000 us = 10 Hz
-
+
     constexpr unsigned long BUZZER_PRIORITY = 20;
     constexpr unsigned long BUZZER_WRITE_PERIOD = 100000;         // 100 000 us = 10 Hz
-
+    
     constexpr unsigned long DASH_SAMPLE_PRIORITY = 21;
     constexpr unsigned long DASH_SAMPLE_PERIOD = 100000;          // 100 000 us = 10 Hz
-
+    
     constexpr unsigned long DASH_SEND_PRIORITY = 7;
     constexpr unsigned long DASH_SEND_PERIOD = 100000;            // 100 000 us = 10 Hz
-
+    
     constexpr unsigned long DEBUG_PRIORITY = 100;
     constexpr unsigned long DEBUG_PERIOD = 10000;                 // 10 000 us = 2 Hz
-
+    
     constexpr unsigned long NEOPIXEL_UPDATE_PRIORITY = 90;
     constexpr unsigned long NEOPIXEL_UPDATE_PERIOD = 100000;      // 100 000 us = 10 Hz
-
-    constexpr unsigned long STEERING_SEND_PRIORITY = 25;
-    constexpr unsigned long STEERING_SEND_PERIOD = 4000;          // 4 000 us = 250 Hz
-
+
+
     constexpr unsigned long LOADCELL_SAMPLE_PRIORITY = 24;
     constexpr unsigned long LOADCELL_SAMPLE_PERIOD = 250;         // 250 us = 4 kHz
-
+    constexpr unsigned long LOADCELL_SEND_PRIORITY = 25;
+    constexpr unsigned long LOADCELL_SEND_PERIOD = 4000;          // 4 000 us = 250 Hz
+
     constexpr unsigned long ETHERNET_SEND_PRIORITY = 20;
     constexpr unsigned long ETHERNET_SEND_PERIOD = 100000;        // 100 000 us = 10Hz
+
+    constexpr unsigned long PEDALS_PRIORITY = 5;
+    constexpr unsigned long PEDALS_SEND_PERIOD = 4000;            // 4 000 us = 250 Hz
+    constexpr unsigned long PEDALS_SAMPLE_PERIOD = 500;           // 500 us = 2 kHz
+
+    constexpr unsigned long PEDALS_RECALIBRATION_PRIORITY = 150;
+    constexpr unsigned long PEDALS_RECALIBRATION_PERIOD = 100000; // 100 000 us = 10 Hz
 
-    constexpr unsigned long LOADCELL_SEND_PRIORITY = 25;
-    constexpr unsigned long LOADCELL_SEND_PERIOD = 4000;          // 4 000 us = 250 Hz
+    constexpr unsigned long STEERING_SEND_PRIORITY = 25;
+    constexpr unsigned long STEERING_SEND_PERIOD = 4000;          // 4 000 us = 250 Hz
 
+    constexpr unsigned long STEERING_SAMPLE_PERIOD = 1000;         // 2000 us = 500 Hz
+    constexpr unsigned long STEERING_SAMPLE_PRIORITY = 10;
+
+    constexpr unsigned long STEERING_RECALIBRATION_PRIORITY = 150;
+    constexpr unsigned long STEERING_RECALIBRATION_PERIOD = 100000;
+
     // IIR filter alphas
     constexpr float LOADCELL_IIR_FILTER_ALPHA = 0.01f;
 
     constexpr unsigned long WATCHDOG_PRIORITY = 1;
     constexpr unsigned long WATCHDOG_KICK_PERIOD = 10000;          // 10 000 us = 100 Hz
 }
 
-#endif /* VCF_CONSTANTS */
+#endif /* VCF_CONSTANTS */

include/VCF_Tasks.h

@@ -34,6 +34,7 @@
 #include "VCFCANInterfaceImpl.h"
 #include "VCFEthernetInterface.h"
 #include "PedalsSystem.h"
+#include "SteeringSystem.h"
 #include "ht_sched.hpp"
 #include "ht_task.hpp"
 #include "BuzzerController.h"
@@ -42,6 +43,7 @@
 #include "WatchdogSystem.h"
 #include "ADCInterface.h"
 #include "CANInterface.h"
+#include "SteeringEncoderInterface.h"
 
 /**
  * The read_adc0 task will command the ADCInterface to sample, convert, and store 
@@ -60,6 +62,7 @@ HT_TASK::TaskResponse run_kick_watchdog(const unsigned long& sysMicros, const HT
 
 HT_TASK::TaskResponse update_pedals_calibration_task(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo);
 
+HT_TASK::TaskResponse update_steering_calibration_task(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo);
 /**
  * The buzzer_control task will control the buzzer control pin. This function
  * relies on the buzzer_control pin definition in VCF_Constants.h;
@@ -77,20 +80,9 @@ HT_TASK::TaskResponse run_buzzer_control_task(const unsigned long& sysMicros, co
 HT_TASK::TaskResponse init_handle_send_vcf_ethernet_data(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo);
 HT_TASK::TaskResponse run_handle_send_vcf_ethernet_data(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo);
 
-/**
- * The handle_receive_VCR_ethernet_data task will receive a protobuf message
- * from VCR. This function relies on the VCF (receiving) socket and vcf_data
- * defined in VCFGlobals.h, and Ethernet constants defined in
- * EthernetAddressDefs.h.
- *
- */
-HT_TASK::TaskResponse init_handle_receive_vcr_ethernet_data(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo);
-HT_TASK::TaskResponse run_handle_receive_vcr_ethernet_data(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo);
-
 // this task attempts to send any data that is enqueued at 250hz. this will be the max rate that you can send over the CAN bus.
 // you dont have to enqeue at this rate, but this allows us to have 2 layers of rate limiting on CAN sending
 HT_TASK::TaskResponse handle_CAN_send(const unsigned long &sysMicros, const HT_TASK::TaskInfo &taskInfo); // NOLINT (capitalization of CAN)
-HT_TASK::TaskResponse handle_CAN_receive(const unsigned long& sysMicros, const HT_TASK::TaskInfo& taskInfo); // NOLINT (capitalization of CAN)
 
 HT_TASK::TaskResponse run_dash_GPIOs_task(const unsigned long& sys_micros, const HT_TASK::TaskInfo& task_info); // NOLINT (capitalization of GPIOs)
 HT_TASK::TaskResponse send_dash_data(const unsigned long &sysMicros, const HT_TASK::TaskInfo &taskInfo);

lib/interfaces/include/NeopixelController.h

@@ -18,7 +18,6 @@
 #include "etl/singleton.h"
 #include "VCFCANInterfaceImpl.h"
 
-
 struct MinCellMonitoringThresholds_s
 {
     float max_level = 3.85;

lib/interfaces/include/SteeringEncoderInterface.h

@@ -1,32 +1,6 @@
 #ifndef STEERING_ENCODER_INTERFACE_H
 #define STEERING_ENCODER_INTERFACE_H
-
-#include <Arduino.h>
-
-enum class SteeringEncoderStatus_e
-{
-    NOMINAL = 0,
-    ERROR = 1,
-};
-
-/// @brief Error and warning flags from the encoder.
-/// @note Specific encoders may populate only a subset of these flags.
-struct EncoderErrorFlags_s
-{
-    bool dataInvalid              = false;
-    bool operatingLimit           = false;
-    bool noData                   = false;
-};
-
-/// @brief Complete steering angle measurement with status and error flags.
-/// @note This struct is transmitted across the CAN bus to other vehicle systems.
-struct SteeringEncoderReading_s
-{
-    float angle = 0.0f;
-    int rawValue = 0;
-    SteeringEncoderStatus_e status = SteeringEncoderStatus_e::NOMINAL;
-    EncoderErrorFlags_s errors;
-};
+#include "SharedFirmwareTypes.h"
 
 class SteeringEncoderInterface
 {
@@ -40,4 +14,4 @@ class SteeringEncoderInterface
     virtual SteeringEncoderReading_s getLastReading() = 0;
 };
 
-#endif /* STEERING_ENCODER_INTERFACE_H */
+#endif /* STEERING_ENCODER_INTERFACE_H */

lib/interfaces/include/VCFEthernetInterface.h

@@ -7,6 +7,7 @@
 #include "ADCInterface.h"
 #include "DashboardInterface.h"
 #include "PedalsSystem.h"
+#include "SteeringSystem.h"
 
 namespace VCFEthernetInterface 
 {
@@ -17,7 +18,7 @@ namespace VCFEthernetInterface
      * @return A populated instance of the outgoing protoc struct.
      */
     // hytech_msgs_VCFData_s make_vcf_data_msg(VCFData_s &shared_state);
-    hytech_msgs_VCFData_s make_vcf_data_msg(ADCInterface &ADCInterfaceInstance, DashboardInterface &dashInstance, PedalsSystem &pedalsInstance);
+    hytech_msgs_VCFData_s make_vcf_data_msg(ADCInterface &ADCInterfaceInstance, DashboardInterface &dashInstance, PedalsSystem &pedalsInstance, SteeringSystem &steeringInstance);
 
     /**
      * Function to take a populated protoc struct from VCR and update the VCF state. This is ONLY critical

lib/interfaces/include/VCRInterface.h

@@ -8,15 +8,13 @@
 #include "hytech.h"
 #include "FlexCAN_T4.h"
 
-
 struct InverterErrorFlags_s{
 
     veh_vec<bool> error;
 
 };
 
 struct InverterBusVolts_s{
-
     veh_vec<int> voltage;
 };
 
@@ -26,11 +24,15 @@ class VCRInterface
 
         bool is_in_pedals_calibration_state() {return _is_in_pedals_calibration_state;}
         TorqueLimit_e get_torque_limit_mode() {return _torque_limit;}
+
+        bool is_in_steering_calibration_state() {return _is_in_steering_calibration_state;} //steering and pedals calibration states are the same, so we can use the same variable for both
 
         void receive_dash_control_data(const CAN_message_t &can_msg);
 
         void disable_calibration_state() {_is_in_pedals_calibration_state = false;}
 
+        void disable_steering_calibration_state() {_is_in_steering_calibration_state = false;}
+
         void receive_car_states_data(const CAN_message_t &can_msg);
 
         void receive_inverter_status_1(const CAN_message_t &can_msg);
@@ -45,22 +47,22 @@ class VCRInterface
         VehicleState_e get_vehicle_state() {return _vehicle_state_value;}
         DrivetrainState_e get_drivetrain_state() {return _drivetrain_state_value;}
         InverterBusVolts_s get_dc_bus_voltage() {return _bus_voltages;}
-
         bool get_db_in_ctrl() {return _is_db_in_ctrl;}
         bool get_inverter_error();
 
     private: 
 
         bool _is_in_pedals_calibration_state = false;
+        bool _is_in_steering_calibration_state = false;
         VehicleState_e _vehicle_state_value;
         DrivetrainState_e _drivetrain_state_value;
         bool _is_db_in_ctrl;
         TorqueLimit_e _torque_limit = TorqueLimit_e::TCMUX_LOW_TORQUE;
         InverterErrorFlags_s _inv_error_status; //creates object that reflects the inverter error status...the object 
         //holds the error flags for each inverter, the getter above returns True if there's an error in any of the 4
-        InverterBusVolts_s _bus_voltages;        
+        InverterBusVolts_s _bus_voltages;
 };
 
 using VCRInterfaceInstance = etl::singleton<VCRInterface>;
 
-#endif /* VCR_INTERFACE_H */
+#endif /* VCR_INTERFACE_H */

lib/interfaces/src/ACUInterface.cpp

@@ -4,6 +4,5 @@ void ACUInterface::receive_ACU_voltages(const CAN_message_t &can_msg)
 {
     BMS_VOLTAGES_t unpacked_msg;
     Unpack_BMS_VOLTAGES_hytech(&unpacked_msg, can_msg.buf, can_msg.len); // NOLINT (implicitly decay pointer)
-
     _min_cell_voltage = HYTECH_min_cell_voltage_ro_fromS(unpacked_msg.min_cell_voltage_ro);
 }

lib/interfaces/src/ADCInterface.cpp

@@ -172,4 +172,4 @@ AnalogConversion_s ADCInterface::get_brake_pressure_front() {
 
 AnalogConversion_s ADCInterface::get_brake_pressure_rear() {
     return _adc1.data.conversions[_adc_parameters.channels.brake_pressure_rear_channel];
-}
+}

lib/interfaces/src/NeopixelController.cpp

@@ -36,7 +36,7 @@ void NeopixelController::set_neopixel(uint16_t id, uint32_t c)
 void NeopixelController::refresh_neopixels(const PedalsSystemData_s &pedals_data, CANInterfaces &interfaces) 
 {
     // If we are in pedals recalibration state, LIGHT UP DASHBOARD ALL RED.
-    if (interfaces.vcr_interface.is_in_pedals_calibration_state()) {
+    if (interfaces.vcr_interface.is_in_pedals_calibration_state() || interfaces.vcr_interface.is_in_steering_calibration_state()) {
         set_neopixel_color(LED_ID_e::BRAKE, LED_color_e::RED);
         set_neopixel_color(LED_ID_e::TORQUE_MODE, LED_color_e::RED);
         set_neopixel_color(LED_ID_e::LATCH, LED_color_e::RED);
@@ -136,10 +136,11 @@ void NeopixelController::refresh_neopixels(const PedalsSystemData_s &pedals_data
                       interfaces.vcr_interface.get_dc_bus_voltage().voltage.RL > _hv_threshold_voltage ||
                       interfaces.vcr_interface.get_dc_bus_voltage().voltage.RR > _hv_threshold_voltage;
 
+
     constexpr float glv_critical_voltage = 22.0f;
 
     /* SHUTDOWN LEDS */
-    set_neopixel_color(LED_ID_e::LATCH, hv_present ? LED_color_e::PURPLE : LED_color_e::GREEN); // Unused for now
+    set_neopixel_color(LED_ID_e::LATCH, hv_present ? LED_color_e::PURPLE : LED_color_e::GREEN);
     set_neopixel_color(LED_ID_e::IMD, interfaces.dash_interface.imd_ok ? LED_color_e::GREEN : LED_color_e::RED);
     set_neopixel_color(LED_ID_e::BMS, interfaces.dash_interface.bms_ok ? LED_color_e::GREEN : LED_color_e::RED);
     set_neopixel_color(LED_ID_e::SHUTDOWN, LED_color_e::OFF); // Unused for now

lib/interfaces/src/VCFEthernetInterface.cpp

@@ -4,7 +4,7 @@
 #include "hytech_msgs_version.h"
 #include "device_fw_version.h"
 
-hytech_msgs_VCFData_s VCFEthernetInterface::make_vcf_data_msg(ADCInterface &ADCInterfaceInstance, DashboardInterface &dashInstance, PedalsSystem &pedalsInstance)
+hytech_msgs_VCFData_s VCFEthernetInterface::make_vcf_data_msg(ADCInterface &ADCInterfaceInstance, DashboardInterface &dashInstance, PedalsSystem &pedalsInstance, SteeringSystem &steeringInstance)
 {
 	hytech_msgs_VCFData_s out;
 
@@ -14,6 +14,7 @@ hytech_msgs_VCFData_s VCFEthernetInterface::make_vcf_data_msg(ADCInterface &ADCI
     out.has_front_suspot_data = true;
     out.has_pedals_system_data = true;
     out.has_steering_data = true;
+    out.has_steering_system_data = true;
     out.has_vcf_ethernet_link_data = true;
     out.has_vcf_shutdown_data = true;
     out.has_brake_pressure_data = true;
@@ -30,6 +31,22 @@ hytech_msgs_VCFData_s VCFEthernetInterface::make_vcf_data_msg(ADCInterface &ADCI
     out.steering_data.analog_steering_degrees = ADCInterfaceInstance.get_steering_degrees_cw().conversion;
     out.steering_data.digital_steering_analog = ADCInterfaceInstance.get_steering_degrees_ccw().conversion;
 
+    //SteeringSystem
+    out.steering_system_data.analog_raw = steeringInstance.get_steering_system_data().analog_raw;
+    out.steering_system_data.digital_raw = steeringInstance.get_steering_system_data().digital_raw;
+    out.steering_system_data.analog_steering_angle = steeringInstance.get_steering_system_data().analog_steering_angle;
+    out.steering_system_data.digital_steering_angle = steeringInstance.get_steering_system_data().digital_steering_angle;
+    out.steering_system_data.output_steering_angle = steeringInstance.get_steering_system_data().output_steering_angle;
+    out.steering_system_data.analog_steering_velocity_deg_s = steeringInstance.get_steering_system_data().analog_steering_velocity_deg_s;
+    out.steering_system_data.digital_steering_velocity_deg_s = steeringInstance.get_steering_system_data().digital_steering_velocity_deg_s;
+    out.steering_system_data.digital_oor_implausibility = steeringInstance.get_steering_system_data().digital_oor_implausibility;
+    out.steering_system_data.analog_oor_implausibility = steeringInstance.get_steering_system_data().analog_oor_implausibility;
+    out.steering_system_data.sensor_disagreement_implausibility = steeringInstance.get_steering_system_data().sensor_disagreement_implausibility;
+    out.steering_system_data.dtheta_exceeded_analog = steeringInstance.get_steering_system_data().dtheta_exceeded_analog;
+    out.steering_system_data.dtheta_exceeded_digital = steeringInstance.get_steering_system_data().dtheta_exceeded_digital;
+    out.steering_system_data.both_sensors_fail = steeringInstance.get_steering_system_data().both_sensors_fail;
+    out.steering_system_data.interface_sensor_error = steeringInstance.get_steering_system_data().interface_sensor_error;
+
     //TODO: MODIFY ETH STRUCT
     // Dash
     out.dash_input_state.dim_btn_is_pressed = dashInstance.get_dashboard_outputs().brightness_ctrl_btn_is_pressed;