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_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; 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/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( ); 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;