From 9394cc0892f6d3546f7180cdba35f6c21d31d66b Mon Sep 17 00:00:00 2001 From: Austin Morlan Date: Thu, 5 Oct 2017 14:49:27 -0700 Subject: [PATCH 01/20] Add Kia Soul Niro files --- api/OsccConfig.cmake | 2 + .../can_protocols/brake_can_protocol.h | 2 +- api/include/vehicles.h | 2 + api/include/vehicles/kia_soul_niro.dbc | 109 +++++ api/include/vehicles/kia_soul_niro.h | 420 ++++++++++++++++++ api/src/oscc.c | 2 +- firmware/CMakeLists.txt | 8 +- firmware/brake/CMakeLists.txt | 4 +- .../CMakeLists.txt | 0 .../include/brake_control.h | 0 .../include/communications.h | 0 .../include/globals.h | 0 .../include/init.h | 0 .../include/timers.h | 0 .../src/brake_control.cpp | 0 .../src/communications.cpp | 0 .../src/globals.cpp | 0 .../src/init.cpp | 0 .../src/main.cpp | 0 .../src/timers.cpp | 0 .../tests/CMakeLists.txt | 0 .../tests/features/checking_faults.feature | 0 .../tests/features/receiving_messages.feature | 0 .../tests/features/sending_reports.feature | 0 .../step_definitions/checking_faults.cpp | 0 .../features/step_definitions/common.cpp | 0 .../features/step_definitions/cucumber.wire | 0 .../step_definitions/receiving_messages.cpp | 0 .../step_definitions/sending_reports.cpp | 0 .../tests/features/step_definitions/test.cpp | 0 .../tests/features/support/env.rb | 0 .../tests/property/Cargo.toml | 0 .../tests/property/build.rs | 0 .../tests/property/include/wrapper.hpp | 0 .../tests/property/src/tests.rs | 0 35 files changed, 544 insertions(+), 5 deletions(-) create mode 100644 api/include/vehicles/kia_soul_niro.dbc create mode 100644 api/include/vehicles/kia_soul_niro.h rename firmware/brake/{kia_soul_ev => kia_soul_ev_niro}/CMakeLists.txt (100%) rename firmware/brake/{kia_soul_ev => kia_soul_ev_niro}/include/brake_control.h (100%) rename firmware/brake/{kia_soul_ev => kia_soul_ev_niro}/include/communications.h (100%) rename firmware/brake/{kia_soul_ev => kia_soul_ev_niro}/include/globals.h (100%) rename firmware/brake/{kia_soul_ev => kia_soul_ev_niro}/include/init.h (100%) rename firmware/brake/{kia_soul_ev => kia_soul_ev_niro}/include/timers.h (100%) rename firmware/brake/{kia_soul_ev => kia_soul_ev_niro}/src/brake_control.cpp (100%) rename firmware/brake/{kia_soul_ev => kia_soul_ev_niro}/src/communications.cpp (100%) rename firmware/brake/{kia_soul_ev => kia_soul_ev_niro}/src/globals.cpp (100%) rename firmware/brake/{kia_soul_ev => kia_soul_ev_niro}/src/init.cpp (100%) rename firmware/brake/{kia_soul_ev => kia_soul_ev_niro}/src/main.cpp (100%) rename firmware/brake/{kia_soul_ev => kia_soul_ev_niro}/src/timers.cpp (100%) rename firmware/brake/{kia_soul_ev => kia_soul_ev_niro}/tests/CMakeLists.txt (100%) rename firmware/brake/{kia_soul_ev => kia_soul_ev_niro}/tests/features/checking_faults.feature (100%) rename firmware/brake/{kia_soul_ev => kia_soul_ev_niro}/tests/features/receiving_messages.feature (100%) rename firmware/brake/{kia_soul_ev => kia_soul_ev_niro}/tests/features/sending_reports.feature (100%) rename firmware/brake/{kia_soul_ev => kia_soul_ev_niro}/tests/features/step_definitions/checking_faults.cpp (100%) rename firmware/brake/{kia_soul_ev => kia_soul_ev_niro}/tests/features/step_definitions/common.cpp (100%) rename firmware/brake/{kia_soul_ev => kia_soul_ev_niro}/tests/features/step_definitions/cucumber.wire (100%) rename firmware/brake/{kia_soul_ev => kia_soul_ev_niro}/tests/features/step_definitions/receiving_messages.cpp (100%) rename firmware/brake/{kia_soul_ev => kia_soul_ev_niro}/tests/features/step_definitions/sending_reports.cpp (100%) rename firmware/brake/{kia_soul_ev => kia_soul_ev_niro}/tests/features/step_definitions/test.cpp (100%) rename firmware/brake/{kia_soul_ev => kia_soul_ev_niro}/tests/features/support/env.rb (100%) rename firmware/brake/{kia_soul_ev => kia_soul_ev_niro}/tests/property/Cargo.toml (100%) rename firmware/brake/{kia_soul_ev => kia_soul_ev_niro}/tests/property/build.rs (100%) rename firmware/brake/{kia_soul_ev => kia_soul_ev_niro}/tests/property/include/wrapper.hpp (100%) rename firmware/brake/{kia_soul_ev => kia_soul_ev_niro}/tests/property/src/tests.rs (100%) diff --git a/api/OsccConfig.cmake b/api/OsccConfig.cmake index 061fb1d1..fa9a0283 100644 --- a/api/OsccConfig.cmake +++ b/api/OsccConfig.cmake @@ -2,6 +2,8 @@ if(KIA_SOUL) add_definitions(-DKIA_SOUL) elseif(KIA_SOUL_EV) add_definitions(-DKIA_SOUL_EV) +elseif(KIA_SOUL_NIRO) + add_definitions(-DKIA_SOUL_NIRO) else() message(FATAL_ERROR "No platform selected") endif() diff --git a/api/include/can_protocols/brake_can_protocol.h b/api/include/can_protocols/brake_can_protocol.h index 78815aa2..57bc6010 100644 --- a/api/include/can_protocols/brake_can_protocol.h +++ b/api/include/can_protocols/brake_can_protocol.h @@ -113,7 +113,7 @@ typedef struct uint16_t pedal_command; /*!< Pedal command. [65535 == 100%] */ uint8_t reserved[4]; /*!< Reserved. */ -#elif defined(KIA_SOUL_EV) +#elif defined(KIA_SOUL_EV) || defined(KIA_SOUL_NIRO) uint16_t spoof_value_low; /*!< Value to be sent on the low spoof signal. */ uint16_t spoof_value_high; /*!< Value to be sent on the high spoof signal. */ diff --git a/api/include/vehicles.h b/api/include/vehicles.h index 32439604..b7ea9b2c 100644 --- a/api/include/vehicles.h +++ b/api/include/vehicles.h @@ -13,6 +13,8 @@ #include "vehicles/kia_soul_petrol.h" #elif defined(KIA_SOUL_EV) #include "vehicles/kia_soul_ev.h" +#elif defined(KIA_SOUL_NIRO) +#include "vehicles/kia_soul_niro.h" #endif diff --git a/api/include/vehicles/kia_soul_niro.dbc b/api/include/vehicles/kia_soul_niro.dbc new file mode 100644 index 00000000..87304cf1 --- /dev/null +++ b/api/include/vehicles/kia_soul_niro.dbc @@ -0,0 +1,109 @@ +VERSION "" + +NS_ : + BA_ + BA_DEF_ + BA_DEF_DEF_ + BA_DEF_DEF_REL_ + BA_DEF_REL_ + BA_DEF_SGTYPE_ + BA_REL_ + BA_SGTYPE_ + BO_TX_BU_ + BU_BO_REL_ + BU_EV_REL_ + BU_SG_REL_ + CAT_ + CAT_DEF_ + CM_ + ENVVAR_DATA_ + EV_DATA_ + FILTER + NS_DESC_ + SGTYPE_ + SGTYPE_VAL_ + SG_MUL_VAL_ + SIGTYPE_VALTYPE_ + SIG_GROUP_ + SIG_TYPE_REF_ + SIG_VALTYPE_ + VAL_ + VAL_TABLE_ + +BS_: + +BU_: BRAKE STEERING THROTTLE FAULT + +BO_ 80 BRAKE_ENABLE: 8 BRAKE + SG_ BRAKE_ENABLE_magic : 0|16@1+ (1,0) [0|0] "" BRAKE + SG_ BRAKE_ENABLE_reserved : 16|48@1+ (1,0) [0|0] "" BRAKE + +BO_ 81 BRAKE_DISABLE: 8 BRAKE + SG_ BRAKE_DISABLE_magic : 0|16@1+ (1,0) [0|0] "" BRAKE + SG_ BRAKE_DISABLE_reserved : 16|48@1+ (1,0) [0|0] "" BRAKE + +BO_ 96 BRAKE_COMMAND: 8 BRAKE + SG_ BRAKE_COMMAND_magic : 0|16@1+ (1,0) [0|0] "" BRAKE + SG_ BRAKE_COMMAND_spoof_low : 16|16@1+ (1,0) [0|0] "" BRAKE + SG_ BRAKE_COMMAND_spoof_high : 32|16@1+ (1,0) [0|0] "" BRAKE + SG_ BRAKE_COMMAND_reserved : 48|16@1+ (1,0) [0|0] "" BRAKE + +BO_ 97 BRAKE_REPORT: 8 BRAKE + SG_ BRAKE_REPORT_magic : 0|16@1+ (1,0) [0|0] "" BRAKE + SG_ BRAKE_REPORT_enabled : 16|8@1+ (1,0) [0|0] "" BRAKE + SG_ BRAKE_REPORT_operator_override : 24|8@1+ (1,0) [0|0] "" BRAKE + SG_ BRAKE_REPORT_dtcs : 32|8@1+ (1,0) [0|0] "" BRAKE + SG_ BRAKE_REPORT_reserved : 40|24@1+ (1,0) [0|0] "" BRAKE + +BO_ 84 STEERING_ENABLE: 8 STEERING + SG_ STEERING_ENABLE_magic : 0|16@1+ (1,0) [0|0] "" STEERING + SG_ STEERING_ENABLE_reserved : 16|48@1+ (1,0) [0|0] "" STEERING + +BO_ 85 STEERING_DISABLE: 8 STEERING + SG_ STEERING_DISABLE_magic : 0|16@1+ (1,0) [0|0] "" STEERING + SG_ STEERING_DISABLE_reserved : 16|48@1+ (1,0) [0|0] "" STEERING + +BO_ 100 STEERING_COMMAND: 8 STEERING + SG_ STEERING_COMMAND_magic : 0|16@1+ (1,0) [0|0] "" STEERING + SG_ STEERING_COMMAND_spoof_low : 16|16@1+ (1,0) [0|0] "" STEERING + SG_ STEERING_COMMAND_spoof_high : 32|16@1+ (1,0) [0|0] "" STEERING + SG_ STEERING_COMMAND_reserved : 48|16@1+ (1,0) [0|0] "" STEERING + +BO_ 101 STEERING_REPORT: 8 STEERING + SG_ STEERING_REPORT_magic : 0|16@1+ (1,0) [0|0] "" STEERING + SG_ STEERING_REPORT_enabled : 16|8@1+ (1,0) [0|0] "" STEERING + SG_ STEERING_REPORT_operator_override : 24|8@1+ (1,0) [0|0] "" STEERING + SG_ STEERING_REPORT_dtcs : 32|8@1+ (1,0) [0|0] "" STEERING + SG_ STEERING_REPORT_reserved : 40|24@1+ (1,0) [0|0] "" STEERING + +BO_ 82 THROTTLE_ENABLE: 8 THROTTLE + SG_ THROTTLE_ENABLE_magic : 0|16@1+ (1,0) [0|0] "" THROTTLE + SG_ THROTTLE_ENABLE_reserved : 16|48@1+ (1,0) [0|0] "" THROTTLE + +BO_ 83 THROTTLE_DISABLE: 8 THROTTLE + SG_ THROTTLE_DISABLE_magic : 0|16@1+ (1,0) [0|0] "" THROTTLE + SG_ THROTTLE_DISABLE_reserved : 16|48@1+ (1,0) [0|0] "" THROTTLE + +BO_ 98 THROTTLE_COMMAND: 8 THROTTLE + SG_ THROTTLE_COMMAND_magic : 0|16@1+ (1,0) [0|0] "" THROTTLE + SG_ THROTTLE_COMMAND_spoof_low : 16|16@1+ (1,0) [0|0] "" THROTTLE + SG_ THROTTLE_COMMAND_spoof_high : 32|16@1+ (1,0) [0|0] "" THROTTLE + SG_ THROTTLE_COMMAND_reserved : 48|16@1+ (1,0) [0|0] "" THROTTLE + +BO_ 99 THROTTLE_REPORT: 8 THROTTLE + SG_ THROTTLE_REPORT_magic : 0|16@1+ (1,0) [0|0] "" THROTTLE + SG_ THROTTLE_REPORT_enabled : 16|8@1+ (1,0) [0|0] "" THROTTLE + SG_ THROTTLE_REPORT_operator_override : 24|8@1+ (1,0) [0|0] "" THROTTLE + SG_ THROTTLE_REPORT_dtcs : 32|8@1+ (1,0) [0|0] "" THROTTLE + SG_ THROTTLE_REPORT_reserved : 40|24@1+ (1,0) [0|0] "" THROTTLE + +BO_ 153 FAULT_REPORT: 8 FAULT + SG_ FAULT_REPORT_magic : 0|16@1+ (1,0) [0|0] "" FAULT + SG_ FAULT_REPORT_fault_origin_id : 16|32@1+ (1,0) [0|0] "" FAULT + SG_ FAULT_REPORT_dtcs : 48|8@1+ (1,0) [0|0] "" FAULT + SG_ FAULT_REPORT_reserved : 56|8@1+ (1,0) [0|0] "" FAULT + +CM_ BU_ BRAKE "The OSCC brake module"; +CM_ BU_ STEERING "The OSCC steering module"; +CM_ BU_ THROTTLE "The OSCC throttle module"; +CM_ BU_ FAULT "The OSCC fault report"; diff --git a/api/include/vehicles/kia_soul_niro.h b/api/include/vehicles/kia_soul_niro.h new file mode 100644 index 00000000..c3cb433d --- /dev/null +++ b/api/include/vehicles/kia_soul_niro.h @@ -0,0 +1,420 @@ +/** + * @file kia_soul_niro.h + * @brief Kia Soul Niro specific macros. + * + */ + + +#ifndef _KIA_SOUL_NIRO_PLATFORM_INFO_H_ +#define _KIA_SOUL_NIRO_PLATFORM_INFO_H_ + + +#include + + +// ******************************************************************** +// +// WARNING +// +// The values listed here are carefully tested to ensure that the vehicle's +// components are not actuated outside of the range of what they can handle. +// By changing any of these values you risk attempting to actuate outside of the +// vehicle's valid range. This can cause damage to the hardware and/or a +// vehicle fault. Clearing this fault state requires additional tools. +// +// It is NOT recommended to modify any of these values without expert knowledge. +// +// ************************************************************************ + + +// **************************************************************************** +// OBD MESSAGES +// **************************************************************************** + +/* + * @brief ID of the Kia Soul's OBD steering wheel angle CAN frame. + * + */ +#define KIA_SOUL_OBD_STEERING_WHEEL_ANGLE_CAN_ID ( 0x2B0 ) + +/* + * @brief ID of the Kia Soul's OBD wheel speed CAN frame. + * + */ +#define KIA_SOUL_OBD_WHEEL_SPEED_CAN_ID ( 0x4B0 ) + +/* + * @brief ID of the Kia Soul's OBD brake pressure CAN frame. + * + */ +#define KIA_SOUL_OBD_BRAKE_PRESSURE_CAN_ID ( 0x220 ) + +/* + * @brief Factor to scale OBD steering angle to degrees + * + */ +#define KIA_SOUL_OBD_STEERING_ANGLE_SCALAR ( 0.1 ) + +/** + * @brief Steering wheel angle message data. + * + */ +typedef struct +{ + int16_t steering_wheel_angle; /* 1/10 degrees */ + + uint8_t reserved[6]; /* Reserved. */ +} kia_soul_obd_steering_wheel_angle_data_s; + +/** + * @brief Wheel speed message data. + * + */ +typedef struct +{ + int16_t wheel_speed_front_left; /* 1/50 mph */ + + int16_t wheel_speed_front_right; /* 1/50 mph */ + + int16_t wheel_speed_rear_left; /* 1/50 mph */ + + int16_t wheel_speed_rear_right; /* 1/50 mph */ +} kia_soul_obd_wheel_speed_data_s; + +/** + * @brief Brake pressure message data. + * + */ +typedef struct +{ + int16_t master_cylinder_pressure; /* 1/10th of a bar per bit */ + + uint8_t reserved[6]; /* Reserved. */ +} kia_soul_obd_brake_pressure_data_s; + + + /* + * @brief Number of steps per volt corresponding to 4096 steps (2^12) across 5 volts. + * + */ +#define STEPS_PER_VOLT ( 819.2 ) + + + + +// **************************************************************************** +// BRAKE MODULE +// **************************************************************************** + +/* + * @brief Minimum allowable brake value. + * + */ +#define MINIMUM_BRAKE_COMMAND ( 0.0 ) + +/* + * @brief Maximum allowable brake value. + * + */ +#define MAXIMUM_BRAKE_COMMAND ( 1.0 ) + +/* + * @brief Minimum allowed voltage for the low spoof signal voltage. [volts] + * + */ +#define BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN ( 0.39 ) + +/* + * @brief Maximum allowed voltage for the low spoof signal voltage. [volts] + * + */ +#define BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX ( 2.10 ) + +/** + * @brief Minimum allowed voltage for the high spoof signal voltage. [volts] + * + */ +#define BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN ( 0.76 ) + +/** + * @brief Maximum allowed voltage for the high spoof signal voltage. [volts] + * + */ +#define BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX ( 4.20 ) + +/* + * @brief Minimum allowed value for the low spoof signal value. [steps] + * + * Equal to \ref BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN * \ref STEPS_PER_VOLT. + */ +#define BRAKE_SPOOF_LOW_SIGNAL_RANGE_MIN ( 320 ) + +/* + * @brief Minimum allowed value for the low spoof signal value. [steps] + * + * Equal to \ref BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX * \ref STEPS_PER_VOLT. + */ +#define BRAKE_SPOOF_LOW_SIGNAL_RANGE_MAX ( 1720 ) + +/* + * @brief Minimum allowed value for the low spoof signal value. [steps] + * + * Equal to \ref BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN * \ref STEPS_PER_VOLT. + */ +#define BRAKE_SPOOF_HIGH_SIGNAL_RANGE_MIN ( 623 ) + +/* + * @brief Minimum allowed value for the low spoof signal value. [steps] + * + * Equal to \ref BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX * \ref STEPS_PER_VOLT. + */ +#define BRAKE_SPOOF_HIGH_SIGNAL_RANGE_MAX ( 3440 ) + +/* + * @brief Calculation to convert a brake position to a low spoof voltage. + * + */ +#define BRAKE_POSITION_TO_VOLTS_LOW( position ) ( (position) *\ + (BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX - BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN) +\ + BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN ) + +/* + * @brief Calculation to convert a brake position to a high spoof voltage. + * + */ +#define BRAKE_POSITION_TO_VOLTS_HIGH( position ) ( (position) *\ + (BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX - BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN) +\ + BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN ) + +/* + * @brief Value of the accelerator position that indicates operator override. [steps] + * + */ +#define BRAKE_PEDAL_OVERRIDE_THRESHOLD ( 130 ) + +/* + * @brief Minimum value of the low spoof signal that activates the brake lights. [steps] + * + */ +#define BRAKE_LIGHT_SPOOF_LOW_THRESHOLD ( 300 ) + +/* + * @brief Minimum value of the high spoof signal that activates the brake lights. [steps] + * + */ +#define BRAKE_LIGHT_SPOOF_HIGH_THRESHOLD ( 600 ) + + + + +// **************************************************************************** +// STEERING MODULE +// **************************************************************************** + +/* + * @brief Minimum allowable torque value. + * + */ +#define MINIMUM_TORQUE_COMMAND ( -12.8 ) + +/* + * @brief Maximum allowable torque value. + * + */ +#define MAXIMUM_TORQUE_COMMAND ( 12.7 ) + +/* + * @brief Minimum allowable steering DAC output. [volts] + * + */ +#define STEERING_SPOOF_LOW_SIGNAL_VOLTAGE_MIN ( 0.80 ) + +/* + * @brief Maximum allowable steering DAC output. [volts] + * + */ +#define STEERING_SPOOF_LOW_SIGNAL_VOLTAGE_MAX ( 4.10 ) + +/* + * @brief Minimum allowable steering DAC output. [volts] + * + */ +#define STEERING_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN ( 0.90 ) + +/* + * @brief Maximum allowable steering DAC output. [volts] + * + */ +#define STEERING_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX ( 4.20 ) + +/* + * @brief Minimum allowed value for the low spoof signal value. [steps] + * + * Equal to \ref STEERING_SPOOF_LOW_SIGNAL_VOLTAGE_MIN * \ref STEPS_PER_VOLT. + */ +#define STEERING_SPOOF_LOW_SIGNAL_RANGE_MIN ( 656 ) + +/* + * @brief Minimum allowed value for the low spoof signal value. [steps] + * + * Equal to \ref STEERING_SPOOF_LOW_SIGNAL_VOLTAGE_MAX * \ref STEPS_PER_VOLT. + */ +#define STEERING_SPOOF_LOW_SIGNAL_RANGE_MAX ( 3358 ) + +/* + * @brief Minimum allowed value for the low spoof signal value. [steps] + * + * Equal to \ref STEERING_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN * \ref STEPS_PER_VOLT. + */ +#define STEERING_SPOOF_HIGH_SIGNAL_RANGE_MIN ( 738 ) + +/* + * @brief Minimum allowed value for the low spoof signal value. [steps] + * + * Equal to \ref STEERING_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX * \ref STEPS_PER_VOLT. + */ +#define STEERING_SPOOF_HIGH_SIGNAL_RANGE_MAX ( 3440 ) + +/* + * @brief Scalar value for the low spoof signal taken from a calibration curve. + * + */ +#define TORQUE_SPOOF_LOW_SIGNAL_CALIBRATION_CURVE_SCALE ( 0.135 ) + +/* + * @brief Offset value for the low spoof signal taken from a calibration curve. + * + */ +#define TORQUE_SPOOF_LOW_SIGNAL_CALIBRATION_CURVE_OFFSET ( 2.39 ) + +/* + * @brief Scalar value for the high spoof signal taken from a calibration curve. + * + */ +#define TORQUE_SPOOF_HIGH_SIGNAL_CALIBRATION_CURVE_SCALE ( -0.145 ) + +/* + * @brief Offset value for the high spoof signal taken from a calibration curve. + * + */ +#define TORQUE_SPOOF_HIGH_SIGNAL_CALIBRATION_CURVE_OFFSET ( 2.42 ) + +/* + * @brief Minimum allowed value for the high spoof signal value. + * + */ +#define STEERING_TORQUE_TO_VOLTS_HIGH( torque ) (\ + ((TORQUE_SPOOF_HIGH_SIGNAL_CALIBRATION_CURVE_SCALE * (torque))\ + + TORQUE_SPOOF_HIGH_SIGNAL_CALIBRATION_CURVE_OFFSET)) + +/* + * @brief Calculation to convert a steering torque to a low spoof value. + * + */ +#define STEERING_TORQUE_TO_VOLTS_LOW( torque ) (\ + ((TORQUE_SPOOF_LOW_SIGNAL_CALIBRATION_CURVE_SCALE * (torque))\ + + TORQUE_SPOOF_LOW_SIGNAL_CALIBRATION_CURVE_OFFSET)) + +/* + * @brief Value of torque sensor difference that indicates likely operator + * override. + * + */ +#define TORQUE_DIFFERENCE_OVERRIDE_THRESHOLD ( 1600 ) + + + + +// **************************************************************************** +// THROTTLE MODULE +// **************************************************************************** + +/* + * @brief Minimum allowable throttle value. + * + */ +#define MINIMUM_THROTTLE_COMMAND ( 0.0 ) + +/* + * @brief Maximum allowable throttle value. + * + */ +#define MAXIMUM_THROTTLE_COMMAND ( 1.0 ) + +/* + * @brief Minimum allowed voltage for the low spoof signal voltage. [volts] + * + */ +#define THROTTLE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN ( 0.32 ) + +/* + * @brief Maximum allowed voltage for the low spoof signal voltage. [volts] + * + */ +#define THROTTLE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX ( 1.38 ) + +/** + * @brief Minimum allowed voltage for the high spoof signal voltage. [volts] + * + */ +#define THROTTLE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN ( 0.71 ) + +/** + * @brief Maximum allowed voltage for the high spoof signal voltage. [volts] + * + */ +#define THROTTLE_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX ( 2.80 ) + +/* + * @brief Minimum allowed value for the low spoof signal value. [steps] + * + * Equal to \ref THROTTLE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN * \ref STEPS_PER_VOLT. + */ +#define THROTTLE_SPOOF_LOW_SIGNAL_RANGE_MIN ( 263 ) + +/* + * @brief Minimum allowed value for the low spoof signal value. [steps] + * + * Equal to \ref THROTTLE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX * \ref STEPS_PER_VOLT. + */ +#define THROTTLE_SPOOF_LOW_SIGNAL_RANGE_MAX ( 1130 ) + +/* + * @brief Minimum allowed value for the low spoof signal value. [steps] + * + * Equal to \ref THROTTLE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN * \ref STEPS_PER_VOLT. + */ +#define THROTTLE_SPOOF_HIGH_SIGNAL_RANGE_MIN ( 582 ) + +/* + * @brief Minimum allowed value for the low spoof signal value. [steps] + * + * Equal to \ref THROTTLE_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX * \ref STEPS_PER_VOLT. + */ +#define THROTTLE_SPOOF_HIGH_SIGNAL_RANGE_MAX ( 2293 ) + +/* + * @brief Calculation to convert a throttle position to a low spoof voltage. + * + */ +#define THROTTLE_POSITION_TO_VOLTS_LOW( position ) ( (position) *\ + (THROTTLE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX - THROTTLE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN) +\ + THROTTLE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN ) + +/* + * @brief Calculation to convert a throttle position to a high spoof voltage. + * + */ +#define THROTTLE_POSITION_TO_VOLTS_HIGH( position ) ( (position) *\ + (THROTTLE_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX - THROTTLE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN) +\ + THROTTLE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN ) + +/* + * @brief Value of the accelerator position that indicates operator override. [steps] + * + */ +#define ACCELERATOR_OVERRIDE_THRESHOLD ( 185.0 ) + + + +#endif diff --git a/api/src/oscc.c b/api/src/oscc.c index c700ebbc..87c03976 100644 --- a/api/src/oscc.c +++ b/api/src/oscc.c @@ -115,7 +115,7 @@ oscc_result_t oscc_publish_brake_position( double brake_position ) brake_cmd.pedal_command = ( uint16_t ) BRAKE_POSITION_TO_PEDAL( clamped_position ); -#elif defined(KIA_SOUL_EV) +#elif defined(KIA_SOUL_EV) || defined(KIA_SOUL_NIRO) const double clamped_position = CONSTRAIN( brake_position, MINIMUM_BRAKE_COMMAND, diff --git a/firmware/CMakeLists.txt b/firmware/CMakeLists.txt index 8c6d6c7b..4dc22196 100644 --- a/firmware/CMakeLists.txt +++ b/firmware/CMakeLists.txt @@ -12,7 +12,10 @@ if(TESTS) add_subdirectory(brake/kia_soul_petrol/tests) elseif(KIA_SOUL_EV) add_definitions(-DKIA_SOUL_EV) - add_subdirectory(brake/kia_soul_ev/tests) + add_subdirectory(brake/kia_soul_ev_niro/tests) + elseif(KIA_SOUL_NIRO) + add_definitions(-DKIA_SOUL_NIRO) + add_subdirectory(brake/kia_soul_ev_niro/tests) else() message(FATAL_ERROR "No platform selected - no firmware will be tested") endif() @@ -60,6 +63,7 @@ else() option(DEBUG "Enable debug mode" OFF) option(KIA_SOUL "Build firmware for the petrol Kia Soul" OFF) option(KIA_SOUL_EV "Build firmware for the Kia Soul EV" OFF) + option(KIA_SOUL_NIRO "Build firmware for the Kia Soul Niro" OFF) option(BRAKE_STARTUP_TEST "Enable brake startup sensor tests" ON) option(STEERING_OVERRIDE "Enable steering override" ON) @@ -83,6 +87,8 @@ else() add_definitions(-DKIA_SOUL) elseif(KIA_SOUL_EV) add_definitions(-DKIA_SOUL_EV) + elseif(KIA_SOUL_NIRO) + add_definitions(-DKIA_SOUL_NIRO) else() message(FATAL_ERROR "No platform selected - no firmware will be built") endif() diff --git a/firmware/brake/CMakeLists.txt b/firmware/brake/CMakeLists.txt index f01a1ddd..63bb3d86 100644 --- a/firmware/brake/CMakeLists.txt +++ b/firmware/brake/CMakeLists.txt @@ -15,6 +15,6 @@ add_custom_target( if(KIA_SOUL) add_subdirectory(kia_soul_petrol) -elseif(KIA_SOUL_EV) - add_subdirectory(kia_soul_ev) +elseif(KIA_SOUL_EV OR KIA_SOUL_NIRO) + add_subdirectory(kia_soul_ev_niro) endif() diff --git a/firmware/brake/kia_soul_ev/CMakeLists.txt b/firmware/brake/kia_soul_ev_niro/CMakeLists.txt similarity index 100% rename from firmware/brake/kia_soul_ev/CMakeLists.txt rename to firmware/brake/kia_soul_ev_niro/CMakeLists.txt diff --git a/firmware/brake/kia_soul_ev/include/brake_control.h b/firmware/brake/kia_soul_ev_niro/include/brake_control.h similarity index 100% rename from firmware/brake/kia_soul_ev/include/brake_control.h rename to firmware/brake/kia_soul_ev_niro/include/brake_control.h diff --git a/firmware/brake/kia_soul_ev/include/communications.h b/firmware/brake/kia_soul_ev_niro/include/communications.h similarity index 100% rename from firmware/brake/kia_soul_ev/include/communications.h rename to firmware/brake/kia_soul_ev_niro/include/communications.h diff --git a/firmware/brake/kia_soul_ev/include/globals.h b/firmware/brake/kia_soul_ev_niro/include/globals.h similarity index 100% rename from firmware/brake/kia_soul_ev/include/globals.h rename to firmware/brake/kia_soul_ev_niro/include/globals.h diff --git a/firmware/brake/kia_soul_ev/include/init.h b/firmware/brake/kia_soul_ev_niro/include/init.h similarity index 100% rename from firmware/brake/kia_soul_ev/include/init.h rename to firmware/brake/kia_soul_ev_niro/include/init.h diff --git a/firmware/brake/kia_soul_ev/include/timers.h b/firmware/brake/kia_soul_ev_niro/include/timers.h similarity index 100% rename from firmware/brake/kia_soul_ev/include/timers.h rename to firmware/brake/kia_soul_ev_niro/include/timers.h diff --git a/firmware/brake/kia_soul_ev/src/brake_control.cpp b/firmware/brake/kia_soul_ev_niro/src/brake_control.cpp similarity index 100% rename from firmware/brake/kia_soul_ev/src/brake_control.cpp rename to firmware/brake/kia_soul_ev_niro/src/brake_control.cpp diff --git a/firmware/brake/kia_soul_ev/src/communications.cpp b/firmware/brake/kia_soul_ev_niro/src/communications.cpp similarity index 100% rename from firmware/brake/kia_soul_ev/src/communications.cpp rename to firmware/brake/kia_soul_ev_niro/src/communications.cpp diff --git a/firmware/brake/kia_soul_ev/src/globals.cpp b/firmware/brake/kia_soul_ev_niro/src/globals.cpp similarity index 100% rename from firmware/brake/kia_soul_ev/src/globals.cpp rename to firmware/brake/kia_soul_ev_niro/src/globals.cpp diff --git a/firmware/brake/kia_soul_ev/src/init.cpp b/firmware/brake/kia_soul_ev_niro/src/init.cpp similarity index 100% rename from firmware/brake/kia_soul_ev/src/init.cpp rename to firmware/brake/kia_soul_ev_niro/src/init.cpp diff --git a/firmware/brake/kia_soul_ev/src/main.cpp b/firmware/brake/kia_soul_ev_niro/src/main.cpp similarity index 100% rename from firmware/brake/kia_soul_ev/src/main.cpp rename to firmware/brake/kia_soul_ev_niro/src/main.cpp diff --git a/firmware/brake/kia_soul_ev/src/timers.cpp b/firmware/brake/kia_soul_ev_niro/src/timers.cpp similarity index 100% rename from firmware/brake/kia_soul_ev/src/timers.cpp rename to firmware/brake/kia_soul_ev_niro/src/timers.cpp diff --git a/firmware/brake/kia_soul_ev/tests/CMakeLists.txt b/firmware/brake/kia_soul_ev_niro/tests/CMakeLists.txt similarity index 100% rename from firmware/brake/kia_soul_ev/tests/CMakeLists.txt rename to firmware/brake/kia_soul_ev_niro/tests/CMakeLists.txt diff --git a/firmware/brake/kia_soul_ev/tests/features/checking_faults.feature b/firmware/brake/kia_soul_ev_niro/tests/features/checking_faults.feature similarity index 100% rename from firmware/brake/kia_soul_ev/tests/features/checking_faults.feature rename to firmware/brake/kia_soul_ev_niro/tests/features/checking_faults.feature diff --git a/firmware/brake/kia_soul_ev/tests/features/receiving_messages.feature b/firmware/brake/kia_soul_ev_niro/tests/features/receiving_messages.feature similarity index 100% rename from firmware/brake/kia_soul_ev/tests/features/receiving_messages.feature rename to firmware/brake/kia_soul_ev_niro/tests/features/receiving_messages.feature diff --git a/firmware/brake/kia_soul_ev/tests/features/sending_reports.feature b/firmware/brake/kia_soul_ev_niro/tests/features/sending_reports.feature similarity index 100% rename from firmware/brake/kia_soul_ev/tests/features/sending_reports.feature rename to firmware/brake/kia_soul_ev_niro/tests/features/sending_reports.feature diff --git a/firmware/brake/kia_soul_ev/tests/features/step_definitions/checking_faults.cpp b/firmware/brake/kia_soul_ev_niro/tests/features/step_definitions/checking_faults.cpp similarity index 100% rename from firmware/brake/kia_soul_ev/tests/features/step_definitions/checking_faults.cpp rename to firmware/brake/kia_soul_ev_niro/tests/features/step_definitions/checking_faults.cpp diff --git a/firmware/brake/kia_soul_ev/tests/features/step_definitions/common.cpp b/firmware/brake/kia_soul_ev_niro/tests/features/step_definitions/common.cpp similarity index 100% rename from firmware/brake/kia_soul_ev/tests/features/step_definitions/common.cpp rename to firmware/brake/kia_soul_ev_niro/tests/features/step_definitions/common.cpp diff --git a/firmware/brake/kia_soul_ev/tests/features/step_definitions/cucumber.wire b/firmware/brake/kia_soul_ev_niro/tests/features/step_definitions/cucumber.wire similarity index 100% rename from firmware/brake/kia_soul_ev/tests/features/step_definitions/cucumber.wire rename to firmware/brake/kia_soul_ev_niro/tests/features/step_definitions/cucumber.wire diff --git a/firmware/brake/kia_soul_ev/tests/features/step_definitions/receiving_messages.cpp b/firmware/brake/kia_soul_ev_niro/tests/features/step_definitions/receiving_messages.cpp similarity index 100% rename from firmware/brake/kia_soul_ev/tests/features/step_definitions/receiving_messages.cpp rename to firmware/brake/kia_soul_ev_niro/tests/features/step_definitions/receiving_messages.cpp diff --git a/firmware/brake/kia_soul_ev/tests/features/step_definitions/sending_reports.cpp b/firmware/brake/kia_soul_ev_niro/tests/features/step_definitions/sending_reports.cpp similarity index 100% rename from firmware/brake/kia_soul_ev/tests/features/step_definitions/sending_reports.cpp rename to firmware/brake/kia_soul_ev_niro/tests/features/step_definitions/sending_reports.cpp diff --git a/firmware/brake/kia_soul_ev/tests/features/step_definitions/test.cpp b/firmware/brake/kia_soul_ev_niro/tests/features/step_definitions/test.cpp similarity index 100% rename from firmware/brake/kia_soul_ev/tests/features/step_definitions/test.cpp rename to firmware/brake/kia_soul_ev_niro/tests/features/step_definitions/test.cpp diff --git a/firmware/brake/kia_soul_ev/tests/features/support/env.rb b/firmware/brake/kia_soul_ev_niro/tests/features/support/env.rb similarity index 100% rename from firmware/brake/kia_soul_ev/tests/features/support/env.rb rename to firmware/brake/kia_soul_ev_niro/tests/features/support/env.rb diff --git a/firmware/brake/kia_soul_ev/tests/property/Cargo.toml b/firmware/brake/kia_soul_ev_niro/tests/property/Cargo.toml similarity index 100% rename from firmware/brake/kia_soul_ev/tests/property/Cargo.toml rename to firmware/brake/kia_soul_ev_niro/tests/property/Cargo.toml diff --git a/firmware/brake/kia_soul_ev/tests/property/build.rs b/firmware/brake/kia_soul_ev_niro/tests/property/build.rs similarity index 100% rename from firmware/brake/kia_soul_ev/tests/property/build.rs rename to firmware/brake/kia_soul_ev_niro/tests/property/build.rs diff --git a/firmware/brake/kia_soul_ev/tests/property/include/wrapper.hpp b/firmware/brake/kia_soul_ev_niro/tests/property/include/wrapper.hpp similarity index 100% rename from firmware/brake/kia_soul_ev/tests/property/include/wrapper.hpp rename to firmware/brake/kia_soul_ev_niro/tests/property/include/wrapper.hpp diff --git a/firmware/brake/kia_soul_ev/tests/property/src/tests.rs b/firmware/brake/kia_soul_ev_niro/tests/property/src/tests.rs similarity index 100% rename from firmware/brake/kia_soul_ev/tests/property/src/tests.rs rename to firmware/brake/kia_soul_ev_niro/tests/property/src/tests.rs From 2c342953a107cd527315e5b49c8bf8daf267605a Mon Sep 17 00:00:00 2001 From: Lucas Buckland Date: Tue, 10 Oct 2017 10:38:15 -0700 Subject: [PATCH 02/20] Add updated voltage ranges Prior to this commit the voltage ranges for the Kia Niro definition file were slightly off. This commit corrects the voltage ranges and adds a correct threshold for brake disable. --- api/include/vehicles/kia_soul_niro.h | 34 ++++++++++++++-------------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/api/include/vehicles/kia_soul_niro.h b/api/include/vehicles/kia_soul_niro.h index c3cb433d..65c16c3c 100644 --- a/api/include/vehicles/kia_soul_niro.h +++ b/api/include/vehicles/kia_soul_niro.h @@ -122,53 +122,53 @@ typedef struct * @brief Minimum allowed voltage for the low spoof signal voltage. [volts] * */ -#define BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN ( 0.39 ) +#define BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN ( 0.279 ) /* * @brief Maximum allowed voltage for the low spoof signal voltage. [volts] * */ -#define BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX ( 2.10 ) +#define BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX ( 1.386 ) /** * @brief Minimum allowed voltage for the high spoof signal voltage. [volts] * */ -#define BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN ( 0.76 ) +#define BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN ( 0.609 ) /** * @brief Maximum allowed voltage for the high spoof signal voltage. [volts] * */ -#define BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX ( 4.20 ) +#define BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX ( 2.880 ) /* * @brief Minimum allowed value for the low spoof signal value. [steps] * * Equal to \ref BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN * \ref STEPS_PER_VOLT. */ -#define BRAKE_SPOOF_LOW_SIGNAL_RANGE_MIN ( 320 ) +#define BRAKE_SPOOF_LOW_SIGNAL_RANGE_MIN ( 229 ) /* * @brief Minimum allowed value for the low spoof signal value. [steps] * * Equal to \ref BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX * \ref STEPS_PER_VOLT. */ -#define BRAKE_SPOOF_LOW_SIGNAL_RANGE_MAX ( 1720 ) +#define BRAKE_SPOOF_LOW_SIGNAL_RANGE_MAX ( 1135 ) /* * @brief Minimum allowed value for the low spoof signal value. [steps] * * Equal to \ref BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN * \ref STEPS_PER_VOLT. */ -#define BRAKE_SPOOF_HIGH_SIGNAL_RANGE_MIN ( 623 ) +#define BRAKE_SPOOF_HIGH_SIGNAL_RANGE_MIN ( 499 ) /* * @brief Minimum allowed value for the low spoof signal value. [steps] * * Equal to \ref BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX * \ref STEPS_PER_VOLT. */ -#define BRAKE_SPOOF_HIGH_SIGNAL_RANGE_MAX ( 3440 ) +#define BRAKE_SPOOF_HIGH_SIGNAL_RANGE_MAX ( 2359 ) /* * @brief Calculation to convert a brake position to a low spoof voltage. @@ -190,7 +190,7 @@ typedef struct * @brief Value of the accelerator position that indicates operator override. [steps] * */ -#define BRAKE_PEDAL_OVERRIDE_THRESHOLD ( 130 ) +#define BRAKE_PEDAL_OVERRIDE_THRESHOLD ( 150 ) /* * @brief Minimum value of the low spoof signal that activates the brake lights. [steps] @@ -345,53 +345,53 @@ typedef struct * @brief Minimum allowed voltage for the low spoof signal voltage. [volts] * */ -#define THROTTLE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN ( 0.32 ) +#define THROTTLE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN ( 0.380 ) /* * @brief Maximum allowed voltage for the low spoof signal voltage. [volts] * */ -#define THROTTLE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX ( 1.38 ) +#define THROTTLE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX ( 2.104 ) /** * @brief Minimum allowed voltage for the high spoof signal voltage. [volts] * */ -#define THROTTLE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN ( 0.71 ) +#define THROTTLE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN ( 0.757 ) /** * @brief Maximum allowed voltage for the high spoof signal voltage. [volts] * */ -#define THROTTLE_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX ( 2.80 ) +#define THROTTLE_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX ( 4.207 ) /* * @brief Minimum allowed value for the low spoof signal value. [steps] * * Equal to \ref THROTTLE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN * \ref STEPS_PER_VOLT. */ -#define THROTTLE_SPOOF_LOW_SIGNAL_RANGE_MIN ( 263 ) +#define THROTTLE_SPOOF_LOW_SIGNAL_RANGE_MIN ( 311 ) /* * @brief Minimum allowed value for the low spoof signal value. [steps] * * Equal to \ref THROTTLE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX * \ref STEPS_PER_VOLT. */ -#define THROTTLE_SPOOF_LOW_SIGNAL_RANGE_MAX ( 1130 ) +#define THROTTLE_SPOOF_LOW_SIGNAL_RANGE_MAX ( 1724 ) /* * @brief Minimum allowed value for the low spoof signal value. [steps] * * Equal to \ref THROTTLE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN * \ref STEPS_PER_VOLT. */ -#define THROTTLE_SPOOF_HIGH_SIGNAL_RANGE_MIN ( 582 ) +#define THROTTLE_SPOOF_HIGH_SIGNAL_RANGE_MIN ( 620 ) /* * @brief Minimum allowed value for the low spoof signal value. [steps] * * Equal to \ref THROTTLE_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX * \ref STEPS_PER_VOLT. */ -#define THROTTLE_SPOOF_HIGH_SIGNAL_RANGE_MAX ( 2293 ) +#define THROTTLE_SPOOF_HIGH_SIGNAL_RANGE_MAX ( 3446 ) /* * @brief Calculation to convert a throttle position to a low spoof voltage. From 1bf29a67aec141351da5f74a5c9bdfacfe7e7518 Mon Sep 17 00:00:00 2001 From: Austin Morlan Date: Fri, 5 Jan 2018 12:01:21 -0800 Subject: [PATCH 03/20] Rename Soul to Niro --- .../{kia_soul_niro.dbc => kia_niro.dbc} | 0 .../vehicles/{kia_soul_niro.h => kia_niro.h} | 38 ++++++++++++------- 2 files changed, 24 insertions(+), 14 deletions(-) rename api/include/vehicles/{kia_soul_niro.dbc => kia_niro.dbc} (100%) rename api/include/vehicles/{kia_soul_niro.h => kia_niro.h} (92%) diff --git a/api/include/vehicles/kia_soul_niro.dbc b/api/include/vehicles/kia_niro.dbc similarity index 100% rename from api/include/vehicles/kia_soul_niro.dbc rename to api/include/vehicles/kia_niro.dbc diff --git a/api/include/vehicles/kia_soul_niro.h b/api/include/vehicles/kia_niro.h similarity index 92% rename from api/include/vehicles/kia_soul_niro.h rename to api/include/vehicles/kia_niro.h index 65c16c3c..4c1ccd7a 100644 --- a/api/include/vehicles/kia_soul_niro.h +++ b/api/include/vehicles/kia_niro.h @@ -1,12 +1,12 @@ /** - * @file kia_soul_niro.h - * @brief Kia Soul Niro specific macros. + * @file kia_niro.h + * @brief Kia Niro specific macros. * */ -#ifndef _KIA_SOUL_NIRO_PLATFORM_INFO_H_ -#define _KIA_SOUL_NIRO_PLATFORM_INFO_H_ +#ifndef _KIA_NIRO_PLATFORM_INFO_H_ +#define _KIA_NIRO_PLATFORM_INFO_H_ #include @@ -32,22 +32,22 @@ // **************************************************************************** /* - * @brief ID of the Kia Soul's OBD steering wheel angle CAN frame. + * @brief ID of the Kia Niro's OBD steering wheel angle CAN frame. * */ #define KIA_SOUL_OBD_STEERING_WHEEL_ANGLE_CAN_ID ( 0x2B0 ) /* - * @brief ID of the Kia Soul's OBD wheel speed CAN frame. + * @brief ID of the Kia Niro's OBD wheel speed CAN frame. * */ -#define KIA_SOUL_OBD_WHEEL_SPEED_CAN_ID ( 0x4B0 ) +#define KIA_SOUL_OBD_WHEEL_SPEED_CAN_ID ( 0x386 ) /* - * @brief ID of the Kia Soul's OBD brake pressure CAN frame. + * @brief ID of the Kia Niro's OBD brake pressure CAN frame. * */ -#define KIA_SOUL_OBD_BRAKE_PRESSURE_CAN_ID ( 0x220 ) +#define KIA_SOUL_OBD_BRAKE_PRESSURE_CAN_ID ( 0x371 ) /* * @brief Factor to scale OBD steering angle to degrees @@ -72,13 +72,21 @@ typedef struct */ typedef struct { - int16_t wheel_speed_front_left; /* 1/50 mph */ + uint8_t wheel_speed_front_left; /* 1/4 kph */ - int16_t wheel_speed_front_right; /* 1/50 mph */ + uint8_t reserved_0; /* Reserved. */ - int16_t wheel_speed_rear_left; /* 1/50 mph */ + uint8_t wheel_speed_front_right; /* 1/4 kph */ - int16_t wheel_speed_rear_right; /* 1/50 mph */ + uint8_t reserved_1; /* Reserved. */ + + uint8_t wheel_speed_rear_left; /* 1/4 kph */ + + uint8_t reserved_2; /* Reserved. */ + + uint8_t wheel_speed_rear_right; /* 1/4 kph */ + + uint8_t reserved_3; /* Reserved. */ } kia_soul_obd_wheel_speed_data_s; /** @@ -87,9 +95,11 @@ typedef struct */ typedef struct { + uint8_t reserved_0[4]; /* Reserved. */ + int16_t master_cylinder_pressure; /* 1/10th of a bar per bit */ - uint8_t reserved[6]; /* Reserved. */ + uint8_t reserved_1[2]; /* Reserved. */ } kia_soul_obd_brake_pressure_data_s; From 3396fb3c70fce49741ddaff7a1eee91002224ee9 Mon Sep 17 00:00:00 2001 From: Austin Morlan Date: Fri, 5 Jan 2018 15:11:55 -0800 Subject: [PATCH 04/20] Add vehicle speed message --- api/include/vehicles/kia_niro.h | 21 ++++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/api/include/vehicles/kia_niro.h b/api/include/vehicles/kia_niro.h index 4c1ccd7a..179de90b 100644 --- a/api/include/vehicles/kia_niro.h +++ b/api/include/vehicles/kia_niro.h @@ -47,7 +47,13 @@ * @brief ID of the Kia Niro's OBD brake pressure CAN frame. * */ -#define KIA_SOUL_OBD_BRAKE_PRESSURE_CAN_ID ( 0x371 ) +#define KIA_SOUL_OBD_BRAKE_PRESSURE_CAN_ID ( 0x220 ) + +/* + * @brief ID of the Kia Niro's OBD speed CAN frame. + * + */ +#define KIA_SOUL_OBD_SPEED_CAN_ID ( 0x371 ) /* * @brief Factor to scale OBD steering angle to degrees @@ -102,6 +108,19 @@ typedef struct uint8_t reserved_1[2]; /* Reserved. */ } kia_soul_obd_brake_pressure_data_s; +/** + * @brief Speed message data. + * + */ +typedef struct +{ + uint8_t reserved_0[3]; + + int16_t vehicle_speed; /* kph */ + + uint8_t reserved_1[3]; +} kia_soul_obd_speed_data_s; + /* * @brief Number of steps per volt corresponding to 4096 steps (2^12) across 5 volts. From 95f1510422cfdb96ebd316bfa1a997b5b4da265c Mon Sep 17 00:00:00 2001 From: Austin Morlan Date: Fri, 5 Jan 2018 15:12:43 -0800 Subject: [PATCH 05/20] Rename Kia Niro macros --- api/OsccConfig.cmake | 4 ++-- api/include/can_protocols/brake_can_protocol.h | 2 +- api/include/vehicles.h | 4 ++-- api/src/oscc.c | 2 +- firmware/CMakeLists.txt | 10 +++++----- firmware/brake/CMakeLists.txt | 2 +- 6 files changed, 12 insertions(+), 12 deletions(-) diff --git a/api/OsccConfig.cmake b/api/OsccConfig.cmake index fa9a0283..5fc0cc72 100644 --- a/api/OsccConfig.cmake +++ b/api/OsccConfig.cmake @@ -2,8 +2,8 @@ if(KIA_SOUL) add_definitions(-DKIA_SOUL) elseif(KIA_SOUL_EV) add_definitions(-DKIA_SOUL_EV) -elseif(KIA_SOUL_NIRO) - add_definitions(-DKIA_SOUL_NIRO) +elseif(KIA_NIRO) + add_definitions(-DKIA_NIRO) else() message(FATAL_ERROR "No platform selected") endif() diff --git a/api/include/can_protocols/brake_can_protocol.h b/api/include/can_protocols/brake_can_protocol.h index 167b7355..7bd0c6e5 100644 --- a/api/include/can_protocols/brake_can_protocol.h +++ b/api/include/can_protocols/brake_can_protocol.h @@ -117,7 +117,7 @@ typedef struct uint16_t pedal_command; /*!< Pedal command. [65535 == 100%] */ uint8_t reserved[4]; /*!< Reserved. */ -#elif defined(KIA_SOUL_EV) || defined(KIA_SOUL_NIRO) +#elif defined(KIA_SOUL_EV) || defined(KIA_NIRO) uint16_t spoof_value_low; /*!< Value to be sent on the low spoof signal. */ uint16_t spoof_value_high; /*!< Value to be sent on the high spoof signal. */ diff --git a/api/include/vehicles.h b/api/include/vehicles.h index b7ea9b2c..82cd4a60 100644 --- a/api/include/vehicles.h +++ b/api/include/vehicles.h @@ -13,8 +13,8 @@ #include "vehicles/kia_soul_petrol.h" #elif defined(KIA_SOUL_EV) #include "vehicles/kia_soul_ev.h" -#elif defined(KIA_SOUL_NIRO) -#include "vehicles/kia_soul_niro.h" +#elif defined(KIA_NIRO) +#include "vehicles/kia_niro.h" #endif diff --git a/api/src/oscc.c b/api/src/oscc.c index 87c03976..8ac888cf 100644 --- a/api/src/oscc.c +++ b/api/src/oscc.c @@ -115,7 +115,7 @@ oscc_result_t oscc_publish_brake_position( double brake_position ) brake_cmd.pedal_command = ( uint16_t ) BRAKE_POSITION_TO_PEDAL( clamped_position ); -#elif defined(KIA_SOUL_EV) || defined(KIA_SOUL_NIRO) +#elif defined(KIA_SOUL_EV) || defined(KIA_NIRO) const double clamped_position = CONSTRAIN( brake_position, MINIMUM_BRAKE_COMMAND, diff --git a/firmware/CMakeLists.txt b/firmware/CMakeLists.txt index 0801d4b4..6fbab699 100644 --- a/firmware/CMakeLists.txt +++ b/firmware/CMakeLists.txt @@ -17,8 +17,8 @@ if(TESTS) elseif(KIA_SOUL_EV) add_definitions(-DKIA_SOUL_EV) add_subdirectory(brake/kia_soul_ev_niro/tests) - elseif(KIA_SOUL_NIRO) - add_definitions(-DKIA_SOUL_NIRO) + elseif(KIA_NIRO) + add_definitions(-DKIA_NIRO) add_subdirectory(brake/kia_soul_ev_niro/tests) else() message(FATAL_ERROR "No platform selected - no firmware will be tested") @@ -67,7 +67,7 @@ else() option(DEBUG "Enable debug mode" OFF) option(KIA_SOUL "Build firmware for the petrol Kia Soul" OFF) option(KIA_SOUL_EV "Build firmware for the Kia Soul EV" OFF) - option(KIA_SOUL_NIRO "Build firmware for the Kia Soul Niro" OFF) + option(KIA_NIRO "Build firmware for the Kia Niro" OFF) option(BRAKE_STARTUP_TEST "Enable brake startup sensor tests" ON) option(STEERING_OVERRIDE "Enable steering override" ON) @@ -91,8 +91,8 @@ else() add_definitions(-DKIA_SOUL) elseif(KIA_SOUL_EV) add_definitions(-DKIA_SOUL_EV) - elseif(KIA_SOUL_NIRO) - add_definitions(-DKIA_SOUL_NIRO) + elseif(KIA_NIRO) + add_definitions(-DKIA_NIRO) else() message(FATAL_ERROR "No platform selected - no firmware will be built") endif() diff --git a/firmware/brake/CMakeLists.txt b/firmware/brake/CMakeLists.txt index 63bb3d86..9b0aa0d3 100644 --- a/firmware/brake/CMakeLists.txt +++ b/firmware/brake/CMakeLists.txt @@ -15,6 +15,6 @@ add_custom_target( if(KIA_SOUL) add_subdirectory(kia_soul_petrol) -elseif(KIA_SOUL_EV OR KIA_SOUL_NIRO) +elseif(KIA_SOUL_EV OR KIA_NIRO) add_subdirectory(kia_soul_ev_niro) endif() From af4f4a28a87a8f3d4aebc89859de51bbdeeea2d0 Mon Sep 17 00:00:00 2001 From: Trey German Date: Fri, 12 Jan 2018 12:15:59 -0600 Subject: [PATCH 06/20] Swapped high and low brake signal #defines, updated README.md to include Kia Niro --- README.md | 1 + api/include/vehicles/kia_niro.h | 44 ++++++++++++++++----------------- 2 files changed, 23 insertions(+), 22 deletions(-) diff --git a/README.md b/README.md index 43d5a216..7383a2a9 100644 --- a/README.md +++ b/README.md @@ -89,6 +89,7 @@ appropriate build flag: | --------------- | ---------------- | | Kia Soul Petrol | -DKIA_SOUL=ON | | Kia Soul EV | -DKIA_SOUL_EV=ON | +| Kia Niro | -DKIA_NIRO=ON | For example, if you want to build firmware for the petrol Kia Soul: diff --git a/api/include/vehicles/kia_niro.h b/api/include/vehicles/kia_niro.h index 179de90b..4205bbb9 100644 --- a/api/include/vehicles/kia_niro.h +++ b/api/include/vehicles/kia_niro.h @@ -148,56 +148,56 @@ typedef struct #define MAXIMUM_BRAKE_COMMAND ( 1.0 ) /* - * @brief Minimum allowed voltage for the low spoof signal voltage. [volts] + * @brief Minimum allowed voltage for the high spoof signal voltage. [volts] * */ -#define BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN ( 0.279 ) +#define BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN ( 0.279 ) /* - * @brief Maximum allowed voltage for the low spoof signal voltage. [volts] + * @brief Maximum allowed voltage for the high spoof signal voltage. [volts] * */ -#define BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX ( 1.386 ) +#define BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX ( 1.386 ) /** - * @brief Minimum allowed voltage for the high spoof signal voltage. [volts] + * @brief Minimum allowed voltage for the low spoof signal voltage. [volts] * */ -#define BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN ( 0.609 ) +#define BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN ( 0.609 ) /** - * @brief Maximum allowed voltage for the high spoof signal voltage. [volts] + * @brief Maximum allowed voltage for the low spoof signal voltage. [volts] * */ -#define BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX ( 2.880 ) +#define BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX ( 2.880 ) /* - * @brief Minimum allowed value for the low spoof signal value. [steps] + * @brief Minimum allowed value for the high spoof signal value. [steps] * - * Equal to \ref BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN * \ref STEPS_PER_VOLT. + * Equal to \ref BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN * \ref STEPS_PER_VOLT. */ -#define BRAKE_SPOOF_LOW_SIGNAL_RANGE_MIN ( 229 ) +#define BRAKE_SPOOF_HIGH_SIGNAL_RANGE_MIN ( 229 ) /* - * @brief Minimum allowed value for the low spoof signal value. [steps] + * @brief Minimum allowed value for the high spoof signal value. [steps] * - * Equal to \ref BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX * \ref STEPS_PER_VOLT. + * Equal to \ref BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX * \ref STEPS_PER_VOLT. */ -#define BRAKE_SPOOF_LOW_SIGNAL_RANGE_MAX ( 1135 ) +#define BRAKE_SPOOF_HIGH_SIGNAL_RANGE_MAX ( 1135 ) /* * @brief Minimum allowed value for the low spoof signal value. [steps] * - * Equal to \ref BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN * \ref STEPS_PER_VOLT. + * Equal to \ref BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN * \ref STEPS_PER_VOLT. */ -#define BRAKE_SPOOF_HIGH_SIGNAL_RANGE_MIN ( 499 ) +#define BRAKE_SPOOF_LOW_SIGNAL_RANGE_MIN ( 499 ) /* * @brief Minimum allowed value for the low spoof signal value. [steps] * - * Equal to \ref BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX * \ref STEPS_PER_VOLT. + * Equal to \ref BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX * \ref STEPS_PER_VOLT. */ -#define BRAKE_SPOOF_HIGH_SIGNAL_RANGE_MAX ( 2359 ) +#define BRAKE_SPOOF_LOW_SIGNAL_RANGE_MAX ( 2359 ) /* * @brief Calculation to convert a brake position to a low spoof voltage. @@ -222,16 +222,16 @@ typedef struct #define BRAKE_PEDAL_OVERRIDE_THRESHOLD ( 150 ) /* - * @brief Minimum value of the low spoof signal that activates the brake lights. [steps] + * @brief Minimum value of the high spoof signal that activates the brake lights. [steps] * */ -#define BRAKE_LIGHT_SPOOF_LOW_THRESHOLD ( 300 ) +#define BRAKE_LIGHT_SPOOF_HIGH_THRESHOLD ( 300 ) /* - * @brief Minimum value of the high spoof signal that activates the brake lights. [steps] + * @brief Minimum value of the low spoof signal that activates the brake lights. [steps] * */ -#define BRAKE_LIGHT_SPOOF_HIGH_THRESHOLD ( 600 ) +#define BRAKE_LIGHT_SPOOF_LOW_THRESHOLD ( 600 ) From 8108f36eda0463dc5f9fc3206ab8dd35ecb39ac8 Mon Sep 17 00:00:00 2001 From: Adrien Thebo Date: Thu, 18 Jan 2018 07:48:11 -0800 Subject: [PATCH 07/20] Support variable arity for DEBUG fns The Arduino Serial functions are variadic and accept optional format arguments, but our wrappers for printing debug messages have a fixed arity of 1. This commit updates the DEBUG_ macros to forward variadic args to the underlying Serial functions to expose the richer functionality. --- firmware/common/include/debug.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/firmware/common/include/debug.h b/firmware/common/include/debug.h index 77ff5b4e..c0b550ba 100644 --- a/firmware/common/include/debug.h +++ b/firmware/common/include/debug.h @@ -14,11 +14,11 @@ #ifdef DEBUG #warning "DEBUG defined" - #define DEBUG_PRINT(x) Serial.print(x) - #define DEBUG_PRINTLN(x) Serial.println(x) + #define DEBUG_PRINT(...) Serial.print(__VA_ARGS__) + #define DEBUG_PRINTLN(...) Serial.println(__VA_ARGS__) #else - #define DEBUG_PRINT(x) - #define DEBUG_PRINTLN(x) + #define DEBUG_PRINT(...) + #define DEBUG_PRINTLN(...) #endif From 04ad2310f6acac550a2857f055332931c9b9b2e9 Mon Sep 17 00:00:00 2001 From: DavidSosnow Date: Wed, 24 Jan 2018 17:40:53 -0800 Subject: [PATCH 08/20] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index ba4d4431..ef95bbfe 100644 --- a/README.md +++ b/README.md @@ -5,7 +5,7 @@ Open Source Car Control (OSCC) is an assemblage of software and hardware designs OSCC enables developers to send control commands to the vehicle, read control messages from the vehicle’s OBD-II CAN network, and forward reports for current vehicle control state. Such as steering angle & wheel speeds. Control commands are issued to the vehicle component ECUs via the steering wheel torque sensor, throttle position sensor, and brake position sensor. (Because the gas-powered Kia Soul isn’t brake by-wire capable, an auxiliary actuator is added to enable braking.) This low-level interface means that OSCC offers full-range control of the vehicle without altering the factory safety-case, spoofing CAN messages, or hacking ADAS features. -Although we currently support only the 2014 or later Kia Soul (w/ Kia Soul EV & Kia Niro support coming in Q3/Q4 2017, respectively), the API and firmware have been designed to make it easy to add new vehicle support. Additionally, the separation between API and firmware means it is easier to modify and test parts of your program without having to update the flashed OSCC modules. +Although OSCC currently supports only the 2014 or later Kia Soul (petrol and EV), the API and firmware have been designed to make it easy to add new vehicle support. Additionally, the separation between API and firmware means it is easier to modify and test parts of your program without having to update the flashed OSCC modules. Our [Wiki](https://github.com/PolySync/OSCC/wiki) is in the process of being updated to reflect the new changes, but contains a bunch of valuable information to help you get started in understanding the details of the system. From 08a6489ebdcf838a8e2f024bd82e1d90a5f60318 Mon Sep 17 00:00:00 2001 From: Robert Brown Date: Wed, 31 Jan 2018 11:38:58 -0800 Subject: [PATCH 09/20] Keep header organization and add hysteresis Prior to this commit the kia niro header did not have the hysteresis parameter in the header file and the HIGH LOW variables were in the opposite order of the other vehicles. This commit fixes that by making the header file follow the format common among existing header files. --- api/include/vehicles/kia_niro.h | 29 ++++++++++++++++++++--------- 1 file changed, 20 insertions(+), 9 deletions(-) diff --git a/api/include/vehicles/kia_niro.h b/api/include/vehicles/kia_niro.h index 4205bbb9..39ed27b2 100644 --- a/api/include/vehicles/kia_niro.h +++ b/api/include/vehicles/kia_niro.h @@ -122,12 +122,23 @@ typedef struct } kia_soul_obd_speed_data_s; +// **************************************************************************** +// VEHICLE AND BOARD PARAMETERS +// **************************************************************************** + /* * @brief Number of steps per volt corresponding to 4096 steps (2^12) across 5 volts. * */ #define STEPS_PER_VOLT ( 819.2 ) +/* + * @brief Length of time in ms for delay of signal reads to ensure fault is + * outside the range of noise in the signal. + * + */ +#define FAULT_HYSTERESIS ( 150 ) + @@ -151,53 +162,53 @@ typedef struct * @brief Minimum allowed voltage for the high spoof signal voltage. [volts] * */ -#define BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN ( 0.279 ) +#define BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN ( 0.609 ) /* * @brief Maximum allowed voltage for the high spoof signal voltage. [volts] * */ -#define BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX ( 1.386 ) +#define BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX ( 2.880 ) /** * @brief Minimum allowed voltage for the low spoof signal voltage. [volts] * */ -#define BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN ( 0.609 ) +#define BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN ( 0.279 ) /** * @brief Maximum allowed voltage for the low spoof signal voltage. [volts] * */ -#define BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX ( 2.880 ) +#define BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX ( 1.386 ) /* * @brief Minimum allowed value for the high spoof signal value. [steps] * * Equal to \ref BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN * \ref STEPS_PER_VOLT. */ -#define BRAKE_SPOOF_HIGH_SIGNAL_RANGE_MIN ( 229 ) +#define BRAKE_SPOOF_LOW_SIGNAL_RANGE_MIN ( 499 ) /* * @brief Minimum allowed value for the high spoof signal value. [steps] * * Equal to \ref BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX * \ref STEPS_PER_VOLT. */ -#define BRAKE_SPOOF_HIGH_SIGNAL_RANGE_MAX ( 1135 ) +#define BRAKE_SPOOF_LOW_SIGNAL_RANGE_MAX ( 2359 ) /* * @brief Minimum allowed value for the low spoof signal value. [steps] * * Equal to \ref BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN * \ref STEPS_PER_VOLT. */ -#define BRAKE_SPOOF_LOW_SIGNAL_RANGE_MIN ( 499 ) +#define BRAKE_SPOOF_HIGH_SIGNAL_RANGE_MIN ( 229 ) /* * @brief Minimum allowed value for the low spoof signal value. [steps] * * Equal to \ref BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX * \ref STEPS_PER_VOLT. */ -#define BRAKE_SPOOF_LOW_SIGNAL_RANGE_MAX ( 2359 ) +#define BRAKE_SPOOF_HIGH_SIGNAL_RANGE_MAX ( 1135 ) /* * @brief Calculation to convert a brake position to a low spoof voltage. @@ -219,7 +230,7 @@ typedef struct * @brief Value of the accelerator position that indicates operator override. [steps] * */ -#define BRAKE_PEDAL_OVERRIDE_THRESHOLD ( 150 ) +#define BRAKE_PEDAL_OVERRIDE_THRESHOLD ( 200 ) /* * @brief Minimum value of the high spoof signal that activates the brake lights. [steps] From 9b789cd778d181744664c06f9697622d06aced5d Mon Sep 17 00:00:00 2001 From: Adrien Thebo Date: Wed, 31 Jan 2018 12:08:59 -0800 Subject: [PATCH 10/20] Move fault detection state into caller function The original implementation for checking for grounded signal inputs uses a function local static variable to store state between calls, which prevented the reuse of that logic for detecting other errors that have some time component. This commit extracts that state to a struct that caller functions can make static, so that the common fault checking logic can be reused for other cases. --- .../common/libs/fault_check/oscc_check.cpp | 61 ++++++++++++++----- firmware/common/libs/fault_check/oscc_check.h | 48 +++++++++++++-- firmware/steering/src/steering_control.cpp | 10 ++- firmware/throttle/src/throttle_control.cpp | 11 +++- 4 files changed, 107 insertions(+), 23 deletions(-) diff --git a/firmware/common/libs/fault_check/oscc_check.cpp b/firmware/common/libs/fault_check/oscc_check.cpp index 3a6ab8ea..b9be6dd4 100644 --- a/firmware/common/libs/fault_check/oscc_check.cpp +++ b/firmware/common/libs/fault_check/oscc_check.cpp @@ -9,26 +9,57 @@ #include "oscc_check.h" #include "vehicles.h" +bool fault_exceeded_duration( + bool fault_active, + unsigned long max_fault_duration, + fault_state_s *state) +{ + bool faulted = false; -bool check_voltage_grounded( uint16_t high, uint16_t low ) { - - static unsigned long elapsed_detection_time = 0; - unsigned long current_time = millis(); - - bool ret = false; - if( (high == 0) || (low == 0) ) + if( fault_active == false ) { - if ( elapsed_detection_time == 0 ) - { - elapsed_detection_time = millis(); - } - - ret = ( current_time - elapsed_detection_time ) > FAULT_HYSTERESIS; + /* + * If a fault is not active, update the state to clear the fault active + * flag and clear the last fault time. + */ + state->fault_active = false; + state->fault_start_time = 0; } else { - elapsed_detection_time = 0; + unsigned long now = millis(); + + if( state->fault_active == false ) + { + /* We detected that a fault has just become active, update the state + * to indicate the fault is active and track the fault start time + * so we can determine if the fault has been active for longer than + * the maximum fault duration. + */ + state->fault_active = true; + state->fault_start_time = now; + } + + unsigned long fault_duration = now - state->fault_start_time; + + if( fault_duration >= max_fault_duration ) + { + /* The fault has been active for longer than the maximum + * acceptable duration. + */ + faulted = true; + } } - return ret; + return faulted; +} + +bool check_voltage_grounded( + uint16_t high, + uint16_t low, + unsigned long max_fault_duration, + fault_state_s *fault_state ) +{ + bool fault_active = (high == 0) || (low == 0); + return fault_exceeded_duration(fault_active, max_fault_duration, fault_state); } diff --git a/firmware/common/libs/fault_check/oscc_check.h b/firmware/common/libs/fault_check/oscc_check.h index a3a82138..87c58cc6 100644 --- a/firmware/common/libs/fault_check/oscc_check.h +++ b/firmware/common/libs/fault_check/oscc_check.h @@ -10,19 +10,57 @@ #include +typedef struct { + bool fault_active; + unsigned long fault_start_time; +} fault_state_s; + + +// **************************************************************************** +// Function: fault_exceeded_duration +// +// Purpose: Check if a fault has been active for longer than +// `max_fault_duration`. +// +// Returns: bool where true means a fault has been active for longer than +// `max_fault_duration`. +// +// Parameters: [in] fault_active - true if a fault has been detected. +// [in] max_fault_duration - the maximum time that the fault can be +// active without being treated as an +// error. +// [in] fault_state - whether the fault is active and at what time +// the fault was first detected. +// +// **************************************************************************** +bool fault_exceeded_duration( + bool fault_active, + unsigned long max_fault_duration, + fault_state_s *state); + + // **************************************************************************** // Function: check_voltage_grounded // -// Purpose: Check if the voltage is ground for the period of time determined -// by the configuration define of HYSTERESIS. This function is -// assumed to be called periodicly. +// Purpose: Check if the voltage is grounded for the period of time +// determined by `max_fault_duration`. This function is assumed to +// be called periodicly. // -// Returns: bool where true means there is a fault after the HYSTERESIS time +// Returns: bool where true means a fault has been active for longer than +// `max_fault_duration`. // // Parameters: [in] high - the high pin to check for ground // [in] low - the low pin to check for ground +// [in] max_fault_duration - the maximum time that the fault can be +// active +// [in] fault_state - whether the fault is active and at what time +// the fault was first detected // // **************************************************************************** -bool check_voltage_grounded( uint16_t high, uint16_t low ); +bool check_voltage_grounded( + uint16_t high, + uint16_t low, + unsigned long max_fault_duration, + fault_state_s *fault_state ); #endif /* _OSCC_CHECK_H_ */ diff --git a/firmware/steering/src/steering_control.cpp b/firmware/steering/src/steering_control.cpp index 45b2ebd5..257f3e72 100644 --- a/firmware/steering/src/steering_control.cpp +++ b/firmware/steering/src/steering_control.cpp @@ -36,6 +36,8 @@ static uint16_t filtered_diff = 0; void check_for_faults( void ) { + static fault_state_s grounded_fault_state = { .fault_active = false }; + steering_torque_s torque; if ( ( g_steering_control_state.enabled == true ) @@ -59,8 +61,14 @@ void check_for_faults( void ) filtered_diff); #endif + bool inputs_grounded = check_voltage_grounded( + torque.high, + torque.low, + FAULT_HYSTERESIS, + &grounded_fault_state); + // sensor pins tied to ground - a value of zero indicates disconnection - if( check_voltage_grounded( torque.high, torque.low ) ) + if( inputs_grounded == true ) { disable_control( ); diff --git a/firmware/throttle/src/throttle_control.cpp b/firmware/throttle/src/throttle_control.cpp index 39c10bae..46805558 100644 --- a/firmware/throttle/src/throttle_control.cpp +++ b/firmware/throttle/src/throttle_control.cpp @@ -24,6 +24,8 @@ static void read_accelerator_position_sensor( void check_for_faults( void ) { + static fault_state_s grounded_fault_state = { .fault_active = false }; + accelerator_position_s accelerator_position; if ( ( g_throttle_control_state.enabled == true ) @@ -34,9 +36,14 @@ void check_for_faults( void ) uint32_t accelerator_position_average = (accelerator_position.low + accelerator_position.high) / 2; + bool inputs_grounded = check_voltage_grounded( + accelerator_position.high, + accelerator_position.low, + FAULT_HYSTERESIS, + &grounded_fault_state); + // sensor pins tied to ground - a value of zero indicates disconnection - if( check_voltage_grounded( accelerator_position.high, - accelerator_position.low ) ) + if( inputs_grounded == true ) { disable_control( ); From d73b109e2996e1b748fea319d2b46ff82008c506 Mon Sep 17 00:00:00 2001 From: Robert Brown Date: Wed, 31 Jan 2018 17:01:44 -0800 Subject: [PATCH 11/20] Remove kia_niro specific DBC. The v1.2.0 release made the CAN command messages common across vehicle platforms. This commit removes the kia_niro specific dbc since it was added before v1.2.0 was integrated into this branch. --- api/include/vehicles/kia_niro.dbc | 109 ------------------------------ 1 file changed, 109 deletions(-) delete mode 100644 api/include/vehicles/kia_niro.dbc diff --git a/api/include/vehicles/kia_niro.dbc b/api/include/vehicles/kia_niro.dbc deleted file mode 100644 index 87304cf1..00000000 --- a/api/include/vehicles/kia_niro.dbc +++ /dev/null @@ -1,109 +0,0 @@ -VERSION "" - -NS_ : - BA_ - BA_DEF_ - BA_DEF_DEF_ - BA_DEF_DEF_REL_ - BA_DEF_REL_ - BA_DEF_SGTYPE_ - BA_REL_ - BA_SGTYPE_ - BO_TX_BU_ - BU_BO_REL_ - BU_EV_REL_ - BU_SG_REL_ - CAT_ - CAT_DEF_ - CM_ - ENVVAR_DATA_ - EV_DATA_ - FILTER - NS_DESC_ - SGTYPE_ - SGTYPE_VAL_ - SG_MUL_VAL_ - SIGTYPE_VALTYPE_ - SIG_GROUP_ - SIG_TYPE_REF_ - SIG_VALTYPE_ - VAL_ - VAL_TABLE_ - -BS_: - -BU_: BRAKE STEERING THROTTLE FAULT - -BO_ 80 BRAKE_ENABLE: 8 BRAKE - SG_ BRAKE_ENABLE_magic : 0|16@1+ (1,0) [0|0] "" BRAKE - SG_ BRAKE_ENABLE_reserved : 16|48@1+ (1,0) [0|0] "" BRAKE - -BO_ 81 BRAKE_DISABLE: 8 BRAKE - SG_ BRAKE_DISABLE_magic : 0|16@1+ (1,0) [0|0] "" BRAKE - SG_ BRAKE_DISABLE_reserved : 16|48@1+ (1,0) [0|0] "" BRAKE - -BO_ 96 BRAKE_COMMAND: 8 BRAKE - SG_ BRAKE_COMMAND_magic : 0|16@1+ (1,0) [0|0] "" BRAKE - SG_ BRAKE_COMMAND_spoof_low : 16|16@1+ (1,0) [0|0] "" BRAKE - SG_ BRAKE_COMMAND_spoof_high : 32|16@1+ (1,0) [0|0] "" BRAKE - SG_ BRAKE_COMMAND_reserved : 48|16@1+ (1,0) [0|0] "" BRAKE - -BO_ 97 BRAKE_REPORT: 8 BRAKE - SG_ BRAKE_REPORT_magic : 0|16@1+ (1,0) [0|0] "" BRAKE - SG_ BRAKE_REPORT_enabled : 16|8@1+ (1,0) [0|0] "" BRAKE - SG_ BRAKE_REPORT_operator_override : 24|8@1+ (1,0) [0|0] "" BRAKE - SG_ BRAKE_REPORT_dtcs : 32|8@1+ (1,0) [0|0] "" BRAKE - SG_ BRAKE_REPORT_reserved : 40|24@1+ (1,0) [0|0] "" BRAKE - -BO_ 84 STEERING_ENABLE: 8 STEERING - SG_ STEERING_ENABLE_magic : 0|16@1+ (1,0) [0|0] "" STEERING - SG_ STEERING_ENABLE_reserved : 16|48@1+ (1,0) [0|0] "" STEERING - -BO_ 85 STEERING_DISABLE: 8 STEERING - SG_ STEERING_DISABLE_magic : 0|16@1+ (1,0) [0|0] "" STEERING - SG_ STEERING_DISABLE_reserved : 16|48@1+ (1,0) [0|0] "" STEERING - -BO_ 100 STEERING_COMMAND: 8 STEERING - SG_ STEERING_COMMAND_magic : 0|16@1+ (1,0) [0|0] "" STEERING - SG_ STEERING_COMMAND_spoof_low : 16|16@1+ (1,0) [0|0] "" STEERING - SG_ STEERING_COMMAND_spoof_high : 32|16@1+ (1,0) [0|0] "" STEERING - SG_ STEERING_COMMAND_reserved : 48|16@1+ (1,0) [0|0] "" STEERING - -BO_ 101 STEERING_REPORT: 8 STEERING - SG_ STEERING_REPORT_magic : 0|16@1+ (1,0) [0|0] "" STEERING - SG_ STEERING_REPORT_enabled : 16|8@1+ (1,0) [0|0] "" STEERING - SG_ STEERING_REPORT_operator_override : 24|8@1+ (1,0) [0|0] "" STEERING - SG_ STEERING_REPORT_dtcs : 32|8@1+ (1,0) [0|0] "" STEERING - SG_ STEERING_REPORT_reserved : 40|24@1+ (1,0) [0|0] "" STEERING - -BO_ 82 THROTTLE_ENABLE: 8 THROTTLE - SG_ THROTTLE_ENABLE_magic : 0|16@1+ (1,0) [0|0] "" THROTTLE - SG_ THROTTLE_ENABLE_reserved : 16|48@1+ (1,0) [0|0] "" THROTTLE - -BO_ 83 THROTTLE_DISABLE: 8 THROTTLE - SG_ THROTTLE_DISABLE_magic : 0|16@1+ (1,0) [0|0] "" THROTTLE - SG_ THROTTLE_DISABLE_reserved : 16|48@1+ (1,0) [0|0] "" THROTTLE - -BO_ 98 THROTTLE_COMMAND: 8 THROTTLE - SG_ THROTTLE_COMMAND_magic : 0|16@1+ (1,0) [0|0] "" THROTTLE - SG_ THROTTLE_COMMAND_spoof_low : 16|16@1+ (1,0) [0|0] "" THROTTLE - SG_ THROTTLE_COMMAND_spoof_high : 32|16@1+ (1,0) [0|0] "" THROTTLE - SG_ THROTTLE_COMMAND_reserved : 48|16@1+ (1,0) [0|0] "" THROTTLE - -BO_ 99 THROTTLE_REPORT: 8 THROTTLE - SG_ THROTTLE_REPORT_magic : 0|16@1+ (1,0) [0|0] "" THROTTLE - SG_ THROTTLE_REPORT_enabled : 16|8@1+ (1,0) [0|0] "" THROTTLE - SG_ THROTTLE_REPORT_operator_override : 24|8@1+ (1,0) [0|0] "" THROTTLE - SG_ THROTTLE_REPORT_dtcs : 32|8@1+ (1,0) [0|0] "" THROTTLE - SG_ THROTTLE_REPORT_reserved : 40|24@1+ (1,0) [0|0] "" THROTTLE - -BO_ 153 FAULT_REPORT: 8 FAULT - SG_ FAULT_REPORT_magic : 0|16@1+ (1,0) [0|0] "" FAULT - SG_ FAULT_REPORT_fault_origin_id : 16|32@1+ (1,0) [0|0] "" FAULT - SG_ FAULT_REPORT_dtcs : 48|8@1+ (1,0) [0|0] "" FAULT - SG_ FAULT_REPORT_reserved : 56|8@1+ (1,0) [0|0] "" FAULT - -CM_ BU_ BRAKE "The OSCC brake module"; -CM_ BU_ STEERING "The OSCC steering module"; -CM_ BU_ THROTTLE "The OSCC throttle module"; -CM_ BU_ FAULT "The OSCC fault report"; From 045a9335cb8538187dec9047f185807c9a9062f5 Mon Sep 17 00:00:00 2001 From: Adrien Thebo Date: Thu, 1 Feb 2018 09:01:32 -0800 Subject: [PATCH 12/20] fault detection: distinguish between faults and fault conditions Some fault conditions have a time component where an error state has to be present for longer than an acceptable time period. In order to distinguish between these bad states and an actual fault detection this uses `condition` to indicate the anomalous state and `fault` to indicate a state that has been anomalous for longer than an acceptable period. --- .../common/libs/fault_check/oscc_check.cpp | 48 +++++++-------- firmware/common/libs/fault_check/oscc_check.h | 59 ++++++++++--------- firmware/steering/src/steering_control.cpp | 2 +- firmware/throttle/src/throttle_control.cpp | 2 +- 4 files changed, 58 insertions(+), 53 deletions(-) diff --git a/firmware/common/libs/fault_check/oscc_check.cpp b/firmware/common/libs/fault_check/oscc_check.cpp index b9be6dd4..6beebe65 100644 --- a/firmware/common/libs/fault_check/oscc_check.cpp +++ b/firmware/common/libs/fault_check/oscc_check.cpp @@ -9,42 +9,41 @@ #include "oscc_check.h" #include "vehicles.h" -bool fault_exceeded_duration( - bool fault_active, - unsigned long max_fault_duration, - fault_state_s *state) +bool condition_exceeded_duration( + bool condition_active, + unsigned long max_duration, + condition_state_s *state) { bool faulted = false; - if( fault_active == false ) + if( condition_active == false ) { /* - * If a fault is not active, update the state to clear the fault active - * flag and clear the last fault time. + * If a fault condition is not active, update the state to clear + * the condition active flag and reset the last detection time. */ - state->fault_active = false; - state->fault_start_time = 0; + state->condition_active = false; + state->condition_start_time = 0; } else { unsigned long now = millis(); - if( state->fault_active == false ) + if( state->condition_active == false ) { - /* We detected that a fault has just become active, update the state - * to indicate the fault is active and track the fault start time - * so we can determine if the fault has been active for longer than - * the maximum fault duration. + /* We just detected a condition that may lead to a fault. Update + * the state to track that the condition is active and store the + * first time of detection. */ - state->fault_active = true; - state->fault_start_time = now; + state->condition_active = true; + state->condition_start_time = now; } - unsigned long fault_duration = now - state->fault_start_time; + unsigned long duration = now - state->condition_start_time; - if( fault_duration >= max_fault_duration ) + if( duration >= max_duration ) { - /* The fault has been active for longer than the maximum + /* The fault fault has been active for longer than the maximum * acceptable duration. */ faulted = true; @@ -57,9 +56,12 @@ bool fault_exceeded_duration( bool check_voltage_grounded( uint16_t high, uint16_t low, - unsigned long max_fault_duration, - fault_state_s *fault_state ) + unsigned long max_duration, + condition_state_s *state ) { - bool fault_active = (high == 0) || (low == 0); - return fault_exceeded_duration(fault_active, max_fault_duration, fault_state); + bool condition_active = (high == 0) || (low == 0); + return condition_exceeded_duration( + condition_active, + max_duration, + state ); } diff --git a/firmware/common/libs/fault_check/oscc_check.h b/firmware/common/libs/fault_check/oscc_check.h index 87c58cc6..adac29aa 100644 --- a/firmware/common/libs/fault_check/oscc_check.h +++ b/firmware/common/libs/fault_check/oscc_check.h @@ -11,56 +11,59 @@ #include typedef struct { - bool fault_active; - unsigned long fault_start_time; -} fault_state_s; + bool condition_active; //!< Fault condition active flag + unsigned long condition_start_time; //!< Fault condition first detection time +} condition_state_s; // **************************************************************************** -// Function: fault_exceeded_duration +// Function: condition_exceeded_duration // -// Purpose: Check if a fault has been active for longer than -// `max_fault_duration`. +// Purpose: Check if a fault condition has been active for longer than +// `max_duration`. // -// Returns: bool where true means a fault has been active for longer than -// `max_fault_duration`. -// -// Parameters: [in] fault_active - true if a fault has been detected. -// [in] max_fault_duration - the maximum time that the fault can be -// active without being treated as an -// error. -// [in] fault_state - whether the fault is active and at what time -// the fault was first detected. +// Returns: bool where true means a fault condition has been active for +// longer than `max_duration`. // +// Parameters: [in] condition_active - true if a fault condition has been +// detected. +// [in] max_duration - the maximum time that the fault condition +// can be active before being reported as an +// actual fault. +// [in] condition_state - whether the fault condition is active +// and at what time the condition was first +// detected. // **************************************************************************** -bool fault_exceeded_duration( - bool fault_active, - unsigned long max_fault_duration, - fault_state_s *state); +bool condition_exceeded_duration( + bool condition_active, + unsigned long max_duration, + condition_state_s *state); // **************************************************************************** // Function: check_voltage_grounded // // Purpose: Check if the voltage is grounded for the period of time -// determined by `max_fault_duration`. This function is assumed to +// determined by `max_duration`. This function is assumed to // be called periodicly. // -// Returns: bool where true means a fault has been active for longer than -// `max_fault_duration`. +// Returns: bool where true means a fault condition has been active for +// longer than `max_duration`. // // Parameters: [in] high - the high pin to check for ground // [in] low - the low pin to check for ground -// [in] max_fault_duration - the maximum time that the fault can be -// active -// [in] fault_state - whether the fault is active and at what time -// the fault was first detected +// [in] max_duration - the maximum time that the fault condition +// can be active before being reported as an +// actual fault. +// [in] condition_state - whether the fault condition is active +// and at what time the condition was first +// detected. // // **************************************************************************** bool check_voltage_grounded( uint16_t high, uint16_t low, - unsigned long max_fault_duration, - fault_state_s *fault_state ); + unsigned long max_duration, + condition_state_s *condition_state ); #endif /* _OSCC_CHECK_H_ */ diff --git a/firmware/steering/src/steering_control.cpp b/firmware/steering/src/steering_control.cpp index 257f3e72..ab3c769e 100644 --- a/firmware/steering/src/steering_control.cpp +++ b/firmware/steering/src/steering_control.cpp @@ -36,7 +36,7 @@ static uint16_t filtered_diff = 0; void check_for_faults( void ) { - static fault_state_s grounded_fault_state = { .fault_active = false }; + static condition_state_s grounded_fault_state = { .condition_active = false }; steering_torque_s torque; diff --git a/firmware/throttle/src/throttle_control.cpp b/firmware/throttle/src/throttle_control.cpp index 46805558..87e4b36c 100644 --- a/firmware/throttle/src/throttle_control.cpp +++ b/firmware/throttle/src/throttle_control.cpp @@ -24,7 +24,7 @@ static void read_accelerator_position_sensor( void check_for_faults( void ) { - static fault_state_s grounded_fault_state = { .fault_active = false }; + static condition_state_s grounded_fault_state = { .condition_active = false }; accelerator_position_s accelerator_position; From e443448629c9a356c1100222848d02dcee3402c5 Mon Sep 17 00:00:00 2001 From: Adrien Thebo Date: Thu, 1 Feb 2018 12:31:50 -0800 Subject: [PATCH 13/20] fault detection: Clarify condition_state monitoring flag --- firmware/common/libs/fault_check/oscc_check.cpp | 6 +++--- firmware/common/libs/fault_check/oscc_check.h | 7 +++++-- firmware/steering/src/steering_control.cpp | 4 +++- firmware/throttle/src/throttle_control.cpp | 4 +++- 4 files changed, 14 insertions(+), 7 deletions(-) diff --git a/firmware/common/libs/fault_check/oscc_check.cpp b/firmware/common/libs/fault_check/oscc_check.cpp index 6beebe65..5f81e559 100644 --- a/firmware/common/libs/fault_check/oscc_check.cpp +++ b/firmware/common/libs/fault_check/oscc_check.cpp @@ -22,20 +22,20 @@ bool condition_exceeded_duration( * If a fault condition is not active, update the state to clear * the condition active flag and reset the last detection time. */ - state->condition_active = false; + state->monitoring_active = false; state->condition_start_time = 0; } else { unsigned long now = millis(); - if( state->condition_active == false ) + if( state->monitoring_active == false ) { /* We just detected a condition that may lead to a fault. Update * the state to track that the condition is active and store the * first time of detection. */ - state->condition_active = true; + state->monitoring_active = true; state->condition_start_time = now; } diff --git a/firmware/common/libs/fault_check/oscc_check.h b/firmware/common/libs/fault_check/oscc_check.h index adac29aa..3edf6a16 100644 --- a/firmware/common/libs/fault_check/oscc_check.h +++ b/firmware/common/libs/fault_check/oscc_check.h @@ -11,8 +11,11 @@ #include typedef struct { - bool condition_active; //!< Fault condition active flag - unsigned long condition_start_time; //!< Fault condition first detection time + //! Flag set if a fault condition has been detected and is being monitored. + bool monitoring_active; + + //! The time (in ms) when the fault condition was first detected. + unsigned long condition_start_time; } condition_state_s; diff --git a/firmware/steering/src/steering_control.cpp b/firmware/steering/src/steering_control.cpp index ab3c769e..637199de 100644 --- a/firmware/steering/src/steering_control.cpp +++ b/firmware/steering/src/steering_control.cpp @@ -36,7 +36,9 @@ static uint16_t filtered_diff = 0; void check_for_faults( void ) { - static condition_state_s grounded_fault_state = { .condition_active = false }; + static condition_state_s grounded_fault_state = { + .monitoring_active = false + }; steering_torque_s torque; diff --git a/firmware/throttle/src/throttle_control.cpp b/firmware/throttle/src/throttle_control.cpp index 87e4b36c..bef3fef3 100644 --- a/firmware/throttle/src/throttle_control.cpp +++ b/firmware/throttle/src/throttle_control.cpp @@ -24,7 +24,9 @@ static void read_accelerator_position_sensor( void check_for_faults( void ) { - static condition_state_s grounded_fault_state = { .condition_active = false }; + static condition_state_s grounded_fault_state = { + .monitoring_active = false + }; accelerator_position_s accelerator_position; From 2c21247cf2a903368f14d13c92f58d0c88b28342 Mon Sep 17 00:00:00 2001 From: Adrien Thebo Date: Thu, 1 Feb 2018 12:37:47 -0800 Subject: [PATCH 14/20] fault detection: initialize condition start time The static `condition_state_s` structs should be initialized at compilation time so initializing the variable is zero cost, and ensuring that all struct members are initialized to sensible values is always a safe bet. --- firmware/steering/src/steering_control.cpp | 3 ++- firmware/throttle/src/throttle_control.cpp | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/firmware/steering/src/steering_control.cpp b/firmware/steering/src/steering_control.cpp index 637199de..7c136314 100644 --- a/firmware/steering/src/steering_control.cpp +++ b/firmware/steering/src/steering_control.cpp @@ -37,7 +37,8 @@ static uint16_t filtered_diff = 0; void check_for_faults( void ) { static condition_state_s grounded_fault_state = { - .monitoring_active = false + .monitoring_active = false, + .condition_start_time = 0, }; steering_torque_s torque; diff --git a/firmware/throttle/src/throttle_control.cpp b/firmware/throttle/src/throttle_control.cpp index bef3fef3..6f7d9633 100644 --- a/firmware/throttle/src/throttle_control.cpp +++ b/firmware/throttle/src/throttle_control.cpp @@ -25,7 +25,8 @@ static void read_accelerator_position_sensor( void check_for_faults( void ) { static condition_state_s grounded_fault_state = { - .monitoring_active = false + .monitoring_active = false, + .condition_start_time = 0, }; accelerator_position_s accelerator_position; From 963571ee2de64505b151a163d885c0a3de22d6e4 Mon Sep 17 00:00:00 2001 From: Adrien Thebo Date: Thu, 1 Feb 2018 12:44:07 -0800 Subject: [PATCH 15/20] fault detection: fix a tyop'd comment. --- firmware/common/libs/fault_check/oscc_check.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/firmware/common/libs/fault_check/oscc_check.cpp b/firmware/common/libs/fault_check/oscc_check.cpp index 5f81e559..aab9a021 100644 --- a/firmware/common/libs/fault_check/oscc_check.cpp +++ b/firmware/common/libs/fault_check/oscc_check.cpp @@ -43,7 +43,7 @@ bool condition_exceeded_duration( if( duration >= max_duration ) { - /* The fault fault has been active for longer than the maximum + /* The fault condition has been active for longer than the maximum * acceptable duration. */ faulted = true; From 36852e6ce6b76b519e533cab981f631769f12288 Mon Sep 17 00:00:00 2001 From: Adrien Thebo Date: Thu, 1 Feb 2018 12:49:44 -0800 Subject: [PATCH 16/20] fault detection: Update soul_ev braking for new fn signature --- firmware/brake/kia_soul_ev/src/brake_control.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/firmware/brake/kia_soul_ev/src/brake_control.cpp b/firmware/brake/kia_soul_ev/src/brake_control.cpp index f92486a3..1cf69367 100644 --- a/firmware/brake/kia_soul_ev/src/brake_control.cpp +++ b/firmware/brake/kia_soul_ev/src/brake_control.cpp @@ -26,6 +26,11 @@ static void read_brake_pedal_position_sensor( void check_for_faults( void ) { + static condition_state_s grounded_fault_state = { + .monitoring_active = false, + .condition_start_time = 0, + }; + brake_pedal_position_s brake_pedal_position; if ( (g_brake_control_state.enabled == true) @@ -36,9 +41,13 @@ void check_for_faults( void ) uint32_t brake_pedal_position_average = (brake_pedal_position.low + brake_pedal_position.high) / 2; + bool inputs_grounded = check_voltage_grounded( + brake_pedal_position.high, + brake_pedal_position.low, + FAULT_HYSTERESIS, + &grounded_fault_state); // sensor pins tied to ground - a value of zero indicates disconnection - if( check_voltage_grounded( brake_pedal_position.high, - brake_pedal_position.low ) ) + if( inputs_grounded == true ) { disable_control( ); From 44bfdd845cf9e0e60604dc7953eae09146a4fc41 Mon Sep 17 00:00:00 2001 From: Adrien Thebo Date: Thu, 1 Feb 2018 12:58:16 -0800 Subject: [PATCH 17/20] fault detection: initialize condition state objects with macro Using a macro to initialize condition state objects deduplicates code, hides implementation details, and ensures consistent behaviors. --- firmware/brake/kia_soul_ev/src/brake_control.cpp | 5 +---- firmware/common/libs/fault_check/oscc_check.h | 2 ++ firmware/steering/src/steering_control.cpp | 5 +---- firmware/throttle/src/throttle_control.cpp | 5 +---- 4 files changed, 5 insertions(+), 12 deletions(-) diff --git a/firmware/brake/kia_soul_ev/src/brake_control.cpp b/firmware/brake/kia_soul_ev/src/brake_control.cpp index 1cf69367..2cde1912 100644 --- a/firmware/brake/kia_soul_ev/src/brake_control.cpp +++ b/firmware/brake/kia_soul_ev/src/brake_control.cpp @@ -26,10 +26,7 @@ static void read_brake_pedal_position_sensor( void check_for_faults( void ) { - static condition_state_s grounded_fault_state = { - .monitoring_active = false, - .condition_start_time = 0, - }; + static condition_state_s grounded_fault_state = CONDITION_STATE_INIT; brake_pedal_position_s brake_pedal_position; diff --git a/firmware/common/libs/fault_check/oscc_check.h b/firmware/common/libs/fault_check/oscc_check.h index 3edf6a16..f7432da1 100644 --- a/firmware/common/libs/fault_check/oscc_check.h +++ b/firmware/common/libs/fault_check/oscc_check.h @@ -18,6 +18,8 @@ typedef struct { unsigned long condition_start_time; } condition_state_s; +//! Initialize a condition state object to safe defaults. +#define CONDITION_STATE_INIT { .monitoring_active = false, .condition_start_time = 0 } // **************************************************************************** // Function: condition_exceeded_duration diff --git a/firmware/steering/src/steering_control.cpp b/firmware/steering/src/steering_control.cpp index 7c136314..3edb5229 100644 --- a/firmware/steering/src/steering_control.cpp +++ b/firmware/steering/src/steering_control.cpp @@ -36,10 +36,7 @@ static uint16_t filtered_diff = 0; void check_for_faults( void ) { - static condition_state_s grounded_fault_state = { - .monitoring_active = false, - .condition_start_time = 0, - }; + static condition_state_s grounded_fault_state = CONDITION_STATE_INIT; steering_torque_s torque; diff --git a/firmware/throttle/src/throttle_control.cpp b/firmware/throttle/src/throttle_control.cpp index 6f7d9633..c82b88f6 100644 --- a/firmware/throttle/src/throttle_control.cpp +++ b/firmware/throttle/src/throttle_control.cpp @@ -24,10 +24,7 @@ static void read_accelerator_position_sensor( void check_for_faults( void ) { - static condition_state_s grounded_fault_state = { - .monitoring_active = false, - .condition_start_time = 0, - }; + static condition_state_s grounded_fault_state = CONDITION_STATE_INIT; accelerator_position_s accelerator_position; From e9a6f48961f321a5c5b55f042a5a5576e5854eb2 Mon Sep 17 00:00:00 2001 From: Adrien Thebo Date: Thu, 1 Feb 2018 13:58:16 -0800 Subject: [PATCH 18/20] operator override: Add time component to detection This commit adds a time factor for detecting operator overrides. Spurious noise induced by vehicle systems may cause the ADC ground reference to jitter which in turn can cause spurious overrides. This commit mitigates the noise induced overrides by requiring that the override condition is present for longer than the maximum transient noise duration. --- .../brake/kia_soul_ev_niro/src/brake_control.cpp | 8 +++++++- firmware/brake/kia_soul_petrol/CMakeLists.txt | 2 ++ .../brake/kia_soul_petrol/src/brake_control.cpp | 16 +++++++++++++--- .../utils/serial_actuator/CMakeLists.txt | 2 ++ firmware/throttle/src/throttle_control.cpp | 8 +++++++- 5 files changed, 31 insertions(+), 5 deletions(-) diff --git a/firmware/brake/kia_soul_ev_niro/src/brake_control.cpp b/firmware/brake/kia_soul_ev_niro/src/brake_control.cpp index 2cde1912..fcfc3ef8 100644 --- a/firmware/brake/kia_soul_ev_niro/src/brake_control.cpp +++ b/firmware/brake/kia_soul_ev_niro/src/brake_control.cpp @@ -27,6 +27,7 @@ static void read_brake_pedal_position_sensor( void check_for_faults( void ) { static condition_state_s grounded_fault_state = CONDITION_STATE_INIT; + static condition_state_s operator_override_state = CONDITION_STATE_INIT; brake_pedal_position_s brake_pedal_position; @@ -38,6 +39,11 @@ void check_for_faults( void ) uint32_t brake_pedal_position_average = (brake_pedal_position.low + brake_pedal_position.high) / 2; + bool operator_overridden = condition_exceeded_duration( + brake_pedal_position_average >= BRAKE_PEDAL_OVERRIDE_THRESHOLD, + FAULT_HYSTERESIS, + &operator_override_state); + bool inputs_grounded = check_voltage_grounded( brake_pedal_position.high, brake_pedal_position.low, @@ -56,7 +62,7 @@ void check_for_faults( void ) DEBUG_PRINTLN( "Bad value read from brake pedal position sensor" ); } - else if ( brake_pedal_position_average >= BRAKE_PEDAL_OVERRIDE_THRESHOLD ) + else if ( operator_overridden == true ) { disable_control( ); diff --git a/firmware/brake/kia_soul_petrol/CMakeLists.txt b/firmware/brake/kia_soul_petrol/CMakeLists.txt index 3c809742..b854ddc9 100644 --- a/firmware/brake/kia_soul_petrol/CMakeLists.txt +++ b/firmware/brake/kia_soul_petrol/CMakeLists.txt @@ -9,6 +9,7 @@ generate_arduino_firmware( ${CMAKE_SOURCE_DIR}/common/libs/serial/oscc_serial.cpp ${CMAKE_SOURCE_DIR}/common/libs/can/oscc_can.cpp ${CMAKE_SOURCE_DIR}/common/libs/timer/oscc_timer.cpp + ${CMAKE_SOURCE_DIR}/common/libs/fault_check/oscc_check.cpp src/main.cpp src/globals.cpp src/accumulator.cpp @@ -30,6 +31,7 @@ target_include_directories( ${CMAKE_SOURCE_DIR}/common/libs/serial ${CMAKE_SOURCE_DIR}/common/libs/can ${CMAKE_SOURCE_DIR}/common/libs/timer + ${CMAKE_SOURCE_DIR}/common/libs/fault_check ${CMAKE_SOURCE_DIR}/../api/include) add_subdirectory(utils) diff --git a/firmware/brake/kia_soul_petrol/src/brake_control.cpp b/firmware/brake/kia_soul_petrol/src/brake_control.cpp index ca361054..56114288 100644 --- a/firmware/brake/kia_soul_petrol/src/brake_control.cpp +++ b/firmware/brake/kia_soul_petrol/src/brake_control.cpp @@ -18,7 +18,7 @@ #include "master_cylinder.h" #include "oscc_pid.h" #include "vehicles.h" - +#include "oscc_check.h" /* * @brief Number of consecutive faults that can occur when reading the @@ -100,6 +100,8 @@ void disable_control( void ) void check_for_operator_override( void ) { + static condition_state_s operator_override_state = CONDITION_STATE_INIT; + if( g_brake_control_state.enabled == true || g_brake_control_state.operator_override == true ) { @@ -107,8 +109,16 @@ void check_for_operator_override( void ) master_cylinder_read_pressure( &master_cylinder_pressure ); - if ( ( master_cylinder_pressure.sensor_1_pressure >= BRAKE_OVERRIDE_PEDAL_THRESHOLD_IN_DECIBARS ) || - ( master_cylinder_pressure.sensor_2_pressure >= BRAKE_OVERRIDE_PEDAL_THRESHOLD_IN_DECIBARS ) ) + bool override_detected = + ( master_cylinder_pressure.sensor_1_pressure >= BRAKE_OVERRIDE_PEDAL_THRESHOLD_IN_DECIBARS ) || + ( master_cylinder_pressure.sensor_2_pressure >= BRAKE_OVERRIDE_PEDAL_THRESHOLD_IN_DECIBARS ); + + bool operator_overridden = condition_exceeded_duration( + override_detected, + FAULT_HYSTERESIS, + &operator_override_state); + + if ( operator_overridden == true ) { disable_control( ); diff --git a/firmware/brake/kia_soul_petrol/utils/serial_actuator/CMakeLists.txt b/firmware/brake/kia_soul_petrol/utils/serial_actuator/CMakeLists.txt index ea132815..4627e237 100644 --- a/firmware/brake/kia_soul_petrol/utils/serial_actuator/CMakeLists.txt +++ b/firmware/brake/kia_soul_petrol/utils/serial_actuator/CMakeLists.txt @@ -9,6 +9,7 @@ generate_arduino_firmware( ${CMAKE_SOURCE_DIR}/common/libs/serial/oscc_serial.cpp ${CMAKE_SOURCE_DIR}/common/libs/can/oscc_can.cpp ${CMAKE_SOURCE_DIR}/common/libs/timer/oscc_timer.cpp + ${CMAKE_SOURCE_DIR}/common/libs/fault_check/oscc_check.cpp ../../src/globals.cpp ../../src/accumulator.cpp ../../src/helper.cpp @@ -29,4 +30,5 @@ target_include_directories( ${CMAKE_SOURCE_DIR}/common/libs/serial ${CMAKE_SOURCE_DIR}/common/libs/can ${CMAKE_SOURCE_DIR}/common/libs/timer + ${CMAKE_SOURCE_DIR}/common/libs/fault_check ${CMAKE_SOURCE_DIR}/../api/include) diff --git a/firmware/throttle/src/throttle_control.cpp b/firmware/throttle/src/throttle_control.cpp index c82b88f6..3745a0e8 100644 --- a/firmware/throttle/src/throttle_control.cpp +++ b/firmware/throttle/src/throttle_control.cpp @@ -25,6 +25,7 @@ static void read_accelerator_position_sensor( void check_for_faults( void ) { static condition_state_s grounded_fault_state = CONDITION_STATE_INIT; + static condition_state_s operator_override_state = CONDITION_STATE_INIT; accelerator_position_s accelerator_position; @@ -36,6 +37,11 @@ void check_for_faults( void ) uint32_t accelerator_position_average = (accelerator_position.low + accelerator_position.high) / 2; + bool operator_overridden = condition_exceeded_duration( + accelerator_position_average >= ACCELERATOR_OVERRIDE_THRESHOLD, + FAULT_HYSTERESIS, + &operator_override_state); + bool inputs_grounded = check_voltage_grounded( accelerator_position.high, accelerator_position.low, @@ -55,7 +61,7 @@ void check_for_faults( void ) DEBUG_PRINTLN( "Bad value read from accelerator position sensor" ); } - else if ( accelerator_position_average >= ACCELERATOR_OVERRIDE_THRESHOLD + else if ( operator_overridden == true && g_throttle_control_state.operator_override == false ) { disable_control( ); From 76d8be41659728a0ed3ae0aae051bb2c4717a536 Mon Sep 17 00:00:00 2001 From: Adrien Thebo Date: Fri, 2 Feb 2018 09:47:02 -0800 Subject: [PATCH 19/20] operator override: update test coverage for hysteresis --- .../kia_soul_petrol/tests/CMakeLists.txt | 2 ++ .../tests/features/checking_faults.feature | 2 +- .../step_definitions/checking_faults.cpp | 20 +++++++++++-------- .../features/step_definitions/common.cpp | 1 + .../tests/features/checking_faults.feature | 2 +- .../step_definitions/checking_faults.cpp | 12 +++++++---- .../features/step_definitions/common.cpp | 1 + 7 files changed, 26 insertions(+), 14 deletions(-) diff --git a/firmware/brake/kia_soul_petrol/tests/CMakeLists.txt b/firmware/brake/kia_soul_petrol/tests/CMakeLists.txt index a65710c0..5da335ed 100644 --- a/firmware/brake/kia_soul_petrol/tests/CMakeLists.txt +++ b/firmware/brake/kia_soul_petrol/tests/CMakeLists.txt @@ -11,6 +11,7 @@ add_library( ../src/helper.cpp ${CMAKE_SOURCE_DIR}/common/libs/can/oscc_can.cpp ${CMAKE_SOURCE_DIR}/common/libs/pid/oscc_pid.cpp + ${CMAKE_SOURCE_DIR}/common/libs/fault_check/oscc_check.cpp ${CMAKE_SOURCE_DIR}/common/testing/mocks/Arduino_mock.cpp ${CMAKE_SOURCE_DIR}/common/testing/mocks/mcp_can_mock.cpp ${CMAKE_SOURCE_DIR}/common/testing/mocks/DAC_MCP49xx_mock.cpp) @@ -22,6 +23,7 @@ target_include_directories( ${CMAKE_SOURCE_DIR}/common/include ${CMAKE_SOURCE_DIR}/common/libs/can ${CMAKE_SOURCE_DIR}/common/libs/pid + ${CMAKE_SOURCE_DIR}/common/libs/fault_check ${CMAKE_SOURCE_DIR}/common/libs/signal_smoothing ${CMAKE_SOURCE_DIR}/common/testing/mocks ${CMAKE_SOURCE_DIR}/../api/include) diff --git a/firmware/brake/kia_soul_petrol/tests/features/checking_faults.feature b/firmware/brake/kia_soul_petrol/tests/features/checking_faults.feature index 3c036eeb..9aa560b0 100644 --- a/firmware/brake/kia_soul_petrol/tests/features/checking_faults.feature +++ b/firmware/brake/kia_soul_petrol/tests/features/checking_faults.feature @@ -27,7 +27,7 @@ Feature: Timeouts and overrides Scenario Outline: Operator override Given brake control is enabled - When the operator applies to the brake pedal + When the operator applies to the brake pedal for 200 ms Then control should be disabled And a fault report should be published diff --git a/firmware/brake/kia_soul_petrol/tests/features/step_definitions/checking_faults.cpp b/firmware/brake/kia_soul_petrol/tests/features/step_definitions/checking_faults.cpp index f0b078c2..79c7f1bb 100644 --- a/firmware/brake/kia_soul_petrol/tests/features/step_definitions/checking_faults.cpp +++ b/firmware/brake/kia_soul_petrol/tests/features/step_definitions/checking_faults.cpp @@ -3,10 +3,10 @@ WHEN("^a sensor becomes temporarily disconnected$") g_mock_arduino_analog_read_return[0] = 0; g_mock_arduino_analog_read_return[1] = 0; + g_mock_arduino_millis_return = 1; check_for_sensor_faults(); - check_for_sensor_faults(); - + g_mock_arduino_millis_return += FAULT_HYSTERESIS / 2; check_for_sensor_faults(); } @@ -16,23 +16,27 @@ WHEN("^a sensor becomes permanently disconnected$") g_mock_arduino_analog_read_return[0] = 0; g_mock_arduino_analog_read_return[1] = 0; - // must call function enough times to exceed the fault limit - for( int i = 0; i < 100; ++i ) - { - check_for_sensor_faults(); - } + g_mock_arduino_millis_return = 1; + check_for_sensor_faults(); + + g_mock_arduino_millis_return += FAULT_HYSTERESIS * 2; + check_for_sensor_faults(); } -WHEN("^the operator applies (.*) to the brake pedal$") +WHEN("^the operator applies (.*) to the brake pedal for (\\d+) ms$") { REGEX_PARAM(int, pedal_val); + REGEX_PARAM(int, duration); g_mock_arduino_analog_read_return[10] = pedal_val; g_mock_arduino_analog_read_return[11] = pedal_val; + g_mock_arduino_millis_return = 1; check_for_operator_override(); + g_mock_arduino_millis_return += duration; + check_for_operator_override(); } diff --git a/firmware/brake/kia_soul_petrol/tests/features/step_definitions/common.cpp b/firmware/brake/kia_soul_petrol/tests/features/step_definitions/common.cpp index db167b02..d7c806e6 100644 --- a/firmware/brake/kia_soul_petrol/tests/features/step_definitions/common.cpp +++ b/firmware/brake/kia_soul_petrol/tests/features/step_definitions/common.cpp @@ -17,6 +17,7 @@ using namespace cgreen; extern unsigned long g_mock_arduino_micros_return; +extern unsigned long g_mock_arduino_millis_return; extern uint8_t g_mock_arduino_digital_write_pins[100]; extern uint8_t g_mock_arduino_digital_write_val[100]; diff --git a/firmware/throttle/tests/features/checking_faults.feature b/firmware/throttle/tests/features/checking_faults.feature index bc6f480b..e3f0c823 100644 --- a/firmware/throttle/tests/features/checking_faults.feature +++ b/firmware/throttle/tests/features/checking_faults.feature @@ -17,7 +17,7 @@ Feature: Checking for faults Scenario Outline: Operator override Given throttle control is enabled - When the operator applies to the accelerator + When the operator applies to the accelerator for 200 ms Then control should be disabled And a fault report should be published diff --git a/firmware/throttle/tests/features/step_definitions/checking_faults.cpp b/firmware/throttle/tests/features/step_definitions/checking_faults.cpp index 34f7f118..e32b2cb9 100644 --- a/firmware/throttle/tests/features/step_definitions/checking_faults.cpp +++ b/firmware/throttle/tests/features/step_definitions/checking_faults.cpp @@ -3,22 +3,26 @@ WHEN("^a sensor becomes permanently disconnected$") g_mock_arduino_analog_read_return[0] = 0; g_mock_arduino_analog_read_return[1] = 0; + g_mock_arduino_millis_return = 1; check_for_faults(); - // must call function enough times to exceed the fault limit - g_mock_arduino_millis_return = 105; - + g_mock_arduino_millis_return += FAULT_HYSTERESIS * 2; check_for_faults(); } -WHEN("^the operator applies (.*) to the accelerator$") +WHEN("^the operator applies (.*) to the accelerator for (\\d+) ms$") { REGEX_PARAM(int, throttle_sensor_val); + REGEX_PARAM(int, duration); g_mock_arduino_analog_read_return[0] = throttle_sensor_val; g_mock_arduino_analog_read_return[1] = throttle_sensor_val; + g_mock_arduino_millis_return = 1; + check_for_faults(); + + g_mock_arduino_millis_return += duration; check_for_faults(); } diff --git a/firmware/throttle/tests/features/step_definitions/common.cpp b/firmware/throttle/tests/features/step_definitions/common.cpp index f4166445..5440350e 100644 --- a/firmware/throttle/tests/features/step_definitions/common.cpp +++ b/firmware/throttle/tests/features/step_definitions/common.cpp @@ -12,6 +12,7 @@ #include "can_protocols/fault_can_protocol.h" #include "can_protocols/throttle_can_protocol.h" #include "globals.h" +#include "vehicles.h" using namespace cgreen; From 162a2c99efb13b33bc537dbeb0d00e161158402a Mon Sep 17 00:00:00 2001 From: Adrien Thebo Date: Fri, 2 Feb 2018 11:19:09 -0800 Subject: [PATCH 20/20] operator override: Update EV brake tests for hysteresis --- .../tests/features/checking_faults.feature | 2 +- .../features/step_definitions/checking_faults.cpp | 11 +++++++---- .../tests/features/step_definitions/common.cpp | 1 + 3 files changed, 9 insertions(+), 5 deletions(-) diff --git a/firmware/brake/kia_soul_ev_niro/tests/features/checking_faults.feature b/firmware/brake/kia_soul_ev_niro/tests/features/checking_faults.feature index 3dd374c0..00daab88 100644 --- a/firmware/brake/kia_soul_ev_niro/tests/features/checking_faults.feature +++ b/firmware/brake/kia_soul_ev_niro/tests/features/checking_faults.feature @@ -18,7 +18,7 @@ Feature: Checking for faults Scenario Outline: Operator override Given brake control is enabled - When the operator applies to the accelerator + When the operator applies to the brake pedal for 200 ms Then control should be disabled And a fault report should be published diff --git a/firmware/brake/kia_soul_ev_niro/tests/features/step_definitions/checking_faults.cpp b/firmware/brake/kia_soul_ev_niro/tests/features/step_definitions/checking_faults.cpp index aef811fe..245b8b8d 100644 --- a/firmware/brake/kia_soul_ev_niro/tests/features/step_definitions/checking_faults.cpp +++ b/firmware/brake/kia_soul_ev_niro/tests/features/step_definitions/checking_faults.cpp @@ -3,23 +3,26 @@ WHEN("^a sensor becomes permanently disconnected$") g_mock_arduino_analog_read_return[0] = 0; g_mock_arduino_analog_read_return[1] = 0; + g_mock_arduino_millis_return = 1; check_for_faults(); - // must call function enough times to exceed the fault limit - g_mock_arduino_millis_return = 105; - + g_mock_arduino_millis_return += FAULT_HYSTERESIS * 2; check_for_faults(); } -WHEN("^the operator applies (.*) to the accelerator$") +WHEN("^the operator applies (.*) to the brake pedal for (\\d+) ms$") { REGEX_PARAM(int, brake_sensor_val); + REGEX_PARAM(int, duration); g_mock_arduino_analog_read_return[0] = brake_sensor_val; g_mock_arduino_analog_read_return[1] = brake_sensor_val; + g_mock_arduino_millis_return = 1; + check_for_faults(); + g_mock_arduino_millis_return += duration; check_for_faults(); } diff --git a/firmware/brake/kia_soul_ev_niro/tests/features/step_definitions/common.cpp b/firmware/brake/kia_soul_ev_niro/tests/features/step_definitions/common.cpp index 2e1ec6e7..d3ca2dd0 100644 --- a/firmware/brake/kia_soul_ev_niro/tests/features/step_definitions/common.cpp +++ b/firmware/brake/kia_soul_ev_niro/tests/features/step_definitions/common.cpp @@ -12,6 +12,7 @@ #include "can_protocols/fault_can_protocol.h" #include "can_protocols/brake_can_protocol.h" #include "globals.h" +#include "vehicles.h" using namespace cgreen;