diff --git a/README.md b/README.md index ba4d4431..205944d9 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. @@ -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/OsccConfig.cmake b/api/OsccConfig.cmake index 061fb1d1..5fc0cc72 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_NIRO) + add_definitions(-DKIA_NIRO) else() message(FATAL_ERROR "No platform selected") endif() diff --git a/api/include/vehicles.h b/api/include/vehicles.h index 1fca1dbf..3e2f418b 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_NIRO) +#include "vehicles/kia_niro.h" #endif #define CONSTRAIN(amt, low, high) ((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt))) diff --git a/api/include/vehicles/kia_niro.h b/api/include/vehicles/kia_niro.h new file mode 100644 index 00000000..39ed27b2 --- /dev/null +++ b/api/include/vehicles/kia_niro.h @@ -0,0 +1,460 @@ +/** + * @file kia_niro.h + * @brief Kia Niro specific macros. + * + */ + + +#ifndef _KIA_NIRO_PLATFORM_INFO_H_ +#define _KIA_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 Niro's OBD steering wheel angle CAN frame. + * + */ +#define KIA_SOUL_OBD_STEERING_WHEEL_ANGLE_CAN_ID ( 0x2B0 ) + +/* + * @brief ID of the Kia Niro's OBD wheel speed CAN frame. + * + */ +#define KIA_SOUL_OBD_WHEEL_SPEED_CAN_ID ( 0x386 ) + +/* + * @brief ID of the Kia Niro's OBD brake pressure CAN frame. + * + */ +#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 + * + */ +#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 +{ + uint8_t wheel_speed_front_left; /* 1/4 kph */ + + uint8_t reserved_0; /* Reserved. */ + + uint8_t wheel_speed_front_right; /* 1/4 kph */ + + 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; + +/** + * @brief Brake pressure message data. + * + */ +typedef struct +{ + uint8_t reserved_0[4]; /* Reserved. */ + + int16_t master_cylinder_pressure; /* 1/10th of a bar per bit */ + + 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; + + +// **************************************************************************** +// 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 ) + + + + +// **************************************************************************** +// 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 high spoof signal voltage. [volts] + * + */ +#define BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN ( 0.609 ) + +/* + * @brief Maximum allowed voltage for the high spoof signal voltage. [volts] + * + */ +#define BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX ( 2.880 ) + +/** + * @brief Minimum allowed voltage for the low spoof signal voltage. [volts] + * + */ +#define BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN ( 0.279 ) + +/** + * @brief Maximum allowed voltage for the low spoof signal voltage. [volts] + * + */ +#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_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_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_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_HIGH_SIGNAL_RANGE_MAX ( 1135 ) + +/* + * @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 ( 200 ) + +/* + * @brief Minimum value of the high spoof signal that activates the brake lights. [steps] + * + */ +#define BRAKE_LIGHT_SPOOF_HIGH_THRESHOLD ( 300 ) + +/* + * @brief Minimum value of the low spoof signal that activates the brake lights. [steps] + * + */ +#define BRAKE_LIGHT_SPOOF_LOW_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.380 ) + +/* + * @brief Maximum allowed voltage for the low spoof signal voltage. [volts] + * + */ +#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.757 ) + +/** + * @brief Maximum allowed voltage for the high spoof signal voltage. [volts] + * + */ +#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 ( 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 ( 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 ( 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 ( 3446 ) + +/* + * @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/firmware/CMakeLists.txt b/firmware/CMakeLists.txt index 649a5c6d..6fbab699 100644 --- a/firmware/CMakeLists.txt +++ b/firmware/CMakeLists.txt @@ -16,7 +16,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_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") endif() @@ -64,6 +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_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) @@ -87,6 +91,8 @@ else() add_definitions(-DKIA_SOUL) elseif(KIA_SOUL_EV) add_definitions(-DKIA_SOUL_EV) + 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 f01a1ddd..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) - add_subdirectory(kia_soul_ev) +elseif(KIA_SOUL_EV OR KIA_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/src/brake_control.cpp b/firmware/brake/kia_soul_ev_niro/src/brake_control.cpp similarity index 85% rename from firmware/brake/kia_soul_ev/src/brake_control.cpp rename to firmware/brake/kia_soul_ev_niro/src/brake_control.cpp index f92486a3..fcfc3ef8 100644 --- a/firmware/brake/kia_soul_ev/src/brake_control.cpp +++ b/firmware/brake/kia_soul_ev_niro/src/brake_control.cpp @@ -26,6 +26,9 @@ 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; if ( (g_brake_control_state.enabled == true) @@ -36,9 +39,18 @@ 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, + 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( ); @@ -50,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_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/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 89% 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 index 3dd374c0..00daab88 100644 --- a/firmware/brake/kia_soul_ev/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/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 78% 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 index aef811fe..245b8b8d 100644 --- 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 @@ -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/tests/features/step_definitions/common.cpp b/firmware/brake/kia_soul_ev_niro/tests/features/step_definitions/common.cpp similarity index 99% 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 index 2e1ec6e7..d3ca2dd0 100644 --- 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 @@ -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; 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 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/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/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/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 diff --git a/firmware/common/libs/fault_check/oscc_check.cpp b/firmware/common/libs/fault_check/oscc_check.cpp index 3a6ab8ea..aab9a021 100644 --- a/firmware/common/libs/fault_check/oscc_check.cpp +++ b/firmware/common/libs/fault_check/oscc_check.cpp @@ -9,26 +9,59 @@ #include "oscc_check.h" #include "vehicles.h" +bool condition_exceeded_duration( + bool condition_active, + unsigned long max_duration, + condition_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( condition_active == false ) { - if ( elapsed_detection_time == 0 ) - { - elapsed_detection_time = millis(); - } - - ret = ( current_time - elapsed_detection_time ) > FAULT_HYSTERESIS; + /* + * If a fault condition is not active, update the state to clear + * the condition active flag and reset the last detection time. + */ + state->monitoring_active = false; + state->condition_start_time = 0; } else { - elapsed_detection_time = 0; + unsigned long now = millis(); + + 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->monitoring_active = true; + state->condition_start_time = now; + } + + unsigned long duration = now - state->condition_start_time; + + if( duration >= max_duration ) + { + /* The fault condition 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_duration, + condition_state_s *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 a3a82138..f7432da1 100644 --- a/firmware/common/libs/fault_check/oscc_check.h +++ b/firmware/common/libs/fault_check/oscc_check.h @@ -10,19 +10,65 @@ #include +typedef struct { + //! 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; + +//! Initialize a condition state object to safe defaults. +#define CONDITION_STATE_INIT { .monitoring_active = false, .condition_start_time = 0 } + +// **************************************************************************** +// Function: condition_exceeded_duration +// +// Purpose: Check if a fault condition has been active for longer than +// `max_duration`. +// +// 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 condition_exceeded_duration( + bool condition_active, + unsigned long max_duration, + condition_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_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 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_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 ); +bool check_voltage_grounded( + uint16_t high, + uint16_t low, + 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 45b2ebd5..3edb5229 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 condition_state_s grounded_fault_state = CONDITION_STATE_INIT; + 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..3745a0e8 100644 --- a/firmware/throttle/src/throttle_control.cpp +++ b/firmware/throttle/src/throttle_control.cpp @@ -24,6 +24,9 @@ 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; if ( ( g_throttle_control_state.enabled == true ) @@ -34,9 +37,19 @@ 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, + 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( ); @@ -48,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( ); 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;