diff --git a/Jenkinsfile b/Jenkinsfile index 61c57c16..506e37a7 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -11,20 +11,16 @@ node('arduino') { } stage('Build') { parallel 'kia soul firmware': { - sh 'cd platforms && mkdir build && cd build && cmake .. -DBUILD_KIA_SOUL=ON -DCMAKE_BUILD_TYPE=Release && make' - }, 'joystick commander': { - sh 'cd utils/joystick_commander && mkdir build && cd build && cmake .. && make' - }, 'diagnostics tool': { - sh 'cd utils/diagnostics_tool && mkdir build && cd build && cmake .. && make' + sh 'cd firmware && mkdir build && cd build && cmake .. -DKIA_SOUL=ON -DCMAKE_BUILD_TYPE=Release && make' } echo 'Build Complete!' } stage('Test') { parallel 'unit tests': { - sh 'cd platforms && mkdir build_unit_tests && cd build_unit_tests && cmake .. -DTESTS=ON -DCMAKE_BUILD_TYPE=Release && make run-unit-tests' + sh 'cd firmware && mkdir build_unit_tests && cd build_unit_tests && cmake .. -DKIA_SOUL=ON -DTESTS=ON -DCMAKE_BUILD_TYPE=Release && make run-unit-tests' echo 'Unit Tests Complete!' }, 'property-based tests': { - sh 'cd platforms && mkdir build_property_tests && cd build_property_tests && cmake .. -DTESTS=ON -DCMAKE_BUILD_TYPE=Release && make run-property-tests' + sh 'cd firmware && mkdir build_property_tests && cd build_property_tests && cmake .. -DKIA_SOUL=ON -DTESTS=ON -DCMAKE_BUILD_TYPE=Release && make run-property-tests' echo 'Property-Based Tests Complete!' }, 'acceptance tests': { echo 'Acceptance Tests Complete!' diff --git a/README.md b/README.md index 1676859b..dd3f9723 100644 --- a/README.md +++ b/README.md @@ -1,20 +1,40 @@ -The Open Source Car Control Project is a hardware and software project detailing the conversion of a -late model vehicle into an autonomous driving research and development vehicle. +Open Source Car Control (OSCC) is an assemblage of software and hardware designs that enable computer control of modern cars in order to facilitate the development of autonomous vehicle technology. It is a modular and stable way of using software to interface with a vehicle’s communications network and control systems. -See the [Wiki](https://github.com/PolySync/OSCC/wiki) for full documentation, details, and other -information. +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. -# Versions +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. + + +## Repository Contents + +* **api** - Software API, so your program can seamlessly talk to our modules. +* **firmware** - Arduino libraries and firmware for the OSCC modules. +* **hardware** - PCB schematics and board designs for control modules. + + +## Boards + +The sensor interface and actuator control board schematics and design files are located in the +`hardware/boards` directory. If you don't have the time or fabrication resources, the boards can be +purchased as a kit from the [OSCC website](http://oscc.io). + +Thanks to [Trey German](https://www.polymorphiclabs.com) and [Macrofab](https://macrofab.com/) for +help designing and manufacturing the custom boards. + +## Versions It's important that the correct version of the firmware is used with the correct versions of the module boards. As the boards are updated with additional pins and other features, the firmware is modified accordingly to use them. Mismatched versions will cause problems. +*Your hardware version is printed on the front of the OSCC shield.* + Consult the following table for version compatibility. | Actuator Board | Firmware | @@ -26,35 +46,13 @@ Consult the following table for version compatibility. | >= v1.1.0 | >= v0.7 | -# Repository Contents -* **3d_models** - Technical drawings and 3D files for board enclosures and other useful parts -* **boards** - PCB schematics and board designs for control modules -* **platforms** - Arduino code and relevant files for the specific platforms -* **utils** - Utilities for controlling and interfacing with a platform +# Building and Uploading Firmware -Within a specific platform (e.g., `kia_soul`), there are: -* **3d_models** - Technical drawings and 3D files related to that platform -* **firmware** - Arduino code for the control modules - - -# Boards - -The sensor interface and actuator control board schematics and design files are located in the -`boards` directory. If you don't have the time or fabrication resources, the boards can be -purchased as a kit from the [OSCC website](http://oscc.io). - -Thanks to [Trey German](https://www.polymorphiclabs.com) and [Macrofab](https://macrofab.com/) for -help designing the boards and getting the boards made. - - -# Building and Uploading Arduino Firmware - -The OSCC Project is built around a number of individual modules that interoperate to create a fully -controllable vehicle. These modules are built from Arduinos and Arduino shields designed specifically -for interfacing with various vehicle components. Once these modules have been programmed with the -accompanying firmware and installed into the vehicle, the vehicle is ready to receive control commands -sent over a CAN bus from a computer running a control program. +The OSCC Project is built around a number of individual firmware modules that inter-operate to allow communication with your vehicle. +These modules are built from Arduinos and Arduino shields designed specifically for interfacing with various vehicle components. +Once these modules have been installed in the vehicle and flashed with the firmware, the API can be used to +receive reports from the car and send spoofed commands. ## Pre-requisites @@ -72,27 +70,27 @@ Check out [Arduino CMake](https://github.com/queezythegreat/arduino-cmake) for m ## Building the Firmware -Navigate to the `platforms` directory and create a build directory inside of it: +Navigate to the `firmware` directory and create a build directory inside of it: ``` -cd platforms +cd firmware mkdir build cd build ``` -To generate Makefiles, tell CMake which platform to build firmware for. If you want to build -the firmware for the Kia Soul: +To generate Makefiles, tell CMake which platform to build firmware for. For example, if you want to build +firmware for the Kia Soul: ``` -cmake .. -DBUILD_KIA_SOUL=ON +cmake .. -DKIA_SOUL=ON ``` By default, your firmware will have debug symbols which is good for debugging but increases -the size of the firmware significantly. To compile without debug symbols and optimizatons +the size of the firmware significantly. To compile without debug symbols and optimizations enabled, use the following instead: ``` -cmake .. -DBUILD_KIA_SOUL=ON -DCMAKE_BUILD_TYPE=Release +cmake .. -DKIA_SOUL=ON -DCMAKE_BUILD_TYPE=Release ``` This will generate the necessary files for building. @@ -107,10 +105,10 @@ If you'd like to build only a specific module, you can provide a target name to `make` for whichever module you'd like to build: ``` -make kia-soul-brake -make kia-soul-can-gateway -make kia-soul-steering -make kia-soul-throttle +make brake +make gateway +make steering +make throttle ``` ## Uploading the Firmware @@ -125,7 +123,7 @@ is configured to expect each module to be `/dev/ttyACM0`, so if you connect a single module to your machine, you can flash it without changing anything: ``` -make kia-soul-throttle-upload +make throttle-upload ``` However, if you want to flash all modules, you need to change the ports in @@ -135,13 +133,13 @@ throttle) so that they are assigned `/dev/ttyACM0` through `/dev/ttyACM3` in a known order. You can then change the ports during the `cmake ..` step: ``` -cmake .. -DBUILD_KIA_SOUL=ON -DSERIAL_PORT_BRAKE=/dev/ttyACM0 -DSERIAL_PORT_CAN_GATEWAY=/dev/ttyACM1 -DSERIAL_PORT_STEERING=/dev/ttyACM2 -DSERIAL_PORT_THROTTLE=/dev/ttyACM3 +cmake .. -DKIA_SOUL=ON -DSERIAL_PORT_BRAKE=/dev/ttyACM0 -DSERIAL_PORT_CAN_GATEWAY=/dev/ttyACM1 -DSERIAL_PORT_STEERING=/dev/ttyACM2 -DSERIAL_PORT_THROTTLE=/dev/ttyACM3 ``` Then you can flash all with one command: ``` -make kia-soul-all-upload +make all-upload ``` Sometimes it takes a little while for the Arduino to initialize once connected, so if there is an @@ -168,16 +166,16 @@ the module you want to monitor is connected to ports for each module). The default baud rate is `115200` but you can change it: ``` -cmake .. -DBUILD_KIA_SOUL=ON -DDEBUG=ON -DSERIAL_PORT_THROTTLE=/dev/ttyACM0 -DSERIAL_BAUD_THROTTLE=19200 +cmake .. -DKIA_SOUL=ON -DDEBUG=ON -DSERIAL_PORT_THROTTLE=/dev/ttyACM0 -DSERIAL_BAUD_THROTTLE=19200 ``` You can use a module's `monitor` target to automatically run `screen`, or a module's `monitor-log` target to run `screen` and output to a file called -`screenlog.0` in your current directory: +`screenlog.0` in the module's build directory: ``` -make kia-soul-brake-monitor -make kia-soul-brake-monitor-log +make brake-monitor +make brake-monitor-log ``` You can exit `screen` with `C-a \`. @@ -190,7 +188,7 @@ Be aware that using serial printing can affect the timing of the firmware. You m strange behavior while printing that does not occur otherwise. -# Tests +## Tests There are two types of tests available: unit and property-based. @@ -198,26 +196,27 @@ Building and running the tests is similar to the firmware itself, but you must i CMake to build the tests instead of the firmware with the `-DTESTS=ON` flag. We also pass the `-DCMAKE_BUILD_TYPE=Release` flag so that CMake will disable debug symbols and enable optimizations, good things to do when running tests to ensure nothing breaks with -optimizations. +optimizations. Lastly, you must tell the tests which vehicle header to use for +the tests (e.g., `-DKIA_SOUL=ON`). ``` -cd platforms +cd firmware mkdir build cd build -cmake .. -DTESTS=ON -DCMAKE_BUILD_TYPE=Release +cmake .. -DTESTS=ON -DCMAKE_BUILD_TYPE=Release -DKIA_SOUL=ON ``` -## Unit Tests +### Unit Tests Each module has a suite of unit tests that use **Cucumber** with **Cgreen**. There are prebuilt -64-bit Linux versions in `platforms/common/testing/framework`. Boost is required for Cucumber-CPP +64-bit Linux versions in `firmware/common/testing/framework`. Boost is required for Cucumber-CPP and has been statically linked into `libcucumber-cpp.a`. If you need to build your own versions you can use the provided script `build_test_framework.sh` which will install the Boost dependencies (needed for building), clone the needed repositories with specific hashes, build the Cgreen and Cucumber-CPP libraries, and place static Boost in the Cucumber-CPP library. The built will be placed in an `oscc_test_framework` directory in the directory that you ran the script from. You can then copy `oscc_test_framework/cucumber-cpp` and `oscc_test_framework/cgreen` to -`platforms/common/testing/framework`. +`firmware/common/testing/framework`. You must have **Cucumber** installed to run the tests: @@ -235,16 +234,10 @@ make run-unit-tests Each module's test can also be run individually: ``` -make run-kia-soul-brake-unit-tests -make run-kia-soul-can-gateway-unit-tests -make run-kia-soul-steering-unit-tests -make run-kia-soul-throttle-unit-tests -``` - -Or run only the tests of a single platform: - -``` -make run-kia-soul-unit-tests +make run-brake-unit-tests +make run-can-gateway-unit-tests +make run-steering-unit-tests +make run-throttle-unit-tests ``` If everything works correctly you should see something like this: @@ -255,14 +248,14 @@ Feature: Receiving commands Commands received from a controller should be processed and acted upon. - Scenario Outline: Enable throttle command sent from controller # platforms/kia_soul/firmware/throttle/tests/features/receiving_commands.feature:8 - Given throttle control is disabled # platforms/kia_soul/firmware/throttle/tests/features/receiving_commands.feature:9 - And the accelerator position sensors have a reading of # platforms/kia_soul/firmware/throttle/tests/features/receiving_commands.feature:10 - When an enable throttle command is received # platforms/kia_soul/firmware/throttle/tests/features/receiving_commands.feature:12 - Then control should be enabled # platforms/kia_soul/firmware/throttle/tests/features/receiving_commands.feature:14 - And the last command timestamp should be set # platforms/kia_soul/firmware/throttle/tests/features/receiving_commands.feature:15 - And should be written to DAC A # platforms/kia_soul/firmware/throttle/tests/features/receiving_commands.feature:16 - And should be written to DAC B # platforms/kia_soul/firmware/throttle/tests/features/receiving_commands.feature:17 + Scenario Outline: Enable throttle command sent from controller # firmware/throttle/tests/features/receiving_commands.feature:8 + Given throttle control is disabled # firmware/throttle/tests/features/receiving_commands.feature:9 + And the accelerator position sensors have a reading of # firmware/throttle/tests/features/receiving_commands.feature:10 + When an enable throttle command is received # firmware/throttle/tests/features/receiving_commands.feature:12 + Then control should be enabled # firmware/throttle/tests/features/receiving_commands.feature:14 + And the last command timestamp should be set # firmware/throttle/tests/features/receiving_commands.feature:15 + And should be written to DAC A # firmware/throttle/tests/features/receiving_commands.feature:16 + And should be written to DAC B # firmware/throttle/tests/features/receiving_commands.feature:17 Examples: | sensor_val | dac_a_val | dac_b_val | @@ -290,23 +283,16 @@ make run-property-tests Each module's test can also be run individually: ``` -make run-kia-soul-brake-property-tests -make run-kia-soul-steering-property-tests -make run-kia-soul-throttle-property-tests +make run-brake-property-tests +make run-steering-property-tests +make run-throttle-property-tests make run-pid-library-property-tests ``` -Or run only the tests of a single platform: - -``` -make run-kia-soul-property-tests -``` - Once the tests have completed, the output should look similar to the following: ``` running 7 tests -test bindgen_test_layout_pid_s ... ok test check_integral_term ... ok test check_derivative_term ... ok test check_proportional_term ... ok @@ -323,7 +309,7 @@ running 0 tests test result: ok. 0 passed; 0 failed; 0 ignored; 0 measured ``` -## Run All Tests +### Run All Tests Finally, you can run all available tests: @@ -332,7 +318,7 @@ make run-all-tests ``` -# Easier CMake Configuration +## Easier CMake Configuration If you have a lot of `-D` commands to pass to CMake (e.g., configuring the serial port and baud rates of all of the modules), you can instead configure with a GUI @@ -345,7 +331,7 @@ sudo apt install cmake-gui Then use `cmake-gui` where you would normally use `cmake`: ``` -cd platforms +cd firmware mkdir build cd build cmake-gui .. @@ -359,81 +345,75 @@ with `-D` commands. When you're done, click `Configure` again and then click the `Generate` button. You can then close `cmake-gui` and run any `make` commands like you normally would. - -# Controlling Your Vehicle +# Controlling Your Vehicle - an Example Application Now that all your Arduino modules are properly setup, it is time to start sending control commands. -There is an example application to do this that uses a gamepad. The example interfaces to the -joystick gamepad via the SDL2 game controller library and sends CAN commands over the control CAN bus -via socketcan. These CAN control commands are interpreted by the respective Arduino -modules and used to actuate the vehicle. This application has been tested with a Logitech F310 gamepad -and a wired Xbox 360 controller, but should work with any SDL2 supported game controller. Controllers -with rumble capabilities will provide feedback when OSCC is enabled or disabled. - -## Pre-requisites: - -An SDL2 supported gamepad is required, and the SDL2 library and CANlib SDK need to -be pre-installed. A CAN interface adapter, such as the [Kvaser Leaf Light](https://www.kvaser.com), -is also required. +We've created an example application, joystick commander, that uses the OSCC API to interface with the firmware, allowing you to send commands using a game controller and receive reports from the on-board OBD-II CAN. These commands are converted into CAN messages, which the OSCC API sends to the respective Arduino modules and are used to actuate the vehicle. -[Xbox 360 Wired Controller](https://www.amazon.com/dp/B004QRKWLA) +[OSCC Joystick Commander](https://github.com/PolySync/oscc-joystick-commander) -[logitech-F310](http://a.co/3GoUlkN) +# OSCC API -Install the SDL2 library with the command below. +**Use provided CAN channel to open and close communications to CAN bus connected to the OSCC modules.** -``` -sudo apt install libsdl2-dev +```c +oscc_result_t oscc_open( uint channel ) +oscc_result_t oscc_close( uint ) ``` -## Building Joystick Commander +These methods are the start and end points of using the OSCC API in your application. ```oscc_open``` will open a socket connection +on the specified CAN channel, enabling it to quickly receive reports from and send commands to the firmware modules. +When you are ready to terminate your application, ```oscc_close``` can terminate the connection. -Navigate to the directory for the joystick commander code. +**Send enable or disable commands to all OSCC modules.** -``` -cd utils/joystick_commander +```c +oscc_result_t oscc_enable( void ) +oscc_result_t oscc_disable( void ) ``` -From this directory, run the following sequence to build joystick commander: +After you have initialized your CAN connection to the firmware modules, these methods can be used to enable or disable the system. This +allows your application to choose when to enable sending commands to the firmware. Although you can only send commands when the system is +enabled, you can receive reports at any time. -``` -mkdir build -cd build -cmake .. -make -``` - -Once you have initialized the CAN interface, you can use the channel number to start joystick commander and begin sending commands to the OSCC modules. - -For example, with a Kvaser Leaf Light attached, using a bitrate of 500000: +**Publish message with requested normalized value to the corresponding module.** -``` - sudo ip link set can0 type can bitrate 500000 - sudo ip link set up can0 +```c +oscc_result_t publish_brake_position( double normalized_position ) +oscc_result_t publish_steering_torque( double normalized_torque ) +oscc_result_t publish_throttle_position( double normalized_position ) ``` +These commands will forward a double value, *[0.0, 1.0]*, to the specified firmware module. The API will construct the appropriate values +to send spoof commands into the vehicle ECU's to achieve the desired state. The API also contains safety checks to ensure no invalid values +can be written onto the hardware. -You would then run: +**Register callback function to be called when OBD message received from vehicle.** +```c +oscc_result_t subscribe_to_brake_reports( void(*callback)(oscc_brake_report_s *report) ) +oscc_result_t subscribe_to_steering_reports( void(*callback)(oscc_steering_report_s *report) ) +oscc_result_t subscribe_to_throttle_reports( void(*callback)(oscc_throttle_report_s *report) ) +oscc_result_t subscribe_to_fault_reports( void(*callback)(oscc_fault_report_s *report) ) +oscc_result_t subscribe_to_obd_messages( void(*callback)(struct can_frame *frame) ) ``` -./joystick-commander 0 -``` - -For more information on setting up a socketcan interface, check out [this guide](http://elinux.org/Bringing_CAN_interface_up). -## Controlling the Vehicle with the Joystick Gamepad - -Once the joystick commander is up and running you can use it to send commands to the Arduino modules. -The controls are listed when the programs start up. Be sure the switch on the back of the controller -is switched to the 'X' position, not 'D'. The vehicle will only respond to commands if control is -enabled with the start button. The back button disables control. +In order to receive reports from the modules, your application will need to register a callback handler with the OSCC API. +When the appropriate report for your callback function is received from the API's socket connection, it will then forward the +report to your software. +In addition to OSCC specific reports, it will also forward any non-OSCC reports to any callback function registered with +```subscribe_to_obd_messages```. This can be used to view CAN frames received from the vehicle's OBD-II CAN channel. If you know +the corresponding CAN frame's id, you can parse reports sent from the car. # Additional Vehicles & Contributing OSCC currently has information regarding the Kia Soul PS (2014-2016), but we want to grow! The repository is structured to facilitate including more vehicles as more is learned about them. +In order to include information related to a new vehicle's specification, follow the format defined in ```api/include/vehicles/kia_soul.h``` and +add a CMake option to choose your new header when compiling the API. + Please see [CONTRIBUTING.md](CONTRIBUTING.md). diff --git a/api/include/can_protocols/brake_can_protocol.h b/api/include/can_protocols/brake_can_protocol.h new file mode 100644 index 00000000..33f5d10c --- /dev/null +++ b/api/include/can_protocols/brake_can_protocol.h @@ -0,0 +1,100 @@ +/** + * @file brake_can_protocol.h + * @brief Brake CAN Protocol. + * + */ + + +#ifndef _OSCC_BRAKE_CAN_PROTOCOL_H_ +#define _OSCC_BRAKE_CAN_PROTOCOL_H_ + + +#include +#include "magic.h" + + +/* + * @brief Brake command message (CAN frame) ID. + * + */ +#define OSCC_BRAKE_COMMAND_CAN_ID (0x60) + +/* + * @brief Brake report message (CAN frame) ID. + * + */ +#define OSCC_BRAKE_REPORT_CAN_ID (0x61) + +/* + * @brief Brake report message (CAN frame) length. + * + */ +#define OSCC_BRAKE_REPORT_CAN_DLC (8) + +/* + * @brief Brake report message publishing frequency. [Hz] + * + */ +#define OSCC_BRAKE_REPORT_PUBLISH_FREQ_IN_HZ (50) + +/* + * @brief Brake DTC bitfield position indicating an invalid sensor value. + * + */ +#define OSCC_BRAKE_DTC_INVALID_SENSOR_VAL (0x0) + + +#pragma pack(push) +#pragma pack(1) + +/** + * @brief Brake command message data. + * + * CAN frame ID: \ref OSCC_BRAKE_COMMAND_CAN_ID + * + */ +typedef struct +{ + uint8_t magic[2]; /* Magic number identifying CAN frame as from OSCC. + Byte 0 should be \ref OSCC_MAGIC_BYTE_0. + Byte 1 should be \ref OSCC_MAGIC_BYTE_1. */ + + uint16_t pedal_command; /*!< Pedal command. [65535 == 100%] */ + + uint8_t enable; /*!< Command to enable or disable steering control. + * Zero value means disable. + * Non-zero value means enable. */ + + uint8_t reserved[3]; /*!< Reserved. */ +} oscc_brake_command_s; + + +/** + * @brief Brake report message data. + * + * CAN frame ID: \ref OSCC_BRAKE_REPORT_CAN_ID + * + */ +typedef struct +{ + uint8_t magic[2]; /* Magic number identifying CAN frame as from OSCC. + Byte 0 should be \ref OSCC_MAGIC_BYTE_0. + Byte 1 should be \ref OSCC_MAGIC_BYTE_1. */ + + uint8_t enabled; /*!< Braking controls enabled state. + * Zero value means disabled (commands are ignored). + * Non-zero value means enabled (no timeouts or overrides have occured). */ + + uint8_t operator_override; /*!< Driver override state. + * Zero value means there has been no operator override. + * Non-zero value means an operator has physically overridden + * the system. */ + + uint8_t dtcs; /* Bitfield of DTCs present in the module. */ + + uint8_t reserved[3]; /*!< Reserved. */ +} oscc_brake_report_s; + +#pragma pack(pop) + +#endif /* _OSCC_BRAKE_CAN_PROTOCOL_H_ */ diff --git a/api/include/can_protocols/fault_can_protocol.h b/api/include/can_protocols/fault_can_protocol.h new file mode 100644 index 00000000..b356ee8c --- /dev/null +++ b/api/include/can_protocols/fault_can_protocol.h @@ -0,0 +1,59 @@ +/** + * @file fault_can_protocol.h + * @brief Fault CAN Protocol. + * + */ + + +#ifndef _OSCC_FAULT_CAN_PROTOCOL_H_ +#define _OSCC_FAULT_CAN_PROTOCOL_H_ + + +#include +#include "magic.h" + +/* + * @brief Fault report message (CAN frame) ID. + * + */ +#define OSCC_FAULT_REPORT_CAN_ID (0x99) + +/* + * @brief Fault report message (CAN frame) length. + * + */ +#define OSCC_FAULT_REPORT_CAN_DLC (8) + + +typedef enum +{ + FAULT_ORIGIN_BRAKE, + FAULT_ORIGIN_STEERING, + FAULT_ORIGIN_THROTTLE +} fault_origin_id_t; + + +#pragma pack(push) +#pragma pack(1) + +/** + * @brief Fault report message data. + * + * Message size (CAN frame DLC): \ref OSCC_FAULT_REPORT_CAN_DLC + * + */ +typedef struct +{ + uint8_t magic[2]; /* Magic number identifying CAN frame as from OSCC. + Byte 0 should be \ref OSCC_MAGIC_BYTE_0. + Byte 1 should be \ref OSCC_MAGIC_BYTE_1. */ + + uint32_t fault_origin_id; /* ID of the module that is sending out the fault. */ + + uint8_t reserved[2]; /* Reserved */ +} oscc_fault_report_s; + +#pragma pack(pop) + + +#endif /* _OSCC_FAULT_CAN_PROTOCOL_H_ */ diff --git a/api/include/can_protocols/magic.h b/api/include/can_protocols/magic.h new file mode 100644 index 00000000..c4391973 --- /dev/null +++ b/api/include/can_protocols/magic.h @@ -0,0 +1,27 @@ +/** + * @file magic.h + * @brief Definitions of the magic bytes identifying messages as from OSCC. + * + */ + + +#ifndef _OSCC_MAGIC_H_ +#define _OSCC_MAGIC_H_ + + +/* + * @brief First magic byte used in commands and reports to distinguish CAN + * frame as coming from OSCC (and not OBD). + * + */ +#define OSCC_MAGIC_BYTE_0 ( 0x05 ) + +/* + * @brief Second magic byte used in commands and reports to distinguish CAN + * frame as coming from OSCC (and not OBD). + * + */ +#define OSCC_MAGIC_BYTE_1 ( 0xCC ) + + +#endif /* _OSCC_MAGIC_H_ */ diff --git a/api/include/can_protocols/steering_can_protocol.h b/api/include/can_protocols/steering_can_protocol.h new file mode 100644 index 00000000..29a578bf --- /dev/null +++ b/api/include/can_protocols/steering_can_protocol.h @@ -0,0 +1,103 @@ +/** + * @file steering_can_protocol.h + * @brief Steering CAN Protocol. + * + */ + + +#ifndef _OSCC_STEERING_CAN_PROTOCOL_H_ +#define _OSCC_STEERING_CAN_PROTOCOL_H_ + + +#include +#include "magic.h" + + +/* + * @brief Steering command message (CAN frame) ID. + * + */ +#define OSCC_STEERING_COMMAND_CAN_ID (0x64) + +/* + * @brief Steering report message (CAN frame) ID. + * + */ +#define OSCC_STEERING_REPORT_CAN_ID (0x65) + +/* + * @brief Steering report message (CAN frame) length. + * + */ +#define OSCC_STEERING_REPORT_CAN_DLC (8) + +/* + * @brief Steering report message publishing frequency. [Hz] + * + */ +#define OSCC_REPORT_STEERING_PUBLISH_FREQ_IN_HZ (50) + +/* + * @brief Steering DTC bitfield position indicating an invalid sensor value. + * + */ +#define OSCC_STEERING_DTC_INVALID_SENSOR_VAL (0x0) + + +#pragma pack(push) +#pragma pack(1) + +/** + * @brief Steering command message data. + * + * CAN frame ID: \ref OSCC_STEERING_COMMAND_CAN_ID + * + */ +typedef struct +{ + uint8_t magic[2]; /* Magic number identifying CAN frame as from OSCC. + Byte 0 should be \ref OSCC_MAGIC_BYTE_0. + Byte 1 should be \ref OSCC_MAGIC_BYTE_1. */ + + 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. */ + + uint8_t enable; /*!< Command to enable or disable steering control. + * Zero value means disable. + * Non-zero value means enable. */ + + uint8_t reserved; /*!< Reserved. */ +} oscc_steering_command_s; + + +/** + * @brief Steering report message data. + * + * CAN frame ID: \ref OSCC_STEERING_REPORT_CAN_ID + * + */ +typedef struct +{ + uint8_t magic[2]; /* Magic number identifying CAN frame as from OSCC. + Byte 0 should be \ref OSCC_MAGIC_BYTE_0. + Byte 1 should be \ref OSCC_MAGIC_BYTE_1. */ + + uint8_t enabled; /*!< Steering controls enabled state. + * Zero value means disabled (commands are ignored). + * Non-zero value means enabled (no timeouts or overrides have occured). */ + + uint8_t operator_override; /*!< Driver override state. + * Zero value means there has been no operator override. + * Non-zero value means an operator has physically overridden + * the system. */ + + uint8_t dtcs; /* Bitfield of DTCs present in the module. */ + + uint8_t reserved[3]; /*!< Reserved. */ +} oscc_steering_report_s; + +#pragma pack(pop) + + +#endif /* _OSCC_STEERING_CAN_PROTOCOL_H_ */ diff --git a/api/include/can_protocols/throttle_can_protocol.h b/api/include/can_protocols/throttle_can_protocol.h new file mode 100644 index 00000000..64ea0192 --- /dev/null +++ b/api/include/can_protocols/throttle_can_protocol.h @@ -0,0 +1,108 @@ +/** + * @file throttle_can_protocol.h + * @brief Throttle CAN Protocol. + * + */ + + +#ifndef _OSCC_THROTTLE_CAN_PROTOCOL_H_ +#define _OSCC_THROTTLE_CAN_PROTOCOL_H_ + + +#include +#include "magic.h" + + +/* + * @brief Throttle command message (CAN frame) ID. + * + */ +#define OSCC_THROTTLE_COMMAND_CAN_ID (0x62) + + +/* + * @brief Throttle report message (CAN frame) ID. + * + */ +#define OSCC_THROTTLE_REPORT_CAN_ID (0x63) + + +/* + * @brief Throttle report message (CAN frame) length. + * + */ +#define OSCC_THROTTLE_REPORT_CAN_DLC (8) + + +/* + * @brief Throttle report message publishing frequency. [Hz] + * + */ +#define OSCC_REPORT_THROTTLE_PUBLISH_FREQ_IN_HZ (50) + + +/* + * @brief Throttle DTC bitfield position indicating an invalid sensor value. + * + */ +#define OSCC_THROTTLE_DTC_INVALID_SENSOR_VAL (0x0) + + +#pragma pack(push) +#pragma pack(1) + + +/** + * @brief Throttle command message. + * + * CAN frame ID: \ref OSCC_THROTTLE_COMMAND_CAN_ID + * + */ +typedef struct +{ + uint8_t magic[2]; /* Magic number identifying CAN frame as from OSCC. + Byte 0 should be \ref OSCC_MAGIC_BYTE_0. + Byte 1 should be \ref OSCC_MAGIC_BYTE_1. */ + + 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. */ + + uint8_t enable; /*!< Command to enable or disable throttle control. + * Zero value means disable. + * Non-zero value means enable. */ + + uint8_t reserved; /*!< Reserved. */ +} oscc_throttle_command_s; + + +/** + * @brief Throttle report message. + * + * CAN frame ID: \ref OSCC_THROTTLE_REPORT_CAN_ID + * + */ +typedef struct +{ + uint8_t magic[2]; /* Magic number identifying CAN frame as from OSCC. + Byte 0 should be \ref OSCC_MAGIC_BYTE_0. + Byte 1 should be \ref OSCC_MAGIC_BYTE_1. */ + + uint8_t enabled; /*!< Steering controls enabled state. + * Zero value means disabled (commands are ignored). + * Non-zero value means enabled (commands are sent to the vehicle). */ + + uint8_t operator_override; /*!< Driver override state. + * Zero value means there has been no operator override. + * Non-zero value means an operator has physically overridden + * the system. */ + + uint8_t dtcs; /* Bitfield of DTCs present in the module. */ + + uint8_t reserved[3]; /*!< Reserved. */ +} oscc_throttle_report_s; + +#pragma pack(pop) + + +#endif /* _OSCC_THROTTLE_CAN_PROTOCOL_H_ */ diff --git a/api/include/dtc.h b/api/include/dtc.h new file mode 100644 index 00000000..42e17061 --- /dev/null +++ b/api/include/dtc.h @@ -0,0 +1,31 @@ +/** + * @file dtc.h + * @brief DTC macros. + * + */ + + +#ifndef _OSCC_DTC_H_ +#define _OSCC_DTC_H_ + + +/* + * @brief Set a DTC bit in a DTC bitfield. + * + */ +#define DTC_SET( bitfield, dtc ) ( (bitfield) |= (1<<(dtc)) ) + +/* + * @brief Clear a DTC bit in a DTC bitfield. + * + */ +#define DTC_CLEAR( bitfield, dtc ) ( (bitfield) &= ~(1<<(dtc)) ) + +/* + * @brief Check if a DTC bit in a DTC bitfield is set. + * + */ +#define DTC_CHECK( bitfield, dtc ) ( (bitfield) & (1<<(dtc)) ) + + +#endif /* _OSCC_DTC_H_ */ diff --git a/api/include/oscc.h b/api/include/oscc.h new file mode 100644 index 00000000..57a81c96 --- /dev/null +++ b/api/include/oscc.h @@ -0,0 +1,178 @@ +/** + * @file oscc.h + * @brief OSCC interface - Register callbacks for retrieving module and vehicle reports, + * and send requested targets to the modules. + */ + + +#ifndef _OSCC_H +#define _OSCC_H + +#include +#include +#include "can_protocols/brake_can_protocol.h" +#include "can_protocols/fault_can_protocol.h" +#include "can_protocols/throttle_can_protocol.h" +#include "can_protocols/steering_can_protocol.h" + + +typedef enum +{ + OSCC_OK, + OSCC_ERROR, + OSCC_WARNING +} oscc_result_t; + + +/** + * @brief Use provided CAN channel to open communications + * to CAN bus connected to the OSCC modules. + * + * @param [in] channel - CAN channel connected to OSCC modules. + * + * @return OSCC_ERROR or OSCC_OK + * + */ +oscc_result_t oscc_open( unsigned int channel ); + + +/** + * @brief Use provided CAN channel to close communications + * to CAN bus connected to the OSCC modules. + * + * @param [in] channel - CAN channel connected to OSCC modules. + * + * @return OSCC_ERROR or OSCC_OK + * + */ +oscc_result_t oscc_close( unsigned int channel ); + + +/** + * @brief Send enable commands to all OSCC modules. + * + * @param [void] + * + * @return OSCC_ERROR or OSCC_OK + * + */ +oscc_result_t oscc_enable( void ); + + +/** + * @brief Send disable commands to all OSCC modules. + * + * @param [void] + * + * @return OSCC_ERROR or OSCC_OK + * + */ +oscc_result_t oscc_disable( void ); + + +/** + * @brief Publish message with requested brake pedal position to + * brake module. + * + * @param [in] position - Normalized requested brake pedal + * position in the range [0, 1]. + * + * @return OSCC_ERROR or OSCC_OK + * + */ +oscc_result_t oscc_publish_brake_position( double brake_position ); + + +/** + * @brief Publish message with requested throttle pedal position to + * throttle module. + * + * @param [in] position - Normalized requested throttle pedal + * position in the range [0, 1]. + * + * @return OSCC_ERROR or OSCC_OK + * + */ +oscc_result_t oscc_publish_throttle_position( double throttle_position ); + + +/** + * @brief Publish message with requested steering torque to + * steering module. + * + * @param [in] angle - Normalized requested steering wheel + * torque in the range [-1, 1]. + * + * @return OSCC_ERROR or OSCC_OK + * + */ +oscc_result_t oscc_publish_steering_torque( double torque ); + + +/** + * @brief Register callback function to be called when brake report + * received from brake module. + * + * @param [in] callback - Pointer to callback function to be called when + * brake report received from brake module. + * + * @return OSCC_ERROR or OSCC_OK + * + */ +oscc_result_t oscc_subscribe_to_brake_reports( void( *callback )( oscc_brake_report_s *report ) ); + + +/** + * @brief Register callback function to be called when throttle report + * received from throttle module. + * + * @param [in] callback - Pointer to callback function to be called when + * throttle report received from throttle module. + * + * @return OSCC_ERROR or OSCC_OK + * + */ +oscc_result_t oscc_subscribe_to_throttle_reports( void( *callback )( oscc_throttle_report_s *report ) ); + + +/** + * @brief Register callback function to be called when steering report + * received from steering module. + * + * @param [in] callback - Pointer to callback function to be called when + * steering report received from steering module. + * + * @return OSCC_ERROR or OSCC_OK + * + */ +oscc_result_t oscc_subscribe_to_steering_reports( void( *callback )( oscc_steering_report_s *report ) ); + + +/** + * @brief Register callback function to be called when fault report + * received from any module. + * + * @param [in] callback - Pointer to callback function to be called when + * fault report received from any module. + * + * @return OSCC_ERROR or OSCC_OK + * + */ +oscc_result_t oscc_subscribe_to_fault_reports( void( *callback )( oscc_fault_report_s *report ) ); + + +/** + * @brief Register callback function to be called when OBD message received + * from vehicle. + * + * @param [in] callback - Pointer to callback function to be called when + * OBD message received. + * + * @return OSCC_ERROR or OSCC_OK + * + */ +oscc_result_t oscc_subscribe_to_obd_messages( void( *callback )( struct can_frame *frame ) ); + + +#endif /* _OSCC_H */ + diff --git a/api/include/vehicles.h b/api/include/vehicles.h new file mode 100644 index 00000000..a236d8d5 --- /dev/null +++ b/api/include/vehicles.h @@ -0,0 +1,17 @@ +/** + * @file vehicles.h + * @brief List of vehicle headers. + * + */ + + +#ifndef _OSCC_VEHICLES_H_ +#define _OSCC_VEHICLES_H_ + + +#ifdef KIA_SOUL +#include "vehicles/kia_soul.h" +#endif + + +#endif /* _OSCC_VEHICLES_H_ */ diff --git a/api/include/vehicles/kia_soul.h b/api/include/vehicles/kia_soul.h new file mode 100644 index 00000000..031c9291 --- /dev/null +++ b/api/include/vehicles/kia_soul.h @@ -0,0 +1,456 @@ +/** + * @file kia_soul.h + * @brief Kia Soul specific macros. + * + */ + + +#ifndef _KIA_SOUL_PLATFORM_INFO_H_ +#define _KIA_SOUL_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; + + + + +// **************************************************************************** +// BRAKE MODULE +// **************************************************************************** + +/* + * @brief Minimum allowable brake value. + * + */ +#define MINIMUM_BRAKE_COMMAND ( 0 ) + +/* + * @brief Maximum allowable brake value. + * + */ +#define MAXIMUM_BRAKE_COMMAND ( 52428 ) + +/* + * @brief Calculation to convert a brake position to a pedal position. + * + */ +#define BRAKE_POSITION_TO_PEDAL( position ) ( (position) ) + +/* + * @brief Calculation to convert a brake pressure to a pedal position. + * + */ +#define BRAKE_PRESSURE_TO_PEDAL( pressure ) ( (pressure) ) + +/* + * @brief Minimum accumulator presure. [decibars] + * + */ +#define BRAKE_ACCUMULATOR_PRESSURE_MIN_IN_DECIBARS ( 777.6 ) + +/* + * @brief Maximum accumulator pressure. [decibars] + * + */ +#define BRAKE_ACCUMULATOR_PRESSURE_MAX_IN_DECIBARS ( 878.0 ) + +/* + * @brief Value of brake pressure that indicates operator override. [decibars] + * + */ +#define BRAKE_OVERRIDE_PEDAL_THRESHOLD_IN_DECIBARS ( 43.2 ) + +/* + * @brief Brake pressure threshold for when to enable the brake light. + * + */ +#define BRAKE_LIGHT_PRESSURE_THRESHOLD_IN_DECIBARS ( 20.0 ) + +/* + * @brief Minimum possible pressure of brake system. [decibars] + * + */ +#define BRAKE_PRESSURE_MIN_IN_DECIBARS ( 12.0 ) + +/* + * @brief Maximum possible pressure of brake system. [decibars] + * + */ +#define BRAKE_PRESSURE_MAX_IN_DECIBARS ( 878.3 ) + +/* + * @brief Minimum possible value expected to be read from the brake pressure + * sensors when the pressure check pins (PCK1/PCK2) are asserted. + * + */ +#define BRAKE_PRESSURE_SENSOR_CHECK_VALUE_MIN ( 665 ) + +/* + * @brief Maximum possible value expected to be read from the brake pressure + * sensors when the pressure check pins (PCK1/PCK2) are asserted. + * + */ +#define BRAKE_PRESSURE_SENSOR_CHECK_VALUE_MAX ( 680 ) + +/* + * @brief Proportional gain of the PID controller. + * + */ +#define BRAKE_PID_PROPORTIONAL_GAIN ( 0.65 ) + +/* + * @brief Integral gain of the PID controller. + * + */ +#define BRAKE_PID_INTEGRAL_GAIN ( 1.75 ) + +/* + * @brief Derivative gain of the PID controller. + * + */ +#define BRAKE_PID_DERIVATIVE_GAIN ( 0.000 ) + +/* + * @brief Windup guard of the PID controller. + * + */ +#define BRAKE_PID_WINDUP_GUARD ( 30 ) + +/* + * @brief Minimum output value of PID to be within a valid pressure range. + * + */ +#define BRAKE_PID_OUTPUT_MIN ( -10.0 ) + +/* + * @brief Maximum output value of PID to be within a valid pressure range. + * + */ +#define BRAKE_PID_OUTPUT_MAX ( 10.0 ) + +/* + * @brief Minimum clamped PID value of the actuation solenoid. + * + */ +#define BRAKE_PID_ACCUMULATOR_SOLENOID_CLAMPED_MIN ( 10.0 ) + +/* + * @brief Maximum clamped PID value of the actuation solenoid. + * + */ +#define BRAKE_PID_ACCUMULATOR_SOLENOID_CLAMPED_MAX ( 110.0 ) + +/* + * @brief Minimum clamped PID value of the release solenoid. + * + */ +#define BRAKE_PID_RELEASE_SOLENOID_CLAMPED_MIN ( 0.0 ) + +/* + * @brief Maximum clamped PID value of the release solenoid. + * + */ +#define BRAKE_PID_RELEASE_SOLENOID_CLAMPED_MAX ( 60.0 ) + +/* + * @brief Minimum duty cycle that begins to actuate the actuation solenoid. + * + * 3.921 KHz PWM frequency + * + */ +#define BRAKE_ACCUMULATOR_SOLENOID_DUTY_CYCLE_MIN ( 80.0 ) + +/* + * @brief Maximum duty cycle where actuation solenoid has reached its stop. + * + * 3.921 KHz PWM frequency + * + */ +#define BRAKE_ACCUMULATOR_SOLENOID_DUTY_CYCLE_MAX ( 105.0 ) + +/* + * @brief Minimum duty cycle that begins to actuate the release solenoid. + * + * 3.921 KHz PWM frequency + * + */ +#define BRAKE_RELEASE_SOLENOID_DUTY_CYCLE_MIN ( 65.0 ) + +/* + * @brief Maximum duty cycle where release solenoid has reached its stop. + * + * 3.921 KHz PWM frequency + * + */ +#define BRAKE_RELEASE_SOLENOID_DUTY_CYCLE_MAX ( 100.0 ) + + + + +// **************************************************************************** +// STEERING MODULE +// **************************************************************************** + +/* + * @brief Minimum allowable steering DAC output. [steps] + * + */ +#define STEERING_SPOOF_SIGNAL_MIN ( 868.0 ) + +/* + * @brief Maximum allowable steering DAC output. [steps] + * + */ +#define STEERING_SPOOF_SIGNAL_MAX ( 3031.0 ) + +/* + * @brief Minimum allowable torque value. + * + */ +#define MINIMUM_TORQUE_COMMAND ( -10 ) + +/* + * @brief Maximum allowable torque value. + * + */ +#define MAXIMUM_TORQUE_COMMAND ( 10 ) + + + /* + * @brief Number of steps per volt corresponding to 4096 steps across 5 volts. + * + */ +#define STEPS_PER_VOLT ( 819.2 ) + +/* + * @brief Scalar value for the low spoof signal taken from a calibration curve. + * + */ +#define TORQUE_SPOOF_LOW_SIGNAL_CALIBRATION_CURVE_SCALE ( 0.05 ) + +/* + * @brief Offset value for the low spoof signal taken from a calibration curve. + * + */ +#define TORQUE_SPOOF_LOW_SIGNAL_CALIBRATION_CURVE_OFFSET ( 2.5 ) + +/* + * @brief Scalar value for the high spoof signal taken from a calibration curve. + * + */ +#define TORQUE_SPOOF_HIGH_SIGNAL_CALIBRATION_CURVE_SCALE ( -0.05 ) + +/* + * @brief Offset value for the high spoof signal taken from a calibration curve. + * + */ +#define TORQUE_SPOOF_HIGH_SIGNAL_CALIBRATION_CURVE_OFFSET ( 2.5 ) + +/* + * @brief Calculation to convert a steering angle to a high spoof value. + * + */ +#define STEERING_ANGLE_TO_SPOOF_HIGH( angle ) ( (angle) ) + +/* + * @brief Calculation to convert a steering angle to a low spoof value. + * + */ +#define STEERING_ANGLE_TO_SPOOF_LOW( angle ) ( (angle) ) + +/* + * @brief Minimum allowed value for the high spoof signal value. + * + */ +#define STEERING_TORQUE_TO_SPOOF_HIGH( torque ) (STEPS_PER_VOLT\ + * ((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_SPOOF_LOW( torque ) (STEPS_PER_VOLT\ + * ((TORQUE_SPOOF_LOW_SIGNAL_CALIBRATION_CURVE_SCALE * (torque))\ + + TORQUE_SPOOF_LOW_SIGNAL_CALIBRATION_CURVE_OFFSET)) + +/* + * @brief Value of the torque sensor that indicates operator override. + * [degrees/microsecond] + * + */ +#define OVERRIDE_WHEEL_THRESHOLD_IN_DEGREES_PER_USEC ( 750 ) + + + + +// **************************************************************************** +// THROTTLE MODULE +// **************************************************************************** + +/* + * @brief Minimum allowable throttle value. + * + */ +#define MINIMUM_THROTTLE_COMMAND ( 0 ) + +/* + * @brief Maximum allowable throttle value. + * + */ +#define MAXIMUM_THROTTLE_COMMAND ( 820 ) + +/* + * @brief Calculation to convert a throttle position to a high spoof value. + * + */ +#define THROTTLE_POSITION_TO_SPOOF_HIGH( position ) (STEPS_PER_VOLT\ + * ((THROTTLE_SPOOF_HIGH_SIGNAL_CALIBRATION_CURVE_SCALE * (position))\ + + THROTTLE_SPOOF_HIGH_SIGNAL_CALIBRATION_CURVE_OFFSET)) + +/* + * @brief Calculation to convert a throttle position to a low spoof value. + * + */ +#define THROTTLE_POSITION_TO_SPOOF_LOW( position ) (STEPS_PER_VOLT\ + * ((THROTTLE_SPOOF_LOW_SIGNAL_CALIBRATION_CURVE_SCALE * (position))\ + + THROTTLE_SPOOF_LOW_SIGNAL_CALIBRATION_CURVE_OFFSET)) + +/* + * @brief Minimum allowed value for the low spoof signal value. + * + */ +#define THROTTLE_SPOOF_LOW_SIGNAL_RANGE_MIN ( 0 ) + +/* + * @brief Maximum allowed value for the low spoof signal value. + * + */ +#define THROTTLE_SPOOF_LOW_SIGNAL_RANGE_MAX ( 1800 ) + +/** + * @brief Wheel speed message data. + * + */ +#define THROTTLE_SPOOF_HIGH_SIGNAL_RANGE_MIN ( 0 ) + +/** + * @brief Wheel speed message data. + * + */ +#define THROTTLE_SPOOF_HIGH_SIGNAL_RANGE_MAX ( 3500 ) + +/* + * @brief Scalar value for the low spoof signal taken from a calibration curve. + * + */ +#define THROTTLE_SPOOF_LOW_SIGNAL_CALIBRATION_CURVE_SCALE (0.0004) + +/* + * @brief Offset value for the low spoof signal taken from a calibration curve. + * + */ +#define THROTTLE_SPOOF_LOW_SIGNAL_CALIBRATION_CURVE_OFFSET (0.366) + +/* + * @brief Scalar value for the high spoof signal taken from a calibration curve. + * + */ +#define THROTTLE_SPOOF_HIGH_SIGNAL_CALIBRATION_CURVE_SCALE (0.0008) + +/* + * @brief Offset value for the high spoof signal taken from a calibration curve. + * + */ +#define THROTTLE_SPOOF_HIGH_SIGNAL_CALIBRATION_CURVE_OFFSET (0.732) + +/* + * @brief Value of the accelerator position that indicates operator override. + * + */ +#define ACCELERATOR_OVERRIDE_THRESHOLD ( 185.0 ) + + + +#endif diff --git a/api/src/oscc.c b/api/src/oscc.c new file mode 100644 index 00000000..652c07ab --- /dev/null +++ b/api/src/oscc.c @@ -0,0 +1,522 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "can_protocols/brake_can_protocol.h" +#include "can_protocols/fault_can_protocol.h" +#include "can_protocols/throttle_can_protocol.h" +#include "can_protocols/steering_can_protocol.h" +#include "vehicles.h" +#include "dtc.h" +#include "oscc.h" + +#define CONSTRAIN(amt, low, high) ((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt))) + +static int can_socket; + +static oscc_brake_command_s brake_cmd; +static oscc_throttle_command_s throttle_cmd; +static oscc_steering_command_s steering_cmd; + +static void (*steering_report_callback)(oscc_steering_report_s *report); +static void (*brake_report_callback)(oscc_brake_report_s *report); +static void (*throttle_report_callback)(oscc_throttle_report_s *report); +static void (*fault_report_callback)(oscc_fault_report_s *report); +static void (*obd_frame_callback)(struct can_frame *frame); + +static oscc_result_t oscc_init_can( const char *can_channel ); +static oscc_result_t oscc_can_write( long id, void *msg, unsigned int dlc ); +static oscc_result_t oscc_async_enable( int socket ); +static oscc_result_t oscc_enable_brakes( void ); +static oscc_result_t oscc_enable_throttle( void ); +static oscc_result_t oscc_enable_steering( void ); +static oscc_result_t oscc_disable_brakes( void ); +static oscc_result_t oscc_disable_throttle( void ); +static oscc_result_t oscc_disable_steering( void ); +static void oscc_update_status( ); +static void oscc_init_commands( void ); + +oscc_result_t oscc_open( unsigned int channel ) +{ + oscc_result_t ret = OSCC_ERROR; + + char buffer[16]; + + snprintf( buffer, 16, "can%1d", channel ); + + printf( "Opening CAN channel: %s\n", buffer ); + + ret = oscc_init_can( buffer ); + + oscc_init_commands( ); + + return ret; +} + +oscc_result_t oscc_close( unsigned int channel ) +{ + oscc_result_t ret = OSCC_ERROR; + + int result = close( can_socket ); + + if ( result > 0 ) + { + ret = OSCC_OK; + } + + return ret; +} + +oscc_result_t oscc_enable( void ) +{ + oscc_result_t ret = oscc_enable_brakes( ); + + if (ret == OSCC_OK ) + { + ret = oscc_enable_throttle( ); + + if (ret == OSCC_OK ) + { + ret = oscc_enable_steering( ); + } + } + + return ret; +} + +oscc_result_t oscc_disable( void ) +{ + oscc_result_t ret = oscc_disable_brakes( ); + + if ( ret == OSCC_OK ) + { + ret = oscc_disable_throttle( ); + + if ( ret == OSCC_OK ) + { + ret = oscc_disable_steering( ); + } + } + + return ret; +} + +oscc_result_t oscc_publish_brake_position( double brake_position ) +{ + oscc_result_t ret = OSCC_ERROR; + + const double scaled_position = (double) CONSTRAIN ( + brake_position * MAXIMUM_BRAKE_COMMAND, + MINIMUM_BRAKE_COMMAND, + MAXIMUM_BRAKE_COMMAND ); + + brake_cmd.pedal_command = ( uint16_t ) BRAKE_POSITION_TO_PEDAL( scaled_position ); + + ret = oscc_can_write( + OSCC_BRAKE_COMMAND_CAN_ID, + (void *) &brake_cmd, + sizeof( brake_cmd ) ); + + return ret; +} + +oscc_result_t oscc_publish_throttle_position( double throttle_position ) +{ + oscc_result_t ret = OSCC_ERROR; + + const double scaled_position = CONSTRAIN( + throttle_position * MAXIMUM_THROTTLE_COMMAND, + MINIMUM_THROTTLE_COMMAND, + MAXIMUM_THROTTLE_COMMAND); + + uint16_t spoof_value_low = THROTTLE_POSITION_TO_SPOOF_LOW( scaled_position ); + + spoof_value_low = CONSTRAIN( + spoof_value_low, + THROTTLE_SPOOF_LOW_SIGNAL_RANGE_MIN, + THROTTLE_SPOOF_LOW_SIGNAL_RANGE_MAX); + + uint16_t spoof_value_high = THROTTLE_POSITION_TO_SPOOF_HIGH( scaled_position ); + + spoof_value_high = CONSTRAIN( + spoof_value_high, + THROTTLE_SPOOF_HIGH_SIGNAL_RANGE_MIN, + THROTTLE_SPOOF_HIGH_SIGNAL_RANGE_MAX); + + throttle_cmd.spoof_value_low = spoof_value_low; + throttle_cmd.spoof_value_high = spoof_value_high; + + ret = oscc_can_write( + OSCC_THROTTLE_COMMAND_CAN_ID, + (void *)&throttle_cmd, + sizeof(throttle_cmd)); + + return ret; +} + +oscc_result_t oscc_publish_steering_torque( double torque ) +{ + oscc_result_t ret = OSCC_ERROR; + + const double scaled_torque = CONSTRAIN( + torque * MAXIMUM_TORQUE_COMMAND, + MINIMUM_TORQUE_COMMAND, + MAXIMUM_TORQUE_COMMAND); + + uint16_t spoof_value_low = STEERING_TORQUE_TO_SPOOF_LOW( scaled_torque ); + + spoof_value_low = CONSTRAIN( + spoof_value_low, + STEERING_SPOOF_SIGNAL_MIN, + STEERING_SPOOF_SIGNAL_MAX); + + uint16_t spoof_value_high = STEERING_TORQUE_TO_SPOOF_HIGH( scaled_torque ); + + spoof_value_high = CONSTRAIN( + spoof_value_high, + STEERING_SPOOF_SIGNAL_MIN, + STEERING_SPOOF_SIGNAL_MAX); + + steering_cmd.spoof_value_low = spoof_value_low; + steering_cmd.spoof_value_high = spoof_value_high; + + ret = oscc_can_write( + OSCC_STEERING_COMMAND_CAN_ID, + (void *)&steering_cmd, + sizeof(steering_cmd)); + + return ret; +} + +oscc_result_t oscc_subscribe_to_brake_reports( void (*callback)(oscc_brake_report_s *report) ) +{ + oscc_result_t ret = OSCC_ERROR; + + if ( callback != NULL ) + { + brake_report_callback = callback; + ret = OSCC_OK; + } + + return ret; +} + +oscc_result_t oscc_subscribe_to_throttle_reports( void (*callback)(oscc_throttle_report_s *report) ) +{ + oscc_result_t ret = OSCC_ERROR; + + if ( callback != NULL ) + { + throttle_report_callback = callback; + ret = OSCC_OK; + } + + return ret; +} + +oscc_result_t oscc_subscribe_to_steering_reports( void (*callback)(oscc_steering_report_s *report)) +{ + oscc_result_t ret = OSCC_ERROR; + + if ( callback != NULL ) + { + steering_report_callback = callback; + ret = OSCC_OK; + } + + return ret; +} + +oscc_result_t oscc_subscribe_to_fault_reports( void (*callback)(oscc_fault_report_s *report)) +{ + oscc_result_t ret = OSCC_ERROR; + + if ( callback != NULL ) + { + fault_report_callback = callback; + ret = OSCC_OK; + } + + return ret; +} + +oscc_result_t oscc_subscribe_to_obd_messages( void (*callback)(struct can_frame *frame)) +{ + oscc_result_t ret = OSCC_ERROR; + + if ( callback != NULL ) + { + obd_frame_callback = callback; + ret = OSCC_OK; + } + + return ret; +} + +static oscc_result_t oscc_enable_brakes( void ) +{ + oscc_result_t ret = OSCC_ERROR; + + brake_cmd.enable = 1; + + ret = oscc_publish_brake_position( 0 ); + + return ret; +} + +static oscc_result_t oscc_enable_throttle( void ) +{ + oscc_result_t ret = OSCC_ERROR; + + throttle_cmd.enable = 1; + + ret = oscc_publish_throttle_position( 0 ); + + return ret; +} + +static oscc_result_t oscc_enable_steering( void ) +{ + oscc_result_t ret = OSCC_ERROR; + + steering_cmd.enable = 1; + + ret = oscc_publish_steering_torque( 0 ); + + return ret; +} + +static oscc_result_t oscc_disable_brakes( void ) +{ + oscc_result_t ret = OSCC_ERROR; + + brake_cmd.enable = 0; + + ret = oscc_publish_brake_position( 0 ); + + return ret; +} + +static oscc_result_t oscc_disable_throttle( void ) +{ + oscc_result_t ret = OSCC_ERROR; + + throttle_cmd.enable = 0; + + ret = oscc_publish_throttle_position( 0 ); + + return ret; +} + +static oscc_result_t oscc_disable_steering( void ) +{ + oscc_result_t ret = OSCC_ERROR; + + steering_cmd.enable = 0; + + ret = oscc_publish_steering_torque( 0 ); + + return ret; +} + +static void oscc_update_status( ) +{ + struct can_frame rx_frame; + + int result = read( can_socket, &rx_frame, CAN_MTU ); + + while ( result > 0 ) + { + if ( (rx_frame.data[0] == OSCC_MAGIC_BYTE_0) + && (rx_frame.data[1] = OSCC_MAGIC_BYTE_1) ) + { + if ( rx_frame.can_id == OSCC_STEERING_REPORT_CAN_ID ) + { + oscc_steering_report_s *steering_report = + (oscc_steering_report_s *)rx_frame.data; + + if ( steering_report_callback != NULL ) + { + steering_report_callback( steering_report ); + } + } + else if ( rx_frame.can_id == OSCC_THROTTLE_REPORT_CAN_ID ) + { + oscc_throttle_report_s *throttle_report = + ( oscc_throttle_report_s *)rx_frame.data; + + if ( throttle_report_callback != NULL ) + { + throttle_report_callback( throttle_report ); + } + } + else if ( rx_frame.can_id == OSCC_BRAKE_REPORT_CAN_ID ) + { + oscc_brake_report_s *brake_report = + ( oscc_brake_report_s *)rx_frame.data; + + if ( brake_report_callback != NULL ) + { + brake_report_callback( brake_report ); + } + } + else if ( rx_frame.can_id == OSCC_FAULT_REPORT_CAN_ID ) + { + oscc_fault_report_s *fault_report = + ( oscc_fault_report_s *)rx_frame.data; + + if ( fault_report_callback != NULL ) + { + fault_report_callback( fault_report ); + } + } + } + else + { + if ( obd_frame_callback != NULL ) + { + obd_frame_callback( &rx_frame ); + } + } + + result = read( can_socket, &rx_frame, CAN_MTU ); + } +} + +static oscc_result_t oscc_can_write( long id, void *msg, unsigned int dlc ) +{ + oscc_result_t ret = OSCC_ERROR; + + struct can_frame tx_frame; + + tx_frame.can_id = id; + tx_frame.can_dlc = dlc; + memcpy( tx_frame.data, msg, dlc); + + int result = write( can_socket, &tx_frame, sizeof(tx_frame )); + + if ( result > 0 ) + { + ret = OSCC_OK; + } + + return ret; +} + +static oscc_result_t oscc_async_enable( int socket ) +{ + oscc_result_t ret = OSCC_ERROR; + + ret = fcntl( socket, F_SETOWN, getpid( ) ); + + if ( ret < 0 ) + { + printf( "set own failed\n" ); + } + + ret = fcntl( socket, F_SETFL, FASYNC | O_NONBLOCK ); + + if ( ret < 0 ) + { + printf( "set async failed\n" ); + } + + return ret; +} + +static oscc_result_t oscc_init_can( const char *can_channel ) +{ + int ret = OSCC_OK; + + struct sigaction sig; + sig.sa_handler = oscc_update_status; + sigaction( SIGIO, &sig, NULL ); + + int s = socket( PF_CAN, SOCK_RAW, CAN_RAW ); + + if ( s < 0 ) + { + printf( "opening can socket failed\n" ); + + ret = OSCC_ERROR; + } + + int status; + + struct ifreq ifr; + + if ( ret != OSCC_ERROR ) + { + strncpy( ifr.ifr_name, can_channel, IFNAMSIZ ); + + status = ioctl( s, SIOCGIFINDEX, &ifr ); + + if ( status < 0 ) + { + printf( "finding can index failed\n" ); + + ret = OSCC_ERROR; + } + } + + if ( ret != OSCC_ERROR ) + { + struct sockaddr_can can_address; + + can_address.can_family = AF_CAN; + can_address.can_ifindex = ifr.ifr_ifindex; + + status = bind( s, + (struct sockaddr *)&can_address, + sizeof(can_address)); + + if ( status < 0 ) + { + printf( "socket binding failed\n" ); + + ret = OSCC_ERROR; + } + } + + if ( ret != OSCC_ERROR ) + { + can_socket = s; + + ret = OSCC_OK; + } + + if ( ret != OSCC_ERROR ) + { + status = oscc_async_enable( s ); + + if ( status != OSCC_OK ) + { + printf( "async enable failed\n" ); + + ret = OSCC_ERROR; + } + } + + return ret; +} + +static void oscc_init_commands( void ) +{ + brake_cmd.magic[0] = ( uint8_t ) OSCC_MAGIC_BYTE_0; + brake_cmd.magic[1] = ( uint8_t ) OSCC_MAGIC_BYTE_1; + + throttle_cmd.magic[0] = ( uint8_t ) OSCC_MAGIC_BYTE_0; + throttle_cmd.magic[1] = ( uint8_t ) OSCC_MAGIC_BYTE_1; + + steering_cmd.magic[0] = ( uint8_t ) OSCC_MAGIC_BYTE_0; + steering_cmd.magic[1] = ( uint8_t ) OSCC_MAGIC_BYTE_1; +} diff --git a/platforms/CMakeLists.txt b/firmware/CMakeLists.txt similarity index 65% rename from platforms/CMakeLists.txt rename to firmware/CMakeLists.txt index f86b7ba4..78326094 100644 --- a/platforms/CMakeLists.txt +++ b/firmware/CMakeLists.txt @@ -1,3 +1,5 @@ + + if(TESTS) cmake_minimum_required(VERSION 2.8) @@ -7,43 +9,36 @@ if(TESTS) add_definitions(-DDEBUG) endif() - add_subdirectory(kia_soul/firmware/brake/tests) - add_subdirectory(kia_soul/firmware/can_gateway/tests) - add_subdirectory(kia_soul/firmware/steering/tests) - add_subdirectory(kia_soul/firmware/throttle/tests) + if(KIA_SOUL) + add_definitions(-DKIA_SOUL) + endif() + + add_subdirectory(brake/tests) + add_subdirectory(can_gateway/tests) + add_subdirectory(steering/tests) + add_subdirectory(throttle/tests) add_subdirectory(common/libs/pid/tests) add_custom_target( - run-kia-soul-unit-tests + run-unit-tests DEPENDS - run-kia-soul-brake-unit-tests - run-kia-soul-can-gateway-unit-tests - run-kia-soul-steering-unit-tests - run-kia-soul-throttle-unit-tests) + run-brake-unit-tests + run-can-gateway-unit-tests + run-steering-unit-tests + run-throttle-unit-tests) add_custom_target( - run-kia-soul-property-tests + run-property-tests DEPENDS - run-kia-soul-brake-property-tests - run-kia-soul-steering-property-tests - run-kia-soul-throttle-property-tests) + run-brake-property-tests + run-steering-property-tests + run-throttle-property-tests) add_custom_target( run-library-property-tests DEPENDS run-pid-library-property-tests) - add_custom_target( - run-unit-tests - DEPENDS - run-kia-soul-unit-tests) - - add_custom_target( - run-property-tests - DEPENDS - run-kia-soul-property-tests - run-library-property-tests) - add_custom_target( run-all-tests DEPENDS @@ -56,8 +51,11 @@ else() project(firmware) + set(CMAKE_CFLAGS "-std=gnu11 -Os") + set(CMAKE_CXX_FLAGS "-std=gnu++11 -Os") + option(DEBUG "Enable debug mode" OFF) - option(BUILD_KIA_SOUL "Build firmware for Kia Soul" OFF) + option(KIA_SOUL "Build firmware for Kia Soul" OFF) set(SERIAL_PORT_BRAKE "/dev/ttyACM0" CACHE STRING "Serial port of the brake module") set(SERIAL_BAUD_BRAKE "115200" CACHE STRING "Serial baud rate of the brake module") @@ -75,9 +73,22 @@ else() add_definitions(-DDEBUG) endif() - if(BUILD_KIA_SOUL) - add_subdirectory(kia_soul/firmware) + if(KIA_SOUL) + add_definitions(-DKIA_SOUL) else() message(WARNING "No platform selected - no firmware will be built") endif() + + add_subdirectory(brake) + add_subdirectory(can_gateway) + add_subdirectory(steering) + add_subdirectory(throttle) + + add_custom_target( + all-upload + DEPENDS + brake-upload + can-gateway-upload + steering-upload + throttle-upload) endif() diff --git a/platforms/kia_soul/firmware/brake/CMakeLists.txt b/firmware/brake/CMakeLists.txt similarity index 76% rename from platforms/kia_soul/firmware/brake/CMakeLists.txt rename to firmware/brake/CMakeLists.txt index 6d571632..0dfa03a4 100644 --- a/platforms/kia_soul/firmware/brake/CMakeLists.txt +++ b/firmware/brake/CMakeLists.txt @@ -1,4 +1,4 @@ -project(kia-soul-brake) +project(brake) set(ARDUINO_DEFAULT_BOARD mega2560) SET(ARDUINO_DEFAULT_PORT ${SERIAL_PORT_BRAKE}) @@ -7,23 +7,22 @@ set(ARDUINO_DEFAULT_BAUDRATE ${SERIAL_BAUD_BRAKE}) add_definitions(-DSERIAL_BAUD=${ARDUINO_DEFAULT_BAUDRATE}) add_custom_target( - kia-soul-brake-monitor + brake-monitor COMMAND screen ${ARDUINO_DEFAULT_PORT} ${ARDUINO_DEFAULT_BAUDRATE}) add_custom_target( - kia-soul-brake-monitor-log + brake-monitor-log COMMAND screen -L ${ARDUINO_DEFAULT_PORT} ${ARDUINO_DEFAULT_BAUDRATE}) generate_arduino_firmware( - kia-soul-brake + brake SRCS ${CMAKE_SOURCE_DIR}/common/libs/arduino_init/arduino_init.cpp ${CMAKE_SOURCE_DIR}/common/libs/mcp_can/mcp_can.cpp ${CMAKE_SOURCE_DIR}/common/libs/pid/oscc_pid.cpp ${CMAKE_SOURCE_DIR}/common/libs/serial/oscc_serial.cpp ${CMAKE_SOURCE_DIR}/common/libs/can/oscc_can.cpp - ${CMAKE_SOURCE_DIR}/common/libs/time/oscc_time.cpp - ${CMAKE_SOURCE_DIR}/common/libs/signal_smoothing/oscc_signal_smoothing.cpp + ${CMAKE_SOURCE_DIR}/common/libs/timer/oscc_timer.cpp src/main.cpp src/globals.cpp src/accumulator.cpp @@ -31,10 +30,11 @@ generate_arduino_firmware( src/master_cylinder.cpp src/brake_control.cpp src/communications.cpp - src/init.cpp) + src/init.cpp + src/timers.cpp) target_include_directories( - kia-soul-brake + brake PRIVATE include ${CMAKE_SOURCE_DIR}/common/include @@ -43,7 +43,7 @@ target_include_directories( ${CMAKE_SOURCE_DIR}/common/libs/pid ${CMAKE_SOURCE_DIR}/common/libs/serial ${CMAKE_SOURCE_DIR}/common/libs/can - ${CMAKE_SOURCE_DIR}/common/libs/time - ${CMAKE_SOURCE_DIR}/common/libs/signal_smoothing) + ${CMAKE_SOURCE_DIR}/common/libs/timer + ${CMAKE_SOURCE_DIR}/../api/include) add_subdirectory(utils) diff --git a/platforms/kia_soul/firmware/brake/include/accumulator.h b/firmware/brake/include/accumulator.h similarity index 83% rename from platforms/kia_soul/firmware/brake/include/accumulator.h rename to firmware/brake/include/accumulator.h index 50a35c53..ca2335e3 100644 --- a/platforms/kia_soul/firmware/brake/include/accumulator.h +++ b/firmware/brake/include/accumulator.h @@ -9,25 +9,6 @@ #define _OSCC_KIA_SOUL_BRAKE_ACCUMULATOR_H_ -/* - * @brief Minimum accumulator presure. [decibars] - * - */ -#define ACCUMULATOR_PRESSURE_MIN_IN_DECIBARS ( 777.6 ) - -/* - * @brief Maximum accumulator pressure. [decibars] - * - */ -#define ACCUMULATOR_PRESSURE_MAX_IN_DECIBARS ( 878.0 ) - -/* - * @brief Alpha term for the exponential filter used to smooth the sensor input. - * - */ -#define ACCUMULATOR_PRESSURE_SENSOR_EXPONENTIAL_FILTER_ALPHA ( 0.05 ) - - // **************************************************************************** // Function: accumulator_init // diff --git a/firmware/brake/include/brake_control.h b/firmware/brake/include/brake_control.h new file mode 100644 index 00000000..3c06ab5e --- /dev/null +++ b/firmware/brake/include/brake_control.h @@ -0,0 +1,148 @@ +/** + * @file brake_control.h + * @brief Control of the brake system. + * + */ + + +#ifndef _OSCC_KIA_SOUL_BRAKE_CONTROL_H_ +#define _OSCC_KIA_SOUL_BRAKE_CONTROL_H_ + + +#include + + +/** + * @brief Current brake control state. + * + * Current state of the throttle module control system. + * + */ +typedef struct +{ + bool enabled; /* Flag indicating control is currently enabled. */ + + bool operator_override; /* Flag indicating whether steering wheel was + manually turned by operator. */ + + uint8_t dtcs; /* Bitfield of faults present in the module. */ + + float commanded_pedal_position; /* Brake pedal position commanded by + controller. */ + + bool startup_pressure_check_error; /* Flag indicating a problem with the actuator. */ + + bool startup_pump_motor_check_error; /* Flag indicating a problem with the pump motor. */ +} brake_control_state_s; + + +// **************************************************************************** +// Function: set_accumulator_solenoid_duty_cycle +// +// Purpose: Set the PWM that controls the "accumulator" solenoids to the +// specified value. +// +// Returns: void +// +// Parameters: [in] duty_cycle - value to send to the PWM +// +// **************************************************************************** +void set_accumulator_solenoid_duty_cycle( + const uint16_t duty_cycle ); + + +// **************************************************************************** +// Function: set_release_solenoid_duty_cycle +// +// Purpose: Set the PWM that controls the "release" solenoids to the +// specified value. +// +// Returns: void +// +// Parameters: [in] duty_cycle - value to send to the PWM +// +// **************************************************************************** +void set_release_solenoid_duty_cycle( + const uint16_t duty_cycle ); + + +// **************************************************************************** +// Function: enable_control +// +// Purpose: Enable control of the brake system. +// +// Returns: void +// +// Parameters: void +// +// **************************************************************************** +void enable_control( void ); + + +// **************************************************************************** +// Function: disable_control +// +// Purpose: Disable control of the brake system. +// +// Returns: void +// +// Parameters: void +// +// **************************************************************************** +void disable_control( void ); + + +// **************************************************************************** +// Function: check_for_operator_override +// +// Purpose: Check to see if the vehicle's operator has manually pressed +// the brake pedal and disable control if they have. +// +// Returns: void +// +// Parameters: void +// +// **************************************************************************** +void check_for_operator_override( void ); + + +// **************************************************************************** +// Function: check_for_sensor_faults +// +// Purpose: Checks to see if valid values are being read from the sensors. +// +// Returns: void +// +// Parameters: void +// +// **************************************************************************** +void check_for_sensor_faults( void ); + + +// **************************************************************************** +// Function: brake_init +// +// Purpose: Initialize the brake system. +// +// Returns: void +// +// Parameters: void +// +// **************************************************************************** +void brake_init( void ); + + +// **************************************************************************** +// Function: update_brake +// +// Purpose: Write brake spoof values to DAC. +// +// Returns: void +// +// Parameters: void +// +// **************************************************************************** +void update_brake( void ); + + +#endif /* _OSCC_KIA_SOUL_BRAKE_CONTROL_H_ */ diff --git a/platforms/kia_soul/firmware/brake/include/communications.h b/firmware/brake/include/communications.h similarity index 59% rename from platforms/kia_soul/firmware/brake/include/communications.h rename to firmware/brake/include/communications.h index d7fd9d24..8f9a6236 100644 --- a/platforms/kia_soul/firmware/brake/include/communications.h +++ b/firmware/brake/include/communications.h @@ -9,51 +9,44 @@ #define _OSCC_KIA_SOUL_BRAKE_COMMUNICATIONS_H_ -#include -#include "oscc_can.h" - - -/* - * @brief Amount of time after controller command that is considered a - * timeout. [milliseconds] - * - */ -#define COMMAND_TIMEOUT_IN_MSEC ( 650 ) - - -/* - * @brief Amount of time after a Chassis State 1 report that is considered a - * timeout. [milliseconds] - * - */ -#define CHASSIS_STATE_1_REPORT_TIMEOUT_IN_MSEC ( 500 ) +// **************************************************************************** +// Function: publish_brake_report +// +// Purpose: Publish brake report to CAN bus. +// +// Returns: void +// +// Parameters: void +// +// **************************************************************************** +void publish_brake_report( void ); // **************************************************************************** -// Function: publish_reports +// Function: publish_fault_report // -// Purpose: Publish all valid reports to CAN bus. +// Purpose: Publish a fault report message to the CAN bus. // // Returns: void // // Parameters: void // // **************************************************************************** -void publish_reports( void ); +void publish_fault_report( void ); // **************************************************************************** -// Function: check_for_timeouts +// Function: check_for_controller_command_timeout // -// Purpose: Check for command and report timeouts. +// Purpose: Check if the last command received from the controller exceeds +// the timeout and disable control if it does. // // Returns: void // // Parameters: void // // **************************************************************************** -void check_for_timeouts( void ); - +void check_for_controller_command_timeout( void ); // **************************************************************************** // Function: check_for_incoming_message diff --git a/platforms/kia_soul/firmware/brake/include/globals.h b/firmware/brake/include/globals.h similarity index 90% rename from platforms/kia_soul/firmware/brake/include/globals.h rename to firmware/brake/include/globals.h index bb6a5ed2..a42a956d 100644 --- a/platforms/kia_soul/firmware/brake/include/globals.h +++ b/firmware/brake/include/globals.h @@ -143,12 +143,9 @@ #endif -EXTERN uint32_t g_brake_command_last_rx_timestamp; -EXTERN uint32_t g_brake_report_last_tx_timestamp; -EXTERN uint32_t g_chassis_state_1_report_last_rx_timestamp; -EXTERN uint32_t g_sensor_validity_last_check_timestamp; +EXTERN volatile bool g_brake_command_timeout; -EXTERN kia_soul_brake_control_state_s g_brake_control_state; +EXTERN volatile brake_control_state_s g_brake_control_state; EXTERN pid_s g_pid; diff --git a/platforms/kia_soul/firmware/brake/include/helper.h b/firmware/brake/include/helper.h similarity index 100% rename from platforms/kia_soul/firmware/brake/include/helper.h rename to firmware/brake/include/helper.h diff --git a/platforms/kia_soul/firmware/brake/include/init.h b/firmware/brake/include/init.h similarity index 100% rename from platforms/kia_soul/firmware/brake/include/init.h rename to firmware/brake/include/init.h diff --git a/platforms/kia_soul/firmware/brake/include/master_cylinder.h b/firmware/brake/include/master_cylinder.h similarity index 91% rename from platforms/kia_soul/firmware/brake/include/master_cylinder.h rename to firmware/brake/include/master_cylinder.h index f2d2e091..f983bdff 100644 --- a/platforms/kia_soul/firmware/brake/include/master_cylinder.h +++ b/firmware/brake/include/master_cylinder.h @@ -16,13 +16,6 @@ typedef struct } master_cylinder_pressure_s; -/* - * @brief Alpha term for the exponential filter used to smooth the sensor input. - * - */ -#define MASTER_CYLINDER_PRESSURE_SENSOR_EXPONENTIAL_FILTER_ALPHA ( 0.1 ) - - // **************************************************************************** // Function: master_cylinder_open // diff --git a/firmware/brake/include/timers.h b/firmware/brake/include/timers.h new file mode 100644 index 00000000..70f6ab0d --- /dev/null +++ b/firmware/brake/include/timers.h @@ -0,0 +1,25 @@ +/** + * @file timers.h + * @brief Timer functionality. + * + */ + + +#ifndef _OSCC_KIA_SOUL_BRAKE_TIMERS_H_ +#define _OSCC_KIA_SOUL_BRAKE_TIMERS_H_ + + +// **************************************************************************** +// Function: start_timers +// +// Purpose: Start timers for report publishing and fault checking. +// +// Returns: void +// +// Parameters: void +// +// **************************************************************************** +void start_timers( void ); + + +#endif /* _OSCC_KIA_SOUL_BRAKE_TIMERS_H_ */ diff --git a/platforms/kia_soul/firmware/brake/src/accumulator.cpp b/firmware/brake/src/accumulator.cpp similarity index 60% rename from platforms/kia_soul/firmware/brake/src/accumulator.cpp rename to firmware/brake/src/accumulator.cpp index de71ea11..6cf745f5 100644 --- a/platforms/kia_soul/firmware/brake/src/accumulator.cpp +++ b/firmware/brake/src/accumulator.cpp @@ -5,7 +5,7 @@ #include -#include "oscc_signal_smoothing.h" +#include "vehicles.h" #include "globals.h" #include "accumulator.h" @@ -23,31 +23,29 @@ void accumulator_init( void ) void accumulator_turn_pump_off( void ) { + cli(); digitalWrite( PIN_ACCUMULATOR_PUMP_MOTOR, LOW ); + sei(); } void accumulator_turn_pump_on( void ) { + cli(); digitalWrite( PIN_ACCUMULATOR_PUMP_MOTOR, HIGH ); + sei(); } float accumulator_read_pressure( void ) { + cli(); int raw_adc = analogRead( PIN_ACCUMULATOR_PRESSURE_SENSOR ); + sei(); - float unfiltered_pressure = raw_adc_to_pressure( raw_adc ); + float pressure = raw_adc_to_pressure( raw_adc ); - const float filter_alpha = ACCUMULATOR_PRESSURE_SENSOR_EXPONENTIAL_FILTER_ALPHA; - static float filtered_pressure = 0.0; - - filtered_pressure = exponential_moving_average( - filter_alpha, - unfiltered_pressure, - filtered_pressure); - - return filtered_pressure; + return pressure; } @@ -55,11 +53,11 @@ void accumulator_maintain_pressure( void ) { float pressure = accumulator_read_pressure( ); - if ( pressure <= ACCUMULATOR_PRESSURE_MIN_IN_DECIBARS ) + if ( pressure <= BRAKE_ACCUMULATOR_PRESSURE_MIN_IN_DECIBARS ) { accumulator_turn_pump_on( ); } - else if ( pressure >= ACCUMULATOR_PRESSURE_MAX_IN_DECIBARS ) + else if ( pressure >= BRAKE_ACCUMULATOR_PRESSURE_MAX_IN_DECIBARS ) { accumulator_turn_pump_off( ); } diff --git a/platforms/kia_soul/firmware/brake/src/brake_control.cpp b/firmware/brake/src/brake_control.cpp similarity index 60% rename from platforms/kia_soul/firmware/brake/src/brake_control.cpp rename to firmware/brake/src/brake_control.cpp index b50753e9..20138cd4 100644 --- a/platforms/kia_soul/firmware/brake/src/brake_control.cpp +++ b/firmware/brake/src/brake_control.cpp @@ -4,19 +4,29 @@ */ -#include #include "debug.h" -#include "oscc_time.h" #include "oscc_pid.h" -#include "oscc_signal_smoothing.h" +#include "dtc.h" +#include "can_protocols/brake_can_protocol.h" +#include "vehicles.h" #include "globals.h" +#include "accumulator.h" #include "brake_control.h" +#include "communications.h" #include "master_cylinder.h" #include "helper.h" -#include "accumulator.h" +/* + * @brief Number of consecutive faults that can occur when reading the + * sensors before control is disabled. + * + */ +#define SENSOR_VALIDITY_CHECK_FAULT_COUNT ( 4 ) + + +static float read_pressure_sensor( void ); static void disable_brake_lights( void ); static void enable_brake_lights( void ); static bool check_master_cylinder_pressure_sensor_for_fault( void ); @@ -91,11 +101,13 @@ void check_for_operator_override( void ) master_cylinder_read_pressure( &master_cylinder_pressure ); - if ( ( master_cylinder_pressure.sensor_1_pressure >= DRIVER_OVERRIDE_PEDAL_THRESHOLD_IN_DECIBARS ) || - ( master_cylinder_pressure.sensor_2_pressure >= DRIVER_OVERRIDE_PEDAL_THRESHOLD_IN_DECIBARS ) ) + 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 ) ) { disable_control( ); + publish_fault_report( ); + g_brake_control_state.operator_override = true; DEBUG_PRINTLN( "Operator override" ); @@ -111,100 +123,67 @@ void check_for_operator_override( void ) void check_for_sensor_faults( void ) { if ( (g_brake_control_state.enabled == true) - || (g_brake_control_state.invalid_sensor_value == true) ) + || DTC_CHECK(g_brake_control_state.dtcs, OSCC_BRAKE_DTC_INVALID_SENSOR_VAL) ) { - uint32_t current_time = GET_TIMESTAMP_MS(); - - bool timeout = is_timeout( - g_sensor_validity_last_check_timestamp, - current_time, - SENSOR_VALIDITY_CHECK_INTERVAL_IN_MSEC ); + bool master_cylinder_pressure_fault = + check_master_cylinder_pressure_sensor_for_fault( ); - if( timeout == true ) + if( master_cylinder_pressure_fault == true ) { - g_sensor_validity_last_check_timestamp = current_time; - + DEBUG_PRINTLN( "Bad value read from master cylinder pressure sensor" ); + } - bool master_cylinder_pressure_fault = - check_master_cylinder_pressure_sensor_for_fault( ); - if( master_cylinder_pressure_fault == true ) - { - DEBUG_PRINTLN( "Bad value read from master cylinder presure sensor" ); - } + bool accumulator_pressure_fault = + check_accumulator_pressure_sensor_for_fault( ); + if( accumulator_pressure_fault == true ) + { + DEBUG_PRINTLN( "Bad value read from accumulator pressure sensor" ); + } - bool accumulator_pressure_fault = - check_accumulator_pressure_sensor_for_fault( ); - - if( accumulator_pressure_fault == true ) - { - DEBUG_PRINTLN( "Bad value read from accumulator pressure sensor" ); - } + bool wheel_pressure_fault = + check_wheel_pressure_sensor_for_fault( ); - bool wheel_pressure_fault = - check_wheel_pressure_sensor_for_fault( ); + if( wheel_pressure_fault == true ) + { + DEBUG_PRINTLN( "Bad value read from wheel pressure sensor" ); + } - if( wheel_pressure_fault == true ) - { - DEBUG_PRINTLN( "Bad value read from wheel pressure sensor" ); - } + if( (master_cylinder_pressure_fault == true) + || (accumulator_pressure_fault == true) + || (wheel_pressure_fault == true) ) + { + disable_control( ); - if( (master_cylinder_pressure_fault == true) - || (accumulator_pressure_fault == true) - || (wheel_pressure_fault == true) ) - { - disable_control( ); + publish_fault_report( ); - g_brake_control_state.invalid_sensor_value = true; - } - else - { - g_brake_control_state.invalid_sensor_value = false; - } + DTC_SET( + g_brake_control_state.dtcs, + OSCC_BRAKE_DTC_INVALID_SENSOR_VAL ); + } + else + { + DTC_CLEAR( + g_brake_control_state.dtcs, + OSCC_BRAKE_DTC_INVALID_SENSOR_VAL ); } } } -void read_pressure_sensor( void ) -{ - int raw_pressure_left = analogRead( PIN_PRESSURE_SENSOR_FRONT_LEFT ); - int raw_pressure_right = analogRead( PIN_PRESSURE_SENSOR_FRONT_RIGHT ); - - float unfiltered_pressure_left = raw_adc_to_pressure( raw_pressure_left ); - float unfiltered_pressure_right = raw_adc_to_pressure( raw_pressure_right ); - - const float filter_alpha = BRAKE_PRESSURE_SENSOR_EXPONENTIAL_FILTER_ALPHA; - static float filtered_pressure_left = 0.0; - static float filtered_pressure_right = 0.0; - - filtered_pressure_left = exponential_moving_average( - filter_alpha, - unfiltered_pressure_left, - filtered_pressure_left); - - filtered_pressure_right = exponential_moving_average( - filter_alpha, - unfiltered_pressure_right, - filtered_pressure_right); - - float pressure_average = ( filtered_pressure_left + filtered_pressure_right ) / 2.0; - - g_brake_control_state.current_sensor_brake_pressure = pressure_average; -} - - void brake_init( void ) { + cli(); digitalWrite( PIN_ACCUMULATOR_SOLENOID_FRONT_LEFT, LOW ); digitalWrite( PIN_ACCUMULATOR_SOLENOID_FRONT_RIGHT, LOW ); digitalWrite( PIN_RELEASE_SOLENOID_FRONT_LEFT, LOW ); digitalWrite( PIN_RELEASE_SOLENOID_FRONT_RIGHT, LOW ); digitalWrite( PIN_WHEEL_PRESSURE_CHECK_1, LOW ); digitalWrite( PIN_WHEEL_PRESSURE_CHECK_2, LOW ); + sei(); pinMode( PIN_ACCUMULATOR_SOLENOID_FRONT_LEFT, OUTPUT ); pinMode( PIN_ACCUMULATOR_SOLENOID_FRONT_RIGHT, OUTPUT ); @@ -227,69 +206,51 @@ void update_brake( void ) { if ( g_brake_control_state.enabled == true ) { - static float pressure_target = 0.0; - static float pressure = 0.0; + cli(); + + static float pressure_at_wheels_target = 0.0; + static float pressure_at_wheels_current = 0.0; static uint32_t control_loop_time = 0; - uint32_t current_time = GET_TIMESTAMP_US(); + uint32_t current_time = micros(); - float loop_delta_t = - (float) get_time_delta( control_loop_time, current_time ); + float time_between_loops = current_time - control_loop_time; control_loop_time = current_time; - loop_delta_t /= 1000.0; - loop_delta_t /= 1000.0; - - read_pressure_sensor( ); - - // ******************************************************************** - // - // WARNING - // - // The ranges selected to do brake control are carefully tested to - // ensure that the pressure actuated is not outside of the range of - // what the brake module can handle. By changing any of this code you - // risk attempting to actuate a pressure outside of the brake modules - // valid range. Actuating a pressure outside of the modules valid - // range will, at best, cause it to go into an unrecoverable fault - // state. This is characterized by the accumulator "continuously - // pumping" without accumulating any actual pressure, or being - // "over pressured." Clearing this fault state requires expert - // knowledge of the braking module. - // - // It is NOT recommended to modify any of the existing control ranges, - // or gains, without expert knowledge. - // - // ************************************************************************ + // Division by 1000 twice overcomes the Arduino's mathematic limitations + // and allows for a conversion from microseconds to seconds + time_between_loops /= 1000.0; + time_between_loops /= 1000.0; static interpolate_range_s pressure_ranges = - { UINT16_MIN, UINT16_MAX, BRAKE_PRESSURE_MIN_IN_DECIBARS, BRAKE_PRESSURE_MAX_IN_DECIBARS }; - - pressure = g_brake_control_state.current_sensor_brake_pressure; + { 0, UINT16_MAX, BRAKE_PRESSURE_MIN_IN_DECIBARS, BRAKE_PRESSURE_MAX_IN_DECIBARS }; - pressure_target = interpolate( + pressure_at_wheels_target = interpolate( g_brake_control_state.commanded_pedal_position, &pressure_ranges ); - int16_t ret = pid_update( &g_pid, - pressure_target, - pressure, - loop_delta_t ); + pressure_at_wheels_current = read_pressure_sensor( ); + + int16_t ret = pid_update( + &g_pid, + pressure_at_wheels_target, + pressure_at_wheels_current, + time_between_loops ); if ( ret == PID_SUCCESS ) { float pid_output = g_pid.control; // pressure too high - if ( pid_output < PID_OUTPUT_MIN ) + if ( pid_output < BRAKE_PID_OUTPUT_MIN ) { static interpolate_range_s slr_ranges = { - PID_RELEASE_SOLENOID_CLAMPED_MIN, - PID_RELEASE_SOLENOID_CLAMPED_MAX, - RELEASE_SOLENOID_DUTY_CYCLE_MIN, - RELEASE_SOLENOID_DUTY_CYCLE_MAX }; + BRAKE_PID_RELEASE_SOLENOID_CLAMPED_MIN, + BRAKE_PID_RELEASE_SOLENOID_CLAMPED_MAX, + BRAKE_RELEASE_SOLENOID_DUTY_CYCLE_MIN, + BRAKE_RELEASE_SOLENOID_DUTY_CYCLE_MAX }; uint16_t slr_duty_cycle = 0; @@ -298,27 +259,27 @@ void update_brake( void ) pid_output = -pid_output; slr_duty_cycle = (uint16_t)interpolate( pid_output, &slr_ranges ); - if ( slr_duty_cycle > ( uint16_t )RELEASE_SOLENOID_DUTY_CYCLE_MAX ) + if ( slr_duty_cycle > ( uint16_t )BRAKE_RELEASE_SOLENOID_DUTY_CYCLE_MAX ) { - slr_duty_cycle = ( uint16_t )RELEASE_SOLENOID_DUTY_CYCLE_MAX; + slr_duty_cycle = ( uint16_t )BRAKE_RELEASE_SOLENOID_DUTY_CYCLE_MAX; } set_release_solenoid_duty_cycle( slr_duty_cycle ); - if ( pressure_target < BRAKE_LIGHT_PRESSURE_THRESHOLD_IN_DECIBARS ) + if ( pressure_at_wheels_target < BRAKE_LIGHT_PRESSURE_THRESHOLD_IN_DECIBARS ) { disable_brake_lights( ); } } // pressure too low - else if ( pid_output > PID_OUTPUT_MAX ) + else if ( pid_output > BRAKE_PID_OUTPUT_MAX ) { static interpolate_range_s sla_ranges = { - PID_ACCUMULATOR_SOLENOID_CLAMPED_MIN, - PID_ACCUMULATOR_SOLENOID_CLAMPED_MAX, - ACCUMULATOR_SOLENOID_DUTY_CYCLE_MIN, - ACCUMULATOR_SOLENOID_DUTY_CYCLE_MAX }; + BRAKE_PID_ACCUMULATOR_SOLENOID_CLAMPED_MIN, + BRAKE_PID_ACCUMULATOR_SOLENOID_CLAMPED_MAX, + BRAKE_ACCUMULATOR_SOLENOID_DUTY_CYCLE_MIN, + BRAKE_ACCUMULATOR_SOLENOID_DUTY_CYCLE_MAX }; uint16_t sla_duty_cycle = 0; @@ -328,36 +289,50 @@ void update_brake( void ) sla_duty_cycle = (uint16_t)interpolate( pid_output, &sla_ranges ); - if ( sla_duty_cycle > ( uint16_t )ACCUMULATOR_SOLENOID_DUTY_CYCLE_MAX ) + if ( sla_duty_cycle > ( uint16_t )BRAKE_ACCUMULATOR_SOLENOID_DUTY_CYCLE_MAX ) { - sla_duty_cycle = ( uint16_t )ACCUMULATOR_SOLENOID_DUTY_CYCLE_MAX; + sla_duty_cycle = ( uint16_t )BRAKE_ACCUMULATOR_SOLENOID_DUTY_CYCLE_MAX; } set_accumulator_solenoid_duty_cycle( sla_duty_cycle ); } - - // pressure within valid range - else - { - if ( g_brake_control_state.commanded_pedal_position < BRAKE_LIGHT_PRESSURE_THRESHOLD_IN_DECIBARS ) - { - disable_brake_lights( ); - } - } } + + sei(); } } +static float read_pressure_sensor( void ) +{ + cli(); + int raw_adc_front_left = + analogRead( PIN_PRESSURE_SENSOR_FRONT_LEFT ); + + int raw_adc_front_right = + analogRead( PIN_PRESSURE_SENSOR_FRONT_RIGHT ); + sei(); + + float pressure_front_left = raw_adc_to_pressure( raw_adc_front_left ); + float pressure_front_right = raw_adc_to_pressure( raw_adc_front_right ); + + return ( (pressure_front_left + pressure_front_right) / 2.0 ); +} + + static void disable_brake_lights( void ) { + cli(); digitalWrite( PIN_BRAKE_LIGHT, LOW ); + sei(); } static void enable_brake_lights( void ) { + cli(); digitalWrite( PIN_BRAKE_LIGHT, HIGH ); + sei(); } @@ -367,8 +342,10 @@ static bool check_master_cylinder_pressure_sensor_for_fault( void ) static int fault_count = 0; + cli(); int master_cylinder_pressure_1 = analogRead( PIN_MASTER_CYLINDER_PRESSURE_SENSOR_1 ); int master_cylinder_pressure_2 = analogRead( PIN_MASTER_CYLINDER_PRESSURE_SENSOR_2 ); + sei(); // sensor pins tied to ground - a value of zero indicates disconnection if( (master_cylinder_pressure_1 == 0) @@ -396,7 +373,9 @@ static bool check_accumulator_pressure_sensor_for_fault( void ) static int fault_count = 0; + cli(); int accumulator_pressure = analogRead( PIN_ACCUMULATOR_PRESSURE_SENSOR ); + sei(); // sensor pins tied to ground - a value of zero indicates disconnection if( accumulator_pressure == 0 ) @@ -423,8 +402,10 @@ static bool check_wheel_pressure_sensor_for_fault( void ) static int fault_count = 0; + cli(); int wheel_pressure_front_left = analogRead( PIN_PRESSURE_SENSOR_FRONT_LEFT ); int wheel_pressure_front_right = analogRead( PIN_PRESSURE_SENSOR_FRONT_RIGHT ); + sei(); // sensor pins tied to ground - a value of zero indicates disconnection if( (wheel_pressure_front_left == 0) @@ -455,13 +436,18 @@ static void startup_check( void ) static void pressure_startup_check( void ) { + cli(); digitalWrite( PIN_WHEEL_PRESSURE_CHECK_1, HIGH ); digitalWrite( PIN_WHEEL_PRESSURE_CHECK_2, HIGH ); + sei(); + delay(250); + cli(); int pressure_front_left = analogRead( PIN_PRESSURE_SENSOR_FRONT_LEFT ); int pressure_front_right = analogRead( PIN_PRESSURE_SENSOR_FRONT_RIGHT ); int pressure_accumulator = analogRead( PIN_ACCUMULATOR_PRESSURE_SENSOR ); + sei(); if( (pressure_front_left < BRAKE_PRESSURE_SENSOR_CHECK_VALUE_MIN) || (pressure_front_left > BRAKE_PRESSURE_SENSOR_CHECK_VALUE_MAX) @@ -477,8 +463,11 @@ static void pressure_startup_check( void ) g_brake_control_state.startup_pressure_check_error = false; } + cli(); digitalWrite( PIN_WHEEL_PRESSURE_CHECK_1, LOW ); digitalWrite( PIN_WHEEL_PRESSURE_CHECK_2, LOW ); + sei(); + delay(250); } @@ -504,3 +493,4 @@ static void pump_startup_check( void ) accumulator_turn_pump_off(); } + diff --git a/firmware/brake/src/communications.cpp b/firmware/brake/src/communications.cpp new file mode 100644 index 00000000..6d84f756 --- /dev/null +++ b/firmware/brake/src/communications.cpp @@ -0,0 +1,152 @@ +/** + * @file communications.cpp + * + */ + + +#include "mcp_can.h" +#include "can_protocols/brake_can_protocol.h" +#include "can_protocols/fault_can_protocol.h" +#include "oscc_can.h" +#include "debug.h" + +#include "globals.h" +#include "communications.h" +#include "brake_control.h" + + +static void process_rx_frame( + const can_frame_s * const frame ); + +static void process_brake_command( + const uint8_t * const data ); + +static void process_fault_report( + const uint8_t * const data ); + + +void publish_brake_report( void ) +{ + oscc_brake_report_s brake_report; + + brake_report.magic[0] = (uint8_t) OSCC_MAGIC_BYTE_0; + brake_report.magic[1] = (uint8_t) OSCC_MAGIC_BYTE_1; + brake_report.enabled = (uint8_t) g_brake_control_state.enabled; + brake_report.operator_override = (uint8_t) g_brake_control_state.operator_override; + brake_report.dtcs = g_brake_control_state.dtcs; + + cli(); + g_control_can.sendMsgBuf( + OSCC_BRAKE_REPORT_CAN_ID, + CAN_STANDARD, + OSCC_BRAKE_REPORT_CAN_DLC, + (uint8_t *) &brake_report ); + sei(); +} + + +void publish_fault_report( void ) +{ + oscc_fault_report_s fault_report; + + fault_report.magic[0] = (uint8_t) OSCC_MAGIC_BYTE_0; + fault_report.magic[1] = (uint8_t) OSCC_MAGIC_BYTE_1; + fault_report.fault_origin_id = FAULT_ORIGIN_BRAKE; + + cli(); + g_control_can.sendMsgBuf( + OSCC_FAULT_REPORT_CAN_ID, + CAN_STANDARD, + OSCC_FAULT_REPORT_CAN_DLC, + (uint8_t *) &fault_report ); + sei(); +} + + +void check_for_controller_command_timeout( void ) +{ + if( g_brake_control_state.enabled == true ) + { + if ( g_brake_command_timeout == true ) + { + disable_control( ); + + publish_fault_report( ); + + DEBUG_PRINTLN( "Timeout - controller command" ); + } + } +} + + +void check_for_incoming_message( void ) +{ + can_frame_s rx_frame; + can_status_t ret = check_for_rx_frame( g_control_can, &rx_frame ); + + if( ret == CAN_RX_FRAME_AVAILABLE ) + { + process_rx_frame( &rx_frame ); + } +} + + +static void process_rx_frame( + const can_frame_s * const frame ) +{ + if ( frame != NULL ) + { + if( (frame->data[0] == OSCC_MAGIC_BYTE_0) + && (frame->data[1] == OSCC_MAGIC_BYTE_1) ) + { + if ( frame->id == OSCC_BRAKE_COMMAND_CAN_ID ) + { + process_brake_command( frame->data ); + } + else if ( frame->id == OSCC_FAULT_REPORT_CAN_ID ) + { + process_fault_report( frame->data ); + } + } + } +} + + +static void process_brake_command( + const uint8_t * const data ) +{ + if (data != NULL ) + { + const oscc_brake_command_s * const brake_command = + (oscc_brake_command_s *) data; + + if( brake_command->enable == true ) + { + enable_control( ); + } + else + { + disable_control( ); + } + + g_brake_control_state.commanded_pedal_position = + brake_command->pedal_command; + + g_brake_command_timeout = false; + } +} + + +static void process_fault_report( + const uint8_t * const data ) +{ + if ( data != NULL ) + { + const oscc_fault_report_s * const fault_report = + (oscc_fault_report_s *) data; + + disable_control( ); + + DEBUG_PRINTLN( "Fault report received" ); + } +} diff --git a/platforms/kia_soul/firmware/brake/src/globals.cpp b/firmware/brake/src/globals.cpp similarity index 100% rename from platforms/kia_soul/firmware/brake/src/globals.cpp rename to firmware/brake/src/globals.cpp diff --git a/platforms/kia_soul/firmware/brake/src/helper.cpp b/firmware/brake/src/helper.cpp similarity index 97% rename from platforms/kia_soul/firmware/brake/src/helper.cpp rename to firmware/brake/src/helper.cpp index a935c321..22a293bf 100644 --- a/platforms/kia_soul/firmware/brake/src/helper.cpp +++ b/firmware/brake/src/helper.cpp @@ -6,6 +6,7 @@ #include #include +#include "vehicles.h" #include "globals.h" #include "helper.h" diff --git a/platforms/kia_soul/firmware/brake/src/init.cpp b/firmware/brake/src/init.cpp similarity index 61% rename from platforms/kia_soul/firmware/brake/src/init.cpp rename to firmware/brake/src/init.cpp index 1483a2e5..71260419 100644 --- a/platforms/kia_soul/firmware/brake/src/init.cpp +++ b/firmware/brake/src/init.cpp @@ -8,10 +8,13 @@ #include "oscc_serial.h" #include "oscc_can.h" #include "debug.h" -#include "oscc_time.h" +#include "oscc_timer.h" +#include "can_protocols/brake_can_protocol.h" +#include "vehicles.h" #include "globals.h" #include "init.h" +#include "communications.h" #include "accumulator.h" #include "master_cylinder.h" #include "brake_control.h" @@ -19,20 +22,16 @@ void init_globals( void ) { - memset( &g_brake_control_state, - 0, - sizeof(g_brake_control_state) ); + g_brake_control_state.enabled = false; + g_brake_control_state.operator_override = false; + g_brake_control_state.dtcs = 0; - // Initialize the timestamps to avoid timeout warnings on start up - g_brake_command_last_rx_timestamp = GET_TIMESTAMP_MS( ); - g_brake_command_last_rx_timestamp = GET_TIMESTAMP_MS( ); - g_chassis_state_1_report_last_rx_timestamp = GET_TIMESTAMP_MS( ); + g_brake_command_timeout = false; - pid_zeroize( &g_pid, PID_WINDUP_GUARD ); - - g_pid.proportional_gain = PID_PROPORTIONAL_GAIN; - g_pid.integral_gain = PID_INTEGRAL_GAIN; - g_pid.derivative_gain = PID_DERIVATIVE_GAIN; + pid_zeroize( &g_pid, BRAKE_PID_WINDUP_GUARD ); + g_pid.proportional_gain = BRAKE_PID_PROPORTIONAL_GAIN; + g_pid.integral_gain = BRAKE_PID_INTEGRAL_GAIN; + g_pid.derivative_gain = BRAKE_PID_DERIVATIVE_GAIN; } diff --git a/platforms/kia_soul/firmware/brake/src/main.cpp b/firmware/brake/src/main.cpp similarity index 75% rename from platforms/kia_soul/firmware/brake/src/main.cpp rename to firmware/brake/src/main.cpp index a2a5120b..8e4ed040 100644 --- a/platforms/kia_soul/firmware/brake/src/main.cpp +++ b/firmware/brake/src/main.cpp @@ -4,13 +4,13 @@ */ -#include #include "arduino_init.h" #include "debug.h" #include "accumulator.h" #include "brake_control.h" #include "communications.h" +#include "timers.h" #include "init.h" @@ -24,26 +24,20 @@ int main( void ) init_devices( ); - wdt_enable( WDTO_120MS ); + init_communication_interfaces( ); + + start_timers( ); DEBUG_PRINTLN( "initialization complete" ); while( true ) { - wdt_reset(); - check_for_incoming_message( ); accumulator_maintain_pressure( ); - check_for_timeouts( ); - - check_for_sensor_faults( ); - check_for_operator_override( ); - publish_reports( ); - update_brake( ); } } diff --git a/platforms/kia_soul/firmware/brake/src/master_cylinder.cpp b/firmware/brake/src/master_cylinder.cpp similarity index 53% rename from platforms/kia_soul/firmware/brake/src/master_cylinder.cpp rename to firmware/brake/src/master_cylinder.cpp index c2f4bf11..739734f2 100644 --- a/platforms/kia_soul/firmware/brake/src/master_cylinder.cpp +++ b/firmware/brake/src/master_cylinder.cpp @@ -6,7 +6,6 @@ #include #include "debug.h" -#include "oscc_signal_smoothing.h" #include "globals.h" #include "helper.h" @@ -23,7 +22,9 @@ void master_cylinder_init( void ) void master_cylinder_open( void ) { + cli(); analogWrite( PIN_MASTER_CYLINDER_SOLENOID, SOLENOID_PWM_OFF ); + sei(); DEBUG_PRINTLN( "Master Cylinder Open" ); } @@ -31,7 +32,9 @@ void master_cylinder_open( void ) void master_cylinder_close( void ) { + cli(); analogWrite( PIN_MASTER_CYLINDER_SOLENOID, SOLENOID_PWM_ON ); + sei(); DEBUG_PRINTLN( "Master Cylinder Close" ); } @@ -39,26 +42,11 @@ void master_cylinder_close( void ) void master_cylinder_read_pressure( master_cylinder_pressure_s * pressure ) { + cli(); int raw_adc_sensor_1 = analogRead( PIN_MASTER_CYLINDER_PRESSURE_SENSOR_1 ); int raw_adc_sensor_2 = analogRead( PIN_MASTER_CYLINDER_PRESSURE_SENSOR_2 ); + sei(); - float unfiltered_pressure_1 = raw_adc_to_pressure( raw_adc_sensor_1 ); - float unfiltered_pressure_2 = raw_adc_to_pressure( raw_adc_sensor_2 ); - - const float filter_alpha = MASTER_CYLINDER_PRESSURE_SENSOR_EXPONENTIAL_FILTER_ALPHA; - static float filtered_pressure_1 = 0.0; - static float filtered_pressure_2 = 0.0; - - filtered_pressure_1 = exponential_moving_average( - filter_alpha, - unfiltered_pressure_1, - filtered_pressure_1); - - filtered_pressure_2 = exponential_moving_average( - filter_alpha, - unfiltered_pressure_2, - filtered_pressure_2); - - pressure->sensor_1_pressure = filtered_pressure_1; - pressure->sensor_2_pressure = filtered_pressure_2; + pressure->sensor_1_pressure = raw_adc_to_pressure( raw_adc_sensor_1 ); + pressure->sensor_2_pressure = raw_adc_to_pressure( raw_adc_sensor_2 ); } diff --git a/firmware/brake/src/timers.cpp b/firmware/brake/src/timers.cpp new file mode 100644 index 00000000..545c64f1 --- /dev/null +++ b/firmware/brake/src/timers.cpp @@ -0,0 +1,44 @@ +/** + * @file timers.cpp + * + */ + + +#include "can_protocols/brake_can_protocol.h" +#include "oscc_timer.h" + +#include "timers.h" +#include "globals.h" +#include "communications.h" +#include "brake_control.h" + + +/* + * @brief Fault checking frequency. [Hz] + * + */ +#define FAULT_CHECK_FREQUENCY_IN_HZ ( 1 ) + + +static void check_for_faults( void ); + + +void start_timers( void ) +{ + timer1_init( FAULT_CHECK_FREQUENCY_IN_HZ, check_for_faults ); + timer2_init( OSCC_BRAKE_REPORT_PUBLISH_FREQ_IN_HZ, publish_brake_report ); +} + + +static void check_for_faults( void ) +{ + cli(); + + check_for_controller_command_timeout( ); + + check_for_sensor_faults( ); + + g_brake_command_timeout = true; + + sei(); +} diff --git a/platforms/kia_soul/firmware/brake/tests/CMakeLists.txt b/firmware/brake/tests/CMakeLists.txt similarity index 67% rename from platforms/kia_soul/firmware/brake/tests/CMakeLists.txt rename to firmware/brake/tests/CMakeLists.txt index 8b517108..8a25a99d 100644 --- a/platforms/kia_soul/firmware/brake/tests/CMakeLists.txt +++ b/firmware/brake/tests/CMakeLists.txt @@ -1,7 +1,7 @@ -project(kia-soul-brake-tests) +project(brake-tests) add_library( - kia-soul-brake + brake SHARED ../src/accumulator.cpp ../src/communications.cpp @@ -9,38 +9,36 @@ add_library( ../src/brake_control.cpp ../src/master_cylinder.cpp ../src/helper.cpp - ${CMAKE_SOURCE_DIR}/common/libs/time/oscc_time.cpp ${CMAKE_SOURCE_DIR}/common/libs/can/oscc_can.cpp ${CMAKE_SOURCE_DIR}/common/libs/pid/oscc_pid.cpp - ${CMAKE_SOURCE_DIR}/common/libs/signal_smoothing/oscc_signal_smoothing.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) target_include_directories( - kia-soul-brake + brake PRIVATE ../include ${CMAKE_SOURCE_DIR}/common/include ${CMAKE_SOURCE_DIR}/common/libs/can - ${CMAKE_SOURCE_DIR}/common/libs/time ${CMAKE_SOURCE_DIR}/common/libs/pid ${CMAKE_SOURCE_DIR}/common/libs/signal_smoothing - ${CMAKE_SOURCE_DIR}/common/testing/mocks) + ${CMAKE_SOURCE_DIR}/common/testing/mocks + ${CMAKE_SOURCE_DIR}/../api/include) add_executable( - kia-soul-brake-unit-test + brake-unit-test features/step_definitions/test.cpp) target_link_libraries( - kia-soul-brake-unit-test + brake-unit-test PRIVATE - kia-soul-brake + brake ${CMAKE_SOURCE_DIR}/common/testing/framework/cucumber-cpp/lib/libcucumber-cpp.a ${CMAKE_SOURCE_DIR}/common/testing/framework/cgreen/lib/libcgreen.so) target_include_directories( - kia-soul-brake-unit-test + brake-unit-test PRIVATE ../include ${CMAKE_SOURCE_DIR}/common/include @@ -48,17 +46,18 @@ target_include_directories( ${CMAKE_SOURCE_DIR}/common/libs/pid ${CMAKE_SOURCE_DIR}/common/testing/mocks ${CMAKE_SOURCE_DIR}/common/testing/framework/cucumber-cpp/include - ${CMAKE_SOURCE_DIR}/common/testing/framework/cgreen/include) + ${CMAKE_SOURCE_DIR}/common/testing/framework/cgreen/include + ${CMAKE_SOURCE_DIR}/../api/include) add_custom_target( - run-kia-soul-brake-unit-tests + run-brake-unit-tests DEPENDS - kia-soul-brake-unit-test + brake-unit-test COMMAND - kia-soul-brake-unit-test --port=3902 >/dev/null & cucumber _2.0.0_ ${CMAKE_CURRENT_SOURCE_DIR}/features ) + brake-unit-test --port=3902 >/dev/null & cucumber _2.0.0_ ${CMAKE_CURRENT_SOURCE_DIR}/features ) add_custom_target( - run-kia-soul-brake-property-tests + run-brake-property-tests COMMAND cargo test -- --test-threads=1 WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/property) diff --git a/platforms/kia_soul/firmware/brake/tests/features/timeouts_and_overrides.feature b/firmware/brake/tests/features/checking_faults.feature similarity index 65% rename from platforms/kia_soul/firmware/brake/tests/features/timeouts_and_overrides.feature rename to firmware/brake/tests/features/checking_faults.feature index c760fc71..5230dd8f 100644 --- a/platforms/kia_soul/firmware/brake/tests/features/timeouts_and_overrides.feature +++ b/firmware/brake/tests/features/checking_faults.feature @@ -7,20 +7,30 @@ Feature: Timeouts and overrides disabled. - Scenario: Controller command timeout + Scenario: A sensor becomes temporarily disconnected Given brake control is enabled - When the time since the last received controller command exceeds the timeout + When a sensor becomes temporarily disconnected + + Then control should remain enabled + + + Scenario: A sensor becomes permanently disconnected + Given brake control is enabled + + When a sensor becomes permanently disconnected Then control should be disabled + And a fault report should be published - Scenario: Chassis State 1 report timeout + Scenario: Controller command timeout Given brake control is enabled - When the time since the last received Chassis State 1 report exceeds the timeout + When the time since the last received controller command exceeds the timeout Then control should be disabled + And a fault report should be published Scenario Outline: Operator override @@ -29,7 +39,7 @@ Feature: Timeouts and overrides When the operator applies to the brake pedal Then control should be disabled - And override flag should be set + And a fault report should be published Examples: | sensor_val | diff --git a/platforms/kia_soul/firmware/brake/tests/features/receiving_commands.feature b/firmware/brake/tests/features/receiving_messages.feature similarity index 79% rename from platforms/kia_soul/firmware/brake/tests/features/receiving_commands.feature rename to firmware/brake/tests/features/receiving_messages.feature index fe20c1b1..83e0be6d 100644 --- a/platforms/kia_soul/firmware/brake/tests/features/receiving_commands.feature +++ b/firmware/brake/tests/features/receiving_messages.feature @@ -4,23 +4,29 @@ Feature: Receiving commands Commands received from a controller should be processed and acted upon. - @enable_command + Scenario: Enable brake command sent from controller Given brake control is disabled When an enable brake command is received Then control should be enabled - And the last command timestamp should be set - @disable_command + Scenario: Disable brake command sent from controller Given brake control is enabled When a disable brake command is received Then control should be disabled - And the last command timestamp should be set + + + Scenario: Fault report sent from a different module + Given brake control is enabled + + When a fault report is received + + Then control should be disabled Scenario Outline: Brake pedal command sent from controller @@ -36,10 +42,10 @@ Feature: Receiving commands Examples: | left_pressure | right_pressure | command | solenoid | duty_cycle | | 120 | 120 | 20000 | ACCUMULATOR | 105 | - | 160 | 160 | 20000 | ACCUMULATOR | 105 | - | 190 | 190 | 20000 | ACCUMULATOR | 105 | - | 230 | 230 | 20000 | ACCUMULATOR | 105 | - | 200 | 200 | 20000 | ACCUMULATOR | 105 | + | 160 | 160 | 20000 | ACCUMULATOR | 104 | + | 190 | 190 | 20000 | ACCUMULATOR | 93 | + | 230 | 230 | 20000 | NONE | 0 | + | 200 | 200 | 20000 | NONE | 0 | | 220 | 220 | 20000 | NONE | 0 | | 205 | 205 | 20000 | NONE | 0 | | 215 | 215 | 20000 | NONE | 0 | diff --git a/firmware/brake/tests/features/sending_reports.feature b/firmware/brake/tests/features/sending_reports.feature new file mode 100644 index 00000000..32786be7 --- /dev/null +++ b/firmware/brake/tests/features/sending_reports.feature @@ -0,0 +1,12 @@ +# language: en + +Feature: Sending reports + + Brake reports should be published to the control CAN bus after an interval. + + + Scenario: Brake report published after an interval + When a brake report is published + + Then a brake report should be put on the control CAN bus + And the brake report's fields should be set diff --git a/firmware/brake/tests/features/step_definitions/checking_faults.cpp b/firmware/brake/tests/features/step_definitions/checking_faults.cpp new file mode 100644 index 00000000..fc333958 --- /dev/null +++ b/firmware/brake/tests/features/step_definitions/checking_faults.cpp @@ -0,0 +1,72 @@ +WHEN("^a sensor becomes temporarily disconnected$") +{ + g_mock_arduino_analog_read_return = 0; + + check_for_sensor_faults(); + + check_for_sensor_faults(); + + check_for_sensor_faults(); +} + + +WHEN("^a sensor becomes permanently disconnected$") +{ + g_mock_arduino_analog_read_return = 0; + + // must call function enough times to exceed the fault limit + for( int i = 0; i < 100; ++i ) + { + check_for_sensor_faults(); + } +} + + +WHEN("^the time since the last received controller command exceeds the timeout$") +{ + g_brake_command_timeout = true; + + check_for_controller_command_timeout(); +} + + +WHEN("^the operator applies (.*) to the brake pedal$") +{ + REGEX_PARAM(int, pedal_val); + + g_mock_arduino_analog_read_return = pedal_val; + + check_for_operator_override(); + +} + + +THEN("^control should remain enabled") +{ + assert_that( + g_brake_control_state.enabled, + is_equal_to(1)); +} + + +THEN("^a fault report should be published$") +{ + assert_that(g_mock_mcp_can_send_msg_buf_id, is_equal_to(OSCC_FAULT_REPORT_CAN_ID)); + assert_that(g_mock_mcp_can_send_msg_buf_ext, is_equal_to(CAN_STANDARD)); + assert_that(g_mock_mcp_can_send_msg_buf_len, is_equal_to(OSCC_FAULT_REPORT_CAN_DLC)); + + oscc_fault_report_s * fault_report = + (oscc_fault_report_s *) g_mock_mcp_can_send_msg_buf_buf; + + assert_that( + fault_report->magic[0], + is_equal_to(OSCC_MAGIC_BYTE_0)); + + assert_that( + fault_report->magic[1], + is_equal_to(OSCC_MAGIC_BYTE_1)); + + assert_that( + fault_report->fault_origin_id, + is_equal_to(FAULT_ORIGIN_BRAKE)); +} diff --git a/platforms/kia_soul/firmware/brake/tests/features/step_definitions/common.cpp b/firmware/brake/tests/features/step_definitions/common.cpp similarity index 75% rename from platforms/kia_soul/firmware/brake/tests/features/step_definitions/common.cpp rename to firmware/brake/tests/features/step_definitions/common.cpp index 5dc6ef9a..69e12257 100644 --- a/platforms/kia_soul/firmware/brake/tests/features/step_definitions/common.cpp +++ b/firmware/brake/tests/features/step_definitions/common.cpp @@ -9,67 +9,56 @@ #include "oscc_can.h" #include "mcp_can.h" #include "brake_control.h" -#include "brake_can_protocol.h" -#include "chassis_state_can_protocol.h" +#include "can_protocols/brake_can_protocol.h" +#include "can_protocols/fault_can_protocol.h" +#include "vehicles.h" #include "globals.h" using namespace cgreen; - -extern unsigned long g_mock_arduino_millis_return; extern unsigned long g_mock_arduino_micros_return; -extern uint8_t g_mock_arduino_analog_write_pins[100]; -extern int g_mock_arduino_analog_write_val[100]; -extern int g_mock_arduino_analog_write_count; + extern uint8_t g_mock_arduino_digital_write_pin; extern uint8_t g_mock_arduino_digital_write_val; -extern int g_mock_arduino_analog_read_return; - -extern INT8U g_mock_mcp_can_check_receive_return; -extern INT32U g_mock_mcp_can_read_msg_buf_id; -extern INT8U g_mock_mcp_can_read_msg_buf_buf[8]; +extern int g_mock_arduino_analog_read_return; +extern int g_mock_arduino_analog_write_count; +extern uint8_t g_mock_arduino_analog_write_pins[100]; +extern int g_mock_arduino_analog_write_val[100]; -extern INT32U g_mock_mcp_can_send_msg_buf_id; -extern INT8U g_mock_mcp_can_send_msg_buf_ext; -extern INT8U g_mock_mcp_can_send_msg_buf_len; -extern INT8U *g_mock_mcp_can_send_msg_buf_buf; +extern uint8_t g_mock_mcp_can_check_receive_return; +extern uint32_t g_mock_mcp_can_read_msg_buf_id; +extern uint8_t g_mock_mcp_can_read_msg_buf_buf[8]; +extern uint32_t g_mock_mcp_can_send_msg_buf_id; +extern uint8_t g_mock_mcp_can_send_msg_buf_ext; +extern uint8_t g_mock_mcp_can_send_msg_buf_len; +extern uint8_t *g_mock_mcp_can_send_msg_buf_buf; -extern kia_soul_brake_control_state_s g_brake_control_state; +extern volatile brake_control_state_s g_brake_control_state; // return to known state before every scenario BEFORE() { - g_mock_mcp_can_check_receive_return = -1; - g_mock_mcp_can_read_msg_buf_id = 0; - g_mock_arduino_millis_return = 555; - - memset( - &g_mock_arduino_analog_write_pins, - 0, - sizeof(g_mock_arduino_analog_write_pins)); - - memset( - &g_mock_arduino_analog_write_val, - 0, - sizeof(g_mock_arduino_analog_write_val)); + g_mock_arduino_digital_write_pin = UINT8_MAX; + g_mock_arduino_digital_write_val = UINT8_MAX; + g_mock_arduino_analog_read_return = INT_MAX; g_mock_arduino_analog_write_count = 0; + memset(&g_mock_arduino_analog_write_pins, 0, sizeof(g_mock_arduino_analog_write_pins)); + memset(&g_mock_arduino_analog_write_val, 0, sizeof(g_mock_arduino_analog_write_val)); - memset( - &g_mock_mcp_can_read_msg_buf_buf, - 0, - sizeof(g_mock_mcp_can_read_msg_buf_buf)); + g_mock_mcp_can_check_receive_return = UINT8_MAX; + g_mock_mcp_can_read_msg_buf_id = UINT32_MAX; + memset(&g_mock_mcp_can_read_msg_buf_buf, 0, sizeof(g_mock_mcp_can_read_msg_buf_buf)); - memset( - &g_brake_control_state, - 0, - sizeof(g_brake_control_state)); + g_mock_mcp_can_send_msg_buf_id = UINT32_MAX; + g_mock_mcp_can_send_msg_buf_ext = UINT8_MAX; + g_mock_mcp_can_send_msg_buf_len = UINT8_MAX; - g_mock_arduino_digital_write_pin = UINT8_MAX; - g_mock_arduino_digital_write_val = UINT8_MAX; - g_mock_arduino_analog_read_return = INT_MAX; + g_brake_control_state.enabled = false; + g_brake_control_state.operator_override = false; + g_brake_control_state.dtcs = 0; } @@ -85,15 +74,6 @@ GIVEN("^brake control is disabled$") } -GIVEN("^the previous brake pedal position command was (.*)$") -{ - REGEX_PARAM(int, command); - - g_brake_control_state.commanded_pedal_position = command; -} - - - THEN("^control should be enabled$") { assert_that( diff --git a/platforms/kia_soul/firmware/brake/tests/features/step_definitions/cucumber.wire b/firmware/brake/tests/features/step_definitions/cucumber.wire similarity index 100% rename from platforms/kia_soul/firmware/brake/tests/features/step_definitions/cucumber.wire rename to firmware/brake/tests/features/step_definitions/cucumber.wire diff --git a/platforms/kia_soul/firmware/brake/tests/features/step_definitions/receiving_commands.cpp b/firmware/brake/tests/features/step_definitions/receiving_messages.cpp similarity index 72% rename from platforms/kia_soul/firmware/brake/tests/features/step_definitions/receiving_commands.cpp rename to firmware/brake/tests/features/step_definitions/receiving_messages.cpp index 2b35fd16..29e4f86b 100644 --- a/platforms/kia_soul/firmware/brake/tests/features/step_definitions/receiving_commands.cpp +++ b/firmware/brake/tests/features/step_definitions/receiving_messages.cpp @@ -25,13 +25,15 @@ GIVEN("^the right brake sensor reads (.*)$") WHEN("^an enable brake command is received$") { - oscc_command_brake_data_s * brake_command_data = - (oscc_command_brake_data_s *) g_mock_mcp_can_read_msg_buf_buf; + g_mock_mcp_can_read_msg_buf_id = OSCC_BRAKE_COMMAND_CAN_ID; + g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL; - brake_command_data->enabled = 1; + oscc_brake_command_s * brake_command = + (oscc_brake_command_s *) g_mock_mcp_can_read_msg_buf_buf; - g_mock_mcp_can_read_msg_buf_id = OSCC_COMMAND_BRAKE_CAN_ID; - g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL; + brake_command->magic[0] = OSCC_MAGIC_BYTE_0; + brake_command->magic[1] = OSCC_MAGIC_BYTE_1; + brake_command->enable = 1; check_for_incoming_message(); } @@ -39,13 +41,30 @@ WHEN("^an enable brake command is received$") WHEN("^a disable brake command is received$") { - oscc_command_brake_data_s * brake_command_data = - (oscc_command_brake_data_s *) g_mock_mcp_can_read_msg_buf_buf; + g_mock_mcp_can_read_msg_buf_id = OSCC_BRAKE_COMMAND_CAN_ID; + g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL; + + oscc_brake_command_s * brake_command = + (oscc_brake_command_s *) g_mock_mcp_can_read_msg_buf_buf; + + brake_command->magic[0] = OSCC_MAGIC_BYTE_0; + brake_command->magic[1] = OSCC_MAGIC_BYTE_1; + brake_command->enable = 0; + + check_for_incoming_message(); +} - brake_command_data->enabled = 0; - g_mock_mcp_can_read_msg_buf_id = OSCC_COMMAND_BRAKE_CAN_ID; +WHEN("^a fault report is received$") +{ g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL; + g_mock_mcp_can_read_msg_buf_id = OSCC_FAULT_REPORT_CAN_ID; + + oscc_fault_report_s * fault_report = + (oscc_fault_report_s *) g_mock_mcp_can_read_msg_buf_buf; + + fault_report->magic[0] = OSCC_MAGIC_BYTE_0; + fault_report->magic[1] = OSCC_MAGIC_BYTE_1; check_for_incoming_message(); } @@ -55,20 +74,22 @@ WHEN("^the brake pedal command (.*) is received$") { REGEX_PARAM(int, command); - oscc_command_brake_data_s * brake_command_data = - (oscc_command_brake_data_s *) g_mock_mcp_can_read_msg_buf_buf; + oscc_brake_command_s * brake_command = + (oscc_brake_command_s *) g_mock_mcp_can_read_msg_buf_buf; - g_mock_mcp_can_read_msg_buf_id = OSCC_COMMAND_BRAKE_CAN_ID; + g_mock_mcp_can_read_msg_buf_id = OSCC_BRAKE_COMMAND_CAN_ID; g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL; - brake_command_data->enabled = 1; - brake_command_data->pedal_command = command; + brake_command->magic[0] = OSCC_MAGIC_BYTE_0; + brake_command->magic[1] = OSCC_MAGIC_BYTE_1; + brake_command->enable = 1; + brake_command->pedal_command = command; - pid_zeroize( &g_pid, PID_WINDUP_GUARD ); + pid_zeroize( &g_pid, BRAKE_PID_WINDUP_GUARD ); - g_pid.proportional_gain = PID_PROPORTIONAL_GAIN; - g_pid.integral_gain = PID_INTEGRAL_GAIN; - g_pid.derivative_gain = PID_DERIVATIVE_GAIN; + g_pid.proportional_gain = BRAKE_PID_PROPORTIONAL_GAIN; + g_pid.integral_gain = BRAKE_PID_INTEGRAL_GAIN; + g_pid.derivative_gain = BRAKE_PID_DERIVATIVE_GAIN; // restore PID params needed for next scenario g_pid.prev_input = mock_pid_prev_input; @@ -88,12 +109,12 @@ WHEN("^the brake pedal command (.*) is received$") THEN("^the brake pedal command should be parsed$") { - oscc_command_brake_data_s * brake_command_data = - (oscc_command_brake_data_s *) g_mock_mcp_can_read_msg_buf_buf; + oscc_brake_command_s * brake_command = + (oscc_brake_command_s *) g_mock_mcp_can_read_msg_buf_buf; assert_that_double( g_brake_control_state.commanded_pedal_position, - is_equal_to_double(brake_command_data->pedal_command)); + is_equal_to_double(brake_command->pedal_command)); } @@ -208,11 +229,3 @@ THEN("^the (.*) solenoid should be activated with duty cycle (.*)$") } } - - -THEN("^the last command timestamp should be set$") -{ - assert_that( - g_brake_command_last_rx_timestamp, - is_equal_to(g_mock_arduino_millis_return)); -} diff --git a/firmware/brake/tests/features/step_definitions/sending_reports.cpp b/firmware/brake/tests/features/step_definitions/sending_reports.cpp new file mode 100644 index 00000000..dd073107 --- /dev/null +++ b/firmware/brake/tests/features/step_definitions/sending_reports.cpp @@ -0,0 +1,43 @@ +WHEN("^a brake report is published$") +{ + g_brake_control_state.enabled = true; + g_brake_control_state.operator_override = true; + g_brake_control_state.dtcs = 0x55; + + publish_brake_report(); +} + + +THEN("^a brake report should be put on the control CAN bus$") +{ + assert_that(g_mock_mcp_can_send_msg_buf_id, is_equal_to(OSCC_BRAKE_REPORT_CAN_ID)); + assert_that(g_mock_mcp_can_send_msg_buf_ext, is_equal_to(CAN_STANDARD)); + assert_that(g_mock_mcp_can_send_msg_buf_len, is_equal_to(OSCC_BRAKE_REPORT_CAN_DLC)); +} + + +THEN("^the brake report's fields should be set$") +{ + oscc_brake_report_s * brake_report = + (oscc_brake_report_s *) g_mock_mcp_can_send_msg_buf_buf; + + assert_that( + brake_report->magic[0], + is_equal_to(OSCC_MAGIC_BYTE_0)); + + assert_that( + brake_report->magic[1], + is_equal_to(OSCC_MAGIC_BYTE_1)); + + assert_that( + brake_report->enabled, + is_equal_to(g_brake_control_state.enabled)); + + assert_that( + brake_report->operator_override, + is_equal_to(g_brake_control_state.operator_override)); + + assert_that( + brake_report->dtcs, + is_equal_to(g_brake_control_state.dtcs)); +} diff --git a/platforms/kia_soul/firmware/throttle/tests/features/step_definitions/test.cpp b/firmware/brake/tests/features/step_definitions/test.cpp similarity index 54% rename from platforms/kia_soul/firmware/throttle/tests/features/step_definitions/test.cpp rename to firmware/brake/tests/features/step_definitions/test.cpp index c44d2334..5f7d398e 100644 --- a/platforms/kia_soul/firmware/throttle/tests/features/step_definitions/test.cpp +++ b/firmware/brake/tests/features/step_definitions/test.cpp @@ -1,6 +1,5 @@ // include source files to prevent step files from conflicting with each other #include "common.cpp" -#include "checking_sensor_validity.cpp" -#include "receiving_commands.cpp" +#include "checking_faults.cpp" +#include "receiving_messages.cpp" #include "sending_reports.cpp" -#include "timeouts_and_overrides.cpp" diff --git a/platforms/kia_soul/firmware/brake/tests/features/support/env.rb b/firmware/brake/tests/features/support/env.rb similarity index 100% rename from platforms/kia_soul/firmware/brake/tests/features/support/env.rb rename to firmware/brake/tests/features/support/env.rb diff --git a/platforms/kia_soul/firmware/brake/tests/property/Cargo.toml b/firmware/brake/tests/property/Cargo.toml similarity index 100% rename from platforms/kia_soul/firmware/brake/tests/property/Cargo.toml rename to firmware/brake/tests/property/Cargo.toml diff --git a/firmware/brake/tests/property/build.rs b/firmware/brake/tests/property/build.rs new file mode 100644 index 00000000..421221d3 --- /dev/null +++ b/firmware/brake/tests/property/build.rs @@ -0,0 +1,67 @@ +extern crate gcc; +extern crate bindgen; + +use std::env; +use std::path::Path; + +fn main() { + gcc::Config::new() + .flag("-w") + .define("KIA_SOUL", Some("ON")) + .include("include") + .include("../../include") + .include("../../../common/testing/mocks") + .include("../../../common/include") + .include("../../../common/libs/can") + .include("../../../common/libs/time") + .include("../../../common/libs/pid") + .include("../../../../api/include") + .include("/usr/include") + .file("../../../common/testing/mocks/Arduino_mock.cpp") + .file("../../../common/testing/mocks/mcp_can_mock.cpp") + .file("../../src/communications.cpp") + .file("../../src/brake_control.cpp") + .file("../../src/globals.cpp") + .file("../../src/master_cylinder.cpp") + .file("../../src/helper.cpp") + .file("../../../common/libs/can/oscc_can.cpp") + .cpp(true) + .compiler("/usr/bin/g++") + .compile("libbrake_test.a"); + + let out_dir = env::var("OUT_DIR").unwrap(); + + let _ = bindgen::builder() + .header("include/wrapper.hpp") + .generate_comments(false) + .clang_arg("-DKIA_SOUL=ON") + .clang_arg("-I../../include") + .clang_arg("-I../../../common/testing/mocks") + .clang_arg("-I../../../common/include") + .clang_arg("-I../../../common/libs/can") + .clang_arg("-I../../../common/libs/pid") + .clang_arg("-I../../../../api/include") + .whitelisted_function("publish_brake_report") + .whitelisted_function("check_for_incoming_message") + .whitelisted_function("check_for_operator_override") + .whitelisted_var("OSCC_MAGIC_BYTE_0") + .whitelisted_var("OSCC_MAGIC_BYTE_1") + .whitelisted_var("OSCC_BRAKE_REPORT_CAN_ID") + .whitelisted_var("OSCC_BRAKE_REPORT_CAN_DLC") + .whitelisted_var("OSCC_BRAKE_COMMAND_CAN_ID") + .whitelisted_var("OSCC_BRAKE_COMMAND_CAN_DLC") + .whitelisted_var("BRAKE_OVERRIDE_PEDAL_THRESHOLD_IN_DECIBARS") + .whitelisted_var("MINIMUM_BRAKE_COMMAND") + .whitelisted_var("MAXIMUM_BRAKE_COMMAND") + .whitelisted_var("CAN_STANDARD") + .whitelisted_var("CAN_MSGAVAIL") + .whitelisted_var("g_control_can") + .whitelisted_var("g_brake_control_state") + .whitelisted_type("oscc_brake_report_s") + .whitelisted_type("oscc_brake_command_s") + .whitelisted_type("can_frame_s") + .generate() + .unwrap() + .write_to_file(Path::new(&out_dir).join("brake_test.rs")) + .expect("Unable to generate bindings"); +} diff --git a/firmware/brake/tests/property/include/wrapper.hpp b/firmware/brake/tests/property/include/wrapper.hpp new file mode 100644 index 00000000..d640cac6 --- /dev/null +++ b/firmware/brake/tests/property/include/wrapper.hpp @@ -0,0 +1,9 @@ + +#include "globals.h" +#include "communications.h" +#include "helper.h" +#include "can_protocols/brake_can_protocol.h" +#include "vehicles.h" +#include "oscc_can.h" +#include "Arduino.h" +#include "mcp_can.h" \ No newline at end of file diff --git a/firmware/brake/tests/property/src/tests.rs b/firmware/brake/tests/property/src/tests.rs new file mode 100644 index 00000000..e8c6f237 --- /dev/null +++ b/firmware/brake/tests/property/src/tests.rs @@ -0,0 +1,142 @@ +#![allow(non_upper_case_globals)] +#![allow(non_camel_case_types)] +#![allow(non_snake_case)] +#![allow(dead_code)] +#![allow(unused_variables)] +#![allow(unused_imports)] +include!(concat!(env!("OUT_DIR"), "/brake_test.rs")); + +extern crate quickcheck; +extern crate rand; + +use quickcheck::{QuickCheck, TestResult, Arbitrary, Gen, StdGen}; + +extern "C" { + #[link_name = "g_mock_mcp_can_check_receive_return"] + pub static mut g_mock_mcp_can_check_receive_return: u8; + #[link_name = "g_mock_mcp_can_read_msg_buf_id"] + pub static mut g_mock_mcp_can_read_msg_buf_id: u32; + #[link_name = "g_mock_mcp_can_read_msg_buf_buf"] + pub static mut g_mock_mcp_can_read_msg_buf_buf: [u8; 8usize]; + #[link_name = "g_mock_mcp_can_send_msg_buf_id"] + pub static mut g_mock_mcp_can_send_msg_buf_id: u32; + #[link_name = "g_mock_mcp_can_send_msg_buf_ext"] + pub static mut g_mock_mcp_can_send_msg_buf_ext: u8; + #[link_name = "g_mock_mcp_can_send_msg_buf_len"] + pub static mut g_mock_mcp_can_send_msg_buf_len: u8; + #[link_name = "g_mock_mcp_can_send_msg_buf_buf"] + pub static mut g_mock_mcp_can_send_msg_buf_buf: *mut u8; + #[link_name = "g_mock_arduino_analog_read_return"] + pub static mut g_mock_arduino_analog_read_return: isize; +} + +impl Arbitrary for oscc_brake_report_s { + fn arbitrary(g: &mut G) -> oscc_brake_report_s { + oscc_brake_report_s { + magic: [OSCC_MAGIC_BYTE_0 as u8, OSCC_MAGIC_BYTE_1 as u8], + enabled: u8::arbitrary(g), + operator_override: u8::arbitrary(g), + dtcs: u8::arbitrary(g), + reserved: [u8::arbitrary(g); 3] + } + } +} + +impl Arbitrary for oscc_brake_command_s { + fn arbitrary(g: &mut G) -> oscc_brake_command_s { + oscc_brake_command_s { + magic: [OSCC_MAGIC_BYTE_0 as u8, OSCC_MAGIC_BYTE_1 as u8], + pedal_command: u16::arbitrary(g), + enable: u8::arbitrary(g), + reserved: [u8::arbitrary(g); 3] + } + } +} + + +impl Arbitrary for can_frame_s { + fn arbitrary(g: &mut G) -> can_frame_s { + can_frame_s { + id: u32::arbitrary(g), + dlc: u8::arbitrary(g), + timestamp: u32::arbitrary(g), + data: [u8::arbitrary(g); 8] + } + } +} + +/// the throttle firmware should not attempt processing any messages +/// that are not throttle commands +fn prop_only_process_valid_messages(rx_can_msg: can_frame_s, current_target: f32) -> TestResult { + // if we generate a throttle can message, ignore the result + if rx_can_msg.id == OSCC_BRAKE_COMMAND_CAN_ID { + return TestResult::discard(); + } + unsafe { + g_brake_control_state.commanded_pedal_position = current_target; + + g_mock_mcp_can_read_msg_buf_id = rx_can_msg.id; + g_mock_mcp_can_read_msg_buf_buf = rx_can_msg.data; + g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL as u8; + + check_for_incoming_message(); + + TestResult::from_bool(g_brake_control_state.commanded_pedal_position == current_target) + } +} + +#[test] +fn check_message_type_validity() { + QuickCheck::new() + .tests(1000) + .gen(StdGen::new(rand::thread_rng(), u16::max_value() as usize)) + .quickcheck(prop_only_process_valid_messages as fn(can_frame_s, f32) -> TestResult) +} + +/// the throttle firmware should set the commanded pedal position +/// upon reciept of a valid command throttle message +fn prop_no_invalid_targets(brake_command_msg: oscc_brake_command_s) -> TestResult { + unsafe { + g_mock_mcp_can_read_msg_buf_id = OSCC_BRAKE_COMMAND_CAN_ID; + g_mock_mcp_can_read_msg_buf_buf = std::mem::transmute(brake_command_msg); + g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL as u8; + + check_for_incoming_message(); + + TestResult::from_bool(g_brake_control_state.commanded_pedal_position == + brake_command_msg.pedal_command as f32) + } +} + +#[test] +fn check_accel_pos_validity() { + QuickCheck::new() + .tests(1000) + .quickcheck(prop_no_invalid_targets as fn(oscc_brake_command_s) -> TestResult) +} + +/// the throttle firmware should set the control state as disabled +/// upon reciept of a valid command throttle message telling it to disable +fn prop_process_disable_command(mut brake_command_msg: oscc_brake_command_s) -> TestResult { + unsafe { + brake_command_msg.enable = 0u8; + + g_brake_control_state.enabled = false; + g_brake_control_state.operator_override = false; + + g_mock_mcp_can_read_msg_buf_id = OSCC_BRAKE_COMMAND_CAN_ID; + g_mock_mcp_can_read_msg_buf_buf = std::mem::transmute(brake_command_msg); + g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL as u8; + + check_for_incoming_message(); + + TestResult::from_bool(g_brake_control_state.enabled == false) + } +} + +#[test] +fn check_process_disable_command() { + QuickCheck::new() + .tests(1000) + .quickcheck(prop_process_disable_command as fn(oscc_brake_command_s) -> TestResult) +} diff --git a/platforms/kia_soul/firmware/brake/utils/CMakeLists.txt b/firmware/brake/utils/CMakeLists.txt similarity index 70% rename from platforms/kia_soul/firmware/brake/utils/CMakeLists.txt rename to firmware/brake/utils/CMakeLists.txt index 7078f266..6f9921ca 100644 --- a/platforms/kia_soul/firmware/brake/utils/CMakeLists.txt +++ b/firmware/brake/utils/CMakeLists.txt @@ -1,4 +1,4 @@ -project(kia-soul-brake-utils) +project(brake-utils) add_subdirectory(release_pressure) add_subdirectory(serial_actuator) diff --git a/platforms/kia_soul/firmware/brake/utils/release_pressure/CMakeLists.txt b/firmware/brake/utils/release_pressure/CMakeLists.txt similarity index 50% rename from platforms/kia_soul/firmware/brake/utils/release_pressure/CMakeLists.txt rename to firmware/brake/utils/release_pressure/CMakeLists.txt index 75c4a66a..a9fcad81 100644 --- a/platforms/kia_soul/firmware/brake/utils/release_pressure/CMakeLists.txt +++ b/firmware/brake/utils/release_pressure/CMakeLists.txt @@ -1,11 +1,11 @@ -project(kia-soul-brake-utils-release-pressure) +project(brake-utils-release-pressure) generate_arduino_firmware( - kia-soul-brake-utils-release-pressure + brake-utils-release-pressure SKETCH src/release_pressure.ino) target_compile_options( - kia-soul-brake-utils-release-pressure + brake-utils-release-pressure PRIVATE "-std=gnu++11" "-Os") diff --git a/platforms/kia_soul/firmware/brake/utils/release_pressure/src/release_pressure.ino b/firmware/brake/utils/release_pressure/src/release_pressure.ino similarity index 100% rename from platforms/kia_soul/firmware/brake/utils/release_pressure/src/release_pressure.ino rename to firmware/brake/utils/release_pressure/src/release_pressure.ino diff --git a/platforms/kia_soul/firmware/brake/utils/serial_actuator/CMakeLists.txt b/firmware/brake/utils/serial_actuator/CMakeLists.txt similarity index 70% rename from platforms/kia_soul/firmware/brake/utils/serial_actuator/CMakeLists.txt rename to firmware/brake/utils/serial_actuator/CMakeLists.txt index 20968f9c..ea132815 100644 --- a/platforms/kia_soul/firmware/brake/utils/serial_actuator/CMakeLists.txt +++ b/firmware/brake/utils/serial_actuator/CMakeLists.txt @@ -1,15 +1,14 @@ -project(kia-soul-brake-utils-serial-actuator) +project(brake-utils-serial-actuator) generate_arduino_firmware( - kia-soul-brake-utils-serial-actuator + brake-utils-serial-actuator SRCS ${CMAKE_SOURCE_DIR}/common/libs/arduino_init/arduino_init.cpp ${CMAKE_SOURCE_DIR}/common/libs/mcp_can/mcp_can.cpp ${CMAKE_SOURCE_DIR}/common/libs/pid/oscc_pid.cpp ${CMAKE_SOURCE_DIR}/common/libs/serial/oscc_serial.cpp ${CMAKE_SOURCE_DIR}/common/libs/can/oscc_can.cpp - ${CMAKE_SOURCE_DIR}/common/libs/time/oscc_time.cpp - ${CMAKE_SOURCE_DIR}/common/libs/signal_smoothing/oscc_signal_smoothing.cpp + ${CMAKE_SOURCE_DIR}/common/libs/timer/oscc_timer.cpp ../../src/globals.cpp ../../src/accumulator.cpp ../../src/helper.cpp @@ -20,7 +19,7 @@ generate_arduino_firmware( src/serial_actuator.cpp) target_include_directories( - kia-soul-brake-utils-serial-actuator + brake-utils-serial-actuator PRIVATE ../../include ${CMAKE_SOURCE_DIR}/common/include @@ -29,5 +28,5 @@ target_include_directories( ${CMAKE_SOURCE_DIR}/common/libs/pid ${CMAKE_SOURCE_DIR}/common/libs/serial ${CMAKE_SOURCE_DIR}/common/libs/can - ${CMAKE_SOURCE_DIR}/common/libs/time - ${CMAKE_SOURCE_DIR}/common/libs/signal_smoothing) + ${CMAKE_SOURCE_DIR}/common/libs/timer + ${CMAKE_SOURCE_DIR}/../api/include) diff --git a/platforms/kia_soul/firmware/brake/utils/serial_actuator/src/serial_actuator.cpp b/firmware/brake/utils/serial_actuator/src/serial_actuator.cpp similarity index 97% rename from platforms/kia_soul/firmware/brake/utils/serial_actuator/src/serial_actuator.cpp rename to firmware/brake/utils/serial_actuator/src/serial_actuator.cpp index 8e05762a..c7f54db1 100644 --- a/platforms/kia_soul/firmware/brake/utils/serial_actuator/src/serial_actuator.cpp +++ b/firmware/brake/utils/serial_actuator/src/serial_actuator.cpp @@ -1,12 +1,10 @@ #include #include "arduino_init.h" #include "mcp_can.h" -#include "chassis_state_can_protocol.h" -#include "brake_can_protocol.h" +#include "can_protocols/brake_can_protocol.h" #include "oscc_pid.h" #include "oscc_serial.h" #include "oscc_can.h" -#include "oscc_time.h" #include "debug.h" #include "globals.h" @@ -240,7 +238,7 @@ int main( void ) { check_for_incoming_message( ); - publish_reports( ); + publish_brake_report( ); process_serial( ); diff --git a/platforms/kia_soul/firmware/can_gateway/CMakeLists.txt b/firmware/can_gateway/CMakeLists.txt similarity index 80% rename from platforms/kia_soul/firmware/can_gateway/CMakeLists.txt rename to firmware/can_gateway/CMakeLists.txt index bcea5d0b..5cd24efe 100644 --- a/platforms/kia_soul/firmware/can_gateway/CMakeLists.txt +++ b/firmware/can_gateway/CMakeLists.txt @@ -1,4 +1,4 @@ -project(kia-soul-can-gateway) +project(can-gateway) set(ARDUINO_DEFAULT_BOARD uno) SET(ARDUINO_DEFAULT_PORT ${SERIAL_PORT_CAN_GATEWAY}) @@ -7,28 +7,27 @@ set(ARDUINO_DEFAULT_BAUDRATE ${SERIAL_BAUD_CAN_GATEWAY}) add_definitions(-DSERIAL_BAUD=${ARDUINO_DEFAULT_BAUDRATE}) add_custom_target( - kia-soul-can-gateway-monitor + can-gateway-monitor COMMAND screen ${ARDUINO_DEFAULT_PORT} ${ARDUINO_DEFAULT_BAUDRATE}) add_custom_target( - kia-soul-can-gateway-monitor-log + can-gateway-monitor-log COMMAND screen -L ${ARDUINO_DEFAULT_PORT} ${ARDUINO_DEFAULT_BAUDRATE}) generate_arduino_firmware( - kia-soul-can-gateway + can-gateway SRCS ${CMAKE_SOURCE_DIR}/common/libs/arduino_init/arduino_init.cpp ${CMAKE_SOURCE_DIR}/common/libs/mcp_can/mcp_can.cpp ${CMAKE_SOURCE_DIR}/common/libs/serial/oscc_serial.cpp ${CMAKE_SOURCE_DIR}/common/libs/can/oscc_can.cpp - ${CMAKE_SOURCE_DIR}/common/libs/time/oscc_time.cpp src/main.cpp src/globals.cpp src/init.cpp src/communications.cpp) target_include_directories( - kia-soul-can-gateway + can-gateway PRIVATE include ${CMAKE_SOURCE_DIR}/common/include @@ -36,4 +35,4 @@ target_include_directories( ${CMAKE_SOURCE_DIR}/common/libs/mcp_can ${CMAKE_SOURCE_DIR}/common/libs/serial ${CMAKE_SOURCE_DIR}/common/libs/can - ${CMAKE_SOURCE_DIR}/common/libs/time) + ${CMAKE_SOURCE_DIR}/../api/include) diff --git a/firmware/can_gateway/include/communications.h b/firmware/can_gateway/include/communications.h new file mode 100644 index 00000000..af8a4758 --- /dev/null +++ b/firmware/can_gateway/include/communications.h @@ -0,0 +1,29 @@ +/** + * @file communications.h + * @brief Communication functionality. + * + */ + + +#ifndef _OSCC_KIA_SOUL_CAN_GATEWAY_COMMUNICATIONS_H_ +#define _OSCC_KIA_SOUL_CAN_GATEWAY_COMMUNICATIONS_H_ + + +#include "globals.h" + + +// **************************************************************************** +// Function: republish_obd_frames_to_control_can_bus +// +// Purpose: Republish pertinent frames on the OBD CAN bus to the Control CAN +// bus. +// +// Returns: void +// +// Parameters: void +// +// **************************************************************************** +void republish_obd_frames_to_control_can_bus( void ); + + +#endif /* _OSCC_KIA_SOUL_CAN_GATEWAY_COMMUNICATIONS_H_ */ diff --git a/platforms/kia_soul/firmware/can_gateway/include/globals.h b/firmware/can_gateway/include/globals.h similarity index 60% rename from platforms/kia_soul/firmware/can_gateway/include/globals.h rename to firmware/can_gateway/include/globals.h index eb51f86a..0de512d5 100644 --- a/platforms/kia_soul/firmware/can_gateway/include/globals.h +++ b/firmware/can_gateway/include/globals.h @@ -10,8 +10,6 @@ #include "mcp_can.h" -#include "gateway_can_protocol.h" -#include "chassis_state_can_protocol.h" /* @@ -40,14 +38,4 @@ #endif -EXTERN oscc_report_heartbeat_s g_tx_heartbeat; -EXTERN oscc_report_chassis_state_1_s g_tx_chassis_state_1; -EXTERN oscc_report_chassis_state_2_s g_tx_chassis_state_2; - -EXTERN uint32_t g_obd_steering_wheel_angle_rx_timestamp; -EXTERN uint32_t g_obd_wheel_speed_rx_timestamp; -EXTERN uint32_t g_obd_brake_pressure_rx_timestamp; -EXTERN uint32_t g_obd_turn_signal_rx_timestamp; - - #endif /* _OSCC_KIA_SOUL_CAN_GATEWAY_GLOBALS_H_ */ diff --git a/platforms/kia_soul/firmware/can_gateway/include/init.h b/firmware/can_gateway/include/init.h similarity index 64% rename from platforms/kia_soul/firmware/can_gateway/include/init.h rename to firmware/can_gateway/include/init.h index 9455a41a..82686bcd 100644 --- a/platforms/kia_soul/firmware/can_gateway/include/init.h +++ b/firmware/can_gateway/include/init.h @@ -9,19 +9,6 @@ #define _OSCC_KIA_SOUL_CAN_GATEWAY_INIT_H_ -// **************************************************************************** -// Function: init_globals -// -// Purpose: Initialize values of global variables. -// -// Returns: void -// -// Parameters: void -// -// **************************************************************************** -void init_globals( void ); - - // **************************************************************************** // Function: init_communication_interfaces // diff --git a/firmware/can_gateway/src/communications.cpp b/firmware/can_gateway/src/communications.cpp new file mode 100644 index 00000000..d1938028 --- /dev/null +++ b/firmware/can_gateway/src/communications.cpp @@ -0,0 +1,33 @@ +/** + * @file communications.cpp + * + */ + + +#include "mcp_can.h" +#include "oscc_can.h" +#include "vehicles.h" + +#include "globals.h" +#include "communications.h" + + +void republish_obd_frames_to_control_can_bus( void ) +{ + can_frame_s rx_frame; + can_status_t ret = check_for_rx_frame( g_obd_can, &rx_frame ); + + if( ret == CAN_RX_FRAME_AVAILABLE ) + { + if( (rx_frame.id == KIA_SOUL_OBD_STEERING_WHEEL_ANGLE_CAN_ID) + || (rx_frame.id == KIA_SOUL_OBD_WHEEL_SPEED_CAN_ID) + || (rx_frame.id == KIA_SOUL_OBD_BRAKE_PRESSURE_CAN_ID) ) + { + g_control_can.sendMsgBuf( + rx_frame.id, + CAN_STANDARD, + sizeof(rx_frame), + (uint8_t *) &rx_frame.data ); + } + } +} diff --git a/platforms/kia_soul/firmware/can_gateway/src/globals.cpp b/firmware/can_gateway/src/globals.cpp similarity index 100% rename from platforms/kia_soul/firmware/can_gateway/src/globals.cpp rename to firmware/can_gateway/src/globals.cpp diff --git a/firmware/can_gateway/src/init.cpp b/firmware/can_gateway/src/init.cpp new file mode 100644 index 00000000..d7374b5d --- /dev/null +++ b/firmware/can_gateway/src/init.cpp @@ -0,0 +1,26 @@ +/** + * @file init.cpp + * + */ + + +#include "oscc_serial.h" +#include "oscc_can.h" +#include "debug.h" + +#include "globals.h" +#include "init.h" + + +void init_communication_interfaces( void ) +{ + #ifdef DEBUG + init_serial(); + #endif + + DEBUG_PRINT( "init OBD CAN - "); + init_can( g_obd_can ); + + DEBUG_PRINT( "init Control CAN - "); + init_can( g_control_can ); +} diff --git a/platforms/kia_soul/firmware/can_gateway/src/main.cpp b/firmware/can_gateway/src/main.cpp similarity index 51% rename from platforms/kia_soul/firmware/can_gateway/src/main.cpp rename to firmware/can_gateway/src/main.cpp index f2d5a8f2..c45f85bc 100644 --- a/platforms/kia_soul/firmware/can_gateway/src/main.cpp +++ b/firmware/can_gateway/src/main.cpp @@ -5,38 +5,27 @@ #include - #include "arduino_init.h" #include "debug.h" -#include "gateway_can_protocol.h" -#include "globals.h" -#include "communications.h" #include "init.h" +#include "communications.h" int main( void ) { init_arduino( ); - init_globals( ); - init_communication_interfaces( ); - SET_HEARTBEAT_STATE( OSCC_REPORT_HEARTBEAT_STATE_OK ); - wdt_enable( WDTO_120MS ); - DEBUG_PRINTLN( "initialization complete" ); + DEBUG_PRINTLN( "init complete" ); while( true ) { wdt_reset(); - check_for_incoming_message( ); - - check_for_obd_timeout( ); - - publish_reports( ); + republish_obd_frames_to_control_can_bus( ); } } diff --git a/platforms/kia_soul/firmware/can_gateway/tests/CMakeLists.txt b/firmware/can_gateway/tests/CMakeLists.txt similarity index 60% rename from platforms/kia_soul/firmware/can_gateway/tests/CMakeLists.txt rename to firmware/can_gateway/tests/CMakeLists.txt index 4baf521e..1fc99d69 100644 --- a/platforms/kia_soul/firmware/can_gateway/tests/CMakeLists.txt +++ b/firmware/can_gateway/tests/CMakeLists.txt @@ -1,48 +1,48 @@ -project(kia-soul-can-gateway-unit-tests) +project(can-gateway-unit-tests) add_library( - kia-soul-can-gateway + can-gateway SHARED ../src/communications.cpp ../src/globals.cpp - ${CMAKE_SOURCE_DIR}/common/libs/time/oscc_time.cpp ${CMAKE_SOURCE_DIR}/common/libs/can/oscc_can.cpp ${CMAKE_SOURCE_DIR}/common/testing/mocks/Arduino_mock.cpp ${CMAKE_SOURCE_DIR}/common/testing/mocks/mcp_can_mock.cpp) target_include_directories( - kia-soul-can-gateway + can-gateway PRIVATE ../include ${CMAKE_SOURCE_DIR}/common/include ${CMAKE_SOURCE_DIR}/common/libs/can - ${CMAKE_SOURCE_DIR}/common/libs/time - ${CMAKE_SOURCE_DIR}/common/testing/mocks) + ${CMAKE_SOURCE_DIR}/common/testing/mocks + ${CMAKE_SOURCE_DIR}/../api/include) add_executable( - kia-soul-can-gateway-unit-test + can-gateway-unit-test features/step_definitions/test.cpp) target_link_libraries( - kia-soul-can-gateway-unit-test + can-gateway-unit-test PRIVATE - kia-soul-can-gateway + can-gateway ${CMAKE_SOURCE_DIR}/common/testing/framework/cucumber-cpp/lib/libcucumber-cpp.a ${CMAKE_SOURCE_DIR}/common/testing/framework/cgreen/lib/libcgreen.so) target_include_directories( - kia-soul-can-gateway-unit-test + can-gateway-unit-test PRIVATE ../include ${CMAKE_SOURCE_DIR}/common/include ${CMAKE_SOURCE_DIR}/common/libs/can ${CMAKE_SOURCE_DIR}/common/testing/mocks ${CMAKE_SOURCE_DIR}/common/testing/framework/cucumber-cpp/include - ${CMAKE_SOURCE_DIR}/common/testing/framework/cgreen/include) + ${CMAKE_SOURCE_DIR}/common/testing/framework/cgreen/include + ${CMAKE_SOURCE_DIR}/../api/include) add_custom_target( - run-kia-soul-can-gateway-unit-tests + run-can-gateway-unit-tests DEPENDS - kia-soul-can-gateway-unit-test + can-gateway-unit-test COMMAND - kia-soul-can-gateway-unit-test --port=3902 >/dev/null & cucumber _2.0.0_ ${CMAKE_CURRENT_SOURCE_DIR}/features ) + can-gateway-unit-test --port=3902 >/dev/null & cucumber _2.0.0_ ${CMAKE_CURRENT_SOURCE_DIR}/features ) diff --git a/firmware/can_gateway/tests/features/republishing.feature b/firmware/can_gateway/tests/features/republishing.feature new file mode 100644 index 00000000..dbfec314 --- /dev/null +++ b/firmware/can_gateway/tests/features/republishing.feature @@ -0,0 +1,11 @@ +# language: en + +Feature: Republishing OBD CAN frames + + All OBD CAN frames should be republished to the Control CAN bus. + + + Scenario: OBD CAN frame received. + When an OBD CAN frame is received on the OBD CAN bus + + Then an OBD CAN frame should be published to the Control CAN bus diff --git a/firmware/can_gateway/tests/features/step_definitions/common.cpp b/firmware/can_gateway/tests/features/step_definitions/common.cpp new file mode 100644 index 00000000..27860334 --- /dev/null +++ b/firmware/can_gateway/tests/features/step_definitions/common.cpp @@ -0,0 +1,26 @@ +#include +#include +#include +#include +#include + +#include "Arduino.h" +#include "communications.h" +#include "oscc_can.h" +#include "mcp_can.h" +#include "globals.h" +#include "vehicles.h" + +using namespace cgreen; + +extern uint8_t g_mock_mcp_can_check_receive_return; +extern uint32_t g_mock_mcp_can_read_msg_buf_id; +extern uint32_t g_mock_mcp_can_send_msg_buf_id; + +// return to known state before every scenario +BEFORE() +{ + g_mock_mcp_can_check_receive_return = UINT8_MAX; + g_mock_mcp_can_read_msg_buf_id = UINT32_MAX; + g_mock_mcp_can_send_msg_buf_id = UINT32_MAX; +} diff --git a/platforms/kia_soul/firmware/can_gateway/tests/features/step_definitions/cucumber.wire b/firmware/can_gateway/tests/features/step_definitions/cucumber.wire similarity index 100% rename from platforms/kia_soul/firmware/can_gateway/tests/features/step_definitions/cucumber.wire rename to firmware/can_gateway/tests/features/step_definitions/cucumber.wire diff --git a/firmware/can_gateway/tests/features/step_definitions/republishing.cpp b/firmware/can_gateway/tests/features/step_definitions/republishing.cpp new file mode 100644 index 00000000..941dfd5f --- /dev/null +++ b/firmware/can_gateway/tests/features/step_definitions/republishing.cpp @@ -0,0 +1,15 @@ +WHEN("^an OBD CAN frame is received on the OBD CAN bus$") +{ + g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL; + g_mock_mcp_can_read_msg_buf_id = KIA_SOUL_OBD_STEERING_WHEEL_ANGLE_CAN_ID; + + republish_obd_frames_to_control_can_bus(); +} + + +THEN("^an OBD CAN frame should be published to the Control CAN bus$") +{ + assert_that( + g_mock_mcp_can_send_msg_buf_id, + is_equal_to(KIA_SOUL_OBD_STEERING_WHEEL_ANGLE_CAN_ID)); +} diff --git a/platforms/kia_soul/firmware/can_gateway/tests/features/step_definitions/test.cpp b/firmware/can_gateway/tests/features/step_definitions/test.cpp similarity index 52% rename from platforms/kia_soul/firmware/can_gateway/tests/features/step_definitions/test.cpp rename to firmware/can_gateway/tests/features/step_definitions/test.cpp index 55e8f155..822f600f 100644 --- a/platforms/kia_soul/firmware/can_gateway/tests/features/step_definitions/test.cpp +++ b/firmware/can_gateway/tests/features/step_definitions/test.cpp @@ -1,5 +1,3 @@ // include source files to prevent step files from conflicting with each other #include "common.cpp" -#include "receiving_reports.cpp" - #include "sending_reports.cpp" - #include "timeouts.cpp" +#include "republishing.cpp" diff --git a/platforms/kia_soul/firmware/can_gateway/tests/features/support/env.rb b/firmware/can_gateway/tests/features/support/env.rb similarity index 100% rename from platforms/kia_soul/firmware/can_gateway/tests/features/support/env.rb rename to firmware/can_gateway/tests/features/support/env.rb diff --git a/platforms/common/include/debug.h b/firmware/common/include/debug.h similarity index 100% rename from platforms/common/include/debug.h rename to firmware/common/include/debug.h diff --git a/platforms/common/libs/DAC_MCP49xx/DAC_MCP49xx.cpp b/firmware/common/libs/DAC_MCP49xx/DAC_MCP49xx.cpp similarity index 100% rename from platforms/common/libs/DAC_MCP49xx/DAC_MCP49xx.cpp rename to firmware/common/libs/DAC_MCP49xx/DAC_MCP49xx.cpp diff --git a/platforms/common/libs/DAC_MCP49xx/DAC_MCP49xx.h b/firmware/common/libs/DAC_MCP49xx/DAC_MCP49xx.h similarity index 100% rename from platforms/common/libs/DAC_MCP49xx/DAC_MCP49xx.h rename to firmware/common/libs/DAC_MCP49xx/DAC_MCP49xx.h diff --git a/platforms/common/libs/DAC_MCP49xx/README.txt b/firmware/common/libs/DAC_MCP49xx/README.txt similarity index 100% rename from platforms/common/libs/DAC_MCP49xx/README.txt rename to firmware/common/libs/DAC_MCP49xx/README.txt diff --git a/platforms/common/libs/DAC_MCP49xx/examples/MCP49x1_single_demo/MCP49x1_single_demo.ino b/firmware/common/libs/DAC_MCP49xx/examples/MCP49x1_single_demo/MCP49x1_single_demo.ino similarity index 100% rename from platforms/common/libs/DAC_MCP49xx/examples/MCP49x1_single_demo/MCP49x1_single_demo.ino rename to firmware/common/libs/DAC_MCP49xx/examples/MCP49x1_single_demo/MCP49x1_single_demo.ino diff --git a/platforms/common/libs/DAC_MCP49xx/examples/MCP49x2_dual_demo/MCP49x2_dual_demo.ino b/firmware/common/libs/DAC_MCP49xx/examples/MCP49x2_dual_demo/MCP49x2_dual_demo.ino similarity index 100% rename from platforms/common/libs/DAC_MCP49xx/examples/MCP49x2_dual_demo/MCP49x2_dual_demo.ino rename to firmware/common/libs/DAC_MCP49xx/examples/MCP49x2_dual_demo/MCP49x2_dual_demo.ino diff --git a/platforms/common/libs/DAC_MCP49xx/keywords.txt b/firmware/common/libs/DAC_MCP49xx/keywords.txt similarity index 100% rename from platforms/common/libs/DAC_MCP49xx/keywords.txt rename to firmware/common/libs/DAC_MCP49xx/keywords.txt diff --git a/platforms/common/libs/arduino_init/arduino_init.cpp b/firmware/common/libs/arduino_init/arduino_init.cpp similarity index 100% rename from platforms/common/libs/arduino_init/arduino_init.cpp rename to firmware/common/libs/arduino_init/arduino_init.cpp diff --git a/platforms/common/libs/arduino_init/arduino_init.h b/firmware/common/libs/arduino_init/arduino_init.h similarity index 100% rename from platforms/common/libs/arduino_init/arduino_init.h rename to firmware/common/libs/arduino_init/arduino_init.h diff --git a/platforms/common/libs/can/oscc_can.cpp b/firmware/common/libs/can/oscc_can.cpp similarity index 77% rename from platforms/common/libs/can/oscc_can.cpp rename to firmware/common/libs/can/oscc_can.cpp index fd8d3192..f3986870 100644 --- a/platforms/common/libs/can/oscc_can.cpp +++ b/firmware/common/libs/can/oscc_can.cpp @@ -2,7 +2,6 @@ #include "mcp_can.h" #include "debug.h" -#include "oscc_time.h" #include "oscc_can.h" @@ -25,16 +24,18 @@ can_status_t check_for_rx_frame( MCP_CAN &can, can_frame_s * const frame ) if( frame != NULL ) { + cli(); + if( can.checkReceive( ) == CAN_MSGAVAIL ) { memset( frame, 0, sizeof(*frame) ); - frame->timestamp = GET_TIMESTAMP_MS( ); + frame->timestamp = millis( ); can.readMsgBufID( - ( INT32U* ) &frame->id, - ( INT8U* ) &frame->dlc, - ( INT8U* ) frame->data ); + ( uint32_t* ) &frame->id, + ( uint8_t* ) &frame->dlc, + ( uint8_t* ) frame->data ); ret = CAN_RX_FRAME_AVAILABLE; } @@ -42,6 +43,8 @@ can_status_t check_for_rx_frame( MCP_CAN &can, can_frame_s * const frame ) { ret = CAN_RX_FRAME_UNAVAILABLE; } + + sei(); } return ret; diff --git a/platforms/common/libs/can/oscc_can.h b/firmware/common/libs/can/oscc_can.h similarity index 100% rename from platforms/common/libs/can/oscc_can.h rename to firmware/common/libs/can/oscc_can.h diff --git a/platforms/common/libs/dac/oscc_dac.cpp b/firmware/common/libs/dac/oscc_dac.cpp similarity index 100% rename from platforms/common/libs/dac/oscc_dac.cpp rename to firmware/common/libs/dac/oscc_dac.cpp diff --git a/platforms/common/libs/dac/oscc_dac.h b/firmware/common/libs/dac/oscc_dac.h similarity index 100% rename from platforms/common/libs/dac/oscc_dac.h rename to firmware/common/libs/dac/oscc_dac.h diff --git a/platforms/common/libs/mcp_can/License.txt b/firmware/common/libs/mcp_can/License.txt similarity index 100% rename from platforms/common/libs/mcp_can/License.txt rename to firmware/common/libs/mcp_can/License.txt diff --git a/firmware/common/libs/mcp_can/MCP2515Calc.xlsx b/firmware/common/libs/mcp_can/MCP2515Calc.xlsx new file mode 100644 index 00000000..66d1a302 Binary files /dev/null and b/firmware/common/libs/mcp_can/MCP2515Calc.xlsx differ diff --git a/platforms/common/libs/mcp_can/README.md b/firmware/common/libs/mcp_can/README.md similarity index 76% rename from platforms/common/libs/mcp_can/README.md rename to firmware/common/libs/mcp_can/README.md index bde92ef8..a67d22e2 100644 --- a/platforms/common/libs/mcp_can/README.md +++ b/firmware/common/libs/mcp_can/README.md @@ -1,11 +1,11 @@ CAN BUS Shield --------------------------------------------------------- -[![CAN BUS Shield](http://www.seeedstudio.com/depot/images/1130300211.jpg)](http://www.seeedstudio.com/depot/CANBUS-Shield-p-2256.html?cPath=19_88) +[![CAN BUS Shield](https://github.com/SeeedDocument/CAN_BUS_Shield/blob/master/image/Can_bus_shield_all.jpg?raw=true)](http://www.seeedstudio.com/depot/CANBUS-Shield-p-2256.html?cPath=19_88)
-CAN-BUS is a common industrial bus because of its long travel distance, medium communication speed and high reliability. It is commonly found on modern machine tools and as an automotive diagnostic bus. This CAN-BUS Shield adopts MCP2515 CAN Bus controller with SPI interface and MCP2551 CAN transceiver to give your Arduino/Seeeduino CAN-BUS capibility. With an OBD-II converter cable added on and the OBD-II library imported, you are ready to build an onboard diagnostic device or data logger. +CAN-BUS is a common industrial bus because of its long travel distance, medium communication speed and high reliability. It is commonly found on modern machine tools and as an automotive diagnostic bus. This CAN-BUS Shield adopts MCP2515 CAN Bus controller with SPI interface and MCP2551 CAN transceiver to give your Arduino/Seeeduino CAN-BUS capability. With an OBD-II converter cable added on and the OBD-II library imported, you are ready to build an onboard diagnostic device or data logger. - Implements CAN V2.0B at up to 1 Mb/s - SPI Interface up to 10 MHz @@ -16,33 +16,49 @@ CAN-BUS is a common industrial bus because of its long travel distance, medium c +
+# Installation: + + git clone https://github.com/Seeed-Studio/CAN_BUS_Shield.git + +or download the zip. +
# Usage: +Simply copy the CAN_BUS_Shield folder to your Arduino library collection. For example, +arduino-1.6.12/libraries. Next time you run the Arduino IDE, you'll have a new option +in Sketch -> Include Library -> CAN_BUS_Shield. Review the included examples in +CAN_BUS_Shield/examples. + + + ## 1. Set the BaudRate This function is used to initialize the baudrate of the CAN Bus system. -The available baudrates are listed as follws: +The available baudrates are listed as follows: #define CAN_5KBPS 1 #define CAN_10KBPS 2 #define CAN_20KBPS 3 - #define CAN_31K25BPS 4 - #define CAN_33KBPS 5 - #define CAN_40KBPS 6 - #define CAN_50KBPS 7 - #define CAN_80KBPS 8 - #define CAN_83K3BPS 9 - #define CAN_95KBPS 10 - #define CAN_100KBPS 11 - #define CAN_125KBPS 12 - #define CAN_200KBPS 13 - #define CAN_250KBPS 14 - #define CAN_500KBPS 15 - #define CAN_1000KBPS 16 + #define CAN_25KBPS 4 + #define CAN_31K25BPS 5 + #define CAN_33KBPS 6 + #define CAN_40KBPS 7 + #define CAN_50KBPS 8 + #define CAN_80KBPS 9 + #define CAN_83K3BPS 10 + #define CAN_95KBPS 11 + #define CAN_100KBPS 12 + #define CAN_125KBPS 13 + #define CAN_200KBPS 14 + #define CAN_250KBPS 15 + #define CAN_500KBPS 16 + #define CAN_666kbps 17 + #define CAN_1000KBPS 18
@@ -77,7 +93,7 @@ The function will return 1 if a frame arrives, and 0 if nothing arrives.
## 4. Get CAN ID -When some data arrive, you can use the following function to get the CAN ID of the "send" node. +When some data arrives, you can use the following function to get the CAN ID of the "send" node. INT32U MCP_CAN::getCanId(void); @@ -86,7 +102,7 @@ When some data arrive, you can use the following function to get the CAN ID of t
## 5. Send Data - CAN.sendMsgBuf(INT8U id, INT8U ext, INT8U len, data_buf); + CAN.sendMsgBuf(INT32U id, INT8U ext, INT8U len, INT8U *buf); This is a function to send data onto the bus. In which: @@ -96,7 +112,7 @@ This is a function to send data onto the bus. In which: **len** represents the length of this frame. -**data_buf** is the content of this message. +**buf** is the content of this message. For example, In the 'send' example, we have: @@ -113,9 +129,9 @@ CAN.sendMsgBuf(0x00, 0, 8, stmp); //send out the message 'stmp' to the bus and t The following function is used to receive data on the 'receive' node: - CAN.readMsgBuf(unsigned char len, unsigned char buf); + CAN.readMsgBuf(INT8U *len, INT8U *buf); -In conditions that masks and filters have been set. This function can only get frames that meet the requirements of masks and filters. +Under the condition that masks and filters have been set, this function will only get frames that meet the requirements of those masks and filters. **len** represents the data length. diff --git a/platforms/common/libs/mcp_can/keywords.txt b/firmware/common/libs/mcp_can/keywords.txt similarity index 87% rename from platforms/common/libs/mcp_can/keywords.txt rename to firmware/common/libs/mcp_can/keywords.txt index e7128e8f..30b0f029 100644 --- a/platforms/common/libs/mcp_can/keywords.txt +++ b/firmware/common/libs/mcp_can/keywords.txt @@ -8,6 +8,7 @@ MCP_CAN KEYWORD1 mcp_can_dfs KEYWORD1 mcp_can KEYWORD1 +CAN KEYWORD1 ####################################### # Methods and Functions (KEYWORD2) @@ -27,14 +28,20 @@ getCanId KEYWORD2 CAN_5KBPS LITERAL1 CAN_10KBPS LITERAL1 CAN_20KBPS LITERAL1 +CAN_25KBPS LITERAL1 +CAN_31KBPS LITERAL1 +CAN_33KBPS LITERAL1 CAN_40KBPS LITERAL1 CAN_50KBPS LITERAL1 CAN_80KBPS LITERAL1 +CAN_83KBPS LITERAL1 +CAN_95KBPS LITERAL1 CAN_100KBPS LITERAL1 CAN_125KBPS LITERAL1 CAN_200KBPS LITERAL1 CAN_250KBPS LITERAL1 CAN_500KBPS LITERAL1 +CAN_666KBPS LITERAL1 CAN_1000KBPS LITERAL1 CAN_OK LITERAL1 CAN_FAILINIT LITERAL1 @@ -45,3 +52,4 @@ CAN_CTRLERROR LITERAL1 CAN_GETTXBFTIMEOUT LITERAL1 CAN_SENDMSGTIMEOUT LITERAL1 CAN_FAIL LITERAL1 +SPI_CS_PIN LITERAL1 \ No newline at end of file diff --git a/firmware/common/libs/mcp_can/library.json b/firmware/common/libs/mcp_can/library.json new file mode 100644 index 00000000..65381ccc --- /dev/null +++ b/firmware/common/libs/mcp_can/library.json @@ -0,0 +1,13 @@ +{ + "name": "CAN_BUS_Shield", + "keywords": "can, bus, mcp2515, MCP-2515", + "description": "Library provides CAN communication on the CAN-Bus Shield.", + "repository": + { + "type": "git", + "url": "https://github.com/Seeed-Studio/CAN_BUS_Shield.git" + }, + "version": "1.20", + "frameworks": "arduino", + "platforms": "atmelavr" +} diff --git a/platforms/common/libs/mcp_can/mcp_can.cpp b/firmware/common/libs/mcp_can/mcp_can.cpp similarity index 61% rename from platforms/common/libs/mcp_can/mcp_can.cpp rename to firmware/common/libs/mcp_can/mcp_can.cpp index bd104d2f..914d72f8 100644 --- a/platforms/common/libs/mcp_can/mcp_can.cpp +++ b/firmware/common/libs/mcp_can/mcp_can.cpp @@ -2,29 +2,55 @@ mcp_can.cpp 2012 Copyright (c) Seeed Technology Inc. All right reserved. - Author:Loovee - Contributor: Cory J. Fowler + Author:Loovee (loovee@seeed.cc) 2014-1-16 - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. - - This library is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with this library; if not, write to the Free Software - Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110- - 1301 USA + + Contributor: + + Cory J. Fowler + Latonita + Woodward1 + Mehtajaghvi + BykeBlast + TheRo0T + Tsipizic + ralfEdmund + Nathancheek + BlueAndi + Adlerweb + Btetz + Hurvajs + xboxpro1 + + The MIT License (MIT) + + Copyright (c) 2013 Seeed Technology Inc. + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ -#include #include "mcp_can.h" +#include -#define spi_readwrite SPI.transfer -#define spi_read() spi_readwrite(0x00) +#define spi_readwrite SPI.transfer +#define spi_read() spi_readwrite(0x00) +#define SPI_BEGIN() SPI.beginTransaction(SPISettings(10000000, MSBFIRST, SPI_MODE0)) +#define SPI_END() SPI.endTransaction() /********************************************************************************************************* ** Function name: mcp2515_reset @@ -32,9 +58,15 @@ *********************************************************************************************************/ void MCP_CAN::mcp2515_reset(void) { +#ifdef SPI_HAS_TRANSACTION + SPI_BEGIN(); +#endif MCP2515_SELECT(); spi_readwrite(MCP_RESET); MCP2515_UNSELECT(); +#ifdef SPI_HAS_TRANSACTION + SPI_END(); +#endif delay(10); } @@ -42,15 +74,21 @@ void MCP_CAN::mcp2515_reset(void) ** Function name: mcp2515_readRegister ** Descriptions: read register *********************************************************************************************************/ -INT8U MCP_CAN::mcp2515_readRegister(const INT8U address) +byte MCP_CAN::mcp2515_readRegister(const byte address) { - INT8U ret; + byte ret; +#ifdef SPI_HAS_TRANSACTION + SPI_BEGIN(); +#endif MCP2515_SELECT(); spi_readwrite(MCP_READ); spi_readwrite(address); ret = spi_read(); MCP2515_UNSELECT(); +#ifdef SPI_HAS_TRANSACTION + SPI_END(); +#endif return ret; } @@ -59,110 +97,140 @@ INT8U MCP_CAN::mcp2515_readRegister(const INT8U address) ** Function name: mcp2515_readRegisterS ** Descriptions: read registerS *********************************************************************************************************/ -void MCP_CAN::mcp2515_readRegisterS(const INT8U address, INT8U values[], const INT8U n) +void MCP_CAN::mcp2515_readRegisterS(const byte address, byte values[], const byte n) { - INT8U i; - MCP2515_SELECT(); - spi_readwrite(MCP_READ); - spi_readwrite(address); - // mcp2515 has auto-increment of address-pointer - for (i=0; i TXBnD7 */ + byte i, a1, a2, a3; + a1 = MCP_TXB0CTRL; a2 = MCP_TXB1CTRL; a3 = MCP_TXB2CTRL; - for (i = 0; i < 14; i++) { /* in-buffer loop */ + for(i = 0; i < 14; i++) // in-buffer loop + { mcp2515_setRegister(a1, 0); mcp2515_setRegister(a2, 0); mcp2515_setRegister(a3, 0); @@ -322,91 +384,85 @@ void MCP_CAN::mcp2515_initCANBuffers(void) ** Function name: mcp2515_init ** Descriptions: init the device *********************************************************************************************************/ -INT8U MCP_CAN::mcp2515_init(const INT8U canSpeed) /* mcp2515init */ +byte MCP_CAN::mcp2515_init(const byte canSpeed) { - INT8U res; + byte res; mcp2515_reset(); res = mcp2515_setCANCTRL_Mode(MODE_CONFIG); if(res > 0) { -#if DEBUG_MODE - Serial.print("Enter setting mode fall\r\n"); +#if DEBUG_EN + Serial.print("Enter setting mode fall\r\n"); #else - delay(10); + delay(10); #endif - return res; + return res; } -#if DEBUG_MODE +#if DEBUG_EN Serial.print("Enter setting mode success \r\n"); #else delay(10); #endif - /* set boadrate */ + // set boadrate if(mcp2515_configRate(canSpeed)) { -#if DEBUG_MODE - Serial.print("set rate fall!!\r\n"); +#if DEBUG_EN + Serial.print("set rate fall!!\r\n"); #else - delay(10); + delay(10); #endif - return res; + return res; } -#if DEBUG_MODE +#if DEBUG_EN Serial.print("set rate success!!\r\n"); #else delay(10); #endif - if ( res == MCP2515_OK ) { + if(res == MCP2515_OK) { - /* init canbuffers */ + // init canbuffers mcp2515_initCANBuffers(); - /* interrupt mode */ + // interrupt mode mcp2515_setRegister(MCP_CANINTE, MCP_RX0IF | MCP_RX1IF); #if (DEBUG_RXANY==1) - /* enable both receive-buffers */ - /* to receive any message */ - /* and enable rollover */ + // enable both receive-buffers to receive any message and enable rollover mcp2515_modifyRegister(MCP_RXB0CTRL, MCP_RXB_RX_MASK | MCP_RXB_BUKT_MASK, MCP_RXB_RX_ANY | MCP_RXB_BUKT_MASK); mcp2515_modifyRegister(MCP_RXB1CTRL, MCP_RXB_RX_MASK, MCP_RXB_RX_ANY); #else - /* enable both receive-buffers */ - /* to receive messages */ - /* with std. and ext. identifie */ - /* rs */ - /* and enable rollover */ + // enable both receive-buffers to receive messages with std. and ext. identifiers and enable rollover mcp2515_modifyRegister(MCP_RXB0CTRL, MCP_RXB_RX_MASK | MCP_RXB_BUKT_MASK, - MCP_RXB_RX_STDEXT | MCP_RXB_BUKT_MASK ); + MCP_RXB_RX_STDEXT | MCP_RXB_BUKT_MASK); mcp2515_modifyRegister(MCP_RXB1CTRL, MCP_RXB_RX_MASK, MCP_RXB_RX_STDEXT); #endif - /* enter normal mode */ - res = mcp2515_setCANCTRL_Mode(MODE_NORMAL); + // enter normal mode + res = mcp2515_setCANCTRL_Mode(MODE_NORMAL); if(res) { -#if DEBUG_MODE - Serial.print("Enter Normal Mode Fall!!\r\n"); +#if DEBUG_EN + Serial.print("Enter Normal Mode Fall!!\r\n"); #else delay(10); -#endif - return res; +#endif + return res; } -#if DEBUG_MODE - Serial.print("Enter Normal Mode Success!!\r\n"); +#if DEBUG_EN + Serial.print("Enter Normal Mode Success!!\r\n"); #else - delay(10); + delay(10); #endif } @@ -418,51 +474,51 @@ INT8U MCP_CAN::mcp2515_init(const INT8U canSpeed) /* mcp25 ** Function name: mcp2515_write_id ** Descriptions: write can id *********************************************************************************************************/ -void MCP_CAN::mcp2515_write_id( const INT8U mcp_addr, const INT8U ext, const INT32U id ) +void MCP_CAN::mcp2515_write_id(const byte mcp_addr, const byte ext, const unsigned long id) { uint16_t canid; - INT8U tbufdata[4]; + byte tbufdata[4]; canid = (uint16_t)(id & 0x0FFFF); - if ( ext == 1) + if(ext == 1) { - tbufdata[MCP_EID0] = (INT8U) (canid & 0xFF); - tbufdata[MCP_EID8] = (INT8U) (canid >> 8); + tbufdata[MCP_EID0] = (byte) (canid & 0xFF); + tbufdata[MCP_EID8] = (byte) (canid >> 8); canid = (uint16_t)(id >> 16); - tbufdata[MCP_SIDL] = (INT8U) (canid & 0x03); - tbufdata[MCP_SIDL] += (INT8U) ((canid & 0x1C) << 3); + tbufdata[MCP_SIDL] = (byte) (canid & 0x03); + tbufdata[MCP_SIDL] += (byte) ((canid & 0x1C) << 3); tbufdata[MCP_SIDL] |= MCP_TXB_EXIDE_M; - tbufdata[MCP_SIDH] = (INT8U) (canid >> 5 ); + tbufdata[MCP_SIDH] = (byte) (canid >> 5); } - else + else { - tbufdata[MCP_SIDH] = (INT8U) (canid >> 3 ); - tbufdata[MCP_SIDL] = (INT8U) ((canid & 0x07 ) << 5); + tbufdata[MCP_SIDH] = (byte) (canid >> 3); + tbufdata[MCP_SIDL] = (byte) ((canid & 0x07) << 5); tbufdata[MCP_EID0] = 0; tbufdata[MCP_EID8] = 0; } - mcp2515_setRegisterS( mcp_addr, tbufdata, 4 ); + mcp2515_setRegisterS(mcp_addr, tbufdata, 4); } /********************************************************************************************************* ** Function name: mcp2515_read_id ** Descriptions: read can id *********************************************************************************************************/ -void MCP_CAN::mcp2515_read_id( const INT8U mcp_addr, INT8U* ext, INT32U* id ) +void MCP_CAN::mcp2515_read_id(const byte mcp_addr, byte* ext, unsigned long* id) { - INT8U tbufdata[4]; + byte tbufdata[4]; - *ext = 0; - *id = 0; + *ext = 0; + *id = 0; - mcp2515_readRegisterS( mcp_addr, tbufdata, 4 ); + mcp2515_readRegisterS(mcp_addr, tbufdata, 4); *id = (tbufdata[MCP_SIDH]<<3) + (tbufdata[MCP_SIDL]>>5); - if ( (tbufdata[MCP_SIDL] & MCP_TXB_EXIDE_M) == MCP_TXB_EXIDE_M ) + if((tbufdata[MCP_SIDL] & MCP_TXB_EXIDE_M) == MCP_TXB_EXIDE_M) { - /* extended id */ + // extended id *id = (*id<<2) + (tbufdata[MCP_SIDL] & 0x03); *id = (*id<<8) + tbufdata[MCP_EID8]; *id = (*id<<8) + tbufdata[MCP_EID0]; @@ -474,17 +530,19 @@ void MCP_CAN::mcp2515_read_id( const INT8U mcp_addr, INT8U* ext, INT32U* id ) ** Function name: mcp2515_write_canMsg ** Descriptions: write msg *********************************************************************************************************/ -void MCP_CAN::mcp2515_write_canMsg( const INT8U buffer_sidh_addr) +void MCP_CAN::mcp2515_write_canMsg(const byte buffer_sidh_addr, int rtrBit) { - INT8U mcp_addr; + byte mcp_addr; mcp_addr = buffer_sidh_addr; - mcp2515_setRegisterS(mcp_addr+5, m_nDta, m_nDlc ); /* write data bytes */ - if ( m_nRtr == 1) /* if RTR set bit in byte */ + mcp2515_setRegisterS(mcp_addr+5, dta, dta_len); // write data bytes + // Serial.print("RTR: "); + // Serial.println(rtrBit); + if(rtrBit == 1) // if RTR set bit in byte { - m_nDlc |= MCP_RTR_MASK; + dta_len |= MCP_RTR_MASK; } - mcp2515_setRegister((mcp_addr+4), m_nDlc ); /* write the RTR and DLC */ - mcp2515_write_id(mcp_addr, m_nExtFlg, m_nID ); /* write CAN id */ + mcp2515_setRegister((mcp_addr+4), dta_len); // write the RTR and DLC + mcp2515_write_id(mcp_addr, ext_flg, can_id); // write CAN id } @@ -492,57 +550,50 @@ void MCP_CAN::mcp2515_write_canMsg( const INT8U buffer_sidh_addr) ** Function name: mcp2515_read_canMsg ** Descriptions: read message *********************************************************************************************************/ -void MCP_CAN::mcp2515_read_canMsg( const INT8U buffer_sidh_addr) /* read can msg */ +void MCP_CAN::mcp2515_read_canMsg(const byte buffer_sidh_addr) // read can msg { - INT8U mcp_addr, ctrl; + byte mcp_addr, ctrl; mcp_addr = buffer_sidh_addr; + mcp2515_read_id(mcp_addr, &ext_flg,&can_id); + ctrl = mcp2515_readRegister(mcp_addr-1); + dta_len = mcp2515_readRegister(mcp_addr+4); - mcp2515_read_id( mcp_addr, &m_nExtFlg,&m_nID ); - - ctrl = mcp2515_readRegister( mcp_addr-1 ); - m_nDlc = mcp2515_readRegister( mcp_addr+4 ); - - if ((ctrl & 0x08)) { - m_nRtr = 1; - } - else { - m_nRtr = 0; - } + rtr = (ctrl & 0x08) ? 1 : 0; - m_nDlc &= MCP_DLC_MASK; - mcp2515_readRegisterS( mcp_addr+5, &(m_nDta[0]), m_nDlc ); + dta_len &= MCP_DLC_MASK; + mcp2515_readRegisterS(mcp_addr+5, &(dta[0]), dta_len); } /********************************************************************************************************* -** Function name: sendMsg -** Descriptions: send message +** Function name: mcp2515_start_transmit +** Descriptions: start transmit *********************************************************************************************************/ -void MCP_CAN::mcp2515_start_transmit(const INT8U mcp_addr) /* start transmit */ +void MCP_CAN::mcp2515_start_transmit(const byte mcp_addr) // start transmit { - mcp2515_modifyRegister( mcp_addr-1 , MCP_TXB_TXREQ_M, MCP_TXB_TXREQ_M ); + mcp2515_modifyRegister(mcp_addr-1 , MCP_TXB_TXREQ_M, MCP_TXB_TXREQ_M); } /********************************************************************************************************* -** Function name: sendMsg -** Descriptions: send message +** Function name: mcp2515_getNextFreeTXBuf +** Descriptions: get Next free txbuf *********************************************************************************************************/ -INT8U MCP_CAN::mcp2515_getNextFreeTXBuf(INT8U *txbuf_n) /* get Next free txbuf */ +byte MCP_CAN::mcp2515_getNextFreeTXBuf(byte *txbuf_n) // get Next free txbuf { - INT8U res, i, ctrlval; - INT8U ctrlregs[MCP_N_TXBUFFERS] = { MCP_TXB0CTRL, MCP_TXB1CTRL, MCP_TXB2CTRL }; + byte res, i, ctrlval; + byte ctrlregs[MCP_N_TXBUFFERS] = { MCP_TXB0CTRL, MCP_TXB1CTRL, MCP_TXB2CTRL }; res = MCP_ALLTXBUSY; *txbuf_n = 0x00; - /* check all 3 TX-Buffers */ - for (i=0; i 0){ -#if DEBUG_MODE - Serial.print("Enter setting mode fall\r\n"); +#if DEBUG_EN + Serial.print("Enter setting mode fall\r\n"); #else - delay(10); + delay(10); #endif - return res; + return res; } - - if (num == 0){ + + if(num == 0){ mcp2515_write_id(MCP_RXM0SIDH, ext, ulData); } @@ -603,17 +651,17 @@ INT8U MCP_CAN::init_Mask(INT8U num, INT8U ext, INT32U ulData) mcp2515_write_id(MCP_RXM1SIDH, ext, ulData); } else res = MCP2515_FAIL; - + res = mcp2515_setCANCTRL_Mode(MODE_NORMAL); if(res > 0){ -#if DEBUG_MODE - Serial.print("Enter normal mode fall\r\n"); +#if DEBUG_EN + Serial.print("Enter normal mode fall\r\n"); #else - delay(10); + delay(10); #endif - return res; - } -#if DEBUG_MODE + return res; + } +#if DEBUG_EN Serial.print("set Mask success!!\r\n"); #else delay(10); @@ -625,10 +673,10 @@ INT8U MCP_CAN::init_Mask(INT8U num, INT8U ext, INT32U ulData) ** Function name: init_Filt ** Descriptions: init canid filters *********************************************************************************************************/ -INT8U MCP_CAN::init_Filt(INT8U num, INT8U ext, INT32U ulData) +byte MCP_CAN::init_Filt(byte num, byte ext, unsigned long ulData) { - INT8U res = MCP2515_OK; -#if DEBUG_MODE + byte res = MCP2515_OK; +#if DEBUG_EN Serial.print("Begin to set Filter!!\r\n"); #else delay(10); @@ -636,15 +684,15 @@ INT8U MCP_CAN::init_Filt(INT8U num, INT8U ext, INT32U ulData) res = mcp2515_setCANCTRL_Mode(MODE_CONFIG); if(res > 0) { -#if DEBUG_MODE - Serial.print("Enter setting mode fall\r\n"); +#if DEBUG_EN + Serial.print("Enter setting mode fall\r\n"); #else - delay(10); + delay(10); #endif - return res; + return res; } - - switch( num ) + + switch(num) { case 0: mcp2515_write_id(MCP_RXF0SIDH, ext, ulData); @@ -673,23 +721,23 @@ INT8U MCP_CAN::init_Filt(INT8U num, INT8U ext, INT32U ulData) default: res = MCP2515_FAIL; } - + res = mcp2515_setCANCTRL_Mode(MODE_NORMAL); if(res > 0) { -#if DEBUG_MODE - Serial.print("Enter normal mode fall\r\nSet filter fail!!\r\n"); +#if DEBUG_EN + Serial.print("Enter normal mode fall\r\nSet filter fail!!\r\n"); #else - delay(10); + delay(10); #endif - return res; + return res; } -#if DEBUG_MODE +#if DEBUG_EN Serial.print("set Filter success!!\r\n"); #else delay(10); #endif - + return res; } @@ -697,16 +745,15 @@ INT8U MCP_CAN::init_Filt(INT8U num, INT8U ext, INT32U ulData) ** Function name: setMsg ** Descriptions: set can message, such as dlc, id, dta[] and so on *********************************************************************************************************/ -INT8U MCP_CAN::setMsg(INT32U id, INT8U ext, INT8U len, INT8U rtr, INT8U *pData) +byte MCP_CAN::setMsg(unsigned long id, byte ext, byte len, byte rtr, byte *pData) { - int i = 0; - m_nExtFlg = ext; - m_nID = id; - m_nDlc = len; - m_nRtr = rtr; - for(i = 0; i #include -#ifndef INT32U -#define INT32U unsigned long -#endif - -#ifndef INT8U -#define INT8U byte -#endif // if print debug information -#define DEBUG_MODE 0 +#define DEBUG_EN 0 + +// Begin mt -/* - * Begin mt - */ #define TIMEOUTVALUE 50 #define MCP_SIDH 0 #define MCP_SIDL 1 #define MCP_EID8 2 #define MCP_EID0 3 -#define MCP_TXB_EXIDE_M 0x08 /* In TXBnSIDL */ -#define MCP_DLC_MASK 0x0F /* 4 LSBits */ -#define MCP_RTR_MASK 0x40 /* (1<<6) Bit 6 */ +#define MCP_TXB_EXIDE_M 0x08 // In TXBnSIDL +#define MCP_DLC_MASK 0x0F // 4 LSBits +#define MCP_RTR_MASK 0x40 // (1<<6) Bit 6 #define MCP_RXB_RX_ANY 0x60 #define MCP_RXB_RX_EXT 0x40 @@ -59,9 +74,9 @@ #define MCP_RXB_RX_MASK 0x60 #define MCP_RXB_BUKT_MASK (1<<2) -/* -** Bits in the TXBnCTRL registers. -*/ + +// Bits in the TXBnCTRL registers. + #define MCP_TXB_TXBUFE_M 0x80 #define MCP_TXB_ABTF_M 0x40 #define MCP_TXB_MLOA_M 0x20 @@ -70,9 +85,9 @@ #define MCP_TXB_TXIE_M 0x04 #define MCP_TXB_TXP10_M 0x03 -#define MCP_TXB_RTR_M 0x40 /* In TXBnDLC */ -#define MCP_RXB_IDE_M 0x08 /* In RXBnSIDL */ -#define MCP_RXB_RTR_M 0x40 /* In RXBnDLC */ +#define MCP_TXB_RTR_M 0x40 // In TXBnDLC +#define MCP_RXB_IDE_M 0x08 // In RXBnSIDL +#define MCP_RXB_RTR_M 0x40 // In RXBnDLC #define MCP_STAT_RXIF_MASK (0x03) #define MCP_STAT_RX0IF (1<<0) @@ -86,12 +101,9 @@ #define MCP_EFLG_TXWAR (1<<2) #define MCP_EFLG_RXWAR (1<<1) #define MCP_EFLG_EWARN (1<<0) -#define MCP_EFLG_ERRORMASK (0xF8) /* 5 MS-Bits */ +#define MCP_EFLG_ERRORMASK (0xF8) // 5 MS-Bits - -/* - * Define MCP2515 register addresses - */ +// Define MCP2515 register addresses #define MCP_RXF0SIDH 0x00 #define MCP_RXF0SIDL 0x01 @@ -142,48 +154,36 @@ #define MCP_RXB0SIDH 0x61 #define MCP_RXB1CTRL 0x70 #define MCP_RXB1SIDH 0x71 +#define MCP_TX_INT 0x1C // Enable all transmit interrup ts +#define MCP_TX01_INT 0x0C // Enable TXB0 and TXB1 interru pts +#define MCP_RX_INT 0x03 // Enable receive interrupts +#define MCP_NO_INT 0x00 // Disable all interrupts +#define MCP_TX01_MASK 0x14 +#define MCP_TX_MASK 0x54 -#define MCP_TX_INT 0x1C // Enable all transmit interrup ts -#define MCP_TX01_INT 0x0C // Enable TXB0 and TXB1 interru pts -#define MCP_RX_INT 0x03 // Enable receive interrupts -#define MCP_NO_INT 0x00 // Disable all interrupts - -#define MCP_TX01_MASK 0x14 -#define MCP_TX_MASK 0x54 - -/* - * Define SPI Instruction Set - */ - -#define MCP_WRITE 0x02 - -#define MCP_READ 0x03 -#define MCP_BITMOD 0x05 +// Define SPI Instruction Set -#define MCP_LOAD_TX0 0x40 -#define MCP_LOAD_TX1 0x42 -#define MCP_LOAD_TX2 0x44 +#define MCP_WRITE 0x02 +#define MCP_READ 0x03 +#define MCP_BITMOD 0x05 +#define MCP_LOAD_TX0 0x40 +#define MCP_LOAD_TX1 0x42 +#define MCP_LOAD_TX2 0x44 -#define MCP_RTS_TX0 0x81 -#define MCP_RTS_TX1 0x82 -#define MCP_RTS_TX2 0x84 -#define MCP_RTS_ALL 0x87 +#define MCP_RTS_TX0 0x81 +#define MCP_RTS_TX1 0x82 +#define MCP_RTS_TX2 0x84 +#define MCP_RTS_ALL 0x87 +#define MCP_READ_RX0 0x90 +#define MCP_READ_RX1 0x94 +#define MCP_READ_STATUS 0xA0 +#define MCP_RX_STATUS 0xB0 +#define MCP_RESET 0xC0 -#define MCP_READ_RX0 0x90 -#define MCP_READ_RX1 0x94 -#define MCP_READ_STATUS 0xA0 - -#define MCP_RX_STATUS 0xB0 - -#define MCP_RESET 0xC0 - - -/* - * CANCTRL Register Values - */ +// CANCTRL Register Values #define MODE_NORMAL 0x00 #define MODE_SLEEP 0x20 @@ -202,9 +202,7 @@ #define CLKOUT_PS8 0x03 -/* - * CNF1 Register Values - */ +// CNF1 Register Values #define SJW1 0x00 #define SJW2 0x40 @@ -212,18 +210,14 @@ #define SJW4 0xC0 -/* - * CNF2 Register Values - */ +// CNF2 Register Values #define BTLMODE 0x80 #define SAMPLE_1X 0x00 #define SAMPLE_3X 0x40 -/* - * CNF3 Register Values - */ +// CNF3 Register Values #define SOF_ENABLE 0x80 #define SOF_DISABLE 0x00 @@ -231,9 +225,7 @@ #define WAKFIL_DISABLE 0x00 -/* - * CANINTF Register Bits - */ +// CANINTF Register Bits #define MCP_RX0IF 0x01 #define MCP_RX1IF 0x02 @@ -244,9 +236,8 @@ #define MCP_WAKIF 0x40 #define MCP_MERRF 0x80 -/* - * speed 16M - */ +// speed 16M + #define MCP_16MHz_1000kBPS_CFG1 (0x00) #define MCP_16MHz_1000kBPS_CFG2 (0xD0) #define MCP_16MHz_1000kBPS_CFG3 (0x82) @@ -271,12 +262,6 @@ #define MCP_16MHz_100kBPS_CFG2 (0xFA) #define MCP_16MHz_100kBPS_CFG3 (0x87) -/* -#define MCP_16MHz_100kBPS_CFG1 (0x03) -#define MCP_16MHz_100kBPS_CFG2 (0xBA) -#define MCP_16MHz_100kBPS_CFG3 (0x07) -*/ - #define MCP_16MHz_95kBPS_CFG1 (0x03) #define MCP_16MHz_95kBPS_CFG2 (0xAD) #define MCP_16MHz_95kBPS_CFG3 (0x07) @@ -305,6 +290,10 @@ #define MCP_16MHz_31k25BPS_CFG2 (0xF1) #define MCP_16MHz_31k25BPS_CFG3 (0x85) +#define MCP_16MHz_25kBPS_CFG1 (0X0F) +#define MCP_16MHz_25kBPS_CFG2 (0XBA) +#define MCP_16MHz_25kBPS_CFG3 (0X07) + #define MCP_16MHz_20kBPS_CFG1 (0x0F) #define MCP_16MHz_20kBPS_CFG2 (0xFF) #define MCP_16MHz_20kBPS_CFG3 (0x87) @@ -317,6 +306,9 @@ #define MCP_16MHz_5kBPS_CFG2 (0xFF) #define MCP_16MHz_5kBPS_CFG3 (0x87) +#define MCP_16MHz_666kBPS_CFG1 (0x00) +#define MCP_16MHz_666kBPS_CFG2 (0xA0) +#define MCP_16MHz_666kBPS_CFG3 (0x04) #define MCPDEBUG (0) @@ -326,7 +318,6 @@ #define MCP_RXBUF_0 (MCP_RXB0SIDH) #define MCP_RXBUF_1 (MCP_RXB1SIDH) -//#define SPICS 10 #define MCP2515_SELECT() digitalWrite(SPICS, LOW) #define MCP2515_UNSELECT() digitalWrite(SPICS, HIGH) @@ -338,51 +329,51 @@ #define CANUSELOOP 0 -#define CANSENDTIMEOUT (200) /* milliseconds */ - -/* - * initial value of gCANAutoProcess - */ -#define CANAUTOPROCESS (1) -#define CANAUTOON (1) -#define CANAUTOOFF (0) - -#define CAN_STDID (0) -#define CAN_EXTID (1) - -#define CANDEFAULTIDENT (0x55CC) -#define CANDEFAULTIDENTEXT (CAN_EXTID) - -#define CAN_5KBPS 1 -#define CAN_10KBPS 2 -#define CAN_20KBPS 3 -#define CAN_31K25BPS 4 -#define CAN_33KBPS 5 -#define CAN_40KBPS 6 -#define CAN_50KBPS 7 -#define CAN_80KBPS 8 -#define CAN_83K3BPS 9 -#define CAN_95KBPS 10 -#define CAN_100KBPS 11 -#define CAN_125KBPS 12 -#define CAN_200KBPS 13 -#define CAN_250KBPS 14 -#define CAN_500KBPS 15 -#define CAN_1000KBPS 16 - -#define CAN_OK (0) -#define CAN_FAILINIT (1) -#define CAN_FAILTX (2) -#define CAN_MSGAVAIL (3) -#define CAN_NOMSG (4) -#define CAN_CTRLERROR (5) -#define CAN_GETTXBFTIMEOUT (6) -#define CAN_SENDMSGTIMEOUT (7) -#define CAN_FAIL (0xff) +#define CANSENDTIMEOUT (200) // milliseconds + + +// initial value of gCANAutoProcess + +#define CANAUTOPROCESS (1) +#define CANAUTOON (1) +#define CANAUTOOFF (0) +#define CAN_STDID (0) +#define CAN_EXTID (1) +#define CANDEFAULTIDENT (0x55CC) +#define CANDEFAULTIDENTEXT (CAN_EXTID) + +#define CAN_5KBPS 1 +#define CAN_10KBPS 2 +#define CAN_20KBPS 3 +#define CAN_25KBPS 4 +#define CAN_31K25BPS 5 +#define CAN_33KBPS 6 +#define CAN_40KBPS 7 +#define CAN_50KBPS 8 +#define CAN_80KBPS 9 +#define CAN_83K3BPS 10 +#define CAN_95KBPS 11 +#define CAN_100KBPS 12 +#define CAN_125KBPS 13 +#define CAN_200KBPS 14 +#define CAN_250KBPS 15 +#define CAN_500KBPS 16 +#define CAN_666KBPS 17 +#define CAN_1000KBPS 18 + +#define CAN_OK (0) +#define CAN_FAILINIT (1) +#define CAN_FAILTX (2) +#define CAN_MSGAVAIL (3) +#define CAN_NOMSG (4) +#define CAN_CTRLERROR (5) +#define CAN_GETTXBFTIMEOUT (6) +#define CAN_SENDMSGTIMEOUT (7) +#define CAN_FAIL (0xff) #define CAN_MAX_CHAR_IN_MESSAGE (8) #endif /********************************************************************************************************* END FILE -*********************************************************************************************************/ +*********************************************************************************************************/ \ No newline at end of file diff --git a/platforms/common/libs/pid/oscc_pid.cpp b/firmware/common/libs/pid/oscc_pid.cpp similarity index 100% rename from platforms/common/libs/pid/oscc_pid.cpp rename to firmware/common/libs/pid/oscc_pid.cpp diff --git a/platforms/common/libs/pid/oscc_pid.h b/firmware/common/libs/pid/oscc_pid.h similarity index 100% rename from platforms/common/libs/pid/oscc_pid.h rename to firmware/common/libs/pid/oscc_pid.h diff --git a/platforms/common/libs/pid/tests/CMakeLists.txt b/firmware/common/libs/pid/tests/CMakeLists.txt similarity index 100% rename from platforms/common/libs/pid/tests/CMakeLists.txt rename to firmware/common/libs/pid/tests/CMakeLists.txt diff --git a/platforms/common/libs/pid/tests/property/Cargo.toml b/firmware/common/libs/pid/tests/property/Cargo.toml similarity index 100% rename from platforms/common/libs/pid/tests/property/Cargo.toml rename to firmware/common/libs/pid/tests/property/Cargo.toml diff --git a/platforms/common/libs/pid/tests/property/build.rs b/firmware/common/libs/pid/tests/property/build.rs similarity index 100% rename from platforms/common/libs/pid/tests/property/build.rs rename to firmware/common/libs/pid/tests/property/build.rs diff --git a/platforms/common/libs/pid/tests/property/include/wrapper.hpp b/firmware/common/libs/pid/tests/property/include/wrapper.hpp similarity index 100% rename from platforms/common/libs/pid/tests/property/include/wrapper.hpp rename to firmware/common/libs/pid/tests/property/include/wrapper.hpp diff --git a/platforms/common/libs/pid/tests/property/src/tests.rs b/firmware/common/libs/pid/tests/property/src/tests.rs similarity index 100% rename from platforms/common/libs/pid/tests/property/src/tests.rs rename to firmware/common/libs/pid/tests/property/src/tests.rs diff --git a/platforms/common/libs/serial/oscc_serial.cpp b/firmware/common/libs/serial/oscc_serial.cpp similarity index 100% rename from platforms/common/libs/serial/oscc_serial.cpp rename to firmware/common/libs/serial/oscc_serial.cpp diff --git a/platforms/common/libs/serial/oscc_serial.h b/firmware/common/libs/serial/oscc_serial.h similarity index 100% rename from platforms/common/libs/serial/oscc_serial.h rename to firmware/common/libs/serial/oscc_serial.h diff --git a/firmware/common/libs/timer/oscc_timer.cpp b/firmware/common/libs/timer/oscc_timer.cpp new file mode 100644 index 00000000..4f0479ad --- /dev/null +++ b/firmware/common/libs/timer/oscc_timer.cpp @@ -0,0 +1,189 @@ +/** + * @file oscc_timer.cpp + * + */ + + +#include "Arduino.h" +#include "oscc_timer.h" + + +static void (*timer_1_isr)(void); +static void (*timer_2_isr)(void); + + +// timer1 interrupt service routine +ISR(TIMER1_COMPA_vect) +{ + timer_1_isr( ); +} + +// timer2 interrupt service routine +ISR(TIMER2_COMPA_vect) +{ + timer_2_isr( ); +} + + +void timer1_init( float frequency, void (*isr)(void) ) +{ + // disable interrupts temporarily + cli(); + + // clear existing config + TCCR1A = 0; + TCCR1B = 0; + + // initialize counter value to 0 + TCNT1 = 0; + + + unsigned long prescaler = F_CPU / ((TIMER1_SIZE+1) * frequency); + + if ( prescaler > 256 ) + { + prescaler = 1024; + + TCCR1B |= TIMER1_PRESCALER_1024; + } + else if ( prescaler > 64 ) + { + prescaler = 256; + + TCCR1B |= TIMER1_PRESCALER_256; + } + else if ( prescaler > 8 ) + { + prescaler = 64; + + TCCR1B |= TIMER1_PRESCALER_64; + } + else if ( prescaler > 1 ) + { + prescaler = 8; + + TCCR1B |= TIMER1_PRESCALER_8; + } + else + { + prescaler = 1; + + TCCR1B |= TIMER1_PRESCALER_1; + } + + + unsigned long compare_match_value = ((F_CPU) / (frequency * prescaler)) - 1; + + if ( compare_match_value > TIMER1_SIZE ) + { + compare_match_value = TIMER1_SIZE; + } + else if ( compare_match_value < 1 ) + { + compare_match_value = 1; + } + + + // set value to compare counter with + OCR1A = compare_match_value; + + // turn on compare mode + TCCR1B |= _BV(WGM12); + + // enable compare interrupt + TIMSK1 |= _BV(OCIE1A); + + // attach interrupt service routine + timer_1_isr = isr; + + // re-enable interrupts + sei(); +} + + +void timer2_init( float frequency, void (*isr)(void) ) +{ + // disable interrupts temporarily + cli(); + + // clear existing config + TCCR2A = 0; + TCCR2B = 0; + + // initialize counter value to 0 + TCNT2 = 0; + + + unsigned long prescaler = F_CPU / ((TIMER2_SIZE+1) * frequency); + + if ( prescaler > 256 ) + { + prescaler = 1024; + + TCCR2B |= TIMER2_PRESCALER_1024; + } + else if ( prescaler > 128 ) + { + prescaler = 256; + + TCCR2B |= TIMER2_PRESCALER_256; + } + else if ( prescaler > 64 ) + { + prescaler = 128; + + TCCR2B |= TIMER2_PRESCALER_128; + } + else if ( prescaler > 32 ) + { + prescaler = 64; + + TCCR2B |= TIMER2_PRESCALER_64; + } + else if ( prescaler > 8 ) + { + prescaler = 32; + + TCCR2B |= TIMER2_PRESCALER_32; + } + else if ( prescaler > 1 ) + { + prescaler = 8; + + TCCR2B |= TIMER2_PRESCALER_8; + } + else + { + prescaler = 1; + + TCCR2B |= TIMER2_PRESCALER_1; + } + + + unsigned long compare_match_value = ((F_CPU) / (frequency * prescaler)) - 1; + + if ( compare_match_value > TIMER2_SIZE ) + { + compare_match_value = TIMER2_SIZE; + } + else if ( compare_match_value < 1 ) + { + compare_match_value = 1; + } + + + // set value to compare counter with + OCR2A = compare_match_value; + + // turn on compare mode + TCCR2B |= _BV(WGM21); + + // enable compare interrupt + TIMSK2 |= _BV(OCIE2A); + + // attach interrupt service routine + timer_2_isr = isr; + + // re-enable interrupts + sei(); +} diff --git a/firmware/common/libs/timer/oscc_timer.h b/firmware/common/libs/timer/oscc_timer.h new file mode 100644 index 00000000..014826a2 --- /dev/null +++ b/firmware/common/libs/timer/oscc_timer.h @@ -0,0 +1,136 @@ +/** + * @file oscc_timer.h + * @brief Timer utilities. + * + */ + + +#ifndef _OSCC_TIMER_H_ +#define _OSCC_TIMER_H_ + + +/* + * @brief Maximum value that timer1 counter can contain. + * + */ +#define TIMER1_SIZE ( 65535 ) + +/* + * @brief Necessary bitshifts for a timer1 prescaler of 1. + * + */ +#define TIMER1_PRESCALER_1 ( (_BV(CS10)) ) + +/* + * @brief Necessary bitshifts for a timer1 prescaler of 8. + * + */ +#define TIMER1_PRESCALER_8 ( (_BV(CS11)) ) + +/* + * @brief Necessary bitshifts for a timer1 prescaler of 64. + * + */ +#define TIMER1_PRESCALER_64 ( (_BV(CS11) | _BV(CS10)) ) + +/* + * @brief Necessary bitshifts for a timer1 prescaler of 256. + * + */ +#define TIMER1_PRESCALER_256 ( (_BV(CS12)) ) + +/* + * @brief Necessary bitshifts for a timer1 prescaler of 1024. + * + */ +#define TIMER1_PRESCALER_1024 ( (_BV(CS12) | _BV(CS10)) ) + +/* + * @brief Maximum value that timer2 counter can contain. + * + */ +#define TIMER2_SIZE ( 255 ) + +/* + * @brief Necessary bitshifts for a timer2 prescaler of 1. + * + */ +#define TIMER2_PRESCALER_1 ( (_BV(CS20)) ) + +/* + * @brief Necessary bitshifts for a timer2 prescaler of 8. + * + */ +#define TIMER2_PRESCALER_8 ( (_BV(CS21)) ) + +/* + * @brief Necessary bitshifts for a timer2 prescaler of 32. + * + */ +#define TIMER2_PRESCALER_32 ( (_BV(CS21) | _BV(CS20)) ) + +/* + * @brief Necessary bitshifts for a timer2 prescaler of 64. + * + */ +#define TIMER2_PRESCALER_64 ( (_BV(CS22)) ) + +/* + * @brief Necessary bitshifts for a timer2 prescaler of 128. + * + */ +#define TIMER2_PRESCALER_128 ( (_BV(CS22) | _BV(CS20)) ) + +/* + * @brief Necessary bitshifts for a timer2 prescaler of 256. + * + */ +#define TIMER2_PRESCALER_256 ( (_BV(CS22) | _BV(CS21)) ) + +/* + * @brief Necessary bitshifts for a timer2 prescaler of 1024. + * + */ +#define TIMER2_PRESCALER_1024 ( (_BV(CS22) | _BV(CS21) | _BV(CS20)) ) + + +// **************************************************************************** +// Function: timer1_init +// +// Purpose: Initializes timer1 to interrupt at a set frequency and run +// an ISR at the time of that interrupt. +// +// Notes: timer1 is a 16-bit timer with a minimum frequency of 0.25Hz. +// +// Returns: void +// +// Parameters: [in] frequency - frequency at which to generate an interrupt [hz] +// [in] isr - pointer to the interrupt service routine to call on +// interrupt +// +// **************************************************************************** +void timer1_init( + float frequency, + void (*isr)(void) ); + +// **************************************************************************** +// Function: timer2_init +// +// Purpose: Initializes timer2 to interrupt at a set frequency and run +// an ISR at the time of that interrupt. +// +// Notes: timer2 is an 8-bit timer with a minimum frequency of 61Hz. +// +// Returns: void +// +// Parameters: [in] frequency - frequency at which to generate an interrupt [hz] +// [in] isr - pointer to the interrupt service routine to call on +// interrupt +// +// **************************************************************************** +void timer2_init( + float frequency, + void (*isr)(void) ); + + +#endif /* _OSCC_TIMER_H_ */ diff --git a/platforms/common/testing/framework/build_test_framework.sh b/firmware/common/testing/framework/build_test_framework.sh similarity index 100% rename from platforms/common/testing/framework/build_test_framework.sh rename to firmware/common/testing/framework/build_test_framework.sh diff --git a/platforms/common/testing/framework/cgreen/LICENSE b/firmware/common/testing/framework/cgreen/LICENSE similarity index 100% rename from platforms/common/testing/framework/cgreen/LICENSE rename to firmware/common/testing/framework/cgreen/LICENSE diff --git a/platforms/common/testing/framework/cgreen/bin/cgreen-runner b/firmware/common/testing/framework/cgreen/bin/cgreen-runner similarity index 100% rename from platforms/common/testing/framework/cgreen/bin/cgreen-runner rename to firmware/common/testing/framework/cgreen/bin/cgreen-runner diff --git a/platforms/common/testing/framework/cgreen/include/cgreen/assertions.h b/firmware/common/testing/framework/cgreen/include/cgreen/assertions.h similarity index 100% rename from platforms/common/testing/framework/cgreen/include/cgreen/assertions.h rename to firmware/common/testing/framework/cgreen/include/cgreen/assertions.h diff --git a/platforms/common/testing/framework/cgreen/include/cgreen/boxed_double.h b/firmware/common/testing/framework/cgreen/include/cgreen/boxed_double.h similarity index 100% rename from platforms/common/testing/framework/cgreen/include/cgreen/boxed_double.h rename to firmware/common/testing/framework/cgreen/include/cgreen/boxed_double.h diff --git a/platforms/common/testing/framework/cgreen/include/cgreen/breadcrumb.h b/firmware/common/testing/framework/cgreen/include/cgreen/breadcrumb.h similarity index 100% rename from platforms/common/testing/framework/cgreen/include/cgreen/breadcrumb.h rename to firmware/common/testing/framework/cgreen/include/cgreen/breadcrumb.h diff --git a/platforms/common/testing/framework/cgreen/include/cgreen/cdash_reporter.h b/firmware/common/testing/framework/cgreen/include/cgreen/cdash_reporter.h similarity index 100% rename from platforms/common/testing/framework/cgreen/include/cgreen/cdash_reporter.h rename to firmware/common/testing/framework/cgreen/include/cgreen/cdash_reporter.h diff --git a/platforms/common/testing/framework/cgreen/include/cgreen/cgreen.h b/firmware/common/testing/framework/cgreen/include/cgreen/cgreen.h similarity index 100% rename from platforms/common/testing/framework/cgreen/include/cgreen/cgreen.h rename to firmware/common/testing/framework/cgreen/include/cgreen/cgreen.h diff --git a/platforms/common/testing/framework/cgreen/include/cgreen/cgreen_value.h b/firmware/common/testing/framework/cgreen/include/cgreen/cgreen_value.h similarity index 100% rename from platforms/common/testing/framework/cgreen/include/cgreen/cgreen_value.h rename to firmware/common/testing/framework/cgreen/include/cgreen/cgreen_value.h diff --git a/platforms/common/testing/framework/cgreen/include/cgreen/constraint.h b/firmware/common/testing/framework/cgreen/include/cgreen/constraint.h similarity index 100% rename from platforms/common/testing/framework/cgreen/include/cgreen/constraint.h rename to firmware/common/testing/framework/cgreen/include/cgreen/constraint.h diff --git a/platforms/common/testing/framework/cgreen/include/cgreen/constraint_syntax_helpers.h b/firmware/common/testing/framework/cgreen/include/cgreen/constraint_syntax_helpers.h similarity index 100% rename from platforms/common/testing/framework/cgreen/include/cgreen/constraint_syntax_helpers.h rename to firmware/common/testing/framework/cgreen/include/cgreen/constraint_syntax_helpers.h diff --git a/platforms/common/testing/framework/cgreen/include/cgreen/cpp_assertions.h b/firmware/common/testing/framework/cgreen/include/cgreen/cpp_assertions.h similarity index 100% rename from platforms/common/testing/framework/cgreen/include/cgreen/cpp_assertions.h rename to firmware/common/testing/framework/cgreen/include/cgreen/cpp_assertions.h diff --git a/platforms/common/testing/framework/cgreen/include/cgreen/cpp_constraint.h b/firmware/common/testing/framework/cgreen/include/cgreen/cpp_constraint.h similarity index 100% rename from platforms/common/testing/framework/cgreen/include/cgreen/cpp_constraint.h rename to firmware/common/testing/framework/cgreen/include/cgreen/cpp_constraint.h diff --git a/platforms/common/testing/framework/cgreen/include/cgreen/cute_reporter.h b/firmware/common/testing/framework/cgreen/include/cgreen/cute_reporter.h similarity index 100% rename from platforms/common/testing/framework/cgreen/include/cgreen/cute_reporter.h rename to firmware/common/testing/framework/cgreen/include/cgreen/cute_reporter.h diff --git a/platforms/common/testing/framework/cgreen/include/cgreen/internal/assertions_internal.h b/firmware/common/testing/framework/cgreen/include/cgreen/internal/assertions_internal.h similarity index 100% rename from platforms/common/testing/framework/cgreen/include/cgreen/internal/assertions_internal.h rename to firmware/common/testing/framework/cgreen/include/cgreen/internal/assertions_internal.h diff --git a/platforms/common/testing/framework/cgreen/include/cgreen/internal/c_assertions.h b/firmware/common/testing/framework/cgreen/include/cgreen/internal/c_assertions.h similarity index 100% rename from platforms/common/testing/framework/cgreen/include/cgreen/internal/c_assertions.h rename to firmware/common/testing/framework/cgreen/include/cgreen/internal/c_assertions.h diff --git a/platforms/common/testing/framework/cgreen/include/cgreen/internal/cgreen_pipe.h b/firmware/common/testing/framework/cgreen/include/cgreen/internal/cgreen_pipe.h similarity index 100% rename from platforms/common/testing/framework/cgreen/include/cgreen/internal/cgreen_pipe.h rename to firmware/common/testing/framework/cgreen/include/cgreen/internal/cgreen_pipe.h diff --git a/platforms/common/testing/framework/cgreen/include/cgreen/internal/cgreen_time.h b/firmware/common/testing/framework/cgreen/include/cgreen/internal/cgreen_time.h similarity index 100% rename from platforms/common/testing/framework/cgreen/include/cgreen/internal/cgreen_time.h rename to firmware/common/testing/framework/cgreen/include/cgreen/internal/cgreen_time.h diff --git a/platforms/common/testing/framework/cgreen/include/cgreen/internal/cpp_assertions.h b/firmware/common/testing/framework/cgreen/include/cgreen/internal/cpp_assertions.h similarity index 100% rename from platforms/common/testing/framework/cgreen/include/cgreen/internal/cpp_assertions.h rename to firmware/common/testing/framework/cgreen/include/cgreen/internal/cpp_assertions.h diff --git a/platforms/common/testing/framework/cgreen/include/cgreen/internal/function_macro.h b/firmware/common/testing/framework/cgreen/include/cgreen/internal/function_macro.h similarity index 100% rename from platforms/common/testing/framework/cgreen/include/cgreen/internal/function_macro.h rename to firmware/common/testing/framework/cgreen/include/cgreen/internal/function_macro.h diff --git a/platforms/common/testing/framework/cgreen/include/cgreen/internal/mock_table.h b/firmware/common/testing/framework/cgreen/include/cgreen/internal/mock_table.h similarity index 100% rename from platforms/common/testing/framework/cgreen/include/cgreen/internal/mock_table.h rename to firmware/common/testing/framework/cgreen/include/cgreen/internal/mock_table.h diff --git a/platforms/common/testing/framework/cgreen/include/cgreen/internal/mocks_internal.h b/firmware/common/testing/framework/cgreen/include/cgreen/internal/mocks_internal.h similarity index 100% rename from platforms/common/testing/framework/cgreen/include/cgreen/internal/mocks_internal.h rename to firmware/common/testing/framework/cgreen/include/cgreen/internal/mocks_internal.h diff --git a/platforms/common/testing/framework/cgreen/include/cgreen/internal/runner_platform.h b/firmware/common/testing/framework/cgreen/include/cgreen/internal/runner_platform.h similarity index 100% rename from platforms/common/testing/framework/cgreen/include/cgreen/internal/runner_platform.h rename to firmware/common/testing/framework/cgreen/include/cgreen/internal/runner_platform.h diff --git a/platforms/common/testing/framework/cgreen/include/cgreen/internal/stringify_token.h b/firmware/common/testing/framework/cgreen/include/cgreen/internal/stringify_token.h similarity index 100% rename from platforms/common/testing/framework/cgreen/include/cgreen/internal/stringify_token.h rename to firmware/common/testing/framework/cgreen/include/cgreen/internal/stringify_token.h diff --git a/platforms/common/testing/framework/cgreen/include/cgreen/internal/suite_internal.h b/firmware/common/testing/framework/cgreen/include/cgreen/internal/suite_internal.h similarity index 100% rename from platforms/common/testing/framework/cgreen/include/cgreen/internal/suite_internal.h rename to firmware/common/testing/framework/cgreen/include/cgreen/internal/suite_internal.h diff --git a/platforms/common/testing/framework/cgreen/include/cgreen/internal/unit_implementation.h b/firmware/common/testing/framework/cgreen/include/cgreen/internal/unit_implementation.h similarity index 100% rename from platforms/common/testing/framework/cgreen/include/cgreen/internal/unit_implementation.h rename to firmware/common/testing/framework/cgreen/include/cgreen/internal/unit_implementation.h diff --git a/platforms/common/testing/framework/cgreen/include/cgreen/legacy.h b/firmware/common/testing/framework/cgreen/include/cgreen/legacy.h similarity index 100% rename from platforms/common/testing/framework/cgreen/include/cgreen/legacy.h rename to firmware/common/testing/framework/cgreen/include/cgreen/legacy.h diff --git a/platforms/common/testing/framework/cgreen/include/cgreen/mocks.h b/firmware/common/testing/framework/cgreen/include/cgreen/mocks.h similarity index 100% rename from platforms/common/testing/framework/cgreen/include/cgreen/mocks.h rename to firmware/common/testing/framework/cgreen/include/cgreen/mocks.h diff --git a/platforms/common/testing/framework/cgreen/include/cgreen/reporter.h b/firmware/common/testing/framework/cgreen/include/cgreen/reporter.h similarity index 100% rename from platforms/common/testing/framework/cgreen/include/cgreen/reporter.h rename to firmware/common/testing/framework/cgreen/include/cgreen/reporter.h diff --git a/platforms/common/testing/framework/cgreen/include/cgreen/runner.h b/firmware/common/testing/framework/cgreen/include/cgreen/runner.h similarity index 100% rename from platforms/common/testing/framework/cgreen/include/cgreen/runner.h rename to firmware/common/testing/framework/cgreen/include/cgreen/runner.h diff --git a/platforms/common/testing/framework/cgreen/include/cgreen/string_comparison.h b/firmware/common/testing/framework/cgreen/include/cgreen/string_comparison.h similarity index 100% rename from platforms/common/testing/framework/cgreen/include/cgreen/string_comparison.h rename to firmware/common/testing/framework/cgreen/include/cgreen/string_comparison.h diff --git a/platforms/common/testing/framework/cgreen/include/cgreen/suite.h b/firmware/common/testing/framework/cgreen/include/cgreen/suite.h similarity index 100% rename from platforms/common/testing/framework/cgreen/include/cgreen/suite.h rename to firmware/common/testing/framework/cgreen/include/cgreen/suite.h diff --git a/platforms/common/testing/framework/cgreen/include/cgreen/text_reporter.h b/firmware/common/testing/framework/cgreen/include/cgreen/text_reporter.h similarity index 100% rename from platforms/common/testing/framework/cgreen/include/cgreen/text_reporter.h rename to firmware/common/testing/framework/cgreen/include/cgreen/text_reporter.h diff --git a/platforms/common/testing/framework/cgreen/include/cgreen/unit.h b/firmware/common/testing/framework/cgreen/include/cgreen/unit.h similarity index 100% rename from platforms/common/testing/framework/cgreen/include/cgreen/unit.h rename to firmware/common/testing/framework/cgreen/include/cgreen/unit.h diff --git a/platforms/common/testing/framework/cgreen/include/cgreen/vector.h b/firmware/common/testing/framework/cgreen/include/cgreen/vector.h similarity index 100% rename from platforms/common/testing/framework/cgreen/include/cgreen/vector.h rename to firmware/common/testing/framework/cgreen/include/cgreen/vector.h diff --git a/platforms/common/testing/framework/cgreen/lib/cmake/cgreen/cgreen-config-version.cmake b/firmware/common/testing/framework/cgreen/lib/cmake/cgreen/cgreen-config-version.cmake similarity index 100% rename from platforms/common/testing/framework/cgreen/lib/cmake/cgreen/cgreen-config-version.cmake rename to firmware/common/testing/framework/cgreen/lib/cmake/cgreen/cgreen-config-version.cmake diff --git a/platforms/common/testing/framework/cgreen/lib/cmake/cgreen/cgreen-config.cmake b/firmware/common/testing/framework/cgreen/lib/cmake/cgreen/cgreen-config.cmake similarity index 100% rename from platforms/common/testing/framework/cgreen/lib/cmake/cgreen/cgreen-config.cmake rename to firmware/common/testing/framework/cgreen/lib/cmake/cgreen/cgreen-config.cmake diff --git a/platforms/common/testing/framework/cgreen/lib/libcgreen.so b/firmware/common/testing/framework/cgreen/lib/libcgreen.so similarity index 100% rename from platforms/common/testing/framework/cgreen/lib/libcgreen.so rename to firmware/common/testing/framework/cgreen/lib/libcgreen.so diff --git a/platforms/common/testing/framework/cgreen/lib/libcgreen.so.1 b/firmware/common/testing/framework/cgreen/lib/libcgreen.so.1 similarity index 100% rename from platforms/common/testing/framework/cgreen/lib/libcgreen.so.1 rename to firmware/common/testing/framework/cgreen/lib/libcgreen.so.1 diff --git a/platforms/common/testing/framework/cgreen/lib/libcgreen.so.1.1.0 b/firmware/common/testing/framework/cgreen/lib/libcgreen.so.1.1.0 similarity index 100% rename from platforms/common/testing/framework/cgreen/lib/libcgreen.so.1.1.0 rename to firmware/common/testing/framework/cgreen/lib/libcgreen.so.1.1.0 diff --git a/platforms/common/testing/framework/cgreen/share/man/man1/cgreen-runner.1 b/firmware/common/testing/framework/cgreen/share/man/man1/cgreen-runner.1 similarity index 100% rename from platforms/common/testing/framework/cgreen/share/man/man1/cgreen-runner.1 rename to firmware/common/testing/framework/cgreen/share/man/man1/cgreen-runner.1 diff --git a/platforms/common/testing/framework/cgreen/share/man/man5/cgreen.5 b/firmware/common/testing/framework/cgreen/share/man/man5/cgreen.5 similarity index 100% rename from platforms/common/testing/framework/cgreen/share/man/man5/cgreen.5 rename to firmware/common/testing/framework/cgreen/share/man/man5/cgreen.5 diff --git a/platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/autodetect.hpp b/firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/autodetect.hpp similarity index 100% rename from platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/autodetect.hpp rename to firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/autodetect.hpp diff --git a/platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/defs.hpp b/firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/defs.hpp similarity index 100% rename from platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/defs.hpp rename to firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/defs.hpp diff --git a/platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/generic.hpp b/firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/generic.hpp similarity index 100% rename from platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/generic.hpp rename to firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/generic.hpp diff --git a/platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/ContextManager.hpp b/firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/ContextManager.hpp similarity index 100% rename from platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/ContextManager.hpp rename to firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/ContextManager.hpp diff --git a/platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/CukeCommands.hpp b/firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/CukeCommands.hpp similarity index 100% rename from platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/CukeCommands.hpp rename to firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/CukeCommands.hpp diff --git a/platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/CukeEngine.hpp b/firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/CukeEngine.hpp similarity index 100% rename from platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/CukeEngine.hpp rename to firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/CukeEngine.hpp diff --git a/platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/CukeEngineImpl.hpp b/firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/CukeEngineImpl.hpp similarity index 100% rename from platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/CukeEngineImpl.hpp rename to firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/CukeEngineImpl.hpp diff --git a/platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/Macros.hpp b/firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/Macros.hpp similarity index 100% rename from platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/Macros.hpp rename to firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/Macros.hpp diff --git a/platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/RegistrationMacros.hpp b/firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/RegistrationMacros.hpp similarity index 100% rename from platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/RegistrationMacros.hpp rename to firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/RegistrationMacros.hpp diff --git a/platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/Scenario.hpp b/firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/Scenario.hpp similarity index 100% rename from platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/Scenario.hpp rename to firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/Scenario.hpp diff --git a/platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/Table.hpp b/firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/Table.hpp similarity index 100% rename from platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/Table.hpp rename to firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/Table.hpp diff --git a/platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/connectors/wire/ProtocolHandler.hpp b/firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/connectors/wire/ProtocolHandler.hpp similarity index 100% rename from platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/connectors/wire/ProtocolHandler.hpp rename to firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/connectors/wire/ProtocolHandler.hpp diff --git a/platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/connectors/wire/WireProtocol.hpp b/firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/connectors/wire/WireProtocol.hpp similarity index 100% rename from platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/connectors/wire/WireProtocol.hpp rename to firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/connectors/wire/WireProtocol.hpp diff --git a/platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/connectors/wire/WireProtocolCommands.hpp b/firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/connectors/wire/WireProtocolCommands.hpp similarity index 100% rename from platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/connectors/wire/WireProtocolCommands.hpp rename to firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/connectors/wire/WireProtocolCommands.hpp diff --git a/platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/connectors/wire/WireServer.hpp b/firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/connectors/wire/WireServer.hpp similarity index 100% rename from platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/connectors/wire/WireServer.hpp rename to firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/connectors/wire/WireServer.hpp diff --git a/platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/defs.hpp b/firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/defs.hpp similarity index 100% rename from platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/defs.hpp rename to firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/defs.hpp diff --git a/platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/drivers/BoostDriver.hpp b/firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/drivers/BoostDriver.hpp similarity index 100% rename from platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/drivers/BoostDriver.hpp rename to firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/drivers/BoostDriver.hpp diff --git a/platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/drivers/CgreenDriver.hpp b/firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/drivers/CgreenDriver.hpp similarity index 100% rename from platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/drivers/CgreenDriver.hpp rename to firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/drivers/CgreenDriver.hpp diff --git a/platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/drivers/DriverSelector.hpp b/firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/drivers/DriverSelector.hpp similarity index 100% rename from platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/drivers/DriverSelector.hpp rename to firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/drivers/DriverSelector.hpp diff --git a/platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/drivers/GTestDriver.hpp b/firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/drivers/GTestDriver.hpp similarity index 100% rename from platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/drivers/GTestDriver.hpp rename to firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/drivers/GTestDriver.hpp diff --git a/platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/drivers/GenericDriver.hpp b/firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/drivers/GenericDriver.hpp similarity index 100% rename from platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/drivers/GenericDriver.hpp rename to firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/drivers/GenericDriver.hpp diff --git a/platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/hook/HookMacros.hpp b/firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/hook/HookMacros.hpp similarity index 100% rename from platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/hook/HookMacros.hpp rename to firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/hook/HookMacros.hpp diff --git a/platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/hook/HookRegistrar.hpp b/firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/hook/HookRegistrar.hpp similarity index 100% rename from platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/hook/HookRegistrar.hpp rename to firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/hook/HookRegistrar.hpp diff --git a/platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/hook/Tag.hpp b/firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/hook/Tag.hpp similarity index 100% rename from platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/hook/Tag.hpp rename to firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/hook/Tag.hpp diff --git a/platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/step/StepMacros.hpp b/firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/step/StepMacros.hpp similarity index 100% rename from platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/step/StepMacros.hpp rename to firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/step/StepMacros.hpp diff --git a/platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/step/StepManager.hpp b/firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/step/StepManager.hpp similarity index 100% rename from platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/step/StepManager.hpp rename to firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/step/StepManager.hpp diff --git a/platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/utils/Regex.hpp b/firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/utils/Regex.hpp similarity index 100% rename from platforms/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/utils/Regex.hpp rename to firmware/common/testing/framework/cucumber-cpp/include/cucumber-cpp/internal/utils/Regex.hpp diff --git a/platforms/common/testing/framework/cucumber-cpp/lib/libcucumber-cpp.a b/firmware/common/testing/framework/cucumber-cpp/lib/libcucumber-cpp.a similarity index 100% rename from platforms/common/testing/framework/cucumber-cpp/lib/libcucumber-cpp.a rename to firmware/common/testing/framework/cucumber-cpp/lib/libcucumber-cpp.a diff --git a/platforms/common/testing/mocks/Arduino.h b/firmware/common/testing/mocks/Arduino.h similarity index 91% rename from platforms/common/testing/mocks/Arduino.h rename to firmware/common/testing/mocks/Arduino.h index 10a3ef11..24bc01e4 100644 --- a/platforms/common/testing/mocks/Arduino.h +++ b/firmware/common/testing/mocks/Arduino.h @@ -7,8 +7,11 @@ #define A0 0 #define A1 1 +#define A2 2 +#define A3 3 #define LOW 0 #define HIGH 1 +#define INPUT 0 #define OUTPUT 1 #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) @@ -27,6 +30,10 @@ void analogWrite(uint8_t pin, int val); void delay(unsigned long ms); +void sei(); + +void cli(); + class _Serial { public: diff --git a/platforms/common/testing/mocks/Arduino_mock.cpp b/firmware/common/testing/mocks/Arduino_mock.cpp similarity index 97% rename from platforms/common/testing/mocks/Arduino_mock.cpp rename to firmware/common/testing/mocks/Arduino_mock.cpp index 25765310..e4fc4ba0 100644 --- a/platforms/common/testing/mocks/Arduino_mock.cpp +++ b/firmware/common/testing/mocks/Arduino_mock.cpp @@ -56,6 +56,14 @@ void delay(unsigned long ms) { } +void cli(void) +{ +} + +void sei(void) +{ +} + void _Serial::print(const char str[]) { printf("%s", str); diff --git a/platforms/common/testing/mocks/DAC_MCP49xx.h b/firmware/common/testing/mocks/DAC_MCP49xx.h similarity index 100% rename from platforms/common/testing/mocks/DAC_MCP49xx.h rename to firmware/common/testing/mocks/DAC_MCP49xx.h diff --git a/platforms/common/testing/mocks/DAC_MCP49xx_mock.cpp b/firmware/common/testing/mocks/DAC_MCP49xx_mock.cpp similarity index 100% rename from platforms/common/testing/mocks/DAC_MCP49xx_mock.cpp rename to firmware/common/testing/mocks/DAC_MCP49xx_mock.cpp diff --git a/platforms/common/testing/mocks/mcp_can.h b/firmware/common/testing/mocks/mcp_can.h similarity index 59% rename from platforms/common/testing/mocks/mcp_can.h rename to firmware/common/testing/mocks/mcp_can.h index 2398bdc4..856f12eb 100644 --- a/platforms/common/testing/mocks/mcp_can.h +++ b/firmware/common/testing/mocks/mcp_can.h @@ -5,14 +5,6 @@ #define CAN_500KBPS 15 -#ifndef INT32U -#define INT32U unsigned long -#endif - -#ifndef INT8U -#define INT8U uint8_t -#endif - #define CAN_OK (0) #define CAN_FAILINIT (1) #define CAN_FAILTX (2) @@ -26,11 +18,11 @@ class MCP_CAN { public: - MCP_CAN(INT8U _CS); - INT8U begin(INT8U speedset); - INT8U sendMsgBuf(INT32U id, INT8U ext, INT8U len, INT8U *buf); - INT8U readMsgBufID(INT32U *ID, INT8U *len, INT8U *buf); - INT8U checkReceive(void); + MCP_CAN(uint8_t _CS); + uint8_t begin(uint8_t speedset); + uint8_t sendMsgBuf(uint32_t id, uint8_t ext, uint8_t len, uint8_t *buf); + uint8_t readMsgBufID(uint32_t *ID, uint8_t *len, uint8_t *buf); + uint8_t checkReceive(void); }; #endif diff --git a/firmware/common/testing/mocks/mcp_can_mock.cpp b/firmware/common/testing/mocks/mcp_can_mock.cpp new file mode 100644 index 00000000..4d2433c5 --- /dev/null +++ b/firmware/common/testing/mocks/mcp_can_mock.cpp @@ -0,0 +1,58 @@ +#include +#include +#include +#include + +#include "mcp_can.h" + + + +uint8_t g_mock_mcp_can_check_receive_return; +uint32_t g_mock_mcp_can_read_msg_buf_id; +uint8_t g_mock_mcp_can_read_msg_buf_buf[8]; + +uint32_t g_mock_mcp_can_send_msg_buf_id; +uint8_t g_mock_mcp_can_send_msg_buf_ext; +uint8_t g_mock_mcp_can_send_msg_buf_len; +uint8_t *g_mock_mcp_can_send_msg_buf_buf; + + +MCP_CAN::MCP_CAN(uint8_t _CS) +{ +} + +uint8_t MCP_CAN::begin(uint8_t speedset) +{ + return CAN_OK; +} + +uint8_t MCP_CAN::checkReceive(void) +{ + return g_mock_mcp_can_check_receive_return; +} + +uint8_t MCP_CAN::sendMsgBuf(uint32_t id, uint8_t ext, uint8_t len, uint8_t *buf) +{ + g_mock_mcp_can_send_msg_buf_id = id; + g_mock_mcp_can_send_msg_buf_ext = ext; + g_mock_mcp_can_send_msg_buf_len = len; + + g_mock_mcp_can_send_msg_buf_buf = (uint8_t *) malloc(len); + + memcpy(g_mock_mcp_can_send_msg_buf_buf, buf, len); + + return CAN_OK; +} + +uint8_t MCP_CAN::readMsgBufID(uint32_t *ID, uint8_t *len, uint8_t *buf) +{ + *ID = g_mock_mcp_can_read_msg_buf_id; + *len = 8; + + for( int i = 0; i < *len; ++i ) + { + buf[i] = g_mock_mcp_can_read_msg_buf_buf[i]; + } + + return CAN_OK; +} diff --git a/platforms/common/toolchain/ArduinoToolchain.cmake b/firmware/common/toolchain/ArduinoToolchain.cmake similarity index 100% rename from platforms/common/toolchain/ArduinoToolchain.cmake rename to firmware/common/toolchain/ArduinoToolchain.cmake diff --git a/platforms/common/toolchain/Platform/Arduino.cmake b/firmware/common/toolchain/Platform/Arduino.cmake similarity index 100% rename from platforms/common/toolchain/Platform/Arduino.cmake rename to firmware/common/toolchain/Platform/Arduino.cmake diff --git a/platforms/kia_soul/firmware/steering/CMakeLists.txt b/firmware/steering/CMakeLists.txt similarity index 76% rename from platforms/kia_soul/firmware/steering/CMakeLists.txt rename to firmware/steering/CMakeLists.txt index 403a8eaa..4231b51d 100644 --- a/platforms/kia_soul/firmware/steering/CMakeLists.txt +++ b/firmware/steering/CMakeLists.txt @@ -1,4 +1,4 @@ -project(kia-soul-steering) +project(steering) set(ARDUINO_DEFAULT_BOARD uno) SET(ARDUINO_DEFAULT_PORT ${SERIAL_PORT_STEERING}) @@ -7,15 +7,15 @@ set(ARDUINO_DEFAULT_BAUDRATE ${SERIAL_BAUD_STEERING}) add_definitions(-DSERIAL_BAUD=${ARDUINO_DEFAULT_BAUDRATE}) add_custom_target( - kia-soul-steering-monitor + steering-monitor COMMAND screen ${ARDUINO_DEFAULT_PORT} ${ARDUINO_DEFAULT_BAUDRATE}) add_custom_target( - kia-soul-steering-monitor-log + steering-monitor-log COMMAND screen -L ${ARDUINO_DEFAULT_PORT} ${ARDUINO_DEFAULT_BAUDRATE}) generate_arduino_firmware( - kia-soul-steering + steering SRCS ${CMAKE_SOURCE_DIR}/common/libs/arduino_init/arduino_init.cpp ${CMAKE_SOURCE_DIR}/common/libs/DAC_MCP49xx/DAC_MCP49xx.cpp @@ -23,17 +23,17 @@ generate_arduino_firmware( ${CMAKE_SOURCE_DIR}/common/libs/pid/oscc_pid.cpp ${CMAKE_SOURCE_DIR}/common/libs/serial/oscc_serial.cpp ${CMAKE_SOURCE_DIR}/common/libs/can/oscc_can.cpp - ${CMAKE_SOURCE_DIR}/common/libs/time/oscc_time.cpp ${CMAKE_SOURCE_DIR}/common/libs/dac/oscc_dac.cpp - ${CMAKE_SOURCE_DIR}/common/libs/signal_smoothing/oscc_signal_smoothing.cpp + ${CMAKE_SOURCE_DIR}/common/libs/timer/oscc_timer.cpp src/main.cpp src/globals.cpp src/init.cpp src/communications.cpp - src/steering_control.cpp) + src/steering_control.cpp + src/timers.cpp) target_include_directories( - kia-soul-steering + steering PRIVATE include ${CMAKE_SOURCE_DIR}/common/include @@ -43,6 +43,6 @@ target_include_directories( ${CMAKE_SOURCE_DIR}/common/libs/pid ${CMAKE_SOURCE_DIR}/common/libs/serial ${CMAKE_SOURCE_DIR}/common/libs/can - ${CMAKE_SOURCE_DIR}/common/libs/time ${CMAKE_SOURCE_DIR}/common/libs/dac - ${CMAKE_SOURCE_DIR}/common/libs/signal_smoothing) + ${CMAKE_SOURCE_DIR}/common/libs/timer + ${CMAKE_SOURCE_DIR}/../api/include) diff --git a/firmware/steering/include/communications.h b/firmware/steering/include/communications.h new file mode 100644 index 00000000..14192aa9 --- /dev/null +++ b/firmware/steering/include/communications.h @@ -0,0 +1,65 @@ +/** + * @file communications.h + * @brief Communication functionality. + * + */ + + +#ifndef _OSCC_KIA_SOUL_STEERING_COMMUNICATIONS_H_ +#define _OSCC_KIA_SOUL_STEERING_COMMUNICATIONS_H_ + + +// **************************************************************************** +// Function: publish_steering_report +// +// Purpose: Publish steering report to CAN bus. +// +// Returns: void +// +// Parameters: void +// +// **************************************************************************** +void publish_steering_report( void ); + + +// **************************************************************************** +// Function: publish_fault_report +// +// Purpose: Publish a fault report message to the CAN bus. +// +// Returns: void +// +// Parameters: void +// +// **************************************************************************** +void publish_fault_report( void ); + + +// **************************************************************************** +// Function: check_for_controller_command_timeout +// +// Purpose: Check if the last command received from the controller exceeds +// the timeout and disable control if it does. +// +// Returns: void +// +// Parameters: void +// +// **************************************************************************** +void check_for_controller_command_timeout( void ); + + +// **************************************************************************** +// Function: check_for_incoming_message +// +// Purpose: Check CAN bus for incoming frames and process any present. +// +// Returns: void +// +// Parameters: void +// +// **************************************************************************** +void check_for_incoming_message( void ); + + +#endif /* _OSCC_KIA_SOUL_STEERING_COMMUNICATIONS_H_ */ diff --git a/platforms/kia_soul/firmware/steering/include/globals.h b/firmware/steering/include/globals.h similarity index 62% rename from platforms/kia_soul/firmware/steering/include/globals.h rename to firmware/steering/include/globals.h index 72d45b97..2a9cc49b 100644 --- a/platforms/kia_soul/firmware/steering/include/globals.h +++ b/firmware/steering/include/globals.h @@ -9,10 +9,8 @@ #define _KIA_SOUL_STEERING_GLOBALS_H_ -#include #include "DAC_MCP49xx.h" #include "mcp_can.h" -#include "oscc_pid.h" #include "steering_control.h" @@ -59,19 +57,6 @@ */ #define PIN_SPOOF_ENABLE ( 6 ) -/* - * @brief Windup guard of the PID controller. - * - */ -#define PID_WINDUP_GUARD ( 1500 ) - -/* - * - * @brief Time between steering control updates (operator override checks and - * steering output updates). - */ - #define CONTROL_LOOP_INTERVAL_IN_MSEC ( 50 ) - #ifdef GLOBAL_DEFINED DAC_MCP49xx g_dac( DAC_MCP49xx::MCP4922, PIN_DAC_CHIP_SELECT ); @@ -86,16 +71,9 @@ #endif -EXTERN uint32_t g_steering_command_last_rx_timestamp; -EXTERN uint32_t g_steering_report_last_tx_timestamp; -EXTERN uint32_t g_chassis_state_1_report_last_rx_timestamp; -EXTERN uint32_t g_sensor_validity_last_check_timestamp; -EXTERN uint32_t g_last_control_loop_timestamp; - -EXTERN kia_soul_steering_control_state_s g_steering_control_state; +EXTERN volatile bool g_steering_command_timeout; -EXTERN pid_s g_pid; -EXTERN uint16_t g_spoofed_torque_output_sum; +EXTERN volatile steering_control_state_s g_steering_control_state; #endif diff --git a/platforms/kia_soul/firmware/steering/include/init.h b/firmware/steering/include/init.h similarity index 100% rename from platforms/kia_soul/firmware/steering/include/init.h rename to firmware/steering/include/init.h diff --git a/firmware/steering/include/steering_control.h b/firmware/steering/include/steering_control.h new file mode 100644 index 00000000..b01296e9 --- /dev/null +++ b/firmware/steering/include/steering_control.h @@ -0,0 +1,115 @@ +/** + * @file steering_control.h + * @brief Control of the steering system. + * + */ + + +#ifndef _OSCC_KIA_SOUL_STEERING_CONTROL_H_ +#define _OSCC_KIA_SOUL_STEERING_CONTROL_H_ + + +#include + + +/** + * @brief Torque values. + * + * Contains the high and low torque values. + * + */ +typedef struct +{ + uint16_t low; /* Low value of torque. */ + + uint16_t high; /* High value of torque. */ +} steering_torque_s; + + +/** + * @brief Current steering control state. + * + * Current state of the throttle module control system. + * + */ +typedef struct +{ + bool enabled; /* Flag indicating control is currently enabled. */ + + bool operator_override; /* Flag indicating whether steering wheel was + manually turned by operator. */ + + uint8_t dtcs; /* Bitfield of faults present in the module. */ +} steering_control_state_s; + + +// **************************************************************************** +// Function: check_for_operator_override +// +// Purpose: Checks to see if the vehicle's operator has manually pressed +// the accelerator and disables control if they have. +// +// Returns: void +// +// Parameters: void +// +// **************************************************************************** +void check_for_operator_override( void ); + + +// **************************************************************************** +// Function: check_for_sensor_faults +// +// Purpose: Checks to see if valid values are being read from the sensors. +// +// Returns: void +// +// Parameters: void +// +// **************************************************************************** +void check_for_sensor_faults( void ); + + +// **************************************************************************** +// Function: update_steering +// +// Purpose: Writes steering spoof values to DAC. +// +// Returns: void +// +// Parameters: spoof_command_high - high value of spoof command +// spoof_command_low - low value of spoof command +// +// **************************************************************************** +void update_steering( + uint16_t spoof_command_high, + uint16_t spoof_command_low ); + + +// ***************************************************** +// Function: enable_control +// +// Purpose: Enable control of the steering system. +// +// Returns: void +// +// Parameters: void +// +// ***************************************************** +void enable_control( void ); + + +// ***************************************************** +// Function: disable_control +// +// Purpose: Disable control of the steering system. +// +// Returns: void +// +// Parameters: void +// +// ***************************************************** +void disable_control( void ); + + +#endif /* _OSCC_KIA_SOUL_STEERING_CONTROL_H_ */ diff --git a/firmware/steering/include/timers.h b/firmware/steering/include/timers.h new file mode 100644 index 00000000..a1a118c9 --- /dev/null +++ b/firmware/steering/include/timers.h @@ -0,0 +1,25 @@ +/** + * @file timers.h + * @brief Timer functionality. + * + */ + + +#ifndef _OSCC_KIA_SOUL_STEERING_TIMERS_H_ +#define _OSCC_KIA_SOUL_STEERING_TIMERS_H_ + + +// **************************************************************************** +// Function: start_timers +// +// Purpose: Start timers for report publishing and fault checking. +// +// Returns: void +// +// Parameters: void +// +// **************************************************************************** +void start_timers( void ); + + +#endif /* _OSCC_KIA_SOUL_TH_OSCC_KIA_SOUL_STEERING_TIMERS_H_ROTTLE_TIMERS_H_ */ diff --git a/firmware/steering/src/communications.cpp b/firmware/steering/src/communications.cpp new file mode 100644 index 00000000..6b2d81b9 --- /dev/null +++ b/firmware/steering/src/communications.cpp @@ -0,0 +1,153 @@ +/** + * @file communications.cpp + * + */ + + +#include "mcp_can.h" +#include "oscc_can.h" +#include "can_protocols/fault_can_protocol.h" +#include "can_protocols/steering_can_protocol.h" +#include "debug.h" + +#include "globals.h" +#include "communications.h" +#include "steering_control.h" + + +static void process_rx_frame( + const can_frame_s * const frame ); + +static void process_steering_command( + const uint8_t * const data ); + +static void process_fault_report( + const uint8_t * const data ); + + +void publish_steering_report( void ) +{ + oscc_steering_report_s steering_report; + + steering_report.magic[0] = (uint8_t) OSCC_MAGIC_BYTE_0; + steering_report.magic[1] = (uint8_t) OSCC_MAGIC_BYTE_1; + steering_report.enabled = (uint8_t) g_steering_control_state.enabled; + steering_report.operator_override = (uint8_t) g_steering_control_state.operator_override; + steering_report.dtcs = g_steering_control_state.dtcs; + + cli(); + g_control_can.sendMsgBuf( + OSCC_STEERING_REPORT_CAN_ID, + CAN_STANDARD, + OSCC_STEERING_REPORT_CAN_DLC, + (uint8_t *) &steering_report ); + sei(); +} + + +void publish_fault_report( void ) +{ + oscc_fault_report_s fault_report; + + fault_report.magic[0] = (uint8_t) OSCC_MAGIC_BYTE_0; + fault_report.magic[1] = (uint8_t) OSCC_MAGIC_BYTE_1; + fault_report.fault_origin_id = FAULT_ORIGIN_STEERING; + + cli(); + g_control_can.sendMsgBuf( + OSCC_FAULT_REPORT_CAN_ID, + CAN_STANDARD, + OSCC_FAULT_REPORT_CAN_DLC, + (uint8_t *) &fault_report ); + sei(); +} + + +void check_for_controller_command_timeout( void ) +{ + if( g_steering_control_state.enabled == true ) + { + if( g_steering_command_timeout == true ) + { + disable_control( ); + + publish_fault_report( ); + + DEBUG_PRINTLN( "Timeout - controller command" ); + } + } +} + + +void check_for_incoming_message( void ) +{ + can_frame_s rx_frame; + can_status_t ret = check_for_rx_frame( g_control_can, &rx_frame ); + + if( ret == CAN_RX_FRAME_AVAILABLE ) + { + process_rx_frame( &rx_frame ); + } +} + + +static void process_rx_frame( + const can_frame_s * const frame ) +{ + if ( frame != NULL ) + { + if( (frame->data[0] == OSCC_MAGIC_BYTE_0) + && (frame->data[1] == OSCC_MAGIC_BYTE_1) ) + { + if ( frame->id == OSCC_STEERING_COMMAND_CAN_ID ) + { + process_steering_command( frame->data ); + } + else if ( frame->id == OSCC_FAULT_REPORT_CAN_ID ) + { + process_fault_report( frame->data ); + } + } + } +} + + +static void process_steering_command( + const uint8_t * const data ) +{ + if ( data != NULL ) + { + const oscc_steering_command_s * const steering_command = + (oscc_steering_command_s *) data; + + if ( steering_command->enable == true ) + { + enable_control( ); + + update_steering( + steering_command->spoof_value_high, + steering_command->spoof_value_low ); + } + else + { + disable_control( ); + } + + g_steering_command_timeout = false; + } +} + + +static void process_fault_report( + const uint8_t * const data ) +{ + if ( data != NULL ) + { + const oscc_fault_report_s * const fault_report = + (oscc_fault_report_s *) data; + + disable_control( ); + + DEBUG_PRINTLN( "Fault report received" ); + } +} diff --git a/platforms/kia_soul/firmware/steering/src/globals.cpp b/firmware/steering/src/globals.cpp similarity index 100% rename from platforms/kia_soul/firmware/steering/src/globals.cpp rename to firmware/steering/src/globals.cpp diff --git a/platforms/kia_soul/firmware/steering/src/init.cpp b/firmware/steering/src/init.cpp similarity index 53% rename from platforms/kia_soul/firmware/steering/src/init.cpp rename to firmware/steering/src/init.cpp index 06785cce..43208982 100644 --- a/platforms/kia_soul/firmware/steering/src/init.cpp +++ b/firmware/steering/src/init.cpp @@ -7,30 +7,21 @@ #include #include "oscc_serial.h" #include "oscc_can.h" +#include "can_protocols/steering_can_protocol.h" #include "debug.h" -#include "oscc_time.h" -#include "oscc_pid.h" -#include "init.h" #include "globals.h" +#include "communications.h" +#include "init.h" void init_globals( void ) { - memset( &g_steering_control_state, - 0, - sizeof(g_steering_control_state) ); - - // Initialize the timestamps to avoid timeout warnings on start up - g_steering_command_last_rx_timestamp = GET_TIMESTAMP_MS( ); - g_steering_report_last_tx_timestamp = GET_TIMESTAMP_MS( ); - g_chassis_state_1_report_last_rx_timestamp = GET_TIMESTAMP_MS( ); - g_last_control_loop_timestamp = 0; - - pid_zeroize( &g_pid, PID_WINDUP_GUARD ); - g_pid.proportional_gain = PID_PROPORTIONAL_GAIN; - g_pid.integral_gain = PID_INTEGRAL_GAIN; - g_pid.derivative_gain = PID_DERIVATIVE_GAIN; + g_steering_control_state.enabled = false; + g_steering_control_state.operator_override = false; + g_steering_control_state.dtcs = 0; + + g_steering_command_timeout = false; } @@ -43,9 +34,10 @@ void init_devices( void ) pinMode( PIN_TORQUE_SPOOF_LOW, INPUT ); pinMode( PIN_SPOOF_ENABLE, OUTPUT ); + cli(); digitalWrite( PIN_DAC_CHIP_SELECT, HIGH ); - digitalWrite( PIN_SPOOF_ENABLE, LOW ); + sei(); } diff --git a/firmware/steering/src/main.cpp b/firmware/steering/src/main.cpp new file mode 100644 index 00000000..e771bba8 --- /dev/null +++ b/firmware/steering/src/main.cpp @@ -0,0 +1,36 @@ +/** + * @file main.cpp + * + */ + + +#include "arduino_init.h" +#include "debug.h" + +#include "init.h" +#include "timers.h" +#include "communications.h" +#include "steering_control.h" + + +int main( void ) +{ + init_arduino( ); + + init_globals( ); + + init_devices( ); + + init_communication_interfaces( ); + + start_timers( ); + + DEBUG_PRINTLN( "init complete" ); + + while( true ) + { + check_for_incoming_message( ); + + check_for_operator_override( ); + } +} diff --git a/firmware/steering/src/steering_control.cpp b/firmware/steering/src/steering_control.cpp new file mode 100644 index 00000000..dc88e6d5 --- /dev/null +++ b/firmware/steering/src/steering_control.cpp @@ -0,0 +1,181 @@ +/** + * @file steering_control.cpp + * + */ + + +#include +#include +#include +#include "debug.h" +#include "oscc_dac.h" +#include "can_protocols/steering_can_protocol.h" +#include "dtc.h" +#include "vehicles.h" + +#include "communications.h" +#include "steering_control.h" +#include "globals.h" + + +/* + * @brief Number of consecutive faults that can occur when reading the + * torque sensor before control is disabled. + * + */ +#define SENSOR_VALIDITY_CHECK_FAULT_COUNT ( 4 ) + + +static void read_torque_sensor( + steering_torque_s * value ); + + +void check_for_operator_override( void ) +{ + if( g_steering_control_state.enabled == true + || g_steering_control_state.operator_override == true ) + { + steering_torque_s torque; + + read_torque_sensor( &torque ); + + if ( (abs(torque.high) >= OVERRIDE_WHEEL_THRESHOLD_IN_DEGREES_PER_USEC) + || (abs(torque.low) >= OVERRIDE_WHEEL_THRESHOLD_IN_DEGREES_PER_USEC) ) + { + disable_control( ); + + publish_fault_report( ); + + g_steering_control_state.operator_override = true; + + DEBUG_PRINTLN( "Operator override" ); + } + else + { + g_steering_control_state.operator_override = false; + } + } +} + + +void check_for_sensor_faults( void ) +{ + if ( (g_steering_control_state.enabled == true) + || DTC_CHECK(g_steering_control_state.dtcs, OSCC_STEERING_DTC_INVALID_SENSOR_VAL) ) + { + static int fault_count = 0; + + steering_torque_s torque; + + read_torque_sensor(&torque); + + // sensor pins tied to ground - a value of zero indicates disconnection + if( (torque.high == 0) + || (torque.low == 0) ) + { + ++fault_count; + + if( fault_count >= SENSOR_VALIDITY_CHECK_FAULT_COUNT ) + { + disable_control( ); + + publish_fault_report( ); + + DTC_SET( + g_steering_control_state.dtcs, + OSCC_STEERING_DTC_INVALID_SENSOR_VAL ); + + DEBUG_PRINTLN( "Bad value read from torque sensor" ); + } + } + else + { + DTC_CLEAR( + g_steering_control_state.dtcs, + OSCC_STEERING_DTC_INVALID_SENSOR_VAL ); + + fault_count = 0; + } + } +} + + +void update_steering( + uint16_t spoof_command_high, + uint16_t spoof_command_low ) +{ + if ( g_steering_control_state.enabled == true ) + { + uint16_t spoof_high = + constrain( + spoof_command_high, + STEERING_SPOOF_SIGNAL_MIN, + STEERING_SPOOF_SIGNAL_MAX ); + + uint16_t spoof_low = + constrain( + spoof_command_low, + STEERING_SPOOF_SIGNAL_MIN, + STEERING_SPOOF_SIGNAL_MAX ); + + cli(); + g_dac.outputA( spoof_high ); + g_dac.outputB( spoof_low ); + sei(); + } +} + + +void enable_control( void ) +{ + if( g_steering_control_state.enabled == false + && g_steering_control_state.operator_override == false ) + { + const uint16_t num_samples = 20; + prevent_signal_discontinuity( + g_dac, + num_samples, + PIN_TORQUE_SENSOR_HIGH, + PIN_TORQUE_SENSOR_LOW ); + + cli(); + digitalWrite( PIN_SPOOF_ENABLE, HIGH ); + sei(); + + g_steering_control_state.enabled = true; + + DEBUG_PRINTLN( "Control enabled" ); + } +} + + +void disable_control( void ) +{ + if( g_steering_control_state.enabled == true ) + { + const uint16_t num_samples = 20; + prevent_signal_discontinuity( + g_dac, + num_samples, + PIN_TORQUE_SENSOR_HIGH, + PIN_TORQUE_SENSOR_LOW ); + + cli(); + digitalWrite( PIN_SPOOF_ENABLE, LOW ); + sei(); + + g_steering_control_state.enabled = false; + + DEBUG_PRINTLN( "Control disabled" ); + } +} + +static void read_torque_sensor( + steering_torque_s * value ) +{ + cli(); + value->high = analogRead( PIN_TORQUE_SENSOR_HIGH ); + value->low = analogRead( PIN_TORQUE_SENSOR_LOW ); + sei(); +} + diff --git a/firmware/steering/src/timers.cpp b/firmware/steering/src/timers.cpp new file mode 100644 index 00000000..8a3b5434 --- /dev/null +++ b/firmware/steering/src/timers.cpp @@ -0,0 +1,44 @@ +/** + * @file timers.cpp + * + */ + + +#include "can_protocols/steering_can_protocol.h" +#include "oscc_timer.h" + +#include "timers.h" +#include "globals.h" +#include "communications.h" +#include "steering_control.h" + + +/* + * @brief Fault checking frequency. [Hz] + * + */ +#define FAULT_CHECK_FREQUENCY_IN_HZ ( 5 ) + + +static void check_for_faults( void ); + + +void start_timers( void ) +{ + timer1_init( FAULT_CHECK_FREQUENCY_IN_HZ, check_for_faults ); + timer2_init( OSCC_REPORT_STEERING_PUBLISH_FREQ_IN_HZ, publish_steering_report ); +} + + +static void check_for_faults( void ) +{ + cli(); + + check_for_controller_command_timeout( ); + + check_for_sensor_faults( ); + + g_steering_command_timeout = true; + + sei(); +} diff --git a/platforms/kia_soul/firmware/steering/tests/CMakeLists.txt b/firmware/steering/tests/CMakeLists.txt similarity index 67% rename from platforms/kia_soul/firmware/steering/tests/CMakeLists.txt rename to firmware/steering/tests/CMakeLists.txt index 86760ebf..c88d601d 100644 --- a/platforms/kia_soul/firmware/steering/tests/CMakeLists.txt +++ b/firmware/steering/tests/CMakeLists.txt @@ -1,46 +1,44 @@ -project(kia-soul-steering-unit-tests) +project(steering-unit-tests) add_library( - kia-soul-steering + steering SHARED ../src/communications.cpp ../src/globals.cpp ../src/steering_control.cpp - ${CMAKE_SOURCE_DIR}/common/libs/time/oscc_time.cpp ${CMAKE_SOURCE_DIR}/common/libs/can/oscc_can.cpp ${CMAKE_SOURCE_DIR}/common/libs/dac/oscc_dac.cpp ${CMAKE_SOURCE_DIR}/common/libs/pid/oscc_pid.cpp - ${CMAKE_SOURCE_DIR}/common/libs/signal_smoothing/oscc_signal_smoothing.cpp ${CMAKE_SOURCE_DIR}/common/testing/mocks/ ${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) target_include_directories( - kia-soul-steering + steering PRIVATE ../include ${CMAKE_SOURCE_DIR}/common/include ${CMAKE_SOURCE_DIR}/common/libs/can - ${CMAKE_SOURCE_DIR}/common/libs/time ${CMAKE_SOURCE_DIR}/common/libs/dac ${CMAKE_SOURCE_DIR}/common/libs/pid ${CMAKE_SOURCE_DIR}/common/libs/signal_smoothing - ${CMAKE_SOURCE_DIR}/common/testing/mocks) + ${CMAKE_SOURCE_DIR}/common/testing/mocks + ${CMAKE_SOURCE_DIR}/../api/include) add_executable( - kia-soul-steering-unit-test + steering-unit-test features/step_definitions/test.cpp) target_link_libraries( - kia-soul-steering-unit-test + steering-unit-test PRIVATE - kia-soul-steering + steering ${CMAKE_SOURCE_DIR}/common/testing/framework/cucumber-cpp/lib/libcucumber-cpp.a ${CMAKE_SOURCE_DIR}/common/testing/framework/cgreen/lib/libcgreen.so) target_include_directories( - kia-soul-steering-unit-test + steering-unit-test PRIVATE ../include ${CMAKE_SOURCE_DIR}/common/include @@ -48,17 +46,18 @@ target_include_directories( ${CMAKE_SOURCE_DIR}/common/libs/pid ${CMAKE_SOURCE_DIR}/common/testing/mocks ${CMAKE_SOURCE_DIR}/common/testing/framework/cucumber-cpp/include - ${CMAKE_SOURCE_DIR}/common/testing/framework/cgreen/include) + ${CMAKE_SOURCE_DIR}/common/testing/framework/cgreen/include + ${CMAKE_SOURCE_DIR}/../api/include) add_custom_target( - run-kia-soul-steering-unit-tests + run-steering-unit-tests DEPENDS - kia-soul-steering-unit-test + steering-unit-test COMMAND - kia-soul-steering-unit-test --port=3902 >/dev/null & cucumber _2.0.0_ ${CMAKE_CURRENT_SOURCE_DIR}/features ) + steering-unit-test --port=3902 >/dev/null & cucumber _2.0.0_ ${CMAKE_CURRENT_SOURCE_DIR}/features ) add_custom_target( - run-kia-soul-steering-property-tests + run-steering-property-tests COMMAND cargo test -- --test-threads=1 WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/property) diff --git a/platforms/kia_soul/firmware/steering/tests/features/timeouts_and_overrides.feature b/firmware/steering/tests/features/checking_faults.feature similarity index 51% rename from platforms/kia_soul/firmware/steering/tests/features/timeouts_and_overrides.feature rename to firmware/steering/tests/features/checking_faults.feature index cf5455c9..2b3e817a 100644 --- a/platforms/kia_soul/firmware/steering/tests/features/timeouts_and_overrides.feature +++ b/firmware/steering/tests/features/checking_faults.feature @@ -1,26 +1,34 @@ # language: en -Feature: Timeouts and overrides +Feature: Checking for fauls - If the module doesn't hear from the controller after an amount of time, - or the operator manually actuates the steering wheel, control should be - disabled. + If the module encounters a fault condition, it should disable control and + publish a fault report. + Scenario: A sensor becomes temporarily disconnected + Given steering control is enabled - Scenario: Controller command timeout + When a sensor becomes temporarily disconnected + + Then control should remain enabled + + + Scenario: A sensor becomes permanently disconnected Given steering control is enabled - When the time since the last received controller command exceeds the timeout + When a sensor becomes permanently disconnected Then control should be disabled + And a fault report should be published - Scenario: Chassis State 1 report timeout + Scenario: Controller command timeout Given steering control is enabled - When the time since the last received Chassis State 1 report exceeds the timeout + When the time since the last received controller command exceeds the timeout Then control should be disabled + And a fault report should be published Scenario Outline: Operator override @@ -29,7 +37,7 @@ Feature: Timeouts and overrides When the operator applies to the steering wheel Then control should be disabled - And override flag should be set + And a fault report should be published Examples: | sensor_val | diff --git a/firmware/steering/tests/features/receiving_messages.feature b/firmware/steering/tests/features/receiving_messages.feature new file mode 100644 index 00000000..6eca1487 --- /dev/null +++ b/firmware/steering/tests/features/receiving_messages.feature @@ -0,0 +1,64 @@ +# language: en + +Feature: Receiving commands + + Commands received from a application should be processed and acted upon. + + + Scenario: Enable steering command sent from application + Given steering control is disabled + + When an enable steering command is received + + Then control should be enabled + + + Scenario: Disable steering command sent from application + Given steering control is enabled + + When a disable steering command is received + + Then control should be disabled + + + Scenario: Fault report sent from a different module + Given steering control is enabled + + When a fault report is received + + Then control should be disabled + + + Scenario Outline: Spoof value sent from application + Given steering control is enabled + + When a command is received with spoof values and + + Then should be sent to DAC A + And should be sent to DAC B + + Examples: + | high | low | + | 3031 | 868 | + | 3000 | 1000 | + | 2500 | 1500 | + | 2000 | 2000 | + | 1500 | 2500 | + | 1000 | 3000 | + | 868 | 3031 | + + + Scenario Outline: Spoof value sent from application outside valid range + Given steering control is enabled + + When a command is received with spoof values and + + Then should be sent to DAC A + And should be sent to DAC B + + Examples: + | high | low | high_clamped | low_clamped | + | 4000 | 0 | 3031 | 868 | + | 3500 | 500 | 3031 | 868 | + | 500 | 3500 | 868 | 3031 | + | 0 | 4000 | 868 | 3031 | diff --git a/firmware/steering/tests/features/sending_reports.feature b/firmware/steering/tests/features/sending_reports.feature new file mode 100644 index 00000000..ff99e248 --- /dev/null +++ b/firmware/steering/tests/features/sending_reports.feature @@ -0,0 +1,12 @@ +# language: en + +Feature: Sending reports + + Steering reports should be published to the control CAN bus. + + + Scenario: Steering report published + When a steering report is published + + Then a steering report should be put on the control CAN bus + And the steering report's fields should be set diff --git a/firmware/steering/tests/features/step_definitions/checking_faults.cpp b/firmware/steering/tests/features/step_definitions/checking_faults.cpp new file mode 100644 index 00000000..811c6a61 --- /dev/null +++ b/firmware/steering/tests/features/step_definitions/checking_faults.cpp @@ -0,0 +1,71 @@ +WHEN("^a sensor becomes temporarily disconnected$") +{ + g_mock_arduino_analog_read_return = 0; + + check_for_sensor_faults(); + + check_for_sensor_faults(); + + check_for_sensor_faults(); +} + + +WHEN("^a sensor becomes permanently disconnected$") +{ + g_mock_arduino_analog_read_return = 0; + + // must call function enough times to exceed the fault limit + for( int i = 0; i < 100; ++i ) + { + check_for_sensor_faults(); + } +} + + +WHEN("^the time since the last received controller command exceeds the timeout$") +{ + g_steering_command_timeout = true; + + check_for_controller_command_timeout(); +} + + +WHEN("^the operator applies (.*) to the steering wheel$") +{ + REGEX_PARAM(int, steering_sensor_val); + + g_mock_arduino_analog_read_return = steering_sensor_val; + + check_for_operator_override(); +} + + +THEN("^control should remain enabled") +{ + assert_that( + g_steering_control_state.enabled, + is_equal_to(1)); +} + + +THEN("^a fault report should be published$") +{ + assert_that(g_mock_mcp_can_send_msg_buf_id, is_equal_to(OSCC_FAULT_REPORT_CAN_ID)); + assert_that(g_mock_mcp_can_send_msg_buf_ext, is_equal_to(CAN_STANDARD)); + assert_that(g_mock_mcp_can_send_msg_buf_len, is_equal_to(OSCC_FAULT_REPORT_CAN_DLC)); + + oscc_fault_report_s * fault_report = + (oscc_fault_report_s *) g_mock_mcp_can_send_msg_buf_buf; + + assert_that( + fault_report->magic[0], + is_equal_to(OSCC_MAGIC_BYTE_0)); + + assert_that( + fault_report->magic[1], + is_equal_to(OSCC_MAGIC_BYTE_1)); + + assert_that( + fault_report->fault_origin_id, + is_equal_to(FAULT_ORIGIN_STEERING)); +} diff --git a/platforms/kia_soul/firmware/steering/tests/features/step_definitions/common.cpp b/firmware/steering/tests/features/step_definitions/common.cpp similarity index 60% rename from platforms/kia_soul/firmware/steering/tests/features/step_definitions/common.cpp rename to firmware/steering/tests/features/step_definitions/common.cpp index a3c4877b..eb49888c 100644 --- a/platforms/kia_soul/firmware/steering/tests/features/step_definitions/common.cpp +++ b/firmware/steering/tests/features/step_definitions/common.cpp @@ -9,58 +9,54 @@ #include "oscc_can.h" #include "mcp_can.h" #include "steering_control.h" -#include "steering_can_protocol.h" -#include "chassis_state_can_protocol.h" +#include "can_protocols/fault_can_protocol.h" +#include "can_protocols/steering_can_protocol.h" #include "globals.h" using namespace cgreen; -extern unsigned long g_mock_arduino_millis_return; extern uint8_t g_mock_arduino_digital_write_pin; extern uint8_t g_mock_arduino_digital_write_val; -extern int g_mock_arduino_analog_read_return; - -extern INT8U g_mock_mcp_can_check_receive_return; -extern INT32U g_mock_mcp_can_read_msg_buf_id; -extern INT8U g_mock_mcp_can_read_msg_buf_buf[8]; +extern int g_mock_arduino_analog_read_return; -extern INT32U g_mock_mcp_can_send_msg_buf_id; -extern INT8U g_mock_mcp_can_send_msg_buf_ext; -extern INT8U g_mock_mcp_can_send_msg_buf_len; -extern INT8U *g_mock_mcp_can_send_msg_buf_buf; +extern uint8_t g_mock_mcp_can_check_receive_return; +extern uint32_t g_mock_mcp_can_read_msg_buf_id; +extern uint8_t g_mock_mcp_can_read_msg_buf_buf[8]; +extern uint32_t g_mock_mcp_can_send_msg_buf_id; +extern uint8_t g_mock_mcp_can_send_msg_buf_ext; +extern uint8_t g_mock_mcp_can_send_msg_buf_len; +extern uint8_t *g_mock_mcp_can_send_msg_buf_buf; extern unsigned short g_mock_dac_output_a; extern unsigned short g_mock_dac_output_b; -extern kia_soul_steering_control_state_s g_steering_control_state; +extern volatile steering_control_state_s g_steering_control_state; // return to known state before every scenario BEFORE() { - g_mock_mcp_can_check_receive_return = -1; - g_mock_mcp_can_read_msg_buf_id = 0; - g_mock_arduino_millis_return = 555; - - memset( - &g_mock_mcp_can_read_msg_buf_buf, - 0, - sizeof(g_mock_mcp_can_read_msg_buf_buf)); - - // Do not clear previous_steering_wheel_angle because it is needed for - // scenario about receiving a command - g_steering_control_state.enabled = 0; - g_steering_control_state.operator_override = 0; - g_steering_control_state.current_steering_wheel_angle = 0; - g_steering_control_state.commanded_steering_wheel_angle = 0; - g_mock_arduino_digital_write_pin = UINT8_MAX; g_mock_arduino_digital_write_val = UINT8_MAX; + g_mock_arduino_analog_read_return = INT_MAX; + + g_mock_mcp_can_check_receive_return = UINT8_MAX; + g_mock_mcp_can_read_msg_buf_id = UINT32_MAX; + memset(&g_mock_mcp_can_read_msg_buf_buf, 0, sizeof(g_mock_mcp_can_read_msg_buf_buf)); + + g_mock_mcp_can_send_msg_buf_id = UINT32_MAX; + g_mock_mcp_can_send_msg_buf_ext = UINT8_MAX; + g_mock_mcp_can_send_msg_buf_len = UINT8_MAX; + g_mock_dac_output_a = USHRT_MAX; g_mock_dac_output_b = USHRT_MAX; + + g_steering_control_state.enabled = false; + g_steering_control_state.operator_override = false; + g_steering_control_state.dtcs = 0; } @@ -84,11 +80,13 @@ GIVEN("^the torque sensors have a reading of (.*)$") } -GIVEN("^the previous steering wheel angle command was (.*)$") +GIVEN("^the operator has applied (.*) to the steering wheel$") { - REGEX_PARAM(int, command); + REGEX_PARAM(int, steering_sensor_val); + + g_mock_arduino_analog_read_return = steering_sensor_val; - g_steering_control_state.commanded_steering_wheel_angle = command; + check_for_operator_override(); } @@ -124,7 +122,7 @@ THEN("^control should be disabled$") } -THEN("^(.*) should be written to DAC A$") +THEN("^(.*) should be sent to DAC A$") { REGEX_PARAM(int, dac_output_a); @@ -134,7 +132,7 @@ THEN("^(.*) should be written to DAC A$") } -THEN("^(.*) should be written to DAC B$") +THEN("^(.*) should be sent to DAC B$") { REGEX_PARAM(int, dac_output_b); diff --git a/platforms/kia_soul/firmware/steering/tests/features/step_definitions/cucumber.wire b/firmware/steering/tests/features/step_definitions/cucumber.wire similarity index 100% rename from platforms/kia_soul/firmware/steering/tests/features/step_definitions/cucumber.wire rename to firmware/steering/tests/features/step_definitions/cucumber.wire diff --git a/firmware/steering/tests/features/step_definitions/receiving_messages.cpp b/firmware/steering/tests/features/step_definitions/receiving_messages.cpp new file mode 100644 index 00000000..e4b6bcf5 --- /dev/null +++ b/firmware/steering/tests/features/step_definitions/receiving_messages.cpp @@ -0,0 +1,71 @@ +WHEN("^an enable steering command is received$") +{ + g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL; + g_mock_mcp_can_read_msg_buf_id = OSCC_STEERING_COMMAND_CAN_ID; + + oscc_steering_command_s * steering_command = + (oscc_steering_command_s *) g_mock_mcp_can_read_msg_buf_buf; + + steering_command->magic[0] = OSCC_MAGIC_BYTE_0; + steering_command->magic[1] = OSCC_MAGIC_BYTE_1; + steering_command->enable = 1; + + check_for_incoming_message(); +} + + +WHEN("^a disable steering command is received$") +{ + g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL; + g_mock_mcp_can_read_msg_buf_id = OSCC_STEERING_COMMAND_CAN_ID; + + oscc_steering_command_s * steering_command = + (oscc_steering_command_s *) g_mock_mcp_can_read_msg_buf_buf; + + steering_command->magic[0] = OSCC_MAGIC_BYTE_0; + steering_command->magic[1] = OSCC_MAGIC_BYTE_1; + steering_command->enable = 0; + + check_for_incoming_message(); +} + + +WHEN("^a fault report is received$") +{ + g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL; + g_mock_mcp_can_read_msg_buf_id = OSCC_FAULT_REPORT_CAN_ID; + + oscc_fault_report_s * fault_report = + (oscc_fault_report_s *) g_mock_mcp_can_read_msg_buf_buf; + + fault_report->magic[0] = OSCC_MAGIC_BYTE_0; + fault_report->magic[1] = OSCC_MAGIC_BYTE_1; + + check_for_incoming_message(); +} + + +WHEN("^a command is received with spoof values (.*) and (.*)$") +{ + REGEX_PARAM(uint16_t, high); + REGEX_PARAM(uint16_t, low); + + g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL; + g_mock_mcp_can_read_msg_buf_id = OSCC_STEERING_COMMAND_CAN_ID; + + + oscc_steering_command_s * steering_command = + (oscc_steering_command_s *) g_mock_mcp_can_read_msg_buf_buf; + + steering_command->magic[0] = OSCC_MAGIC_BYTE_0; + steering_command->magic[1] = OSCC_MAGIC_BYTE_1; + steering_command->enable = 1; + steering_command->spoof_value_high = high; + steering_command->spoof_value_low = low; + + check_for_incoming_message(); + + update_steering( + steering_command->spoof_value_high, + steering_command->spoof_value_low); +} diff --git a/firmware/steering/tests/features/step_definitions/sending_reports.cpp b/firmware/steering/tests/features/step_definitions/sending_reports.cpp new file mode 100644 index 00000000..4ff5ea88 --- /dev/null +++ b/firmware/steering/tests/features/step_definitions/sending_reports.cpp @@ -0,0 +1,43 @@ +WHEN("^a steering report is published$") +{ + g_steering_control_state.enabled = true; + g_steering_control_state.operator_override = true; + g_steering_control_state.dtcs = 0x55; + + publish_steering_report(); +} + + +THEN("^a steering report should be put on the control CAN bus$") +{ + assert_that(g_mock_mcp_can_send_msg_buf_id, is_equal_to(OSCC_STEERING_REPORT_CAN_ID)); + assert_that(g_mock_mcp_can_send_msg_buf_ext, is_equal_to(CAN_STANDARD)); + assert_that(g_mock_mcp_can_send_msg_buf_len, is_equal_to(OSCC_STEERING_REPORT_CAN_DLC)); +} + + +THEN("^the steering report's fields should be set$") +{ + oscc_steering_report_s * steering_report = + (oscc_steering_report_s *) g_mock_mcp_can_send_msg_buf_buf; + + assert_that( + steering_report->magic[0], + is_equal_to(OSCC_MAGIC_BYTE_0)); + + assert_that( + steering_report->magic[1], + is_equal_to(OSCC_MAGIC_BYTE_1)); + + assert_that( + steering_report->enabled, + is_equal_to(g_steering_control_state.enabled)); + + assert_that( + steering_report->operator_override, + is_equal_to(g_steering_control_state.operator_override)); + + assert_that( + steering_report->dtcs, + is_equal_to(g_steering_control_state.dtcs)); +} diff --git a/firmware/steering/tests/features/step_definitions/test.cpp b/firmware/steering/tests/features/step_definitions/test.cpp new file mode 100644 index 00000000..5f7d398e --- /dev/null +++ b/firmware/steering/tests/features/step_definitions/test.cpp @@ -0,0 +1,5 @@ +// include source files to prevent step files from conflicting with each other +#include "common.cpp" +#include "checking_faults.cpp" +#include "receiving_messages.cpp" +#include "sending_reports.cpp" diff --git a/platforms/kia_soul/firmware/steering/tests/features/support/env.rb b/firmware/steering/tests/features/support/env.rb similarity index 100% rename from platforms/kia_soul/firmware/steering/tests/features/support/env.rb rename to firmware/steering/tests/features/support/env.rb diff --git a/platforms/kia_soul/firmware/steering/tests/property/Cargo.toml b/firmware/steering/tests/property/Cargo.toml similarity index 83% rename from platforms/kia_soul/firmware/steering/tests/property/Cargo.toml rename to firmware/steering/tests/property/Cargo.toml index e47b7375..bd96c490 100644 --- a/platforms/kia_soul/firmware/steering/tests/property/Cargo.toml +++ b/firmware/steering/tests/property/Cargo.toml @@ -6,13 +6,12 @@ build = "build.rs" description = "The manifest file for the steering module property-based tests. To run the tests, run the command 'cargo test -- --test-threads=1'" [dependencies] -quickcheck = "0.3" -lazy_static = "0.2.8" +quickcheck = "0.4.1" rand = "0.3.15" [build-dependencies] -bindgen = "0.20.0" -gcc = "0.3" +bindgen = "0.25.0" +gcc = "0.3.51" [lib] name = "tests" diff --git a/firmware/steering/tests/property/build.rs b/firmware/steering/tests/property/build.rs new file mode 100644 index 00000000..9ef7ddb8 --- /dev/null +++ b/firmware/steering/tests/property/build.rs @@ -0,0 +1,64 @@ +extern crate gcc; +extern crate bindgen; + +use std::env; +use std::path::Path; + +fn main() { + gcc::Config::new() + .flag("-w") + .define("KIA_SOUL", Some("ON")) + .include("include") + .include("../../include") + .include("../../../common/include") + .include("../../../common/testing/mocks/") + .include("../../../common/libs/can") + .include("../../../common/libs/dac") + .include("../../../../api/include") + .file("../../../common/testing/mocks/Arduino_mock.cpp") + .file("../../../common/testing/mocks/mcp_can_mock.cpp") + .file("../../../common/testing/mocks/DAC_MCP49xx_mock.cpp") + .file("../../../common/libs/can/oscc_can.cpp") + .file("../../../common/libs/dac/oscc_dac.cpp") + .file("../../src/communications.cpp") + .file("../../src/steering_control.cpp") + .file("../../src/globals.cpp") + .compile("libsteering_test.a"); + + let out_dir = env::var("OUT_DIR").unwrap(); + + let _ = bindgen::builder() + .header("include/wrapper.hpp") + .generate_comments(false) + .layout_tests(false) + .clang_arg("-DKIA_SOUL=ON") + .clang_arg("-I../../include") + .clang_arg("-I../../../common/include") + .clang_arg("-I../../../common/libs/can") + .clang_arg("-I../../../common/testing/mocks") + .clang_arg("-I../../../../api/include") + .whitelisted_function("publish_steering_report") + .whitelisted_function("check_for_incoming_message") + .whitelisted_function("check_for_operator_override") + .whitelisted_var("OSCC_MAGIC_BYTE_0") + .whitelisted_var("OSCC_MAGIC_BYTE_1") + .whitelisted_var("OSCC_STEERING_REPORT_CAN_ID") + .whitelisted_var("OSCC_STEERING_REPORT_CAN_DLC") + .whitelisted_var("OSCC_STEERING_COMMAND_CAN_ID") + .whitelisted_var("OSCC_STEERING_COMMAND_CAN_DLC") + .whitelisted_var("OSCC_FAULT_REPORT_CAN_ID") + .whitelisted_var("OSCC_STEERING_REPORT_PUBLISH_INTERVAL_IN_MSEC") + .whitelisted_var("OVERRIDE_WHEEL_THRESHOLD_IN_DEGREES_PER_USEC") + .whitelisted_var("STEERING_SPOOF_SIGNAL_MIN") + .whitelisted_var("STEERING_SPOOF_SIGNAL_MAX") + .whitelisted_var("CAN_STANDARD") + .whitelisted_var("CAN_MSGAVAIL") + .whitelisted_type("oscc_steering_report_s") + .whitelisted_type("oscc_steering_command_s") + .whitelisted_type("can_frame_s") + .whitelisted_type("steering_control_state_s") + .generate() + .unwrap() + .write_to_file(Path::new(&out_dir).join("steering_test.rs")) + .expect("Unable to generate bindings"); +} diff --git a/firmware/steering/tests/property/include/wrapper.hpp b/firmware/steering/tests/property/include/wrapper.hpp new file mode 100644 index 00000000..1b412155 --- /dev/null +++ b/firmware/steering/tests/property/include/wrapper.hpp @@ -0,0 +1,7 @@ +#include "globals.h" +#include "communications.h" +#include "steering_control.h" +#include "oscc_can.h" +#include "can_protocols/steering_can_protocol.h" +#include "can_protocols/fault_can_protocol.h" +#include "vehicles.h" diff --git a/firmware/steering/tests/property/src/tests.rs b/firmware/steering/tests/property/src/tests.rs new file mode 100644 index 00000000..373a72e3 --- /dev/null +++ b/firmware/steering/tests/property/src/tests.rs @@ -0,0 +1,309 @@ +#![allow(non_upper_case_globals)] +#![allow(non_camel_case_types)] +#![allow(non_snake_case)] +#![allow(dead_code)] +#![allow(unused_variables)] +#![allow(unused_imports)] +include!(concat!(env!("OUT_DIR"), "/steering_test.rs")); + +extern crate quickcheck; +extern crate rand; + +use quickcheck::{QuickCheck, TestResult, Arbitrary, Gen, StdGen}; +use rand::Rng; + +extern "C" { + #[link_name = "g_mock_mcp_can_check_receive_return"] + pub static mut g_mock_mcp_can_check_receive_return: u8; + #[link_name = "g_mock_mcp_can_read_msg_buf_id"] + pub static mut g_mock_mcp_can_read_msg_buf_id: u32; + #[link_name = "g_mock_mcp_can_read_msg_buf_buf"] + pub static mut g_mock_mcp_can_read_msg_buf_buf: [u8; 8usize]; + #[link_name = "g_mock_mcp_can_send_msg_buf_id"] + pub static mut g_mock_mcp_can_send_msg_buf_id: u32; + #[link_name = "g_mock_mcp_can_send_msg_buf_ext"] + pub static mut g_mock_mcp_can_send_msg_buf_ext: u8; + #[link_name = "g_mock_mcp_can_send_msg_buf_len"] + pub static mut g_mock_mcp_can_send_msg_buf_len: u8; + #[link_name = "g_mock_mcp_can_send_msg_buf_buf"] + pub static mut g_mock_mcp_can_send_msg_buf_buf: *mut u8; + #[link_name = "g_mock_arduino_analog_read_return"] + pub static mut g_mock_arduino_analog_read_return: isize; + #[link_name = "g_mock_dac_output_a"] + pub static mut g_mock_dac_output_a: u16; + #[link_name = "g_mock_dac_output_b"] + pub static mut g_mock_dac_output_b: u16; + #[link_name = "g_steering_control_state"] + pub static mut g_steering_control_state: steering_control_state_s; +} + +impl Arbitrary for oscc_steering_report_s { + fn arbitrary(g: &mut G) -> oscc_steering_report_s { + oscc_steering_report_s { + magic: [OSCC_MAGIC_BYTE_0 as u8, OSCC_MAGIC_BYTE_1 as u8], + enabled: u8::arbitrary(g), + operator_override: u8::arbitrary(g), + dtcs: u8::arbitrary(g), + reserved: [u8::arbitrary(g); 3] + } + } +} + +impl Arbitrary for oscc_steering_command_s { + fn arbitrary(g: &mut G) -> oscc_steering_command_s { + oscc_steering_command_s { + magic: [OSCC_MAGIC_BYTE_0 as u8, OSCC_MAGIC_BYTE_1 as u8], + spoof_value_low: u16::arbitrary(g), + spoof_value_high: u16::arbitrary(g), + enable: u8::arbitrary(g), + reserved: u8::arbitrary(g) + } + } +} + +impl Arbitrary for can_frame_s { + fn arbitrary(g: &mut G) -> can_frame_s { + can_frame_s { + id: u32::arbitrary(g), + dlc: u8::arbitrary(g), + timestamp: u32::arbitrary(g), + data: [u8::arbitrary(g); 8] + } + } +} + +/// the steering firmware should not attempt processing any messages +/// that are not steering or fault commands +/// this means that none of the steering control state would +/// change, nor would it output any values onto the DAC. +fn prop_only_process_valid_messages(rx_can_msg: can_frame_s, enabled: bool, operator_override: bool, dtcs: u8) -> TestResult { + if rx_can_msg.id == OSCC_STEERING_COMMAND_CAN_ID || + rx_can_msg.id == OSCC_FAULT_REPORT_CAN_ID + { + return TestResult::discard(); + } + unsafe { + let dac_a = g_mock_dac_output_a; + let dac_b = g_mock_dac_output_b; + g_steering_control_state.enabled = enabled; + g_steering_control_state.operator_override = operator_override; + g_steering_control_state.dtcs = dtcs; + + g_mock_mcp_can_read_msg_buf_id = rx_can_msg.id; + g_mock_mcp_can_read_msg_buf_buf = rx_can_msg.data; + g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL as u8; + + check_for_incoming_message(); + + TestResult::from_bool(g_steering_control_state.enabled == + enabled && + g_steering_control_state.operator_override == operator_override && + g_steering_control_state.dtcs == dtcs && + g_mock_dac_output_a == dac_a && + g_mock_dac_output_b == dac_b) + } +} + +#[test] +fn check_message_type_validity() { + QuickCheck::new() + .tests(1000) + .gen(StdGen::new(rand::thread_rng(), u32::max_value() as usize)) + .quickcheck(prop_only_process_valid_messages as fn(can_frame_s, bool, bool, u8) -> TestResult) +} + +/// the steering firmware should set the control state as enabled +/// upon reciept of a valid command steering message telling it to enable +fn prop_process_enable_command(mut steering_command_msg: oscc_steering_command_s) -> TestResult { + unsafe { + steering_command_msg.enable = 1u8; + + g_steering_control_state.enabled = false; + g_steering_control_state.operator_override = false; + + g_mock_mcp_can_read_msg_buf_id = OSCC_STEERING_COMMAND_CAN_ID; + g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL as u8; + g_mock_mcp_can_read_msg_buf_buf = std::mem::transmute(steering_command_msg); + + check_for_incoming_message(); + + TestResult::from_bool(g_steering_control_state.enabled == true) + } +} + +#[test] +fn check_process_enable_command() { + QuickCheck::new() + .tests(1000) + .quickcheck(prop_process_enable_command as fn(oscc_steering_command_s) -> TestResult) +} + +/// the steering firmware should set the control state as disabled +/// upon reciept of a valid command steering message telling it to disable +fn prop_process_disable_command(mut steering_command_msg: oscc_steering_command_s) -> TestResult { + unsafe { + steering_command_msg.enable = 0u8; + + g_mock_mcp_can_read_msg_buf_id = OSCC_STEERING_COMMAND_CAN_ID; + g_mock_mcp_can_read_msg_buf_buf = std::mem::transmute(steering_command_msg); + g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL as u8; + + check_for_incoming_message(); + + TestResult::from_bool(g_steering_control_state.enabled == false) + } +} + +#[test] +fn check_process_disable_command() { + QuickCheck::new() + .tests(1000) + .quickcheck(prop_process_disable_command as fn(oscc_steering_command_s) -> TestResult) +} + +/// the steering firmware should send requested spoof values +/// upon recieving a steering command message +fn prop_output_accurate_spoofs(mut steering_command_msg: oscc_steering_command_s) -> TestResult { + steering_command_msg.enable = 1u8; + steering_command_msg.spoof_value_low = rand::thread_rng().gen_range(STEERING_SPOOF_SIGNAL_MIN as u16, STEERING_SPOOF_SIGNAL_MAX as u16); + steering_command_msg.spoof_value_high = rand::thread_rng().gen_range(STEERING_SPOOF_SIGNAL_MIN as u16, STEERING_SPOOF_SIGNAL_MAX as u16); + unsafe { + g_steering_control_state.enabled = true; + + g_mock_mcp_can_read_msg_buf_id = OSCC_STEERING_COMMAND_CAN_ID; + g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL as u8; + g_mock_mcp_can_read_msg_buf_buf = std::mem::transmute(steering_command_msg); + + check_for_incoming_message(); + + TestResult::from_bool(g_mock_dac_output_b == steering_command_msg.spoof_value_low && + g_mock_dac_output_a == + steering_command_msg.spoof_value_high ) + } +} + +#[test] +fn check_output_accurate_spoofs() { + QuickCheck::new() + .tests(1000) + .gen(StdGen::new(rand::thread_rng(), u16::max_value() as usize)) + .quickcheck(prop_output_accurate_spoofs as fn(oscc_steering_command_s) -> TestResult) +} + +/// the steering firmware should constrain requested spoof values +/// upon recieving a steering command message +fn prop_output_constrained_spoofs(mut steering_command_msg: oscc_steering_command_s) -> TestResult { + steering_command_msg.enable = 1u8; + unsafe { + if (steering_command_msg.spoof_value_low >= + STEERING_SPOOF_SIGNAL_MIN as u16 && + steering_command_msg.spoof_value_low <= + STEERING_SPOOF_SIGNAL_MAX as u16) || + (steering_command_msg.spoof_value_high >= + STEERING_SPOOF_SIGNAL_MIN as u16 && + steering_command_msg.spoof_value_high <= + STEERING_SPOOF_SIGNAL_MAX as u16) + { + return TestResult::discard(); + } + + g_steering_control_state.enabled = true; + + g_mock_mcp_can_read_msg_buf_id = OSCC_STEERING_COMMAND_CAN_ID; + g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL as u8; + g_mock_mcp_can_read_msg_buf_buf = std::mem::transmute(steering_command_msg); + + check_for_incoming_message(); + + TestResult::from_bool( + g_mock_dac_output_a >= STEERING_SPOOF_SIGNAL_MIN as u16 && + g_mock_dac_output_a <= STEERING_SPOOF_SIGNAL_MAX as u16 && + g_mock_dac_output_b >= STEERING_SPOOF_SIGNAL_MIN as u16 && + g_mock_dac_output_b <= STEERING_SPOOF_SIGNAL_MAX as u16) + } +} + +#[test] +fn check_output_constrained_spoofs() { + QuickCheck::new() + .tests(1000) + .gen(StdGen::new(rand::thread_rng(), u16::max_value() as usize)) + .quickcheck(prop_output_constrained_spoofs as fn(oscc_steering_command_s) -> TestResult) +} + +/// the steering firmware should create only valid CAN frames +fn prop_send_valid_can_fields(enabled: bool, + operator_override: bool, + dtcs: u8) + -> TestResult { + unsafe { + g_steering_control_state.operator_override = operator_override; + g_steering_control_state.enabled = enabled; + g_steering_control_state.dtcs = dtcs; + + publish_steering_report(); + + let steering_report_msg = g_mock_mcp_can_send_msg_buf_buf as *mut oscc_steering_report_s; + + TestResult::from_bool((*steering_report_msg).enabled == enabled as u8 &&(*steering_report_msg).operator_override == operator_override as u8 && + (*steering_report_msg).dtcs == dtcs) + } +} + +#[test] +fn check_valid_can_frame() { + QuickCheck::new() + .tests(10) + .gen(StdGen::new(rand::thread_rng(), u8::max_value() as usize)) + .quickcheck(prop_send_valid_can_fields as fn(bool, bool, u8) -> TestResult) +} + +// the steering firmware should be able to correctly and consistently +// detect operator overrides, disable on reciept, and send a fault report +fn prop_check_operator_override(analog_read_spoof: u16) -> TestResult { + unsafe { + g_steering_control_state.enabled = true; + g_steering_control_state.operator_override = false; + g_mock_arduino_analog_read_return = analog_read_spoof as isize; + + check_for_operator_override(); + + if analog_read_spoof >= (OVERRIDE_WHEEL_THRESHOLD_IN_DEGREES_PER_USEC as u16) { + TestResult::from_bool(g_steering_control_state.operator_override == true && g_steering_control_state.enabled == false && + g_mock_mcp_can_send_msg_buf_id == OSCC_FAULT_REPORT_CAN_ID) + } else { + TestResult::from_bool(g_steering_control_state.operator_override == false) + } + } +} + +#[test] +fn check_operator_override() { + QuickCheck::new() + .tests(1000) + .gen(StdGen::new(rand::thread_rng(), u16::max_value() as usize)) + .quickcheck(prop_check_operator_override as fn(u16) -> TestResult) +} + +/// the steering firmware should set disable itself when it recieves a +/// fault report from any other module +fn prop_process_fault_command(enabled: bool, operator_override: bool) -> TestResult { + unsafe { + g_steering_control_state.enabled = enabled; + g_steering_control_state.operator_override = operator_override; + + g_mock_mcp_can_read_msg_buf_id = OSCC_FAULT_REPORT_CAN_ID; + g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL as u8; + + check_for_incoming_message(); + + TestResult::from_bool(g_steering_control_state.enabled == false) + } +} + +#[test] +fn check_process_fault_command() { + QuickCheck::new() + .tests(1000) + .quickcheck(prop_process_fault_command as fn(bool, bool) -> TestResult) +} diff --git a/platforms/kia_soul/firmware/throttle/CMakeLists.txt b/firmware/throttle/CMakeLists.txt similarity index 75% rename from platforms/kia_soul/firmware/throttle/CMakeLists.txt rename to firmware/throttle/CMakeLists.txt index 12953618..50c505b8 100644 --- a/platforms/kia_soul/firmware/throttle/CMakeLists.txt +++ b/firmware/throttle/CMakeLists.txt @@ -1,4 +1,4 @@ -project(kia-soul-throttle) +project(throttle) set(ARDUINO_DEFAULT_BOARD uno) SET(ARDUINO_DEFAULT_PORT ${SERIAL_PORT_THROTTLE}) @@ -7,32 +7,32 @@ set(ARDUINO_DEFAULT_BAUDRATE ${SERIAL_BAUD_THROTTLE}) add_definitions(-DSERIAL_BAUD=${ARDUINO_DEFAULT_BAUDRATE}) add_custom_target( - kia-soul-throttle-monitor + throttle-monitor COMMAND screen ${ARDUINO_DEFAULT_PORT} ${ARDUINO_DEFAULT_BAUDRATE}) add_custom_target( - kia-soul-throttle-monitor-log + throttle-monitor-log COMMAND screen -L ${ARDUINO_DEFAULT_PORT} ${ARDUINO_DEFAULT_BAUDRATE}) generate_arduino_firmware( - kia-soul-throttle + throttle SRCS ${CMAKE_SOURCE_DIR}/common/libs/arduino_init/arduino_init.cpp ${CMAKE_SOURCE_DIR}/common/libs/DAC_MCP49xx/DAC_MCP49xx.cpp ${CMAKE_SOURCE_DIR}/common/libs/mcp_can/mcp_can.cpp ${CMAKE_SOURCE_DIR}/common/libs/serial/oscc_serial.cpp ${CMAKE_SOURCE_DIR}/common/libs/can/oscc_can.cpp - ${CMAKE_SOURCE_DIR}/common/libs/time/oscc_time.cpp ${CMAKE_SOURCE_DIR}/common/libs/dac/oscc_dac.cpp - ${CMAKE_SOURCE_DIR}/common/libs/signal_smoothing/oscc_signal_smoothing.cpp + ${CMAKE_SOURCE_DIR}/common/libs/timer/oscc_timer.cpp src/main.cpp src/globals.cpp src/init.cpp src/communications.cpp - src/throttle_control.cpp) + src/throttle_control.cpp + src/timers.cpp) target_include_directories( - kia-soul-throttle + throttle PRIVATE include ${CMAKE_SOURCE_DIR}/common/include @@ -41,6 +41,6 @@ target_include_directories( ${CMAKE_SOURCE_DIR}/common/libs/mcp_can ${CMAKE_SOURCE_DIR}/common/libs/serial ${CMAKE_SOURCE_DIR}/common/libs/can - ${CMAKE_SOURCE_DIR}/common/libs/time ${CMAKE_SOURCE_DIR}/common/libs/dac - ${CMAKE_SOURCE_DIR}/common/libs/signal_smoothing) + ${CMAKE_SOURCE_DIR}/common/libs/timer + ${CMAKE_SOURCE_DIR}/../api/include) diff --git a/platforms/kia_soul/firmware/throttle/include/communications.h b/firmware/throttle/include/communications.h similarity index 72% rename from platforms/kia_soul/firmware/throttle/include/communications.h rename to firmware/throttle/include/communications.h index a8a9e68c..b2964774 100644 --- a/platforms/kia_soul/firmware/throttle/include/communications.h +++ b/firmware/throttle/include/communications.h @@ -9,25 +9,30 @@ #define _OSCC_KIA_SOUL_THROTTLE_COMMUNICATIONS_H_ -/* - * @brief Amount of time after controller command that is considered a - * timeout. [milliseconds] - * - */ -#define COMMAND_TIMEOUT_IN_MSEC ( 250 ) +// **************************************************************************** +// Function: publish_throttle_report +// +// Purpose: Publish throttle report to CAN bus. +// +// Returns: void +// +// Parameters: void +// +// **************************************************************************** +void publish_throttle_report( void ); // **************************************************************************** -// Function: publish_reports +// Function: publish_fault_report // -// Purpose: Publish all valid reports to CAN bus. +// Purpose: Publish a fault report message to the CAN bus. // // Returns: void // // Parameters: void // // **************************************************************************** -void publish_reports( void ); +void publish_fault_report( void ); // **************************************************************************** diff --git a/platforms/kia_soul/firmware/throttle/include/globals.h b/firmware/throttle/include/globals.h similarity index 82% rename from platforms/kia_soul/firmware/throttle/include/globals.h rename to firmware/throttle/include/globals.h index 43354114..8e58ec03 100644 --- a/platforms/kia_soul/firmware/throttle/include/globals.h +++ b/firmware/throttle/include/globals.h @@ -71,12 +71,9 @@ #endif -EXTERN uint32_t g_throttle_command_last_rx_timestamp; -EXTERN uint32_t g_throttle_report_last_tx_timestamp; -EXTERN uint32_t g_sensor_validity_last_check_timestamp; +EXTERN volatile bool g_throttle_command_timeout; -EXTERN kia_soul_throttle_control_state_s g_throttle_control_state; -EXTERN uint16_t g_accelerator_spoof_output_sum; +EXTERN volatile throttle_control_state_s g_throttle_control_state; #endif /* _OSCC_KIA_SOUL_THROTTLE_GLOBALS_H_ */ diff --git a/platforms/kia_soul/firmware/throttle/include/init.h b/firmware/throttle/include/init.h similarity index 100% rename from platforms/kia_soul/firmware/throttle/include/init.h rename to firmware/throttle/include/init.h diff --git a/firmware/throttle/include/throttle_control.h b/firmware/throttle/include/throttle_control.h new file mode 100644 index 00000000..26d121f1 --- /dev/null +++ b/firmware/throttle/include/throttle_control.h @@ -0,0 +1,115 @@ +/** + * @file throttle_control.h + * @brief Control of the throttle system. + * + */ + + +#ifndef _OSCC_KIA_SOUL_THROTTLE_CONTROL_H_ +#define _OSCC_KIA_SOUL_THROTTLE_CONTROL_H_ + + +#include + + +/** + * @brief Accelerator position values. + * + * Contains high and low accelerator values. + * + */ +typedef struct +{ + uint16_t low; /* Low value of accelerator position. */ + + uint16_t high; /* High value of accelerator position. */ +} accelerator_position_s; + + +/** + * @brief Current throttle control state. + * + * Current state of the throttle module control system. + * + */ +typedef struct +{ + bool enabled; /* Flag indicating whether control is currently enabled. */ + + bool operator_override; /* Flag indicating whether accelerator was manually + pressed by operator. */ + + uint8_t dtcs; /* Bitfield of faults present in the module. */ +} throttle_control_state_s; + + +// **************************************************************************** +// Function: check_for_operator_override +// +// Purpose: Checks to see if the vehicle's operator has manually pressed +// the accelerator and disables control if they have. +// +// Returns: void +// +// Parameters: void +// +// **************************************************************************** +void check_for_operator_override( void ); + + +// **************************************************************************** +// Function: check_for_sensor_faults +// +// Purpose: Checks to see if valid values are being read from the sensors. +// +// Returns: void +// +// Parameters: void +// +// **************************************************************************** +void check_for_sensor_faults( void ); + + +// **************************************************************************** +// Function: update_throttle +// +// Purpose: Writes throttle spoof values to DAC. +// +// Returns: void +// +// Parameters: spoof_command_high - high value of spoof command +// spoof_command_low - low value of spoof command +// +// **************************************************************************** +void update_throttle( + uint16_t spoof_command_high, + uint16_t spoof_command_low ); + + +// **************************************************************************** +// Function: enable_control +// +// Purpose: Enable control of the throttle system. +// +// Returns: void +// +// Parameters: void +// +// **************************************************************************** +void enable_control( void ); + + +// **************************************************************************** +// Function: disable_control +// +// Purpose: Disable control of the throttle system. +// +// Returns: void +// +// Parameters: void +// +// **************************************************************************** +void disable_control( void ); + + +#endif /* _OSCC_KIA_SOUL_THROTTLE_CONTROL_H_ */ diff --git a/firmware/throttle/include/timers.h b/firmware/throttle/include/timers.h new file mode 100644 index 00000000..cde065e2 --- /dev/null +++ b/firmware/throttle/include/timers.h @@ -0,0 +1,25 @@ +/** + * @file timers.h + * @brief Timer functionality. + * + */ + + +#ifndef _OSCC_KIA_SOUL_THROTTLE_TIMERS_H_ +#define _OSCC_KIA_SOUL_THROTTLE_TIMERS_H_ + + +// **************************************************************************** +// Function: start_timers +// +// Purpose: Start timers for report publishing and fault checking. +// +// Returns: void +// +// Parameters: void +// +// **************************************************************************** +void start_timers( void ); + + +#endif /* _OSCC_KIA_SOUL_THROTTLE_TIMERS_H_ */ diff --git a/firmware/throttle/src/communications.cpp b/firmware/throttle/src/communications.cpp new file mode 100644 index 00000000..27da7430 --- /dev/null +++ b/firmware/throttle/src/communications.cpp @@ -0,0 +1,153 @@ +/** + * @file communications.cpp + * + */ + + +#include "mcp_can.h" +#include "oscc_can.h" +#include "can_protocols/fault_can_protocol.h" +#include "can_protocols/throttle_can_protocol.h" +#include "debug.h" + +#include "globals.h" +#include "communications.h" +#include "throttle_control.h" + + +static void process_rx_frame( + const can_frame_s * const frame ); + +static void process_throttle_command( + const uint8_t * const data ); + +static void process_fault_report( + const uint8_t * const data ); + + +void publish_throttle_report( void ) +{ + oscc_throttle_report_s throttle_report; + + throttle_report.magic[0] = (uint8_t) OSCC_MAGIC_BYTE_0; + throttle_report.magic[1] = (uint8_t) OSCC_MAGIC_BYTE_1; + throttle_report.enabled = (uint8_t) g_throttle_control_state.enabled; + throttle_report.operator_override = (uint8_t) g_throttle_control_state.operator_override; + throttle_report.dtcs = g_throttle_control_state.dtcs; + + cli(); + g_control_can.sendMsgBuf( + OSCC_THROTTLE_REPORT_CAN_ID, + CAN_STANDARD, + OSCC_THROTTLE_REPORT_CAN_DLC, + (uint8_t*) &throttle_report ); + sei(); +} + + +void publish_fault_report( void ) +{ + oscc_fault_report_s fault_report; + + fault_report.magic[0] = (uint8_t) OSCC_MAGIC_BYTE_0; + fault_report.magic[1] = (uint8_t) OSCC_MAGIC_BYTE_1; + fault_report.fault_origin_id = FAULT_ORIGIN_THROTTLE; + + cli(); + g_control_can.sendMsgBuf( + OSCC_FAULT_REPORT_CAN_ID, + CAN_STANDARD, + OSCC_FAULT_REPORT_CAN_DLC, + (uint8_t *) &fault_report ); + sei(); +} + + +void check_for_controller_command_timeout( void ) +{ + if( g_throttle_control_state.enabled == true ) + { + if( g_throttle_command_timeout == true ) + { + disable_control( ); + + publish_fault_report( ); + + DEBUG_PRINTLN( "Timeout waiting for controller command" ); + } + } +} + + +void check_for_incoming_message( void ) +{ + can_frame_s rx_frame; + can_status_t ret = check_for_rx_frame( g_control_can, &rx_frame ); + + if( ret == CAN_RX_FRAME_AVAILABLE ) + { + process_rx_frame( &rx_frame ); + } +} + + +static void process_rx_frame( + const can_frame_s * const frame ) +{ + if ( frame != NULL ) + { + if( (frame->data[0] == OSCC_MAGIC_BYTE_0) + && (frame->data[1] == OSCC_MAGIC_BYTE_1) ) + { + if( frame->id == OSCC_THROTTLE_COMMAND_CAN_ID ) + { + process_throttle_command( frame->data ); + } + else if ( frame->id == OSCC_FAULT_REPORT_CAN_ID ) + { + process_fault_report( frame-> data ); + } + } + } +} + + +static void process_throttle_command( + const uint8_t * const data ) +{ + if ( data != NULL ) + { + const oscc_throttle_command_s * const throttle_command = + (oscc_throttle_command_s *) data; + + if( throttle_command->enable == true ) + { + enable_control( ); + + update_throttle( + throttle_command->spoof_value_high, + throttle_command->spoof_value_low ); + } + else + { + disable_control( ); + } + + g_throttle_command_timeout = false; + } +} + + +static void process_fault_report( + const uint8_t * const data ) +{ + if ( data != NULL ) + { + const oscc_fault_report_s * const fault_report = + (oscc_fault_report_s *) data; + + disable_control( ); + + DEBUG_PRINTLN( "Fault report received" ); + } +} diff --git a/platforms/kia_soul/firmware/throttle/src/globals.cpp b/firmware/throttle/src/globals.cpp similarity index 100% rename from platforms/kia_soul/firmware/throttle/src/globals.cpp rename to firmware/throttle/src/globals.cpp diff --git a/platforms/kia_soul/firmware/throttle/src/init.cpp b/firmware/throttle/src/init.cpp similarity index 62% rename from platforms/kia_soul/firmware/throttle/src/init.cpp rename to firmware/throttle/src/init.cpp index 0875f3f5..8ef62f29 100644 --- a/platforms/kia_soul/firmware/throttle/src/init.cpp +++ b/firmware/throttle/src/init.cpp @@ -7,23 +7,21 @@ #include #include "oscc_serial.h" #include "oscc_can.h" -#include "oscc_time.h" +#include "can_protocols/throttle_can_protocol.h" #include "debug.h" #include "globals.h" +#include "communications.h" #include "init.h" void init_globals( void ) { - memset( &g_throttle_control_state, - 0, - sizeof(g_throttle_control_state) ); - - // update timestamps so we don't set timeout warnings on start up - g_throttle_command_last_rx_timestamp = GET_TIMESTAMP_MS( ); - g_throttle_report_last_tx_timestamp = GET_TIMESTAMP_MS( ); - g_sensor_validity_last_check_timestamp = GET_TIMESTAMP_MS(); + g_throttle_control_state.enabled = false; + g_throttle_control_state.operator_override = false; + g_throttle_control_state.dtcs = 0; + + g_throttle_command_timeout = false; } @@ -36,9 +34,10 @@ void init_devices( void ) pinMode( PIN_ACCELERATOR_POSITION_SPOOF_LOW, INPUT ); pinMode( PIN_SPOOF_ENABLE, OUTPUT ); - digitalWrite( PIN_DAC_CHIP_SELECT, HIGH ); // Deselect DAC CS - + cli(); + digitalWrite( PIN_DAC_CHIP_SELECT, HIGH ); digitalWrite( PIN_SPOOF_ENABLE, LOW ); + sei(); } diff --git a/platforms/kia_soul/firmware/throttle/src/main.cpp b/firmware/throttle/src/main.cpp similarity index 59% rename from platforms/kia_soul/firmware/throttle/src/main.cpp rename to firmware/throttle/src/main.cpp index 2ddf2b39..3f7130e0 100644 --- a/platforms/kia_soul/firmware/throttle/src/main.cpp +++ b/firmware/throttle/src/main.cpp @@ -4,15 +4,14 @@ */ -#include #include "arduino_init.h" #include "debug.h" #include "init.h" +#include "timers.h" #include "communications.h" #include "throttle_control.h" - int main( void ) { init_arduino( ); @@ -23,24 +22,14 @@ int main( void ) init_communication_interfaces( ); - wdt_enable( WDTO_120MS ); + start_timers( ); - DEBUG_PRINTLN( "initialization complete" ); + DEBUG_PRINTLN( "init complete" ); while( true ) { - wdt_reset(); - check_for_incoming_message( ); - check_for_controller_command_timeout( ); - - check_for_sensor_faults( ); - check_for_operator_override( ); - - publish_reports( ); - - update_throttle( ); } } diff --git a/firmware/throttle/src/throttle_control.cpp b/firmware/throttle/src/throttle_control.cpp new file mode 100644 index 00000000..348d0cf4 --- /dev/null +++ b/firmware/throttle/src/throttle_control.cpp @@ -0,0 +1,182 @@ +/** + * @file throttle_control.cpp + * + */ + + +#include +#include +#include "debug.h" +#include "oscc_dac.h" +#include "can_protocols/throttle_can_protocol.h" +#include "dtc.h" +#include "vehicles.h" + +#include "communications.h" +#include "throttle_control.h" +#include "globals.h" + + +/* + * @brief Number of consecutive faults that can occur when reading the + * sensors before control is disabled. + * + */ +#define SENSOR_VALIDITY_CHECK_FAULT_COUNT ( 4 ) + + +static void read_accelerator_position_sensor( + accelerator_position_s * const value ); + + +void check_for_operator_override( void ) +{ + if ( g_throttle_control_state.enabled == true + || g_throttle_control_state.operator_override == true ) + { + accelerator_position_s accelerator_position; + + read_accelerator_position_sensor( &accelerator_position ); + + uint32_t accelerator_position_average = + (accelerator_position.low + accelerator_position.high) / 2; + + if ( accelerator_position_average >= ACCELERATOR_OVERRIDE_THRESHOLD ) + { + disable_control( ); + + publish_fault_report( ); + + g_throttle_control_state.operator_override = true; + + DEBUG_PRINTLN( "Operator override" ); + } + else + { + g_throttle_control_state.operator_override = false; + } + } +} + + +void check_for_sensor_faults( void ) +{ + if ( (g_throttle_control_state.enabled == true) + || DTC_CHECK(g_throttle_control_state.dtcs, OSCC_THROTTLE_DTC_INVALID_SENSOR_VAL) ) + { + static int fault_count = 0; + + accelerator_position_s accelerator_position; + + read_accelerator_position_sensor( &accelerator_position ); + + // sensor pins tied to ground - a value of zero indicates disconnection + if( (accelerator_position.high == 0) + || (accelerator_position.low == 0) ) + { + ++fault_count; + + if( fault_count >= SENSOR_VALIDITY_CHECK_FAULT_COUNT ) + { + disable_control( ); + + publish_fault_report( ); + + DTC_SET( + g_throttle_control_state.dtcs, + OSCC_THROTTLE_DTC_INVALID_SENSOR_VAL ); + + DEBUG_PRINTLN( "Bad value read from accelerator position sensor" ); + } + } + else + { + DTC_CLEAR( + g_throttle_control_state.dtcs, + OSCC_THROTTLE_DTC_INVALID_SENSOR_VAL ); + + fault_count = 0; + } + } +} + + +void update_throttle( + uint16_t spoof_command_high, + uint16_t spoof_command_low ) +{ + if ( g_throttle_control_state.enabled == true ) + { + uint16_t spoof_high = + constrain( + spoof_command_high, + THROTTLE_SPOOF_HIGH_SIGNAL_RANGE_MIN, + THROTTLE_SPOOF_HIGH_SIGNAL_RANGE_MAX ); + + uint16_t spoof_low = + constrain( + spoof_command_low, + THROTTLE_SPOOF_LOW_SIGNAL_RANGE_MIN, + THROTTLE_SPOOF_LOW_SIGNAL_RANGE_MAX ); + + cli(); + g_dac.outputA( spoof_high ); + g_dac.outputB( spoof_low ); + sei(); + } +} + + +void enable_control( void ) +{ + if( g_throttle_control_state.enabled == false + && g_throttle_control_state.operator_override == false ) + { + const uint16_t num_samples = 20; + prevent_signal_discontinuity( + g_dac, + num_samples, + PIN_ACCELERATOR_POSITION_SENSOR_HIGH, + PIN_ACCELERATOR_POSITION_SENSOR_LOW ); + + cli(); + digitalWrite( PIN_SPOOF_ENABLE, HIGH ); + sei(); + + g_throttle_control_state.enabled = true; + + DEBUG_PRINTLN( "Control enabled" ); + } +} + + +void disable_control( void ) +{ + if( g_throttle_control_state.enabled == true ) + { + const uint16_t num_samples = 20; + prevent_signal_discontinuity( + g_dac, + num_samples, + PIN_ACCELERATOR_POSITION_SENSOR_HIGH, + PIN_ACCELERATOR_POSITION_SENSOR_LOW ); + + cli(); + digitalWrite( PIN_SPOOF_ENABLE, LOW ); + sei(); + + g_throttle_control_state.enabled = false; + + DEBUG_PRINTLN( "Control disabled" ); + } +} + + +static void read_accelerator_position_sensor( + accelerator_position_s * const value ) +{ + cli(); + value->high = analogRead( PIN_ACCELERATOR_POSITION_SENSOR_HIGH ); + value->low = analogRead( PIN_ACCELERATOR_POSITION_SENSOR_LOW ); + sei(); +} diff --git a/firmware/throttle/src/timers.cpp b/firmware/throttle/src/timers.cpp new file mode 100644 index 00000000..5b944608 --- /dev/null +++ b/firmware/throttle/src/timers.cpp @@ -0,0 +1,44 @@ +/** + * @file timers.cpp + * + */ + + +#include "can_protocols/throttle_can_protocol.h" +#include "oscc_timer.h" + +#include "timers.h" +#include "globals.h" +#include "communications.h" +#include "throttle_control.h" + + +/* + * @brief Fault checking frequency. [Hz] + * + */ +#define FAULT_CHECK_FREQUENCY_IN_HZ ( 5 ) + + +static void check_for_faults( void ); + + +void start_timers( void ) +{ + timer1_init( FAULT_CHECK_FREQUENCY_IN_HZ, check_for_faults ); + timer2_init( OSCC_REPORT_THROTTLE_PUBLISH_FREQ_IN_HZ, publish_throttle_report ); +} + + +static void check_for_faults( void ) +{ + cli(); + + check_for_controller_command_timeout( ); + + check_for_sensor_faults( ); + + g_throttle_command_timeout = true; + + sei(); +} diff --git a/platforms/kia_soul/firmware/throttle/tests/CMakeLists.txt b/firmware/throttle/tests/CMakeLists.txt similarity index 65% rename from platforms/kia_soul/firmware/throttle/tests/CMakeLists.txt rename to firmware/throttle/tests/CMakeLists.txt index 534e3499..71820f71 100644 --- a/platforms/kia_soul/firmware/throttle/tests/CMakeLists.txt +++ b/firmware/throttle/tests/CMakeLists.txt @@ -1,61 +1,60 @@ -project(kia-soul-throttle-unit-tests) +project(throttle-unit-tests) add_library( - kia-soul-throttle + throttle SHARED ../src/communications.cpp ../src/globals.cpp ../src/throttle_control.cpp - ${CMAKE_SOURCE_DIR}/common/libs/time/oscc_time.cpp ${CMAKE_SOURCE_DIR}/common/libs/can/oscc_can.cpp ${CMAKE_SOURCE_DIR}/common/libs/dac/oscc_dac.cpp - ${CMAKE_SOURCE_DIR}/common/libs/signal_smoothing/oscc_signal_smoothing.cpp ${CMAKE_SOURCE_DIR}/common/testing/mocks/ ${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) target_include_directories( - kia-soul-throttle + throttle PRIVATE ../include ${CMAKE_SOURCE_DIR}/common/include ${CMAKE_SOURCE_DIR}/common/libs/can - ${CMAKE_SOURCE_DIR}/common/libs/time ${CMAKE_SOURCE_DIR}/common/libs/dac ${CMAKE_SOURCE_DIR}/common/libs/signal_smoothing - ${CMAKE_SOURCE_DIR}/common/testing/mocks) + ${CMAKE_SOURCE_DIR}/common/testing/mocks + ${CMAKE_SOURCE_DIR}/../api/include) add_executable( - kia-soul-throttle-unit-test + throttle-unit-test features/step_definitions/test.cpp) target_link_libraries( - kia-soul-throttle-unit-test + throttle-unit-test PRIVATE - kia-soul-throttle + throttle ${CMAKE_SOURCE_DIR}/common/testing/framework/cucumber-cpp/lib/libcucumber-cpp.a ${CMAKE_SOURCE_DIR}/common/testing/framework/cgreen/lib/libcgreen.so) target_include_directories( - kia-soul-throttle-unit-test + throttle-unit-test PRIVATE ../include ${CMAKE_SOURCE_DIR}/common/include ${CMAKE_SOURCE_DIR}/common/libs/can ${CMAKE_SOURCE_DIR}/common/testing/mocks ${CMAKE_SOURCE_DIR}/common/testing/framework/cucumber-cpp/include - ${CMAKE_SOURCE_DIR}/common/testing/framework/cgreen/include) + ${CMAKE_SOURCE_DIR}/common/testing/framework/cgreen/include + ${CMAKE_SOURCE_DIR}/../api/include) add_custom_target( - run-kia-soul-throttle-unit-tests + run-throttle-unit-tests DEPENDS - kia-soul-throttle-unit-test + throttle-unit-test COMMAND - kia-soul-throttle-unit-test --port=3902 >/dev/null & cucumber _2.0.0_ ${CMAKE_CURRENT_SOURCE_DIR}/features ) + throttle-unit-test --port=3902 >/dev/null & cucumber _2.0.0_ ${CMAKE_CURRENT_SOURCE_DIR}/features ) add_custom_target( - run-kia-soul-throttle-property-tests + run-throttle-property-tests COMMAND cargo test -- --test-threads=1 WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/property) diff --git a/firmware/throttle/tests/features/checking_faults.feature b/firmware/throttle/tests/features/checking_faults.feature new file mode 100644 index 00000000..18d6c43a --- /dev/null +++ b/firmware/throttle/tests/features/checking_faults.feature @@ -0,0 +1,46 @@ +# language: en + +Feature: Checking for faults + + If the module encounters a fault condition, it should disable control and + publish a fault report. + + Scenario: A sensor becomes temporarily disconnected + Given throttle control is enabled + + When a sensor becomes temporarily disconnected + + Then control should remain enabled + + + Scenario: A sensor becomes permanently disconnected + Given throttle control is enabled + + When a sensor becomes permanently disconnected + + Then control should be disabled + And a fault report should be published + + + Scenario: Controller command timeout + Given throttle control is enabled + + When the time since the last received controller command exceeds the timeout + + Then control should be disabled + And a fault report should be published + + + Scenario Outline: Operator override + Given throttle control is enabled + + When the operator applies to the accelerator + + Then control should be disabled + And a fault report should be published + + Examples: + | sensor_val | + | 250 | + | 500 | + | 1000 | diff --git a/firmware/throttle/tests/features/receiving_messages.feature b/firmware/throttle/tests/features/receiving_messages.feature new file mode 100644 index 00000000..3d66cad5 --- /dev/null +++ b/firmware/throttle/tests/features/receiving_messages.feature @@ -0,0 +1,65 @@ +# language: en + +Feature: Receiving commands + + Commands received from a application should be processed and acted upon. + + + Scenario: Enable throttle command sent from application + Given throttle control is disabled + + When an enable throttle command is received + + Then control should be enabled + + + Scenario: Disable throttle command sent from application + Given throttle control is enabled + + When a disable throttle command is received + + Then control should be disabled + + + Scenario: Fault report sent from a different module + Given throttle control is enabled + + When a fault report is received + + Then control should be disabled + + + Scenario Outline: Spoof value sent from application + Given throttle control is enabled + + When a command is received with spoof values and + + Then should be sent to DAC A + And should be sent to DAC B + + Examples: + | high | low | + | 3500 | 0 | + | 3000 | 500 | + | 2500 | 1000 | + | 2000 | 1500 | + | 1500 | 1800 | + | 1000 | 1800 | + | 500 | 1800 | + | 0 | 1800 | + + + Scenario Outline: Spoof value sent from application outside valid range + Given throttle control is enabled + + When a command is received with spoof values and + + Then should be sent to DAC A + And should be sent to DAC B + + Examples: + | high | low | high_clamped | low_clamped | + | 4000 | 0 | 3500 | 0 | + | 3500 | 500 | 3500 | 500 | + | 500 | 3500 | 500 | 1800 | + | 0 | 4000 | 0 | 1800 | diff --git a/firmware/throttle/tests/features/sending_reports.feature b/firmware/throttle/tests/features/sending_reports.feature new file mode 100644 index 00000000..9d8e4871 --- /dev/null +++ b/firmware/throttle/tests/features/sending_reports.feature @@ -0,0 +1,12 @@ +# language: en + +Feature: Sending reports + + Throttle reports should be published to the control CAN bus. + + + Scenario: Throttle report published + When a throttle report is published + + Then a throttle report should be put on the control CAN bus + And the throttle report's fields should be set diff --git a/firmware/throttle/tests/features/step_definitions/checking_faults.cpp b/firmware/throttle/tests/features/step_definitions/checking_faults.cpp new file mode 100644 index 00000000..715bf723 --- /dev/null +++ b/firmware/throttle/tests/features/step_definitions/checking_faults.cpp @@ -0,0 +1,71 @@ +WHEN("^a sensor becomes temporarily disconnected$") +{ + g_mock_arduino_analog_read_return = 0; + + check_for_sensor_faults(); + + check_for_sensor_faults(); + + check_for_sensor_faults(); +} + + +WHEN("^a sensor becomes permanently disconnected$") +{ + g_mock_arduino_analog_read_return = 0; + + // must call function enough times to exceed the fault limit + for( int i = 0; i < 100; ++i ) + { + check_for_sensor_faults(); + } +} + + +WHEN("^the time since the last received controller command exceeds the timeout$") +{ + g_throttle_command_timeout = true; + + check_for_controller_command_timeout(); +} + + +WHEN("^the operator applies (.*) to the accelerator$") +{ + REGEX_PARAM(int, throttle_sensor_val); + + g_mock_arduino_analog_read_return = throttle_sensor_val; + + check_for_operator_override(); +} + + +THEN("^control should remain enabled") +{ + assert_that( + g_throttle_control_state.enabled, + is_equal_to(1)); +} + + +THEN("^a fault report should be published$") +{ + assert_that(g_mock_mcp_can_send_msg_buf_id, is_equal_to(OSCC_FAULT_REPORT_CAN_ID)); + assert_that(g_mock_mcp_can_send_msg_buf_ext, is_equal_to(CAN_STANDARD)); + assert_that(g_mock_mcp_can_send_msg_buf_len, is_equal_to(OSCC_FAULT_REPORT_CAN_DLC)); + + oscc_fault_report_s * fault_report = + (oscc_fault_report_s *) g_mock_mcp_can_send_msg_buf_buf; + + assert_that( + fault_report->magic[0], + is_equal_to(OSCC_MAGIC_BYTE_0)); + + assert_that( + fault_report->magic[1], + is_equal_to(OSCC_MAGIC_BYTE_1)); + + assert_that( + fault_report->fault_origin_id, + is_equal_to(FAULT_ORIGIN_THROTTLE)); +} diff --git a/platforms/kia_soul/firmware/throttle/tests/features/step_definitions/common.cpp b/firmware/throttle/tests/features/step_definitions/common.cpp similarity index 69% rename from platforms/kia_soul/firmware/throttle/tests/features/step_definitions/common.cpp rename to firmware/throttle/tests/features/step_definitions/common.cpp index b49d8f07..fde4629f 100644 --- a/platforms/kia_soul/firmware/throttle/tests/features/step_definitions/common.cpp +++ b/firmware/throttle/tests/features/step_definitions/common.cpp @@ -9,48 +9,54 @@ #include "oscc_can.h" #include "mcp_can.h" #include "throttle_control.h" -#include "throttle_can_protocol.h" +#include "can_protocols/fault_can_protocol.h" +#include "can_protocols/throttle_can_protocol.h" #include "globals.h" using namespace cgreen; -extern unsigned long g_mock_arduino_millis_return; extern uint8_t g_mock_arduino_digital_write_pin; extern uint8_t g_mock_arduino_digital_write_val; -extern int g_mock_arduino_analog_read_return; - -extern INT8U g_mock_mcp_can_check_receive_return; -extern INT32U g_mock_mcp_can_read_msg_buf_id; -extern INT8U g_mock_mcp_can_read_msg_buf_buf[8]; +extern int g_mock_arduino_analog_read_return; -extern INT32U g_mock_mcp_can_send_msg_buf_id; -extern INT8U g_mock_mcp_can_send_msg_buf_ext; -extern INT8U g_mock_mcp_can_send_msg_buf_len; -extern INT8U *g_mock_mcp_can_send_msg_buf_buf; +extern uint8_t g_mock_mcp_can_check_receive_return; +extern uint32_t g_mock_mcp_can_read_msg_buf_id; +extern uint8_t g_mock_mcp_can_read_msg_buf_buf[8]; +extern uint32_t g_mock_mcp_can_send_msg_buf_id; +extern uint8_t g_mock_mcp_can_send_msg_buf_ext; +extern uint8_t g_mock_mcp_can_send_msg_buf_len; +extern uint8_t *g_mock_mcp_can_send_msg_buf_buf; extern unsigned short g_mock_dac_output_a; extern unsigned short g_mock_dac_output_b; -extern kia_soul_throttle_control_state_s g_throttle_control_state; +extern volatile throttle_control_state_s g_throttle_control_state; // return to known state before every scenario BEFORE() { - g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL; - g_mock_mcp_can_read_msg_buf_id = OSCC_COMMAND_THROTTLE_CAN_ID; - g_mock_arduino_millis_return = 555; - - memset(&g_mock_mcp_can_read_msg_buf_buf, 0, sizeof(g_mock_mcp_can_read_msg_buf_buf)); - memset(&g_throttle_control_state, 0, sizeof(g_throttle_control_state)); - g_mock_arduino_digital_write_pin = UINT8_MAX; g_mock_arduino_digital_write_val = UINT8_MAX; + g_mock_arduino_analog_read_return = INT_MAX; + + g_mock_mcp_can_check_receive_return = UINT8_MAX; + g_mock_mcp_can_read_msg_buf_id = UINT32_MAX; + memset(&g_mock_mcp_can_read_msg_buf_buf, 0, sizeof(g_mock_mcp_can_read_msg_buf_buf)); + + g_mock_mcp_can_send_msg_buf_id = UINT32_MAX; + g_mock_mcp_can_send_msg_buf_ext = UINT8_MAX; + g_mock_mcp_can_send_msg_buf_len = UINT8_MAX; + g_mock_dac_output_a = USHRT_MAX; g_mock_dac_output_b = USHRT_MAX; + + g_throttle_control_state.enabled = false; + g_throttle_control_state.operator_override = false; + g_throttle_control_state.dtcs = 0; } @@ -74,14 +80,6 @@ GIVEN("^the accelerator position sensors have a reading of (.*)$") } -GIVEN("^the previous accelerator position command was (.*)$") -{ - REGEX_PARAM(int, commanded_accelerator_position); - - g_throttle_control_state.commanded_accelerator_position = commanded_accelerator_position; -} - - GIVEN("^the operator has applied (.*) to the accelerator$") { @@ -125,7 +123,7 @@ THEN("^control should be disabled$") } -THEN("^(.*) should be written to DAC A$") +THEN("^(.*) should be sent to DAC A$") { REGEX_PARAM(int, dac_output_a); @@ -135,7 +133,7 @@ THEN("^(.*) should be written to DAC A$") } -THEN("^(.*) should be written to DAC B$") +THEN("^(.*) should be sent to DAC B$") { REGEX_PARAM(int, dac_output_b); diff --git a/platforms/kia_soul/firmware/throttle/tests/features/step_definitions/cucumber.wire b/firmware/throttle/tests/features/step_definitions/cucumber.wire similarity index 100% rename from platforms/kia_soul/firmware/throttle/tests/features/step_definitions/cucumber.wire rename to firmware/throttle/tests/features/step_definitions/cucumber.wire diff --git a/firmware/throttle/tests/features/step_definitions/receiving_messages.cpp b/firmware/throttle/tests/features/step_definitions/receiving_messages.cpp new file mode 100644 index 00000000..e9f3d22a --- /dev/null +++ b/firmware/throttle/tests/features/step_definitions/receiving_messages.cpp @@ -0,0 +1,70 @@ +WHEN("^an enable throttle command is received$") +{ + g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL; + g_mock_mcp_can_read_msg_buf_id = OSCC_THROTTLE_COMMAND_CAN_ID; + + oscc_throttle_command_s * throttle_command = + (oscc_throttle_command_s *) g_mock_mcp_can_read_msg_buf_buf; + + throttle_command->magic[0] = OSCC_MAGIC_BYTE_0; + throttle_command->magic[1] = OSCC_MAGIC_BYTE_1; + throttle_command->enable = 1; + + check_for_incoming_message(); +} + + +WHEN("^a disable throttle command is received$") +{ + g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL; + g_mock_mcp_can_read_msg_buf_id = OSCC_THROTTLE_COMMAND_CAN_ID; + + oscc_throttle_command_s * throttle_command = + (oscc_throttle_command_s *) g_mock_mcp_can_read_msg_buf_buf; + + throttle_command->magic[0] = OSCC_MAGIC_BYTE_0; + throttle_command->magic[1] = OSCC_MAGIC_BYTE_1; + throttle_command->enable = 0; + + check_for_incoming_message(); +} + + +WHEN("^a fault report is received$") +{ + g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL; + g_mock_mcp_can_read_msg_buf_id = OSCC_FAULT_REPORT_CAN_ID; + + oscc_fault_report_s * fault_report = + (oscc_fault_report_s *) g_mock_mcp_can_read_msg_buf_buf; + + fault_report->magic[0] = OSCC_MAGIC_BYTE_0; + fault_report->magic[1] = OSCC_MAGIC_BYTE_1; + + check_for_incoming_message(); +} + + +WHEN("^a command is received with spoof values (.*) and (.*)$") +{ + REGEX_PARAM(uint16_t, high); + REGEX_PARAM(uint16_t, low); + + g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL; + g_mock_mcp_can_read_msg_buf_id = OSCC_THROTTLE_COMMAND_CAN_ID; + + oscc_throttle_command_s * throttle_command = + (oscc_throttle_command_s *) g_mock_mcp_can_read_msg_buf_buf; + + throttle_command->magic[0] = OSCC_MAGIC_BYTE_0; + throttle_command->magic[1] = OSCC_MAGIC_BYTE_1; + throttle_command->enable = 1; + throttle_command->spoof_value_high = high; + throttle_command->spoof_value_low = low; + + check_for_incoming_message(); + + update_throttle( + throttle_command->spoof_value_high, + throttle_command->spoof_value_low); +} diff --git a/firmware/throttle/tests/features/step_definitions/sending_reports.cpp b/firmware/throttle/tests/features/step_definitions/sending_reports.cpp new file mode 100644 index 00000000..a5877231 --- /dev/null +++ b/firmware/throttle/tests/features/step_definitions/sending_reports.cpp @@ -0,0 +1,43 @@ +WHEN("^a throttle report is published$") +{ + g_throttle_control_state.enabled = true; + g_throttle_control_state.operator_override = true; + g_throttle_control_state.dtcs = 0x55; + + publish_throttle_report(); +} + + +THEN("^a throttle report should be put on the control CAN bus$") +{ + assert_that(g_mock_mcp_can_send_msg_buf_id, is_equal_to(OSCC_THROTTLE_REPORT_CAN_ID)); + assert_that(g_mock_mcp_can_send_msg_buf_ext, is_equal_to(CAN_STANDARD)); + assert_that(g_mock_mcp_can_send_msg_buf_len, is_equal_to(OSCC_THROTTLE_REPORT_CAN_DLC)); +} + + +THEN("^the throttle report's fields should be set$") +{ + oscc_throttle_report_s * throttle_report = + (oscc_throttle_report_s *) g_mock_mcp_can_send_msg_buf_buf; + + assert_that( + throttle_report->magic[0], + is_equal_to(OSCC_MAGIC_BYTE_0)); + + assert_that( + throttle_report->magic[1], + is_equal_to(OSCC_MAGIC_BYTE_1)); + + assert_that( + throttle_report->enabled, + is_equal_to(g_throttle_control_state.enabled)); + + assert_that( + throttle_report->operator_override, + is_equal_to(g_throttle_control_state.operator_override)); + + assert_that( + throttle_report->dtcs, + is_equal_to(g_throttle_control_state.dtcs)); +} diff --git a/firmware/throttle/tests/features/step_definitions/test.cpp b/firmware/throttle/tests/features/step_definitions/test.cpp new file mode 100644 index 00000000..5f7d398e --- /dev/null +++ b/firmware/throttle/tests/features/step_definitions/test.cpp @@ -0,0 +1,5 @@ +// include source files to prevent step files from conflicting with each other +#include "common.cpp" +#include "checking_faults.cpp" +#include "receiving_messages.cpp" +#include "sending_reports.cpp" diff --git a/platforms/kia_soul/firmware/brake/tests/features/step_definitions/test.cpp b/firmware/throttle/tests/features/step_definitions/test.cpp.orig similarity index 71% rename from platforms/kia_soul/firmware/brake/tests/features/step_definitions/test.cpp rename to firmware/throttle/tests/features/step_definitions/test.cpp.orig index c6e16a0e..ac0a65e2 100644 --- a/platforms/kia_soul/firmware/brake/tests/features/step_definitions/test.cpp +++ b/firmware/throttle/tests/features/step_definitions/test.cpp.orig @@ -1,8 +1,10 @@ // include source files to prevent step files from conflicting with each other #include "common.cpp" +<<<<<<< HEAD +#include "checking_faults.cpp" +======= #include "checking_actuator_functionality.cpp" #include "checking_sensor_validity.cpp" +>>>>>>> 7573ad032a09a9325d6dc7400c19e9532fea7547 #include "receiving_commands.cpp" -#include "receiving_reports.cpp" #include "sending_reports.cpp" -#include "timeouts_and_overrides.cpp" diff --git a/platforms/kia_soul/firmware/throttle/tests/features/support/env.rb b/firmware/throttle/tests/features/support/env.rb similarity index 100% rename from platforms/kia_soul/firmware/throttle/tests/features/support/env.rb rename to firmware/throttle/tests/features/support/env.rb diff --git a/platforms/kia_soul/firmware/throttle/tests/property/Cargo.toml b/firmware/throttle/tests/property/Cargo.toml similarity index 83% rename from platforms/kia_soul/firmware/throttle/tests/property/Cargo.toml rename to firmware/throttle/tests/property/Cargo.toml index c8f12ddd..4e96a03d 100644 --- a/platforms/kia_soul/firmware/throttle/tests/property/Cargo.toml +++ b/firmware/throttle/tests/property/Cargo.toml @@ -6,13 +6,12 @@ build = "build.rs" description = "The manifest file for the throttle module property-based tests. To run the tests, run the command 'cargo test -- --test-threads=1'" [dependencies] -quickcheck = "0.3" -lazy_static = "0.2.8" +quickcheck = "0.4.1" rand = "0.3.15" [build-dependencies] -bindgen = "0.20" -gcc = "0.3" +bindgen = "0.25.0" +gcc = "0.3.51" [lib] name = "tests" diff --git a/firmware/throttle/tests/property/build.rs b/firmware/throttle/tests/property/build.rs new file mode 100644 index 00000000..a7984ce7 --- /dev/null +++ b/firmware/throttle/tests/property/build.rs @@ -0,0 +1,66 @@ +extern crate gcc; +extern crate bindgen; + +use std::env; +use std::path::Path; + +fn main() { + gcc::Config::new() + .flag("-w") + .define("KIA_SOUL", Some("ON")) + .include("include") + .include("../../include") + .include("../../../common/include") + .include("../../../common/testing/mocks/") + .include("../../../common/libs/can") + .include("../../../common/libs/dac") + .include("../../../../api/include") + .file("../../../common/testing/mocks/Arduino_mock.cpp") + .file("../../../common/testing/mocks/mcp_can_mock.cpp") + .file("../../../common/testing/mocks/DAC_MCP49xx_mock.cpp") + .file("../../../common/libs/can/oscc_can.cpp") + .file("../../../common/libs/dac/oscc_dac.cpp") + .file("../../src/communications.cpp") + .file("../../src/throttle_control.cpp") + .file("../../src/globals.cpp") + .compile("libthrottle_test.a"); + + let out_dir = env::var("OUT_DIR").unwrap(); + + let _ = bindgen::builder() + .header("include/wrapper.hpp") + .generate_comments(false) + .layout_tests(false) + .clang_arg("-DKIA_SOUL=ON") + .clang_arg("-I../../include") + .clang_arg("-I../../../common/include") + .clang_arg("-I../../../common/libs/can") + .clang_arg("-I../../../common/testing/mocks") + .clang_arg("-I../../../../api/include") + .whitelisted_function("publish_throttle_report") + .whitelisted_function("check_for_incoming_message") + .whitelisted_function("check_for_operator_override") + .whitelisted_var("OSCC_MAGIC_BYTE_0") + .whitelisted_var("OSCC_MAGIC_BYTE_1") + .whitelisted_var("OSCC_THROTTLE_REPORT_CAN_ID") + .whitelisted_var("OSCC_THROTTLE_REPORT_CAN_DLC") + .whitelisted_var("OSCC_THROTTLE_COMMAND_CAN_ID") + .whitelisted_var("OSCC_FAULT_REPORT_CAN_ID") + .whitelisted_var("OSCC_THROTTLE_COMMAND_CAN_DLC") + .whitelisted_var("ACCELERATOR_OVERRIDE_THRESHOLD") + .whitelisted_var("OSCC_THROTTLE_REPORT_PUBLISH_INTERVAL_IN_MSEC") + .whitelisted_var("CAN_MSGAVAIL") + .whitelisted_var("CAN_STANDARD") + .whitelisted_var("THROTTLE_SPOOF_LOW_SIGNAL_RANGE_MIN") + .whitelisted_var("THROTTLE_SPOOF_LOW_SIGNAL_RANGE_MAX") + .whitelisted_var("THROTTLE_SPOOF_HIGH_SIGNAL_RANGE_MIN") + .whitelisted_var("THROTTLE_SPOOF_HIGH_SIGNAL_RANGE_MAX") + .whitelisted_type("oscc_throttle_report_s") + .whitelisted_type("oscc_throttle_command_s") + .whitelisted_type("can_frame_s") + .whitelisted_type("throttle_control_state_s") + .generate() + .unwrap() + .write_to_file(Path::new(&out_dir).join("throttle_test.rs")) + .expect("Unable to generate bindings"); +} diff --git a/firmware/throttle/tests/property/include/wrapper.hpp b/firmware/throttle/tests/property/include/wrapper.hpp new file mode 100644 index 00000000..02fd161d --- /dev/null +++ b/firmware/throttle/tests/property/include/wrapper.hpp @@ -0,0 +1,7 @@ +#include "globals.h" +#include "communications.h" +#include "throttle_control.h" +#include "oscc_can.h" +#include "can_protocols/throttle_can_protocol.h" +#include "can_protocols/fault_can_protocol.h" +#include "vehicles.h" diff --git a/firmware/throttle/tests/property/src/tests.rs b/firmware/throttle/tests/property/src/tests.rs new file mode 100644 index 00000000..c60ae7d0 --- /dev/null +++ b/firmware/throttle/tests/property/src/tests.rs @@ -0,0 +1,310 @@ +#![allow(non_upper_case_globals)] +#![allow(non_camel_case_types)] +#![allow(non_snake_case)] +#![allow(dead_code)] +#![allow(unused_variables)] +#![allow(unused_imports)] +include!(concat!(env!("OUT_DIR"), "/throttle_test.rs")); + +extern crate quickcheck; +extern crate rand; + +use quickcheck::{QuickCheck, TestResult, Arbitrary, StdGen, Gen}; +use rand::Rng; + +extern "C" { + #[link_name = "g_mock_mcp_can_check_receive_return"] + pub static mut g_mock_mcp_can_check_receive_return: u8; + #[link_name = "g_mock_mcp_can_read_msg_buf_id"] + pub static mut g_mock_mcp_can_read_msg_buf_id: u32; + #[link_name = "g_mock_mcp_can_read_msg_buf_buf"] + pub static mut g_mock_mcp_can_read_msg_buf_buf: [u8; 8usize]; + #[link_name = "g_mock_mcp_can_send_msg_buf_id"] + pub static mut g_mock_mcp_can_send_msg_buf_id: u32; + #[link_name = "g_mock_mcp_can_send_msg_buf_ext"] + pub static mut g_mock_mcp_can_send_msg_buf_ext: u8; + #[link_name = "g_mock_mcp_can_send_msg_buf_len"] + pub static mut g_mock_mcp_can_send_msg_buf_len: u8; + #[link_name = "g_mock_mcp_can_send_msg_buf_buf"] + pub static mut g_mock_mcp_can_send_msg_buf_buf: *mut u8; + #[link_name = "g_mock_arduino_analog_read_return"] + pub static mut g_mock_arduino_analog_read_return: isize; + #[link_name = "g_mock_dac_output_a"] + pub static mut g_mock_dac_output_a: u16; + #[link_name = "g_mock_dac_output_b"] + pub static mut g_mock_dac_output_b: u16; + #[link_name = "g_throttle_control_state"] + pub static mut g_throttle_control_state: throttle_control_state_s; +} + +impl Arbitrary for oscc_throttle_report_s { + fn arbitrary(g: &mut G) -> oscc_throttle_report_s { + oscc_throttle_report_s { + magic: [OSCC_MAGIC_BYTE_0 as u8, OSCC_MAGIC_BYTE_1 as u8], + enabled: u8::arbitrary(g), + operator_override: u8::arbitrary(g), + dtcs: u8::arbitrary(g), + reserved: [u8::arbitrary(g); 3] + } + } +} + +impl Arbitrary for oscc_throttle_command_s { + fn arbitrary(g: &mut G) -> oscc_throttle_command_s { + oscc_throttle_command_s { + magic: [OSCC_MAGIC_BYTE_0 as u8, OSCC_MAGIC_BYTE_1 as u8], + spoof_value_low: u16::arbitrary(g), + spoof_value_high: u16::arbitrary(g), + enable: u8::arbitrary(g), + reserved: u8::arbitrary(g) + } + } +} + + +impl Arbitrary for can_frame_s { + fn arbitrary(g: &mut G) -> can_frame_s { + can_frame_s { + id: u32::arbitrary(g), + dlc: u8::arbitrary(g), + timestamp: u32::arbitrary(g), + data: [u8::arbitrary(g); 8] + } + } +} + +/// the throttle firmware should not attempt processing any messages +/// that are not throttle or fault commands +/// this means that none of the throttle control state would +/// change, nor would it output any values onto the DAC. +fn prop_only_process_valid_messages(rx_can_msg: can_frame_s, enabled: bool, operator_override: bool, dtcs: u8) -> TestResult { + if rx_can_msg.id == OSCC_THROTTLE_COMMAND_CAN_ID || + rx_can_msg.id == OSCC_FAULT_REPORT_CAN_ID + { + return TestResult::discard(); + } + unsafe { + let dac_a = g_mock_dac_output_a; + let dac_b = g_mock_dac_output_b; + g_throttle_control_state.enabled = enabled; + g_throttle_control_state.operator_override = operator_override; + g_throttle_control_state.dtcs = dtcs; + + g_mock_mcp_can_read_msg_buf_id = rx_can_msg.id; + g_mock_mcp_can_read_msg_buf_buf = rx_can_msg.data; + g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL as u8; + + check_for_incoming_message(); + + TestResult::from_bool(g_throttle_control_state.enabled == + enabled && + g_throttle_control_state.operator_override == operator_override && + g_throttle_control_state.dtcs == dtcs && + g_mock_dac_output_a == dac_a && + g_mock_dac_output_b == dac_b) + } +} + +#[test] +fn check_message_type_validity() { + QuickCheck::new() + .tests(1000) + .gen(StdGen::new(rand::thread_rng(), u32::max_value() as usize)) + .quickcheck(prop_only_process_valid_messages as fn(can_frame_s, bool, bool, u8) -> TestResult) +} + +/// the throttle firmware should set the control state as enabled +/// upon reciept of a valid command throttle message telling it to enable +fn prop_process_enable_command(mut throttle_command_msg: oscc_throttle_command_s) -> TestResult { + unsafe { + throttle_command_msg.enable = 1u8; + + g_throttle_control_state.enabled = false; + g_throttle_control_state.operator_override = false; + + g_mock_mcp_can_read_msg_buf_id = OSCC_THROTTLE_COMMAND_CAN_ID; + g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL as u8; + g_mock_mcp_can_read_msg_buf_buf = std::mem::transmute(throttle_command_msg); + + check_for_incoming_message(); + + TestResult::from_bool(g_throttle_control_state.enabled == true) + } +} + +#[test] +fn check_process_enable_command() { + QuickCheck::new() + .tests(1000) + .quickcheck(prop_process_enable_command as fn(oscc_throttle_command_s) -> TestResult) +} + +/// the throttle firmware should set the control state as disabled +/// upon reciept of a valid command throttle message telling it to disable +fn prop_process_disable_command(mut throttle_command_msg: oscc_throttle_command_s) -> TestResult { + unsafe { + throttle_command_msg.enable = 0u8; + + g_mock_mcp_can_read_msg_buf_id = OSCC_THROTTLE_COMMAND_CAN_ID; + g_mock_mcp_can_read_msg_buf_buf = std::mem::transmute(throttle_command_msg); + g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL as u8; + + check_for_incoming_message(); + + TestResult::from_bool(g_throttle_control_state.enabled == false) + } +} + +#[test] +fn check_process_disable_command() { + QuickCheck::new() + .tests(1000) + .quickcheck(prop_process_disable_command as fn(oscc_throttle_command_s) -> TestResult) +} + +/// the throttle firmware should send requested spoof values +/// upon recieving a throttle command message +fn prop_output_accurate_spoofs(mut throttle_command_msg: oscc_throttle_command_s) -> TestResult { + throttle_command_msg.enable = 1u8; + throttle_command_msg.spoof_value_low = rand::thread_rng().gen_range(THROTTLE_SPOOF_LOW_SIGNAL_RANGE_MIN as u16, THROTTLE_SPOOF_LOW_SIGNAL_RANGE_MAX as u16); + throttle_command_msg.spoof_value_high = rand::thread_rng().gen_range(THROTTLE_SPOOF_HIGH_SIGNAL_RANGE_MIN as u16, THROTTLE_SPOOF_HIGH_SIGNAL_RANGE_MAX as u16); + unsafe { + g_throttle_control_state.enabled = true; + + g_mock_mcp_can_read_msg_buf_id = OSCC_THROTTLE_COMMAND_CAN_ID; + g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL as u8; + g_mock_mcp_can_read_msg_buf_buf = std::mem::transmute(throttle_command_msg); + + check_for_incoming_message(); + + TestResult::from_bool(g_mock_dac_output_b == throttle_command_msg.spoof_value_low && + g_mock_dac_output_a == + throttle_command_msg.spoof_value_high ) + } +} + +#[test] +fn check_output_accurate_spoofs() { + QuickCheck::new() + .tests(1000) + .gen(StdGen::new(rand::thread_rng(), u16::max_value() as usize)) + .quickcheck(prop_output_accurate_spoofs as fn(oscc_throttle_command_s) -> TestResult) +} + +/// the throttle firmware should constrain requested spoof values +/// upon recieving a throttle command message +fn prop_output_constrained_spoofs(mut throttle_command_msg: oscc_throttle_command_s) -> TestResult { + throttle_command_msg.enable = 1u8; + unsafe { + if (throttle_command_msg.spoof_value_low >= + THROTTLE_SPOOF_HIGH_SIGNAL_RANGE_MIN as u16 && + throttle_command_msg.spoof_value_low <= + THROTTLE_SPOOF_HIGH_SIGNAL_RANGE_MAX as u16) || + (throttle_command_msg.spoof_value_high >= + THROTTLE_SPOOF_HIGH_SIGNAL_RANGE_MIN as u16 && + throttle_command_msg.spoof_value_high <= + THROTTLE_SPOOF_HIGH_SIGNAL_RANGE_MAX as u16) + { + return TestResult::discard(); + } + + g_throttle_control_state.enabled = true; + + g_mock_mcp_can_read_msg_buf_id = OSCC_THROTTLE_COMMAND_CAN_ID; + g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL as u8; + g_mock_mcp_can_read_msg_buf_buf = std::mem::transmute(throttle_command_msg); + + check_for_incoming_message(); + + TestResult::from_bool( + g_mock_dac_output_a >= THROTTLE_SPOOF_HIGH_SIGNAL_RANGE_MIN as u16 && + g_mock_dac_output_a <= THROTTLE_SPOOF_HIGH_SIGNAL_RANGE_MAX as u16 && + g_mock_dac_output_b >= THROTTLE_SPOOF_HIGH_SIGNAL_RANGE_MIN as u16 && + g_mock_dac_output_b <= THROTTLE_SPOOF_HIGH_SIGNAL_RANGE_MAX as u16) + } +} + +#[test] +fn check_output_constrained_spoofs() { + QuickCheck::new() + .tests(1000) + .gen(StdGen::new(rand::thread_rng(), u16::max_value() as usize)) + .quickcheck(prop_output_constrained_spoofs as fn(oscc_throttle_command_s) -> TestResult) +} + +/// the throttle firmware should create only valid CAN frames +fn prop_send_valid_can_fields(enabled: bool, + operator_override: bool, + dtcs: u8) + -> TestResult { + unsafe { + g_throttle_control_state.operator_override = operator_override; + g_throttle_control_state.enabled = enabled; + g_throttle_control_state.dtcs = dtcs; + + publish_throttle_report(); + + let throttle_report_msg = g_mock_mcp_can_send_msg_buf_buf as *mut oscc_throttle_report_s; + + TestResult::from_bool((*throttle_report_msg).enabled == enabled as u8 &&(*throttle_report_msg).operator_override == operator_override as u8 && + (*throttle_report_msg).dtcs == dtcs) + } +} + +#[test] +fn check_valid_can_frame() { + QuickCheck::new() + .tests(10) + .gen(StdGen::new(rand::thread_rng(), u8::max_value() as usize)) + .quickcheck(prop_send_valid_can_fields as fn(bool, bool, u8) -> TestResult) +} + +// the throttle firmware should be able to correctly and consistently +// detect operator overrides, disable on reciept, and send a fault report +fn prop_check_operator_override(analog_read_spoof: u16) -> TestResult { + unsafe { + g_throttle_control_state.enabled = true; + g_throttle_control_state.operator_override = false; + g_mock_arduino_analog_read_return = analog_read_spoof as isize; + + check_for_operator_override(); + + if analog_read_spoof >= (ACCELERATOR_OVERRIDE_THRESHOLD as u16) { + TestResult::from_bool(g_throttle_control_state.operator_override == true && g_throttle_control_state.enabled == false && + g_mock_mcp_can_send_msg_buf_id == OSCC_FAULT_REPORT_CAN_ID) + } else { + TestResult::from_bool(g_throttle_control_state.operator_override == false) + } + } +} + +#[test] +fn check_operator_override() { + QuickCheck::new() + .tests(1000) + .gen(StdGen::new(rand::thread_rng(), u16::max_value() as usize)) + .quickcheck(prop_check_operator_override as fn(u16) -> TestResult) +} + +/// the throttle firmware should set disable itself when it recieves a +/// fault report from any other module +fn prop_process_fault_command(enabled: bool, operator_override: bool) -> TestResult { + unsafe { + g_throttle_control_state.enabled = enabled; + g_throttle_control_state.operator_override = operator_override; + + g_mock_mcp_can_read_msg_buf_id = OSCC_FAULT_REPORT_CAN_ID; + g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL as u8; + + check_for_incoming_message(); + + TestResult::from_bool(g_throttle_control_state.enabled == false) + } +} + +#[test] +fn check_process_fault_command() { + QuickCheck::new() + .tests(1000) + .quickcheck(prop_process_fault_command as fn(bool, bool) -> TestResult) +} diff --git a/3d_models/brake_enclosure/actuator_control_board_1.0.0.SLDASM b/hardware/3d_models/brake_enclosure/actuator_control_board_1.0.0.SLDASM similarity index 100% rename from 3d_models/brake_enclosure/actuator_control_board_1.0.0.SLDASM rename to hardware/3d_models/brake_enclosure/actuator_control_board_1.0.0.SLDASM diff --git a/3d_models/brake_enclosure/actuator_control_board_enclosure_assembly.SLDASM b/hardware/3d_models/brake_enclosure/actuator_control_board_enclosure_assembly.SLDASM similarity index 100% rename from 3d_models/brake_enclosure/actuator_control_board_enclosure_assembly.SLDASM rename to hardware/3d_models/brake_enclosure/actuator_control_board_enclosure_assembly.SLDASM diff --git a/3d_models/brake_enclosure/actuator_control_board_enclosure_bottom.SLDPRT b/hardware/3d_models/brake_enclosure/actuator_control_board_enclosure_bottom.SLDPRT similarity index 100% rename from 3d_models/brake_enclosure/actuator_control_board_enclosure_bottom.SLDPRT rename to hardware/3d_models/brake_enclosure/actuator_control_board_enclosure_bottom.SLDPRT diff --git a/3d_models/brake_enclosure/actuator_control_board_enclosure_bottom.STL b/hardware/3d_models/brake_enclosure/actuator_control_board_enclosure_bottom.STL similarity index 100% rename from 3d_models/brake_enclosure/actuator_control_board_enclosure_bottom.STL rename to hardware/3d_models/brake_enclosure/actuator_control_board_enclosure_bottom.STL diff --git a/3d_models/brake_enclosure/actuator_control_board_enclosure_top.SLDPRT b/hardware/3d_models/brake_enclosure/actuator_control_board_enclosure_top.SLDPRT similarity index 100% rename from 3d_models/brake_enclosure/actuator_control_board_enclosure_top.SLDPRT rename to hardware/3d_models/brake_enclosure/actuator_control_board_enclosure_top.SLDPRT diff --git a/3d_models/brake_enclosure/actuator_control_board_enclosure_top.STL b/hardware/3d_models/brake_enclosure/actuator_control_board_enclosure_top.STL similarity index 100% rename from 3d_models/brake_enclosure/actuator_control_board_enclosure_top.STL rename to hardware/3d_models/brake_enclosure/actuator_control_board_enclosure_top.STL diff --git a/3d_models/brake_enclosure/brake_assembly.SLDASM b/hardware/3d_models/brake_enclosure/brake_assembly.SLDASM similarity index 100% rename from 3d_models/brake_enclosure/brake_assembly.SLDASM rename to hardware/3d_models/brake_enclosure/brake_assembly.SLDASM diff --git a/3d_models/brake_enclosure/brake_cable_clamp.SLDPRT b/hardware/3d_models/brake_enclosure/brake_cable_clamp.SLDPRT similarity index 100% rename from 3d_models/brake_enclosure/brake_cable_clamp.SLDPRT rename to hardware/3d_models/brake_enclosure/brake_cable_clamp.SLDPRT diff --git a/3d_models/brake_enclosure/brake_cable_clamp.STL b/hardware/3d_models/brake_enclosure/brake_cable_clamp.STL similarity index 100% rename from 3d_models/brake_enclosure/brake_cable_clamp.STL rename to hardware/3d_models/brake_enclosure/brake_cable_clamp.STL diff --git a/3d_models/brake_enclosure/brake_house_lid.SLDPRT b/hardware/3d_models/brake_enclosure/brake_house_lid.SLDPRT similarity index 100% rename from 3d_models/brake_enclosure/brake_house_lid.SLDPRT rename to hardware/3d_models/brake_enclosure/brake_house_lid.SLDPRT diff --git a/3d_models/brake_enclosure/brake_house_lid.STL b/hardware/3d_models/brake_enclosure/brake_house_lid.STL similarity index 100% rename from 3d_models/brake_enclosure/brake_house_lid.STL rename to hardware/3d_models/brake_enclosure/brake_house_lid.STL diff --git a/3d_models/brake_enclosure/brake_housing.SLDPRT b/hardware/3d_models/brake_enclosure/brake_housing.SLDPRT similarity index 100% rename from 3d_models/brake_enclosure/brake_housing.SLDPRT rename to hardware/3d_models/brake_enclosure/brake_housing.SLDPRT diff --git a/3d_models/brake_enclosure/brake_housing.STL b/hardware/3d_models/brake_enclosure/brake_housing.STL similarity index 100% rename from 3d_models/brake_enclosure/brake_housing.STL rename to hardware/3d_models/brake_enclosure/brake_housing.STL diff --git a/3d_models/dash_enclosure/ArduinoUnoMockup.SLDPRT b/hardware/3d_models/dash_enclosure/ArduinoUnoMockup.SLDPRT similarity index 100% rename from 3d_models/dash_enclosure/ArduinoUnoMockup.SLDPRT rename to hardware/3d_models/dash_enclosure/ArduinoUnoMockup.SLDPRT diff --git a/3d_models/dash_enclosure/CANShieldMockup.SLDPRT b/hardware/3d_models/dash_enclosure/CANShieldMockup.SLDPRT similarity index 100% rename from 3d_models/dash_enclosure/CANShieldMockup.SLDPRT rename to hardware/3d_models/dash_enclosure/CANShieldMockup.SLDPRT diff --git a/3d_models/dash_enclosure/STC_assembly.SLDASM b/hardware/3d_models/dash_enclosure/STC_assembly.SLDASM similarity index 100% rename from 3d_models/dash_enclosure/STC_assembly.SLDASM rename to hardware/3d_models/dash_enclosure/STC_assembly.SLDASM diff --git a/3d_models/dash_enclosure/STC_housing.STL b/hardware/3d_models/dash_enclosure/STC_housing.STL similarity index 100% rename from 3d_models/dash_enclosure/STC_housing.STL rename to hardware/3d_models/dash_enclosure/STC_housing.STL diff --git a/3d_models/dash_enclosure/STC_housing.bfb b/hardware/3d_models/dash_enclosure/STC_housing.bfb similarity index 100% rename from 3d_models/dash_enclosure/STC_housing.bfb rename to hardware/3d_models/dash_enclosure/STC_housing.bfb diff --git a/3d_models/dash_enclosure/STC_housing.cubepro b/hardware/3d_models/dash_enclosure/STC_housing.cubepro similarity index 100% rename from 3d_models/dash_enclosure/STC_housing.cubepro rename to hardware/3d_models/dash_enclosure/STC_housing.cubepro diff --git a/3d_models/dash_enclosure/STC_housing.sldprt b/hardware/3d_models/dash_enclosure/STC_housing.sldprt similarity index 100% rename from 3d_models/dash_enclosure/STC_housing.sldprt rename to hardware/3d_models/dash_enclosure/STC_housing.sldprt diff --git a/3d_models/dash_enclosure/STC_lid.SLDPRT b/hardware/3d_models/dash_enclosure/STC_lid.SLDPRT similarity index 100% rename from 3d_models/dash_enclosure/STC_lid.SLDPRT rename to hardware/3d_models/dash_enclosure/STC_lid.SLDPRT diff --git a/3d_models/dash_enclosure/STC_lid.STL b/hardware/3d_models/dash_enclosure/STC_lid.STL similarity index 100% rename from 3d_models/dash_enclosure/STC_lid.STL rename to hardware/3d_models/dash_enclosure/STC_lid.STL diff --git a/3d_models/estop_cupholder/EStopHousing.SLDPRT b/hardware/3d_models/estop_cupholder/EStopHousing.SLDPRT similarity index 100% rename from 3d_models/estop_cupholder/EStopHousing.SLDPRT rename to hardware/3d_models/estop_cupholder/EStopHousing.SLDPRT diff --git a/3d_models/estop_cupholder/EStopHousing.STL b/hardware/3d_models/estop_cupholder/EStopHousing.STL similarity index 100% rename from 3d_models/estop_cupholder/EStopHousing.STL rename to hardware/3d_models/estop_cupholder/EStopHousing.STL diff --git a/platforms/kia_soul/3d_models/actuator_bracket_assembly.SLDASM b/hardware/3d_models/kia_soul/actuator_bracket_assembly.SLDASM similarity index 100% rename from platforms/kia_soul/3d_models/actuator_bracket_assembly.SLDASM rename to hardware/3d_models/kia_soul/actuator_bracket_assembly.SLDASM diff --git a/platforms/kia_soul/3d_models/actuator_mockup.SLDPRT b/hardware/3d_models/kia_soul/actuator_mockup.SLDPRT similarity index 100% rename from platforms/kia_soul/3d_models/actuator_mockup.SLDPRT rename to hardware/3d_models/kia_soul/actuator_mockup.SLDPRT diff --git a/platforms/kia_soul/3d_models/bottom_bracket.PDF b/hardware/3d_models/kia_soul/bottom_bracket.PDF similarity index 100% rename from platforms/kia_soul/3d_models/bottom_bracket.PDF rename to hardware/3d_models/kia_soul/bottom_bracket.PDF diff --git a/platforms/kia_soul/3d_models/bottom_bracket.SLDDRW b/hardware/3d_models/kia_soul/bottom_bracket.SLDDRW similarity index 100% rename from platforms/kia_soul/3d_models/bottom_bracket.SLDDRW rename to hardware/3d_models/kia_soul/bottom_bracket.SLDDRW diff --git a/platforms/kia_soul/3d_models/bottom_bracket.SLDDRW.PDF b/hardware/3d_models/kia_soul/bottom_bracket.SLDDRW.PDF similarity index 100% rename from platforms/kia_soul/3d_models/bottom_bracket.SLDDRW.PDF rename to hardware/3d_models/kia_soul/bottom_bracket.SLDDRW.PDF diff --git a/platforms/kia_soul/3d_models/bottom_bracket.SLDPRT b/hardware/3d_models/kia_soul/bottom_bracket.SLDPRT similarity index 100% rename from platforms/kia_soul/3d_models/bottom_bracket.SLDPRT rename to hardware/3d_models/kia_soul/bottom_bracket.SLDPRT diff --git a/platforms/kia_soul/3d_models/reservoir_bracket.PDF b/hardware/3d_models/kia_soul/reservoir_bracket.PDF similarity index 100% rename from platforms/kia_soul/3d_models/reservoir_bracket.PDF rename to hardware/3d_models/kia_soul/reservoir_bracket.PDF diff --git a/platforms/kia_soul/3d_models/reservoir_bracket.SLDDRW b/hardware/3d_models/kia_soul/reservoir_bracket.SLDDRW similarity index 100% rename from platforms/kia_soul/3d_models/reservoir_bracket.SLDDRW rename to hardware/3d_models/kia_soul/reservoir_bracket.SLDDRW diff --git a/platforms/kia_soul/3d_models/reservoir_bracket.SLDPRT b/hardware/3d_models/kia_soul/reservoir_bracket.SLDPRT similarity index 100% rename from platforms/kia_soul/3d_models/reservoir_bracket.SLDPRT rename to hardware/3d_models/kia_soul/reservoir_bracket.SLDPRT diff --git a/platforms/kia_soul/3d_models/side_bracket.PDF b/hardware/3d_models/kia_soul/side_bracket.PDF similarity index 100% rename from platforms/kia_soul/3d_models/side_bracket.PDF rename to hardware/3d_models/kia_soul/side_bracket.PDF diff --git a/platforms/kia_soul/3d_models/side_bracket.SLDDRW b/hardware/3d_models/kia_soul/side_bracket.SLDDRW similarity index 100% rename from platforms/kia_soul/3d_models/side_bracket.SLDDRW rename to hardware/3d_models/kia_soul/side_bracket.SLDDRW diff --git a/platforms/kia_soul/3d_models/side_bracket.SLDPRT b/hardware/3d_models/kia_soul/side_bracket.SLDPRT similarity index 100% rename from platforms/kia_soul/3d_models/side_bracket.SLDPRT rename to hardware/3d_models/kia_soul/side_bracket.SLDPRT diff --git a/boards/actuator_control_board/actuator_control_board_1.0.0.brd b/hardware/boards/actuator_control_board/actuator_control_board_1.0.0.brd similarity index 100% rename from boards/actuator_control_board/actuator_control_board_1.0.0.brd rename to hardware/boards/actuator_control_board/actuator_control_board_1.0.0.brd diff --git a/boards/actuator_control_board/actuator_control_board_1.0.0.sch b/hardware/boards/actuator_control_board/actuator_control_board_1.0.0.sch similarity index 100% rename from boards/actuator_control_board/actuator_control_board_1.0.0.sch rename to hardware/boards/actuator_control_board/actuator_control_board_1.0.0.sch diff --git a/boards/actuator_control_board/actuator_control_board_sch_1.0.0.pdf b/hardware/boards/actuator_control_board/actuator_control_board_sch_1.0.0.pdf similarity index 100% rename from boards/actuator_control_board/actuator_control_board_sch_1.0.0.pdf rename to hardware/boards/actuator_control_board/actuator_control_board_sch_1.0.0.pdf diff --git a/boards/sensor_interface_board/sensor_interface.brd b/hardware/boards/sensor_interface_board/sensor_interface.brd similarity index 100% rename from boards/sensor_interface_board/sensor_interface.brd rename to hardware/boards/sensor_interface_board/sensor_interface.brd diff --git a/boards/sensor_interface_board/sensor_interface_BOM.xlsx b/hardware/boards/sensor_interface_board/sensor_interface_BOM.xlsx similarity index 100% rename from boards/sensor_interface_board/sensor_interface_BOM.xlsx rename to hardware/boards/sensor_interface_board/sensor_interface_BOM.xlsx diff --git a/boards/sensor_interface_board/sensor_interface_board_1.0.0.sch b/hardware/boards/sensor_interface_board/sensor_interface_board_1.0.0.sch similarity index 100% rename from boards/sensor_interface_board/sensor_interface_board_1.0.0.sch rename to hardware/boards/sensor_interface_board/sensor_interface_board_1.0.0.sch diff --git a/boards/sensor_interface_board/sensor_interface_board_sch_1.0.0.pdf b/hardware/boards/sensor_interface_board/sensor_interface_board_sch_1.0.0.pdf similarity index 100% rename from boards/sensor_interface_board/sensor_interface_board_sch_1.0.0.pdf rename to hardware/boards/sensor_interface_board/sensor_interface_board_sch_1.0.0.pdf diff --git a/boards/tests/brake_controller_r0.md b/hardware/boards/tests/brake_controller_r0.md similarity index 100% rename from boards/tests/brake_controller_r0.md rename to hardware/boards/tests/brake_controller_r0.md diff --git a/boards/tests/steering_controller_r0.md b/hardware/boards/tests/steering_controller_r0.md similarity index 100% rename from boards/tests/steering_controller_r0.md rename to hardware/boards/tests/steering_controller_r0.md diff --git a/boards/tests/throttle_controller_r0.md b/hardware/boards/tests/throttle_controller_r0.md similarity index 100% rename from boards/tests/throttle_controller_r0.md rename to hardware/boards/tests/throttle_controller_r0.md diff --git a/platforms/common/include/brake_can_protocol.h b/platforms/common/include/brake_can_protocol.h deleted file mode 100644 index df909288..00000000 --- a/platforms/common/include/brake_can_protocol.h +++ /dev/null @@ -1,169 +0,0 @@ -/** - * @file brake_can_protocol.h - * @brief Brake CAN Protocol. - * - */ - - -#ifndef _OSCC_BRAKE_CAN_PROTOCOL_H_ -#define _OSCC_BRAKE_CAN_PROTOCOL_H_ - - -#include - - -/* - * @brief Brake command message (CAN frame) ID. - * - */ -#define OSCC_COMMAND_BRAKE_CAN_ID (0x060) - -/* - * @brief Brake report message (CAN frame) ID. - * - */ -#define OSCC_REPORT_BRAKE_CAN_ID (0x061) - -/* - * @brief Brake report message (CAN frame) length. - * - */ -#define OSCC_REPORT_BRAKE_CAN_DLC (8) - -/* - * @brief Brake report message publishing interval. [milliseconds] - * - */ -#define OSCC_REPORT_BRAKE_PUBLISH_INTERVAL_IN_MSEC (50) - - -/** - * @brief Brake command message data. - * - * Message size (CAN frame DLC): 8 bytes - * - */ -typedef struct -{ - uint16_t pedal_command; /*!< Pedal command. [65535 == 100%] */ - - uint8_t reserved_0 : 1; /*!< Reserved. */ - - uint8_t reserved_1 : 7; /*!< Reserved. */ - - uint8_t enabled : 1; /*!< This brake control command/request enabled. - * Value zero means off/disabled. - * Value one means on/enabled. */ - - uint8_t reserved_2 : 1; /*!< Reserved. */ - - uint8_t reserved_3 : 1; /*!< Reserved. */ - - uint8_t reserved_4 : 5; /*!< Reserved. */ - - uint8_t reserved_5; /*!< Reserved. */ - - uint8_t reserved_6; /*!< Reserved. */ - - uint8_t reserved_7; /*!< Reserved. */ - - uint8_t reserved_8; /*!< Reserved. */ -} oscc_command_brake_data_s; - - -/** - * @brief Brake command message. - * - * CAN frame ID: \ref OSCC_COMMAND_BRAKE_CAN_ID - * - */ -typedef struct -{ - uint32_t timestamp; /* Timestamp when command was received by the firmware. */ - - oscc_command_brake_data_s data; /* CAN frame data. */ -} oscc_command_brake_s; - - -/** - * @brief Brake report message data. - * - * Message size (CAN frame DLC): \ref OSCC_REPORT_BRAKE_CAN_DLC - * - */ -typedef struct -{ - int16_t pedal_input; /*!< Pedal input value from - * the physical pedal. [65535 == 100%] */ - - uint16_t pedal_command; /*!< Pedal command value from - * the command message. [65535 == 100%] */ - - uint16_t pedal_output; /*!< Pedal output value. - * Set to the maximum of - * pedal_input and pedal command. [65535 == 100%] */ - - uint8_t reserved_0 : 1; /*!< Reserved. */ - - uint8_t reserved_1 : 1; /*!< Reserved. */ - - uint8_t reserved_2 : 1; /*!< Reserved. */ - - uint8_t reserved_3 : 1; /*!< Reserved. */ - - uint8_t reserved_4 : 4; /*!< Reserved. */ - - uint8_t enabled : 1; /*!< Brake controls enabled state. - * Value zero means off/disabled (commands are ignored). - * Value one means on/enabled (no timeouts or overrides have occured). */ - - uint8_t override : 1; /*!< Driver override state. - * Value zero means controls are provided autonomously (no override). - * Value one means controls are provided by the driver. */ - - uint8_t fault_invalid_sensor_value : 1; /*!< Invalid sensor value indicator. - * Value zero means the values read - * from the sensors are valid. - * - * Value one means the values read - * from the sensors are invalid. */ - - uint8_t fault_startup_pressure_check_error : 1; /*!< Actuator sensors report - * values that do not match - * the static values - * expected with PCK1 and - * PCK2 asserted. */ - - uint8_t fault_obd_timeout : 1; /*!< OBD timeout indicator. - * Value zero means no timeout occurred. - * Value one means timeout occurred. */ - - uint8_t fault_startup_pump_motor_check_error : 1; /*!< Pump motor check signal - * (MTT) is not asserted - * when pump is on. */ - - uint8_t reserved_5 : 1; /*!< Reserved. */ - - uint8_t reserved_6 : 1; /*!< Reserved. */ -} oscc_report_brake_data_s; - - -/** - * @brief Brake report message. - * - * CAN frame ID: \ref OSCC_REPORT_BRAKE_CAN_ID - * - */ -typedef struct -{ - uint32_t id; /* CAN frame ID. */ - - uint8_t dlc; /* CAN frame data length. */ - - uint32_t timestamp; /* Timestamp when report was put on the bus. */ - - oscc_report_brake_data_s data; /* CAN frame data. */ -} oscc_report_brake_s; - - -#endif /* _OSCC_BRAKE_CAN_PROTOCOL_H_ */ diff --git a/platforms/common/include/chassis_state_can_protocol.h b/platforms/common/include/chassis_state_can_protocol.h deleted file mode 100644 index a6e82b60..00000000 --- a/platforms/common/include/chassis_state_can_protocol.h +++ /dev/null @@ -1,168 +0,0 @@ -/** - * @file chassis_state_can_protocol.h - * @brief Chassis State CAN Protocol. - * - */ - - -#ifndef _OSCC_CHASSIS_STATE_CAN_PROTOCOL_H_ -#define _OSCC_CHASSIS_STATE_CAN_PROTOCOL_H_ - - -#include - - -/* - * @brief Chassis State 1 report message (CAN frame) ID. - * - */ -#define OSCC_REPORT_CHASSIS_STATE_1_CAN_ID (0x210) - -/* - * @brief Chassis State 1 report message (CAN frame) length. - * - */ -#define OSCC_REPORT_CHASSIS_STATE_1_CAN_DLC (8) - -/* - * @brief Chassis State 1 report message publishing interval. [milliseconds] - * - */ -#define OSCC_REPORT_CHASSIS_STATE_1_PUBLISH_INTERVAL_IN_MSEC (50) - -/* - * @brief Chassis State 1 report message flag indicating valid steering wheel angle. - * - */ -#define OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_STEER_WHEEL_ANGLE_VALID (0x02) - -/* - * @brief Chassis State 1 report message flag indicating valid steering wheel angle rate. - * - */ -#define OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_STEER_WHEEL_ANGLE_RATE_VALID (0x04) - -/* - * @brief Chassis State 1 report message flag indicating valid brake pressure. - * - */ -#define OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_BRAKE_PRESSURE_VALID (0x08) - -/* - * @brief Chassis State 1 report message flag indicating valid wheel speed. - * - */ -#define OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_WHEEL_SPEED_VALID (0x10) - -/* - * @brief Chassis State 1 report message flag indicating left turn signal is on. - * - */ -#define OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_LEFT_TURN_SIGNAL_ON (0x20) - -/* - * @brief Chassis State 1 report message flag indicating right turn signal is on. - * - */ -#define OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_RIGHT_TURN_SIGNAL_ON (0x40) - -/* - * @brief Chassis State 1 report message flag indicating brake signal is on. - * - */ -#define OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_BRAKE_SIGNAL_ON (0x80) - -/* - * @brief Chassis State 2 report message (CAN frame) ID. - * - */ -#define OSCC_REPORT_CHASSIS_STATE_2_CAN_ID (0x211) - -/* - * @brief Chassis State 2 report message (CAN frame) length. - * - */ -#define OSCC_REPORT_CHASSIS_STATE_2_CAN_DLC (8) - -/* - * @brief Chassis State 2 report message publishing interval. [milliseconds] - * - */ -#define OSCC_REPORT_CHASSIS_STATE_2_PUBLISH_INTERVAL_IN_MSEC (50) - - -/** - * @brief Chassis State 1 report message data. - * - * Message size (CAN frame DLC): \ref OSCC_REPORT_CHASSIS_STATE_1_CAN_DLC - * - */ -typedef struct -{ - uint8_t flags; /* Stauts flags. */ - - uint8_t reserved; /* Reserved. */ - - int16_t steering_wheel_angle; /* Steering wheel angle. */ - - int16_t steering_wheel_angle_rate; /* Steering wheel angle rate. */ - - int16_t brake_pressure; /* Brake pressure. */ -} oscc_report_chassis_state_1_data_s; - - -/** - * @brief Chassis State 1 report message. - * - * CAN frame ID: \ref OSCC_REPORT_CHASSIS_STATE_1_CAN_ID - * - */ -typedef struct -{ - uint32_t id; /* CAN frame ID. */ - - uint8_t dlc; /* CAN frame data length. */ - - uint32_t timestamp; /* Timestamp when report was put on the bus. */ - - oscc_report_chassis_state_1_data_s data; /* CAN frame data. */ -} oscc_report_chassis_state_1_s; - - -/** - * @brief Chassis State 2 report message data. - * - * Message size (CAN frame DLC): \ref OSCC_REPORT_CHASSIS_STATE_2_CAN_DLC - * - */ -typedef struct -{ - int16_t wheel_speed_front_left; /* Speed of front left wheel. */ - - int16_t wheel_speed_front_right; /* Speed of front right wheel. */ - - int16_t wheel_speed_rear_left; /* Speed of rear left wheel. */ - - int16_t wheel_speed_rear_right; /* Speed of rear right wheel. */ -} oscc_report_chassis_state_2_data_s; - - -/** - * @brief Chassis State 2 report message. - * - * CAN frame ID: \ref OSCC_REPORT_CHASSIS_STATE_2_CAN_ID - * - */ -typedef struct -{ - uint32_t id; /* CAN frame ID. */ - - uint8_t dlc; /* CAN frame data length. */ - - uint32_t timestamp; /* Timestamp when report was put on the bus. */ - - oscc_report_chassis_state_2_data_s data; /* CAN frame data. */ -} oscc_report_chassis_state_2_s; - - -#endif /* _OSCC_CHASSIS_STATE_CAN_PROTOCOL_H_ */ diff --git a/platforms/common/include/gateway_can_protocol.h b/platforms/common/include/gateway_can_protocol.h deleted file mode 100644 index 85de393c..00000000 --- a/platforms/common/include/gateway_can_protocol.h +++ /dev/null @@ -1,110 +0,0 @@ -/** - * @file gateway_can_protocol.h - * @brief Gateway CAN Protocol. - * - */ - - -#ifndef _OSCC_GATEWAY_CAN_PROTOCOL_H_ -#define _OSCC_GATEWAY_CAN_PROTOCOL_H_ - - -#include - - -/* - * @brief Node ID of CAN Gateway module. - * - */ -#define OSCC_MODULE_CAN_GATEWAY_NODE_ID (0x10) - -/* - * @brief Hardware version of CAN Gateway module. - * - */ -#define OSCC_MODULE_CAN_GATEWAY_VERSION_HARDWARE (0x01) - -/* - * @brief Firmware version of CAN Gateway module. - * - */ -#define OSCC_MODULE_CAN_GATEWAY_VERSION_FIRMWARE (0x01) - -/* - * @brief Heartbeat report message (CAN frame) ID. - * - */ -#define OSCC_REPORT_HEARTBEAT_CAN_ID (0x100) - -/* - * @brief Heartbeat report message (CAN frame) length. - * - */ -#define OSCC_REPORT_HEARTBEAT_CAN_DLC (8) - -/* - * @brief Heartbeat report message publishing interval. [milliseconds] - * - */ -#define OSCC_REPORT_HEARTBEAT_PUBLISH_INTERVAL_IN_MSEC (50) - -/* - * @brief Heartbeat state indicating heartbeat is invalid. - * - */ -#define OSCC_REPORT_HEARTBEAT_STATE_INVALID (0x00) - -/* - * @brief Heartbeat state indicating heartbeat is initializing. - * - */ -#define OSCC_REPORT_HEARTBEAT_STATE_INIT (0x01) - -/* - * @brief Heartbeat state indicating heartbeat is okay. - * - */ -#define OSCC_REPORT_HEARTBEAT_STATE_OK (0x02) - - -/** - * @brief Heartbeat report message data. - * - * Message size (CAN frame DLC): \ref OSCC_REPORT_HEARTBEAT_CAN_DLC - * - */ -typedef struct -{ - uint8_t hardware_version : 4; /* Module hardware version. */ - - uint8_t firmware_version : 4; /* Module firmware version. */ - - uint8_t state; /* Heartbeat state. */ - - uint16_t reserved; /* Reserved. */ - - uint16_t error_register; /* Register containing error flags. */ - - uint16_t warning_register; /* Register containing warning flags. */ -} oscc_report_heartbeat_data_s; - - -/** - * @brief Heartbeat report message. - * - * CAN frame ID: \ref OSCC_REPORT_HEARTBEAT_CAN_ID - * - */ -typedef struct -{ - uint32_t id; /* CAN frame ID. */ - - uint8_t dlc; /* CAN frame data length. */ - - uint32_t timestamp; /* Timestamp when report was put on the bus. */ - - oscc_report_heartbeat_data_s data; /* CAN frame data. */ -} oscc_report_heartbeat_s; - - -#endif /* _OSCC_GATEWAY_CAN_PROTOCOL_H_ */ diff --git a/platforms/common/include/steering_can_protocol.h b/platforms/common/include/steering_can_protocol.h deleted file mode 100644 index a6663cdd..00000000 --- a/platforms/common/include/steering_can_protocol.h +++ /dev/null @@ -1,166 +0,0 @@ -/** - * @file steering_can_protocol.h - * @brief Steering CAN Protocol. - * - */ - - -#ifndef _OSCC_STEERING_CAN_PROTOCOL_H_ -#define _OSCC_STEERING_CAN_PROTOCOL_H_ - - -#include - - -/* - * @brief Steering command message (CAN frame) ID. - * - */ -#define OSCC_COMMAND_STEERING_CAN_ID (0x64) - -/* - * @brief Steering report message (CAN frame) ID. - * - */ -#define OSCC_REPORT_STEERING_CAN_ID (0x65) - -/* - * @brief Steering report message (CAN frame) length. - * - */ -#define OSCC_REPORT_STEERING_CAN_DLC (8) - -/* - * @brief Steering report message publishing interval. [milliseconds] - * - */ -#define OSCC_REPORT_STEERING_PUBLISH_INTERVAL_IN_MSEC (20) - - -/** - * @brief Steering command message data. - * - * Message size (CAN frame DLC): 8 bytes - * - */ -typedef struct -{ - int16_t commanded_steering_wheel_angle; /*!< Steering wheel angle command. - * Positive means to the - * left (counter clockwise). - * [0.1 degrees per bit] */ - - uint8_t commanded_steering_wheel_angle_rate; /*!< Steering wheel angle rate - * command. - * Value zero means no limit. - * Value 0x01 means 2 degrees/second. - * Value 0xFA means 500 degrees/second. - * [2 degrees/second per bit] */ - - uint8_t enabled : 1; /*!< Steering control command/request enabled. - * Value zero means off/disabled. - * Value one means on/enabled. */ - - uint8_t reserved_0 : 1; /*!< Reserved. */ - - uint8_t reserved_1 : 1; /*!< Reserved. */ - - uint8_t reserved_2 : 5; /*!< Reserved. */ - - uint8_t reserved_3; /*!< Reserved. */ - - uint8_t reserved_4; /*!< Reserved. */ - - uint8_t reserved_5; /*!< Reserved. */ - - uint8_t reserved_6; /*!< Reserved. */ -} oscc_command_steering_data_s; - - -/** - * @brief Steering command message. - * - * CAN frame ID: \ref OSCC_COMMAND_STEERING_CAN_ID - * - */ -typedef struct -{ - uint32_t timestamp; /* Timestamp when command was received by the firmware. */ - - oscc_command_steering_data_s data; /* CAN frame data. */ -} oscc_command_steering_s; - - -/** - * @brief Steering report message data. - * - * Message size (CAN frame DLC): \ref OSCC_REPORT_STEERING_CAN_DLC - * - */ -typedef struct -{ - - int16_t current_steering_wheel_angle; /*!< Steering wheel angle reported by - * vehicle. - * Positive means to the left - * (counter clockwise). - * [0.1 degrees per bit] */ - - int16_t commanded_steering_wheel_angle; /*!< Steering wheel angle command. - * Positive means to the left - * (counter clockwise). - * [0.1 degrees per bit] */ - - uint16_t reserved_0; /*!< Reserved. */ - - int8_t spoofed_torque_output; /*!< Spoofed steering wheel torque output to the - * vehicle. [0.0625 Newton meters per bit] */ - - uint8_t enabled : 1; /*!< Steering controls enabled state. - * Value zero means off/disabled (commands are ignored). - * Value one means on/enabled (no timeouts or overrides have occured). */ - - uint8_t override : 1; /*!< Driver override state. - * Value zero means controls are provided autonomously (no override). - * Value one means controls are provided by the driver. */ - - uint8_t fault_invalid_sensor_value : 1; /*!< Invalid sensor value indicator. - * Value zero means the values read - * from the sensors are valid. - * - * Value one means the values read - * from the sensors are invalid. */ - - uint8_t reserved_1 : 1; /*!< Reserved. */ - - uint8_t fault_obd_timeout : 1; /*!< OBD timeout indicator. - * Value zero means no timeout occurred. - * Value one means timeout occurred. */ - - uint8_t reserved_2 : 1; /*!< Reserved */ - - uint8_t reserved_3 : 1; /*!< Reserved. */ - - uint8_t reserved_4 : 1; /*!< Reserved. */ -} oscc_report_steering_data_s; - - -/** - * @brief Steering report message. - * - * CAN frame ID: \ref OSCC_REPORT_STEERING_CAN_ID - * - */ -typedef struct -{ - uint32_t id; /* CAN frame ID. */ - - uint8_t dlc; /* CAN frame data length. */ - - uint32_t timestamp; /* Timestamp when report was put on the bus. */ - - oscc_report_steering_data_s data; /* CAN frame data. */ -} oscc_report_steering_s; - - -#endif /* _OSCC_STEERING_CAN_PROTOCOL_H_ */ diff --git a/platforms/common/include/throttle_can_protocol.h b/platforms/common/include/throttle_can_protocol.h deleted file mode 100644 index c7c068aa..00000000 --- a/platforms/common/include/throttle_can_protocol.h +++ /dev/null @@ -1,159 +0,0 @@ -/** - * @file throttle_can_protocol.h - * @brief Throttle CAN Protocol. - * - */ - - -#ifndef _OSCC_THROTTLE_CAN_PROTOCOL_H_ -#define _OSCC_THROTTLE_CAN_PROTOCOL_H_ - - -#include - - -/* - * @brief Throttle command message (CAN frame) ID. - * - */ -#define OSCC_COMMAND_THROTTLE_CAN_ID (0x62) - - -/* - * @brief Throttle report message (CAN frame) ID. - * - */ -#define OSCC_REPORT_THROTTLE_CAN_ID (0x63) - - -/* - * @brief Throttle report message (CAN frame) length. - * - */ -#define OSCC_REPORT_THROTTLE_CAN_DLC (8) - - -/* - * @brief Throttle report message publishing interval. [milliseconds] - * - */ -#define OSCC_REPORT_THROTTLE_PUBLISH_INTERVAL_IN_MSEC (20) - - -/** - * @brief Throttle command message data. - * - * Message size (CAN frame DLC): 8 bytes - * - */ -typedef struct -{ - uint16_t commanded_accelerator_position; /*!< Accelerator position command. - * [65535 == 100%] */ - - uint8_t reserved_0; /*!< Reserved. */ - - uint8_t enabled : 1; /*!< Throttle control command/request enabled. - * Value zero means off/disabled. - * Value one means on/enabled. */ - - uint8_t reserved_1 : 1; /*!< Reserved. */ - - uint8_t reserved_2 : 1; /*!< Reserved. */ - - uint8_t reserved_3 : 5; /*!< Reserved. */ - - uint8_t reserved_4; /*!< Reserved. */ - - uint8_t reserved_5; /*!< Reserved. */ - - uint8_t reserved_6; /*!< Reserved. */ - - uint8_t reserved_7; /*!< Reserved. */ -} oscc_command_throttle_data_s; - - -/** - * @brief Throttle command message. - * - * CAN frame ID: \ref OSCC_COMMAND_THROTTLE_CAN_ID - * - */ -typedef struct -{ - uint32_t timestamp; /* Timestamp when command was received by the firmware. */ - - oscc_command_throttle_data_s data; /* CAN frame data. */ -} oscc_command_throttle_s; - - -/** - * @brief Throttle report message data. - * - * Message size (CAN frame DLC): \ref OSCC_REPORT_THROTTLE_CAN_DLC - * - */ -typedef struct -{ - uint16_t current_accelerator_position; /*!< Current accelerator position as - * read by the acceleration position - * sensor. [65535 == 100%] */ - - uint16_t commanded_accelerator_position; /*!< Commanded accelerator position - * from the throttle command message. - * [65535 == 100%] */ - - uint16_t spoofed_accelerator_output; /*!< Spoof accelerator value output to - * the vehicle. - * [65535 == 100%] */ - - uint8_t reserved_0 : 4; /*!< Reserved. */ - - uint8_t reserved_1 : 4; /*!< Reserved. */ - - uint8_t enabled : 1; /*!< Throttle controls enabled state. - * Value zero means off/disabled (commands are ignored). - * Value one means on/enabled (no timeouts or overrides have occured). */ - - uint8_t override : 1; /*!< Driver override state. - * Value zero means controls are provided autonomously (no override). - * Value one means controls are provided by the driver. */ - - uint8_t fault_invalid_sensor_value : 1; /*!< Invalid sensor value indicator. - * Value zero means the values read - * from the sensors are valid. - * - * Value one means the values read - * from the sensors are invalid. */ - - uint8_t reserved_2 : 1; /*!< Reserved. */ - - uint8_t reserved_3 : 1; /*!< Reserved. */ - - uint8_t reserved_4 : 1; /*!< Reserved. */ - - uint8_t reserved_5 : 1; /*!< Reserved. */ - - uint8_t reserved_6 : 1; /*!< Reserved. */ -} oscc_report_throttle_data_s; - - -/** - * @brief Throttle report message. - * - * CAN frame ID: \ref OSCC_REPORT_THROTTLE_CAN_ID - * - */ -typedef struct -{ - uint32_t id; /* CAN frame ID. */ - - uint8_t dlc; /* CAN frame data length. */ - - uint32_t timestamp; /* Timestamp when report was put on the bus. */ - - oscc_report_throttle_data_s data; /* CAN frame data. */ -} oscc_report_throttle_s; - - -#endif /* _OSCC_THROTTLE_CAN_PROTOCOL_H_ */ diff --git a/platforms/common/libs/mcp_can/mcp_can.h b/platforms/common/libs/mcp_can/mcp_can.h deleted file mode 100644 index cbef86fd..00000000 --- a/platforms/common/libs/mcp_can/mcp_can.h +++ /dev/null @@ -1,117 +0,0 @@ -/* - mcp_can.h - 2012 Copyright (c) Seeed Technology Inc. All right reserved. - - Author:Loovee - Contributor: Cory J. Fowler - 2014-1-16 - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. - - This library is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with this library; if not, write to the Free Software - Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110- - 1301 USA -*/ -#ifndef _MCP2515_H_ -#define _MCP2515_H_ - -#include "mcp_can_dfs.h" - -#define MAX_CHAR_IN_MESSAGE 8 - -class MCP_CAN -{ - private: - - INT8U m_nExtFlg; /* identifier xxxID */ - /* either extended (the 29 LSB) */ - /* or standard (the 11 LSB) */ - INT32U m_nID; /* can id */ - INT8U m_nDlc; /* data length: */ - INT8U m_nDta[MAX_CHAR_IN_MESSAGE]; /* data */ - INT8U m_nRtr; /* rtr */ - INT8U m_nfilhit; - INT8U SPICS; - -/* -* mcp2515 driver function -*/ - // private: -private: - - void mcp2515_reset(void); /* reset mcp2515 */ - - INT8U mcp2515_readRegister(const INT8U address); /* read mcp2515's register */ - - void mcp2515_readRegisterS(const INT8U address, - INT8U values[], - const INT8U n); - void mcp2515_setRegister(const INT8U address, /* set mcp2515's register */ - const INT8U value); - - void mcp2515_setRegisterS(const INT8U address, /* set mcp2515's registers */ - const INT8U values[], - const INT8U n); - - void mcp2515_initCANBuffers(void); - - void mcp2515_modifyRegister(const INT8U address, /* set bit of one register */ - const INT8U mask, - const INT8U data); - - INT8U mcp2515_readStatus(void); /* read mcp2515's Status */ - INT8U mcp2515_setCANCTRL_Mode(const INT8U newmode); /* set mode */ - INT8U mcp2515_configRate(const INT8U canSpeed); /* set boadrate */ - INT8U mcp2515_init(const INT8U canSpeed); /* mcp2515init */ - - void mcp2515_write_id( const INT8U mcp_addr, /* write can id */ - const INT8U ext, - const INT32U id ); - - void mcp2515_read_id( const INT8U mcp_addr, /* read can id */ - INT8U* ext, - INT32U* id ); - - void mcp2515_write_canMsg( const INT8U buffer_sidh_addr ); /* write can msg */ - void mcp2515_read_canMsg( const INT8U buffer_sidh_addr); /* read can msg */ - void mcp2515_start_transmit(const INT8U mcp_addr); /* start transmit */ - INT8U mcp2515_getNextFreeTXBuf(INT8U *txbuf_n); /* get Next free txbuf */ - -/* -* can operator function -*/ - - INT8U setMsg(INT32U id, INT8U ext, INT8U len, INT8U rtr, INT8U *pData); /* set message */ - INT8U setMsg(INT32U id, INT8U ext, INT8U len, INT8U *pData); /* set message */ - INT8U clearMsg(); /* clear all message to zero */ - INT8U readMsg(); /* read message */ - INT8U sendMsg(); /* send message */ - -public: - MCP_CAN(INT8U _CS); - INT8U begin(INT8U speedset); /* init can */ - INT8U init_Mask(INT8U num, INT8U ext, INT32U ulData); /* init Masks */ - INT8U init_Filt(INT8U num, INT8U ext, INT32U ulData); /* init filters */ - INT8U sendMsgBuf(INT32U id, INT8U ext, INT8U rtr, INT8U len, INT8U *buf); /* send buf */ - INT8U sendMsgBuf(INT32U id, INT8U ext, INT8U len, INT8U *buf); /* send buf */ - INT8U readMsgBuf(INT8U *len, INT8U *buf); /* read buf */ - INT8U readMsgBufID(INT32U *ID, INT8U *len, INT8U *buf); /* read buf with object ID */ - INT8U checkReceive(void); /* if something received */ - INT8U checkError(void); /* if something error */ - INT32U getCanId(void); /* get can id when receive */ - INT8U isRemoteRequest(void); /* get RR flag when receive */ - INT8U isExtendedFrame(void); /* did we recieve 29bit frame? */ -}; - -#endif -/********************************************************************************************************* - END FILE -*********************************************************************************************************/ diff --git a/platforms/common/libs/signal_smoothing/oscc_signal_smoothing.cpp b/platforms/common/libs/signal_smoothing/oscc_signal_smoothing.cpp deleted file mode 100644 index 019c3484..00000000 --- a/platforms/common/libs/signal_smoothing/oscc_signal_smoothing.cpp +++ /dev/null @@ -1,16 +0,0 @@ -/** - * @file oscc_signal_smoothing.cpp - * - */ - - - #include "oscc_signal_smoothing.h" - - -float exponential_moving_average( - const float alpha, - const float current_value, - const float previous_value ) -{ - return ( (alpha * current_value) + ((1.0 - alpha) * previous_value) ); -} diff --git a/platforms/common/libs/signal_smoothing/oscc_signal_smoothing.h b/platforms/common/libs/signal_smoothing/oscc_signal_smoothing.h deleted file mode 100644 index d941409a..00000000 --- a/platforms/common/libs/signal_smoothing/oscc_signal_smoothing.h +++ /dev/null @@ -1,39 +0,0 @@ -/** - * @file oscc_signal_smoothing.h - * @brief Signal smoothing utilities. - * - */ - - -#ifndef _OSCC_SIGNAL_SMOOTHING_H_ -#define _OSCC_SIGNAL_SMOOTHING_H_ - - -// **************************************************************************** -// Function: exponential_moving_average -// -// Purpose: Calculates an exponential moving average. -// -// s(t) = (a * x(t)) + ((1-a) * s(t-1)) -// -// s(t) - smoothed signal -// a - alpha -// x(t) - current noisy value of signal -// s(t-1) - previous output of the function -// -// Returns: float - smoothed value -// -// Parameters: alpha - alpha term of the exponential moving average -// current_value - current value to be fed into the function -// for smoothing -// previous_value - previous value that was returned from the -// function after smoothing -// -// **************************************************************************** -float exponential_moving_average( - const float alpha, - const float current_value, - const float previous_value); - - -#endif /* _OSCC_SIGNAL_SMOOTHING_H_ */ diff --git a/platforms/common/libs/time/oscc_time.cpp b/platforms/common/libs/time/oscc_time.cpp deleted file mode 100644 index 943eddd8..00000000 --- a/platforms/common/libs/time/oscc_time.cpp +++ /dev/null @@ -1,49 +0,0 @@ -/** - * @file oscc_time.cpp - * - */ - - -#include - -#include "oscc_time.h" - - -uint32_t get_time_delta( - const uint32_t timestamp_a, - const uint32_t timestamp_b) -{ - uint32_t delta = 0; - - // check for overflow - if( timestamp_b < timestamp_a ) - { - // time remainder, prior to the overflow - delta = UINT32_MAX - timestamp_a; - - // add time since zero - delta += timestamp_b; - } - else - { - // normal delta - delta = timestamp_b - timestamp_a; - } - - return delta; -} - - -bool is_timeout( uint32_t timestamp_a, uint32_t timestamp_b, int timeout ) -{ - bool ret = false; - - uint32_t delta = get_time_delta(timestamp_a, timestamp_b); - - if( delta >= timeout ) - { - ret = true; - } - - return ret; -} diff --git a/platforms/common/libs/time/oscc_time.h b/platforms/common/libs/time/oscc_time.h deleted file mode 100644 index 0274cd77..00000000 --- a/platforms/common/libs/time/oscc_time.h +++ /dev/null @@ -1,68 +0,0 @@ -/** - * @file time.h - * @brief Time utilities. - * - */ - - -#ifndef _OSCC_TIME_H_ -#define _OSCC_TIME_H_ - - -#include -#include - - -/* - * @brief Get current system timestamp in milliseconds. - * - */ -#define GET_TIMESTAMP_MS() ((uint32_t) millis()) - -/* - * @brief Get current system timestamp in microseconds. - * - */ -#define GET_TIMESTAMP_US() ((uint32_t) micros()) - -/* - * @brief Delay execution in milliseconds. - * - */ -#define SLEEP_MS(x) delay(x) - - -// **************************************************************************** -// Function: get_time_delta -// -// Purpose: Calculate the difference between timestamp_a and timestamp_b. -// -// Returns: uint32_t - difference between timestamp_a and timestamp_b -// -// Parameters: [in] timestamp_a - first timestamp for comparison -// [in] timestamp_b - second timestamp for comparison -// -// **************************************************************************** -uint32_t get_time_delta( - const uint32_t timestamp_a, - const uint32_t timestamp_b); - - -// **************************************************************************** -// Function: is_timeout -// -// Purpose: Check if a timestamp is greater than a timeout period. -// -// Returns: bool - true if a timeout has occurred -// -// Parameters: [in] timestamp - time against which to compare -// [in] timeout - timeout period to check against -// -// **************************************************************************** -bool is_timeout( - const uint32_t timestamp_a, - const uint32_t timestamp_b, - const int timeout ); - - -#endif /* _OSCC_TIME_H_ */ diff --git a/platforms/common/testing/mocks/mcp_can_mock.cpp b/platforms/common/testing/mocks/mcp_can_mock.cpp deleted file mode 100644 index cd3d944a..00000000 --- a/platforms/common/testing/mocks/mcp_can_mock.cpp +++ /dev/null @@ -1,58 +0,0 @@ -#include -#include -#include -#include - -#include "mcp_can.h" - - - -INT8U g_mock_mcp_can_check_receive_return; -INT32U g_mock_mcp_can_read_msg_buf_id; -INT8U g_mock_mcp_can_read_msg_buf_buf[8]; - -INT32U g_mock_mcp_can_send_msg_buf_id; -INT8U g_mock_mcp_can_send_msg_buf_ext; -INT8U g_mock_mcp_can_send_msg_buf_len; -INT8U *g_mock_mcp_can_send_msg_buf_buf; - - -MCP_CAN::MCP_CAN(INT8U _CS) -{ -} - -INT8U MCP_CAN::begin(INT8U speedset) -{ - return CAN_OK; -} - -INT8U MCP_CAN::checkReceive(void) -{ - return g_mock_mcp_can_check_receive_return; -} - -INT8U MCP_CAN::sendMsgBuf(INT32U id, INT8U ext, INT8U len, INT8U *buf) -{ - g_mock_mcp_can_send_msg_buf_id = id; - g_mock_mcp_can_send_msg_buf_ext = ext; - g_mock_mcp_can_send_msg_buf_len = len; - - g_mock_mcp_can_send_msg_buf_buf = (INT8U *) malloc(len); - - memcpy(g_mock_mcp_can_send_msg_buf_buf, buf, len); - - return CAN_OK; -} - -INT8U MCP_CAN::readMsgBufID(INT32U *ID, INT8U *len, INT8U *buf) -{ - *ID = g_mock_mcp_can_read_msg_buf_id; - *len = 8; - - for( int i = 0; i < *len; ++i ) - { - buf[i] = g_mock_mcp_can_read_msg_buf_buf[i]; - } - - return CAN_OK; -} diff --git a/platforms/kia_soul/firmware/CMakeLists.txt b/platforms/kia_soul/firmware/CMakeLists.txt deleted file mode 100644 index fc14edf6..00000000 --- a/platforms/kia_soul/firmware/CMakeLists.txt +++ /dev/null @@ -1,17 +0,0 @@ -project(kia-soul) - -set(CMAKE_CFLAGS "-std=gnu11 -Os") -set(CMAKE_CXX_FLAGS "-std=gnu++11 -Os") - -add_subdirectory(brake) -add_subdirectory(can_gateway) -add_subdirectory(steering) -add_subdirectory(throttle) - -add_custom_target( - kia-soul-all-upload - DEPENDS - kia-soul-brake-upload - kia-soul-can-gateway-upload - kia-soul-steering-upload - kia-soul-throttle-upload) diff --git a/platforms/kia_soul/firmware/brake/include/brake_control.h b/platforms/kia_soul/firmware/brake/include/brake_control.h deleted file mode 100644 index 9665d496..00000000 --- a/platforms/kia_soul/firmware/brake/include/brake_control.h +++ /dev/null @@ -1,327 +0,0 @@ -/** - * @file brake_control.h - * @brief Control of the brake system. - * - */ - - -#ifndef _OSCC_KIA_SOUL_BRAKE_CONTROL_H_ -#define _OSCC_KIA_SOUL_BRAKE_CONTROL_H_ - - -#include - - -/* - * @brief Minimum value of an unsigned 16-bit integer. - * - */ -#define UINT16_MIN ( 0 ) - -/* - * @brief Value of brake pressure that indicates operator override. [decibars] - * - */ -#define DRIVER_OVERRIDE_PEDAL_THRESHOLD_IN_DECIBARS ( 43.2 ) - -/* - * @brief Brake pressure threshold for when to enable the brake light. - * - */ -#define BRAKE_LIGHT_PRESSURE_THRESHOLD_IN_DECIBARS (20.0) - -/* - * @brief Minimum possible pressure of brake system. [decibars] - * - */ -#define BRAKE_PRESSURE_MIN_IN_DECIBARS ( 12.0 ) - -/* - * @brief Maximum possible pressure of brake system. [decibars] - * - */ -#define BRAKE_PRESSURE_MAX_IN_DECIBARS ( 878.3 ) - -/* - * @brief Alpha term for the exponential filter used to smooth the sensor input. - * - */ -#define BRAKE_PRESSURE_SENSOR_EXPONENTIAL_FILTER_ALPHA ( 0.05 ) - -/* - * @brief Minimum possible value expected to be read from the brake pressure - * sensors when the pressure check pins (PCK1/PCK2) are asserted. - * - */ -#define BRAKE_PRESSURE_SENSOR_CHECK_VALUE_MIN ( 665 ) - -/* - * @brief Maximum possible value expected to be read from the brake pressure - * sensors when the pressure check pins (PCK1/PCK2) are asserted. - * - */ -#define BRAKE_PRESSURE_SENSOR_CHECK_VALUE_MAX ( 680 ) - -/* - * @brief Amount of time between sensor checks. [milliseconds] - * - */ -#define SENSOR_VALIDITY_CHECK_INTERVAL_IN_MSEC ( 250 ) - -/* - * @brief Number of consecutive faults that can occur when reading the - * sensors before control is disabled. - * - */ -#define SENSOR_VALIDITY_CHECK_FAULT_COUNT ( 4 ) - -/* - * @brief Proportional gain of the PID controller. - * - */ -#define PID_PROPORTIONAL_GAIN ( 0.65 ) - -/* - * @brief Integral gain of the PID controller. - * - */ -#define PID_INTEGRAL_GAIN ( 1.75 ) - -/* - * @brief Derivative gain of the PID controller. - * - */ -#define PID_DERIVATIVE_GAIN ( 0.001 ) - -/* - * @brief Windup guard of the PID controller. - * - */ -#define PID_WINDUP_GUARD ( 30 ) - -/* - * @brief Minimum output value of PID to be within a valid pressure range. - * - */ -#define PID_OUTPUT_MIN ( -10.0 ) - -/* - * @brief Maximum output value of PID to be within a valid pressure range. - * - */ -#define PID_OUTPUT_MAX ( 10.0 ) - -/* - * @brief Minimum clamped PID value of the actuation solenoid. - * - */ -#define PID_ACCUMULATOR_SOLENOID_CLAMPED_MIN ( 10.0 ) - -/* - * @brief Maximum clamped PID value of the actuation solenoid. - * - */ -#define PID_ACCUMULATOR_SOLENOID_CLAMPED_MAX ( 110.0 ) - -/* - * @brief Minimum clamped PID value of the release solenoid. - * - */ -#define PID_RELEASE_SOLENOID_CLAMPED_MIN ( 0.0 ) - -/* - * @brief Maximum clamped PID value of the release solenoid. - * - */ -#define PID_RELEASE_SOLENOID_CLAMPED_MAX ( 60.0 ) - -/* - * @brief Minimum duty cycle that begins to actuate the actuation solenoid. - * - * 3.921 KHz PWM frequency - * - */ -#define ACCUMULATOR_SOLENOID_DUTY_CYCLE_MIN ( 80.0 ) - -/* - * @brief Maximum duty cycle where actuation solenoid has reached its stop. - * - * 3.921 KHz PWM frequency - * - */ -#define ACCUMULATOR_SOLENOID_DUTY_CYCLE_MAX ( 105.0 ) - -/* - * @brief Minimum duty cycle that begins to actuate the release solenoid. - * - * 3.921 KHz PWM frequency - * - */ -#define RELEASE_SOLENOID_DUTY_CYCLE_MIN ( 65.0 ) - -/* - * @brief Maximum duty cycle where release solenoid has reached its stop. - * - * 3.921 KHz PWM frequency - * - */ -#define RELEASE_SOLENOID_DUTY_CYCLE_MAX ( 100.0 ) - - -/** - * @brief Current brake control state. - * - * Current state of the throttle module control system. - * - */ -typedef struct -{ - bool enabled; /* Flag indicating control is currently enabled */ - - bool operator_override; /* Flag indicating whether brake pedal was - manually pressed by operator. */ - - bool invalid_sensor_value; /* Flag indicating a value read from one of the - sensors is invalid. */ - - bool obd_timeout; /* Flag indicating whether an OBD timeout has occurred. */ - - bool startup_pressure_check_error; /* Flag indicating a problem with the actuator. */ - - bool startup_pump_motor_check_error; /* Flag indicating a problem with the pump motor. */ - - float current_sensor_brake_pressure; /* Current brake pressure as read - from the brake pressure sensor. */ - - float current_vehicle_brake_pressure; /* Current brake pressure as reported - by the vehicle. */ - - float commanded_pedal_position; /* Brake pedal position commanded by - controller. */ -} kia_soul_brake_control_state_s; - - -// **************************************************************************** -// Function: set_accumulator_solenoid_duty_cycle -// -// Purpose: Set the PWM that controls the "accumulator" solenoids to the -// the specified value. -// -// Returns: void -// -// Parameters: [in] duty_cycle - value to send to the PWM -// -// **************************************************************************** -void set_accumulator_solenoid_duty_cycle( - const uint16_t duty_cycle ); - - -// **************************************************************************** -// Function: set_release_solenoid_duty_cycle -// -// Purpose: Set the PWM that controls the "release" solenoids to the -// the specified value. -// -// Returns: void -// -// Parameters: [in] duty_cycle - value to send to the PWM -// -// **************************************************************************** -void set_release_solenoid_duty_cycle( - const uint16_t duty_cycle ); - - -// **************************************************************************** -// Function: enable_control -// -// Purpose: Enable control of the brake system. -// -// Returns: void -// -// Parameters: void -// -// **************************************************************************** -void enable_control( void ); - - -// **************************************************************************** -// Function: disable_control -// -// Purpose: Disable control of the brake system. -// -// Returns: void -// -// Parameters: void -// -// **************************************************************************** -void disable_control( void ); - - -// **************************************************************************** -// Function: check_for_operator_override -// -// Purpose: Check to see if the vehicle's operator has manually pressed -// the brake pedal and disable control if they have. -// -// Returns: void -// -// Parameters: void -// -// **************************************************************************** -void check_for_operator_override( void ); - - -// **************************************************************************** -// Function: check_for_sensor_faults -// -// Purpose: Checks to see if valid values are being read from the sensors. -// -// Returns: void -// -// Parameters: void -// -// **************************************************************************** -void check_for_sensor_faults( void ); - - - -// **************************************************************************** -// Function: read_pressure_sensor -// -// Purpose: Update brake pressure. -// -// Returns: void -// -// Parameters: void -// -// **************************************************************************** -void read_pressure_sensor( void ); - - -// **************************************************************************** -// Function: brake_init -// -// Purpose: Initialize the brake system. -// -// Returns: void -// -// Parameters: void -// -// **************************************************************************** -void brake_init( void ); - - -// **************************************************************************** -// Function: update_brake -// -// Purpose: Write brake spoof values to DAC. -// -// Returns: void -// -// Parameters: void -// -// **************************************************************************** -void update_brake( void ); - - -#endif /* _OSCC_KIA_SOUL_BRAKE_CONTROL_H_ */ diff --git a/platforms/kia_soul/firmware/brake/src/communications.cpp b/platforms/kia_soul/firmware/brake/src/communications.cpp deleted file mode 100644 index 13f4fe57..00000000 --- a/platforms/kia_soul/firmware/brake/src/communications.cpp +++ /dev/null @@ -1,191 +0,0 @@ -/** - * @file communications.cpp - * - */ - - -#include "mcp_can.h" -#include "chassis_state_can_protocol.h" -#include "brake_can_protocol.h" -#include "oscc_can.h" -#include "oscc_time.h" -#include "debug.h" - -#include "globals.h" -#include "communications.h" -#include "brake_control.h" - - -static void process_rx_frame( - const can_frame_s * const frame ); - -static void publish_brake_report( void ); - -static void process_brake_command( - const uint8_t * const data ); - -static void process_chassis_state_1( - const uint8_t * const data ); - -static void check_for_controller_command_timeout( void ); - -static void check_for_chassis_state_1_report_timeout( void ); - - -void publish_reports( void ) -{ - uint32_t delta = get_time_delta( g_brake_report_last_tx_timestamp, GET_TIMESTAMP_MS() ); - - if ( delta >= OSCC_REPORT_BRAKE_PUBLISH_INTERVAL_IN_MSEC ) - { - publish_brake_report( ); - } -} - - -void check_for_incoming_message( void ) -{ - can_frame_s rx_frame; - can_status_t ret = check_for_rx_frame( g_control_can, &rx_frame ); - - if( ret == CAN_RX_FRAME_AVAILABLE ) - { - process_rx_frame( &rx_frame ); - } -} - - -void check_for_timeouts( void ) -{ - check_for_controller_command_timeout( ); - - check_for_chassis_state_1_report_timeout( ); -} - - -static void publish_brake_report( void ) -{ - oscc_report_brake_s brake_report; - - brake_report.id = OSCC_REPORT_BRAKE_CAN_ID; - brake_report.dlc = OSCC_REPORT_BRAKE_CAN_DLC; - brake_report.data.enabled = (uint8_t) g_brake_control_state.enabled; - brake_report.data.override = (uint8_t) g_brake_control_state.operator_override; - brake_report.data.pedal_input = (int16_t) g_brake_control_state.current_vehicle_brake_pressure; - brake_report.data.pedal_command = (uint16_t) g_brake_control_state.commanded_pedal_position; - brake_report.data.pedal_output = (uint16_t) g_brake_control_state.current_sensor_brake_pressure; - brake_report.data.fault_obd_timeout = (uint8_t) g_brake_control_state.obd_timeout; - brake_report.data.fault_invalid_sensor_value = (uint8_t) g_brake_control_state.invalid_sensor_value; - brake_report.data.fault_startup_pressure_check_error = (uint8_t) g_brake_control_state.startup_pressure_check_error; - brake_report.data.fault_startup_pump_motor_check_error = (uint8_t) g_brake_control_state.startup_pump_motor_check_error; - - g_control_can.sendMsgBuf( - brake_report.id, - CAN_STANDARD, - brake_report.dlc, - (uint8_t *) &brake_report.data ); - - g_brake_report_last_tx_timestamp = GET_TIMESTAMP_MS( ); -} - - -static void process_brake_command( - const uint8_t * const data ) -{ - if (data != NULL ) - { - const oscc_command_brake_data_s * const brake_command_data = - (oscc_command_brake_data_s *) data; - - if( brake_command_data->enabled == true ) - { - enable_control( ); - } - else - { - disable_control( ); - } - - g_brake_control_state.commanded_pedal_position = brake_command_data->pedal_command; - - g_brake_command_last_rx_timestamp = GET_TIMESTAMP_MS( ); - } -} - - -static void process_chassis_state_1( - const uint8_t * const data ) -{ - if ( data != NULL ) - { - const oscc_report_chassis_state_1_data_s * const chassis_state_1_data = - (oscc_report_chassis_state_1_data_s *) data; - - if( chassis_state_1_data->flags - & OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_BRAKE_PRESSURE_VALID ) - { - g_brake_control_state.current_vehicle_brake_pressure = - chassis_state_1_data->brake_pressure; - - g_chassis_state_1_report_last_rx_timestamp = GET_TIMESTAMP_MS( ); - } - } -} - - -static void process_rx_frame( - const can_frame_s * const frame ) -{ - if ( frame != NULL ) - { - if ( frame->id == OSCC_COMMAND_BRAKE_CAN_ID ) - { - process_brake_command( frame->data ); - } - else if ( frame->id == OSCC_REPORT_CHASSIS_STATE_1_CAN_ID ) - { - process_chassis_state_1( frame->data ); - } - } -} - - -static void check_for_controller_command_timeout( void ) -{ - if( g_brake_control_state.enabled == true ) - { - bool timeout = is_timeout( - g_brake_command_last_rx_timestamp, - GET_TIMESTAMP_MS( ), - COMMAND_TIMEOUT_IN_MSEC ); - - if ( timeout == true ) - { - disable_control( ); - - DEBUG_PRINTLN( "Timeout - controller command" ); - } - } -} - - -static void check_for_chassis_state_1_report_timeout( void ) -{ - bool timeout = is_timeout( - g_chassis_state_1_report_last_rx_timestamp, - GET_TIMESTAMP_MS( ), - CHASSIS_STATE_1_REPORT_TIMEOUT_IN_MSEC); - - if( timeout == true ) - { - disable_control( ); - - g_brake_control_state.obd_timeout = true; - - DEBUG_PRINTLN( "Timeout - Chassis State 1 report" ); - } - else - { - g_brake_control_state.obd_timeout = false; - } -} diff --git a/platforms/kia_soul/firmware/brake/tests/features/checking_actuator_functionality.feature b/platforms/kia_soul/firmware/brake/tests/features/checking_actuator_functionality.feature deleted file mode 100644 index 7677e821..00000000 --- a/platforms/kia_soul/firmware/brake/tests/features/checking_actuator_functionality.feature +++ /dev/null @@ -1,23 +0,0 @@ -# language: en - -Feature: Checking actuator functionality - - Brake actuator should be checked for proper functionality on module startup. - - - Scenario: Startup checks fail - Given the actuator and pump motor are in a bad state - - When the startup checks are run - - Then the actuator error should be set to true - And the pump motor error should be set to true - - - Scenario: Startup checks pass - Given the actuator and pump motor are in a good state - - When the startup checks are run - - Then the actuator error should be set to false - And the pump motor error should be set to false diff --git a/platforms/kia_soul/firmware/brake/tests/features/checking_sensor_validity.feature b/platforms/kia_soul/firmware/brake/tests/features/checking_sensor_validity.feature deleted file mode 100644 index 2713792d..00000000 --- a/platforms/kia_soul/firmware/brake/tests/features/checking_sensor_validity.feature +++ /dev/null @@ -1,20 +0,0 @@ -# language: en - -Feature: Checking sensor validity - - Invalid values read from sensors should cause control to be disabled. - - - Scenario: A sensor becomes temporarily disconnected - Given brake control is enabled - - When a sensor becomes temporarily disconnected - - Then control should remain enabled - - Scenario: A sensor becomes permanently disconnected - Given brake control is enabled - - When a sensor becomes permanently disconnected - - Then control should be disabled diff --git a/platforms/kia_soul/firmware/brake/tests/features/receiving_reports.feature b/platforms/kia_soul/firmware/brake/tests/features/receiving_reports.feature deleted file mode 100644 index cef2dd13..00000000 --- a/platforms/kia_soul/firmware/brake/tests/features/receiving_reports.feature +++ /dev/null @@ -1,31 +0,0 @@ -# language: en - -Feature: Receiving reports - - Chassis state reports should be received and parsed. - - - Scenario Outline: Chassis State 1 report sent from CAN gateway. - When a Chassis State 1 report is received with brake pressure - - Then the control state's current_vehicle_brake_pressure field should be - - Examples: - | pressure | - | -32768 | - | -16384 | - | -8192 | - | -4096 | - | -2048 | - | -1024 | - | -512 | - | -256 | - | 0 | - | 256 | - | 512 | - | 1024 | - | 2048 | - | 4096 | - | 8192 | - | 16348 | - | 32767 | diff --git a/platforms/kia_soul/firmware/brake/tests/features/sending_reports.feature b/platforms/kia_soul/firmware/brake/tests/features/sending_reports.feature deleted file mode 100644 index 4403014a..00000000 --- a/platforms/kia_soul/firmware/brake/tests/features/sending_reports.feature +++ /dev/null @@ -1,24 +0,0 @@ -# language: en - -Feature: Sending reports - - Brake reports should be published to the control CAN bus after an interval. - - - Scenario Outline: Brake report published after an interval - Given the previous brake pedal position command was - And the current vehicle reported brake pressure is - And the current sensor reported brake pressure is - - When the time since the last report publishing exceeds the interval - - Then a brake report should be published to the control CAN bus - And the report's command field should be set to - And the report's current vehicle reported brake pressure field should be set to - And the report's sensor reported brake pressure should be set to - - Examples: - | command | vehicle_brake_pressure | sensor_brake_pressure | - | 0 | -512 | 256 | - | 50 | 256 | 512 | - | 100 | 512 | 1024 | diff --git a/platforms/kia_soul/firmware/brake/tests/features/step_definitions/checking_actuator_functionality.cpp b/platforms/kia_soul/firmware/brake/tests/features/step_definitions/checking_actuator_functionality.cpp deleted file mode 100644 index 444c7814..00000000 --- a/platforms/kia_soul/firmware/brake/tests/features/step_definitions/checking_actuator_functionality.cpp +++ /dev/null @@ -1,57 +0,0 @@ -GIVEN("^the actuator and pump motor are in a (.*) state$") -{ - REGEX_PARAM(std::string, state); - - if( state == "bad" ) - { - g_mock_arduino_analog_read_return = 0; - } - else if( state == "good" ) - { - g_mock_arduino_analog_read_return = BRAKE_PRESSURE_SENSOR_CHECK_VALUE_MIN; - } -} - - -WHEN("^the startup checks are run$") -{ - brake_init(); -} - - -THEN("^the actuator error should be set to (.*)") -{ - REGEX_PARAM(std::string, error_val); - - if( error_val == "false" ) - { - assert_that( - g_brake_control_state.startup_pressure_check_error, - is_equal_to(0)); - } - else if( error_val == "true" ) - { - assert_that( - g_brake_control_state.startup_pressure_check_error, - is_equal_to(1)); - } -} - - -THEN("^the pump motor error should be set to (.*)") -{ - REGEX_PARAM(std::string, error_val); - - if( error_val == "false" ) - { - assert_that( - g_brake_control_state.startup_pressure_check_error, - is_equal_to(0)); - } - else if( error_val == "true" ) - { - assert_that( - g_brake_control_state.startup_pressure_check_error, - is_equal_to(1)); - } -} diff --git a/platforms/kia_soul/firmware/brake/tests/features/step_definitions/checking_sensor_validity.cpp b/platforms/kia_soul/firmware/brake/tests/features/step_definitions/checking_sensor_validity.cpp deleted file mode 100644 index ffcb7682..00000000 --- a/platforms/kia_soul/firmware/brake/tests/features/step_definitions/checking_sensor_validity.cpp +++ /dev/null @@ -1,41 +0,0 @@ -WHEN("^a sensor becomes temporarily disconnected$") -{ - // first check - error value - one fault - g_mock_arduino_analog_read_return = 0; - g_mock_arduino_millis_return = SENSOR_VALIDITY_CHECK_INTERVAL_IN_MSEC; - check_for_sensor_faults(); - - // second check - error value - two faults - g_mock_arduino_analog_read_return = 0; - g_mock_arduino_millis_return += SENSOR_VALIDITY_CHECK_INTERVAL_IN_MSEC; - check_for_sensor_faults(); - - // third check - valid value - faults reset - g_mock_arduino_analog_read_return = 500; - g_mock_arduino_millis_return += SENSOR_VALIDITY_CHECK_INTERVAL_IN_MSEC; - check_for_sensor_faults(); -} - - -WHEN("^a sensor becomes permanently disconnected$") -{ - g_mock_arduino_analog_read_return = 0; - g_mock_arduino_millis_return = SENSOR_VALIDITY_CHECK_INTERVAL_IN_MSEC; - - // must call function enough times to exceed the fault limit - for( int i = 0; i < SENSOR_VALIDITY_CHECK_FAULT_COUNT; ++i ) - { - // continue timing out - g_mock_arduino_millis_return += SENSOR_VALIDITY_CHECK_INTERVAL_IN_MSEC; - - check_for_sensor_faults(); - } -} - - -THEN("^control should remain enabled") -{ - assert_that( - g_brake_control_state.enabled, - is_equal_to(1)); -} diff --git a/platforms/kia_soul/firmware/brake/tests/features/step_definitions/receiving_reports.cpp b/platforms/kia_soul/firmware/brake/tests/features/step_definitions/receiving_reports.cpp deleted file mode 100644 index a45c7d4a..00000000 --- a/platforms/kia_soul/firmware/brake/tests/features/step_definitions/receiving_reports.cpp +++ /dev/null @@ -1,25 +0,0 @@ -WHEN("^a Chassis State 1 report is received with brake pressure (.*)$") -{ - REGEX_PARAM(int, pressure); - - oscc_report_chassis_state_1_data_s * chassis_state_1_data = - (oscc_report_chassis_state_1_data_s *) g_mock_mcp_can_read_msg_buf_buf; - - chassis_state_1_data->brake_pressure = pressure; - chassis_state_1_data->flags = OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_BRAKE_PRESSURE_VALID; - - g_mock_mcp_can_read_msg_buf_id = OSCC_REPORT_CHASSIS_STATE_1_CAN_ID; - g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL; - - check_for_incoming_message(); -} - - -THEN("^the control state's current_vehicle_brake_pressure field should be (.*)$") -{ - REGEX_PARAM(float, pressure); - - assert_that_double( - g_brake_control_state.current_vehicle_brake_pressure, - is_equal_to_double(pressure)); -} diff --git a/platforms/kia_soul/firmware/brake/tests/features/step_definitions/sending_reports.cpp b/platforms/kia_soul/firmware/brake/tests/features/step_definitions/sending_reports.cpp deleted file mode 100644 index 2bb9320d..00000000 --- a/platforms/kia_soul/firmware/brake/tests/features/step_definitions/sending_reports.cpp +++ /dev/null @@ -1,95 +0,0 @@ -GIVEN("^the current vehicle reported brake pressure is (.*)$") -{ - REGEX_PARAM(int, pressure); - - g_brake_control_state.current_vehicle_brake_pressure = pressure; -} - - -GIVEN("^the current sensor reported brake pressure is (.*)$") -{ - REGEX_PARAM(int, pressure); - - g_brake_control_state.current_sensor_brake_pressure = pressure; -} - - -WHEN("^the time since the last report publishing exceeds the interval$") -{ - g_brake_report_last_tx_timestamp = 0; - - g_mock_arduino_millis_return = - OSCC_REPORT_BRAKE_PUBLISH_INTERVAL_IN_MSEC; - - publish_reports(); -} - - -THEN("^a brake report should be published to the control CAN bus$") -{ - assert_that( - g_mock_mcp_can_send_msg_buf_id, - is_equal_to(OSCC_REPORT_BRAKE_CAN_ID)); - - assert_that( - g_mock_mcp_can_send_msg_buf_ext, - is_equal_to(CAN_STANDARD)); - - assert_that( - g_mock_mcp_can_send_msg_buf_len, - is_equal_to(OSCC_REPORT_BRAKE_CAN_DLC)); - - oscc_report_brake_data_s * brake_report_data = - (oscc_report_brake_data_s *) g_mock_mcp_can_send_msg_buf_buf; - - assert_that( - brake_report_data->enabled, - is_equal_to(g_brake_control_state.enabled)); - - assert_that( - brake_report_data->override, - is_equal_to(g_brake_control_state.operator_override)); - - assert_that( - g_brake_report_last_tx_timestamp, - is_equal_to(g_mock_arduino_millis_return)); -} - - -THEN("^the report's command field should be set to (.*)$") -{ - REGEX_PARAM(int, command); - - oscc_report_brake_data_s * brake_report_data = - (oscc_report_brake_data_s *) g_mock_mcp_can_send_msg_buf_buf; - - assert_that( - brake_report_data->pedal_command, - is_equal_to(command)); -} - - -THEN("^the report's current vehicle reported brake pressure field should be set to (.*)$") -{ - REGEX_PARAM(int, pressure); - - oscc_report_brake_data_s * brake_report_data = - (oscc_report_brake_data_s *) g_mock_mcp_can_send_msg_buf_buf; - - assert_that( - brake_report_data->pedal_input, - is_equal_to(pressure)); -} - - -THEN("^the report's sensor reported brake pressure should be set to (.*)$") -{ - REGEX_PARAM(int, pressure); - - oscc_report_brake_data_s * brake_report_data = - (oscc_report_brake_data_s *) g_mock_mcp_can_send_msg_buf_buf; - - assert_that( - brake_report_data->pedal_output, - is_equal_to(pressure)); -} diff --git a/platforms/kia_soul/firmware/brake/tests/features/step_definitions/timeouts_and_overrides.cpp b/platforms/kia_soul/firmware/brake/tests/features/step_definitions/timeouts_and_overrides.cpp deleted file mode 100644 index 83c6c658..00000000 --- a/platforms/kia_soul/firmware/brake/tests/features/step_definitions/timeouts_and_overrides.cpp +++ /dev/null @@ -1,45 +0,0 @@ -WHEN("^the time since the last received controller command exceeds the timeout$") -{ - g_brake_command_last_rx_timestamp = 0; - - g_mock_arduino_millis_return = - COMMAND_TIMEOUT_IN_MSEC; - - check_for_timeouts(); -} - - -WHEN("^the time since the last received Chassis State 1 report exceeds the timeout$") -{ - g_chassis_state_1_report_last_rx_timestamp = 0; - - g_mock_arduino_millis_return = - CHASSIS_STATE_1_REPORT_TIMEOUT_IN_MSEC; - - check_for_timeouts(); -} - - -WHEN("^the operator applies (.*) to the brake pedal$") -{ - REGEX_PARAM(int, pedal_val); - - g_mock_arduino_analog_read_return = pedal_val; - - // The exponential filter used requires multiple passes for it to recognize - // an override - for( int i = 0; i < 3; ++i ) - { - check_for_operator_override(); - } -} - - -THEN("^override flag should be set$") -{ - assert_that( - g_brake_control_state.operator_override, - is_equal_to(true)); -} - - diff --git a/platforms/kia_soul/firmware/brake/tests/property/build.rs b/platforms/kia_soul/firmware/brake/tests/property/build.rs deleted file mode 100644 index 19b8d1d3..00000000 --- a/platforms/kia_soul/firmware/brake/tests/property/build.rs +++ /dev/null @@ -1,75 +0,0 @@ -extern crate gcc; -extern crate bindgen; - -use std::env; -use std::path::Path; - -fn main() { - println!("cargo:rerun-if-changed=../../include"); - println!("cargo:rerun-if-changed=../../include/*"); - println!("cargo:rerun-if-changed=../src"); - println!("cargo:rerun-if-changed=../../src/*"); - println!("cargo:rerun-if-changed=../../../../../common/testing/mocks"); - println!("cargo:rerun-if-changed=../../../../../common/testing/mocks/*"); - - gcc::Config::new() - .flag("-w") - .define("__STDC_LIMIT_MACROS", None) - .include("include") - .include("../../include") - .include("../../../../../common/testing/mocks") - .include("../../../../../common/include") - .include("../../../../../common/libs/can") - .include("../../../../../common/libs/time") - .include("../../../../../common/libs/pid") - .include("../../../../../common/libs/signal_smoothing") - .include("/usr/lib/avr/include") - .file("../../../../../common/testing/mocks/Arduino_mock.cpp") - .file("../../../../../common/testing/mocks/mcp_can_mock.cpp") - .file("../../src/communications.cpp") - .file("../../src/brake_control.cpp") - .file("../../src/globals.cpp") - .file("../../src/master_cylinder.cpp") - .file("../../src/helper.cpp") - .file("../../../../../common/libs/time/oscc_time.cpp") - .file("../../../../../common/libs/can/oscc_can.cpp") - .file("../../../../../common/libs/signal_smoothing/oscc_signal_smoothing.cpp") - .cpp(true) - .compiler("/usr/bin/g++") - .compile("libbrake_test.a"); - - let out_dir = env::var("OUT_DIR").unwrap(); - - let _ = bindgen::builder() - .header("include/wrapper.hpp") - .generate_comments(false) - .clang_arg("-I../../include") - .clang_arg("-I../../../../../common/testing/mocks") - .clang_arg("-I../../../../../common/include") - .clang_arg("-I../../../../../common/libs/can") - .clang_arg("-I../../../../../common/libs/pid") - .clang_arg("-I/usr/lib/avr/include") - .whitelisted_function("publish_reports") - .whitelisted_function("check_for_incoming_message") - .whitelisted_function("raw_adc_to_pressure") - .whitelisted_function("check_for_operator_override") - .whitelisted_var("OSCC_REPORT_BRAKE_CAN_ID") - .whitelisted_var("OSCC_REPORT_BRAKE_CAN_DLC") - .whitelisted_var("OSCC_COMMAND_BRAKE_CAN_ID") - .whitelisted_var("OSCC_COMMAND_BRAKE_CAN_DLC") - .whitelisted_var("DRIVER_OVERRIDE_PEDAL_THRESHOLD_IN_DECIBARS") - .whitelisted_var("OSCC_REPORT_BRAKE_PUBLISH_INTERVAL_IN_MSEC") - .whitelisted_var("CAN_STANDARD") - .whitelisted_var("CAN_MSGAVAIL") - .whitelisted_var("g_control_can") - .whitelisted_var("g_brake_control_state") - .whitelisted_type("oscc_report_brake_data_s") - .whitelisted_type("oscc_report_brake_s") - .whitelisted_type("oscc_command_brake_data_s") - .whitelisted_type("oscc_command_brake_s") - .whitelisted_type("can_frame_s") - .generate() - .unwrap() - .write_to_file(Path::new(&out_dir).join("brake_test.rs")) - .expect("Unable to generate bindings"); -} diff --git a/platforms/kia_soul/firmware/brake/tests/property/include/wrapper.hpp b/platforms/kia_soul/firmware/brake/tests/property/include/wrapper.hpp deleted file mode 100644 index 5807a541..00000000 --- a/platforms/kia_soul/firmware/brake/tests/property/include/wrapper.hpp +++ /dev/null @@ -1,6 +0,0 @@ -#include "globals.h" -#include "communications.h" -#include "helper.h" -#include "brake_can_protocol.h" -#include "Arduino.h" -#include "mcp_can.h" \ No newline at end of file diff --git a/platforms/kia_soul/firmware/brake/tests/property/src/tests.rs b/platforms/kia_soul/firmware/brake/tests/property/src/tests.rs deleted file mode 100644 index 9bd35afe..00000000 --- a/platforms/kia_soul/firmware/brake/tests/property/src/tests.rs +++ /dev/null @@ -1,276 +0,0 @@ -#![allow(non_upper_case_globals)] -#![allow(non_camel_case_types)] -#![allow(non_snake_case)] -#![allow(dead_code)] -#![allow(unused_variables)] -#![allow(unused_imports)] -include!(concat!(env!("OUT_DIR"), "/brake_test.rs")); - -extern crate quickcheck; -extern crate rand; - -use quickcheck::{QuickCheck, TestResult, Arbitrary, Gen, StdGen}; - -extern "C" { - #[link_name = "g_mock_mcp_can_check_receive_return"] - pub static mut g_mock_mcp_can_check_receive_return: u8; - #[link_name = "g_mock_mcp_can_read_msg_buf_id"] - pub static mut g_mock_mcp_can_read_msg_buf_id: ::std::os::raw::c_ulong; - #[link_name = "g_mock_mcp_can_read_msg_buf_buf"] - pub static mut g_mock_mcp_can_read_msg_buf_buf: [u8; 8usize]; - #[link_name = "g_mock_mcp_can_send_msg_buf_id"] - pub static mut g_mock_mcp_can_send_msg_buf_id: ::std::os::raw::c_ulong; - #[link_name = "g_mock_mcp_can_send_msg_buf_ext"] - pub static mut g_mock_mcp_can_send_msg_buf_ext: u8; - #[link_name = "g_mock_mcp_can_send_msg_buf_len"] - pub static mut g_mock_mcp_can_send_msg_buf_len: u8; - #[link_name = "g_mock_mcp_can_send_msg_buf_buf"] - pub static mut g_mock_mcp_can_send_msg_buf_buf: *mut u8; - #[link_name = "g_mock_arduino_millis_return"] - pub static mut g_mock_arduino_millis_return: ::std::os::raw::c_ulong; - #[link_name = "g_mock_arduino_analog_read_return"] - pub static mut g_mock_arduino_analog_read_return: u16; -} - -impl Arbitrary for oscc_report_brake_data_s { - fn arbitrary(g: &mut G) -> oscc_report_brake_data_s { - oscc_report_brake_data_s { - pedal_input: i16::arbitrary(g), - pedal_command: u16::arbitrary(g), - pedal_output: u16::arbitrary(g), - _bitfield_1: u16::arbitrary(g), - } - } -} - -impl Arbitrary for oscc_report_brake_s { - fn arbitrary(g: &mut G) -> oscc_report_brake_s { - oscc_report_brake_s { - id: u32::arbitrary(g), - dlc: u8::arbitrary(g), - timestamp: u32::arbitrary(g), - data: oscc_report_brake_data_s::arbitrary(g), - } - } -} - -impl Arbitrary for oscc_command_brake_data_s { - fn arbitrary(g: &mut G) -> oscc_command_brake_data_s { - oscc_command_brake_data_s { - pedal_command: u16::arbitrary(g), - _bitfield_1: u16::arbitrary(g), - reserved_5: u8::arbitrary(g), - reserved_6: u8::arbitrary(g), - reserved_7: u8::arbitrary(g), - reserved_8: u8::arbitrary(g), - } - } -} - -impl Arbitrary for oscc_command_brake_s { - fn arbitrary(g: &mut G) -> oscc_command_brake_s { - oscc_command_brake_s { - timestamp: u32::arbitrary(g), - data: oscc_command_brake_data_s::arbitrary(g), - } - } -} - - -impl Arbitrary for can_frame_s { - fn arbitrary(g: &mut G) -> can_frame_s { - can_frame_s { - id: u32::arbitrary(g), - dlc: u8::arbitrary(g), - timestamp: u32::arbitrary(g), - data: [u8::arbitrary(g), - u8::arbitrary(g), - u8::arbitrary(g), - u8::arbitrary(g), - u8::arbitrary(g), - u8::arbitrary(g), - u8::arbitrary(g), - u8::arbitrary(g)], - } - } -} - -/// the throttle firmware should not attempt processing any messages -/// that are not throttle commands -fn prop_only_process_valid_messages(rx_can_msg: can_frame_s, current_target: f32) -> TestResult { - // if we generate a throttle can message, ignore the result - if rx_can_msg.id == OSCC_COMMAND_BRAKE_CAN_ID { - return TestResult::discard(); - } - unsafe { - g_brake_control_state.commanded_pedal_position = current_target; - - g_mock_mcp_can_read_msg_buf_id = rx_can_msg.id as u64; - g_mock_mcp_can_read_msg_buf_buf = rx_can_msg.data; - g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL as u8; - - check_for_incoming_message(); - - TestResult::from_bool(g_brake_control_state.commanded_pedal_position == current_target) - } -} - -#[test] -fn check_message_type_validity() { - QuickCheck::new() - .tests(1000) - .gen(StdGen::new(rand::thread_rng(), u16::max_value() as usize)) - .quickcheck(prop_only_process_valid_messages as fn(can_frame_s, f32) -> TestResult) -} - -/// the throttle firmware should set the commanded pedal position -/// upon reciept of a valid command throttle message -fn prop_no_invalid_targets(command_brake_msg: oscc_command_brake_s) -> TestResult { - unsafe { - g_mock_mcp_can_read_msg_buf_id = OSCC_COMMAND_BRAKE_CAN_ID as u64; - g_mock_mcp_can_read_msg_buf_buf = std::mem::transmute(command_brake_msg.data); - g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL as u8; - - check_for_incoming_message(); - - TestResult::from_bool(g_brake_control_state.commanded_pedal_position == - command_brake_msg.data.pedal_command as f32) - } -} - -#[test] -fn check_accel_pos_validity() { - QuickCheck::new() - .tests(1000) - .quickcheck(prop_no_invalid_targets as fn(oscc_command_brake_s) -> TestResult) -} - -/// the throttle firmware should set the control state as enabled -/// upon reciept of a valid command throttle message telling it to enable -fn prop_process_enable_command(mut command_brake_msg: oscc_command_brake_s) -> TestResult { - unsafe { - command_brake_msg.data.set_enabled(1); - - g_brake_control_state.operator_override = false; - - g_mock_mcp_can_read_msg_buf_id = OSCC_COMMAND_BRAKE_CAN_ID as u64; - g_mock_mcp_can_read_msg_buf_buf = std::mem::transmute(command_brake_msg.data); - g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL as u8; - - check_for_incoming_message(); - - TestResult::from_bool(g_brake_control_state.enabled == true) - } -} - -#[test] -fn check_process_enable_command() { - QuickCheck::new() - .tests(1000) - .quickcheck(prop_process_enable_command as fn(oscc_command_brake_s) -> TestResult) -} - -/// the throttle firmware should set the control state as disabled -/// upon reciept of a valid command throttle message telling it to disable -fn prop_process_disable_command(mut command_brake_msg: oscc_command_brake_s) -> TestResult { - unsafe { - command_brake_msg.data.set_enabled(0); - - g_mock_mcp_can_read_msg_buf_id = OSCC_COMMAND_BRAKE_CAN_ID as u64; - g_mock_mcp_can_read_msg_buf_buf = std::mem::transmute(command_brake_msg.data); - g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL as u8; - - check_for_incoming_message(); - - TestResult::from_bool(g_brake_control_state.enabled == false) - } -} - -#[test] -fn check_process_disable_command() { - QuickCheck::new() - .tests(1000) - .quickcheck(prop_process_disable_command as fn(oscc_command_brake_s) -> TestResult) -} - -/// the throttle firmware should create only valid CAN frames -fn prop_send_valid_can_fields(control_enabled: bool, - operator_override: bool, - commanded_pedal_position: f32) - -> TestResult { - static mut time: u64 = 0; - unsafe { - g_brake_control_state.operator_override = operator_override; - g_brake_control_state.commanded_pedal_position = commanded_pedal_position; - - time = time + OSCC_REPORT_BRAKE_PUBLISH_INTERVAL_IN_MSEC as u64; - - g_mock_arduino_millis_return = time; - - publish_reports(); - - let brake_data = oscc_report_brake_data_s { - pedal_input: std::mem::transmute([*g_mock_mcp_can_send_msg_buf_buf, - *g_mock_mcp_can_send_msg_buf_buf.offset(1)]), - pedal_command: std::mem::transmute([*g_mock_mcp_can_send_msg_buf_buf.offset(2), - *g_mock_mcp_can_send_msg_buf_buf.offset(3)]), - pedal_output: std::mem::transmute([*g_mock_mcp_can_send_msg_buf_buf.offset(4), - *g_mock_mcp_can_send_msg_buf_buf.offset(5)]), - _bitfield_1: std::mem::transmute([*g_mock_mcp_can_send_msg_buf_buf.offset(6), - *g_mock_mcp_can_send_msg_buf_buf.offset(7)]), - }; - - TestResult::from_bool((g_mock_mcp_can_send_msg_buf_id == OSCC_REPORT_BRAKE_CAN_ID as u64) && - (g_mock_mcp_can_send_msg_buf_ext == (CAN_STANDARD as u8)) && - (g_mock_mcp_can_send_msg_buf_len == - (OSCC_REPORT_BRAKE_CAN_DLC as u8)) && - (brake_data.pedal_command == commanded_pedal_position as u16) && - (brake_data.enabled() == g_brake_control_state.enabled as u8) && - (brake_data.override_() == operator_override as u8)) - } -} - -#[test] -fn check_valid_can_frame() { - QuickCheck::new() - .tests(1000) - .gen(StdGen::new(rand::thread_rng(), u16::max_value() as usize)) - .quickcheck(prop_send_valid_can_fields as fn(bool, bool, f32) -> TestResult) -} - -// the brake firmware should be able to correctly and consistently -// detect operator overrides -fn prop_check_operator_override(analog_read_spoof: u16) -> TestResult { - unsafe { - static mut filtered_input_1: f32 = 0.0; - static mut filtered_input_2: f32 = 0.0; - const filter_alpha: f32 = 0.5; - - g_brake_control_state.enabled = true; - g_mock_arduino_analog_read_return = analog_read_spoof; - - check_for_operator_override(); - - let sensor_1: f32 = raw_adc_to_pressure(analog_read_spoof as i32); - let sensor_2: f32 = raw_adc_to_pressure(analog_read_spoof as i32); - - filtered_input_1 = (filter_alpha * sensor_1) + ((1.0 - filter_alpha) * filtered_input_1); - - filtered_input_2 = (filter_alpha * sensor_2) + (1.0 - filter_alpha * filtered_input_2); - - if filtered_input_1.abs() >= DRIVER_OVERRIDE_PEDAL_THRESHOLD_IN_DECIBARS || - filtered_input_2.abs() >= DRIVER_OVERRIDE_PEDAL_THRESHOLD_IN_DECIBARS { - TestResult::from_bool(g_brake_control_state.operator_override == true && - g_brake_control_state.enabled == false) - } else { - TestResult::from_bool(g_brake_control_state.operator_override == false) - } - } -} - -#[test] -fn check_operator_override() { - QuickCheck::new() - .tests(1000) - .quickcheck(prop_check_operator_override as fn(u16) -> TestResult) -} diff --git a/platforms/kia_soul/firmware/can_gateway/include/communications.h b/platforms/kia_soul/firmware/can_gateway/include/communications.h deleted file mode 100644 index e8d84a0d..00000000 --- a/platforms/kia_soul/firmware/can_gateway/include/communications.h +++ /dev/null @@ -1,108 +0,0 @@ -/** - * @file communications.h - * @brief Communication functionality. - * - */ - - -#ifndef _OSCC_KIA_SOUL_CAN_GATEWAY_COMMUNICATIONS_H_ -#define _OSCC_KIA_SOUL_CAN_GATEWAY_COMMUNICATIONS_H_ - - -#include - -#include "globals.h" - - -/* - * @brief Set warning flag in heartbeat warning register. - * - */ -#define SET_HEARTBEAT_WARNING(x) (g_tx_heartbeat.data.warning_register |= ((uint16_t) x)) - -/* - * @brief Clear warning flag in heartbeat warning register. - * - */ -#define CLEAR_HEARTBEAT_WARNING(x) (g_tx_heartbeat.data.warning_register &= ~((uint16_t) x)) - -/* - * @brief Set error flag in heartbeat error register. - * - */ -#define SET_HEARTBEAT_ERROR(x) (g_tx_heartbeat.data.error_register |= ((uint16_t) x)) - -/* - * @brief Clear error flag in heartbeat error register. - * - */ -#define CLEAR_HEARTBEAT_ERROR(x) (g_tx_heartbeat.data.error_register &= ~((uint16_t) x)) - -/* - * @brief Set heartbeat state. - * - */ -#define SET_HEARTBEAT_STATE(x) (g_tx_heartbeat.data.state = ((uint8_t) x)) - -/* - * @brief Get heartbeat state. - * - */ -#define GET_HEARTBEAT_STATE() (g_tx_heartbeat.data.state) - -/* - * @brief Set Chassis 1 flag. - * - */ - -#define SET_CHASSIS_1_FLAG(x) (g_tx_chassis_state_1.data.flags |= ((uint8_t) x)) - -/* - * @brief Clear Chassis 1 flag. - * - */ -#define CLEAR_CHASSIS_1_FLAG(x) (g_tx_chassis_state_1.data.flags &= ~((uint8_t) x)) - - -// **************************************************************************** -// Function: publish_reports -// -// Purpose: Publish all valid reports to CAN bus. -// -// Returns: void -// -// Parameters: void -// -// **************************************************************************** -void publish_reports( void ); - - -// **************************************************************************** -// Function: check_for_obd_timeout -// -// Purpose: Check if the last message received from the vehicle's OBD CAN -// bus exceeds the timeout and set appropriate heartbeat and chassis -// flags. -// -// Returns: void -// -// Parameters: void -// -// **************************************************************************** -void check_for_obd_timeout( void ); - - -// **************************************************************************** -// Function: check_for_incoming_message -// -// Purpose: Check CAN bus for incoming messages and process any present. -// -// Returns: void -// -// Parameters: void -// -// **************************************************************************** -void check_for_incoming_message( void ); - - -#endif /* _OSCC_KIA_SOUL_CAN_GATEWAY_COMMUNICATIONS_H_ */ diff --git a/platforms/kia_soul/firmware/can_gateway/include/obd_can_protocol.h b/platforms/kia_soul/firmware/can_gateway/include/obd_can_protocol.h deleted file mode 100644 index 29d0c6d0..00000000 --- a/platforms/kia_soul/firmware/can_gateway/include/obd_can_protocol.h +++ /dev/null @@ -1,237 +0,0 @@ -/** - * @file obd_can_protocol.h - * @brief Kia Soul OBD-II CAN Protocol. - * - */ - - -#ifndef _KIA_SOUL_OBD_CAN_PROTOCOL_H_ -#define _KIA_SOUL_OBD_CAN_PROTOCOL_H_ - - -#include - - -/* - * @brief ID of the Kia Soul's OBD steering wheel angle CAN frame. - * - */ -#define KIA_SOUL_OBD_STEERING_WHEEL_ANGLE_CAN_ID (0x2B0) - -/* - * @brief Amount of time between steering wheel angle CAN frames considered to - * be a timeout. - * - */ -#define KIA_SOUL_OBD_STEERING_WHEEL_ANGLE_RX_WARN_TIMEOUT_IN_MSEC (50) - -/* - * @brief Bit in heartbeat warning register corresponding to steering wheel - * angle. - * - */ -#define KIA_SOUL_OBD_STEERING_WHEEL_ANGLE_HEARTBEAT_WARNING_BIT (0x0100) - -/* - * @brief ID of the Kia Soul's OBD wheel speed CAN frame. - * - */ -#define KIA_SOUL_OBD_WHEEL_SPEED_CAN_ID (0x4B0) - -/* - * @brief Amount of time between wheel speed CAN frames considered to - * be a timeout. - * - */ -#define KIA_SOUL_OBD_WHEEL_SPEED_RX_WARN_TIMEOUT_IN_MSEC (50) - -/* - * @brief Bit in heartbeat warning register corresponding to wheel speed. - * - */ -#define KIA_SOUL_OBD_WHEEL_SPEED_HEARTBEAT_WARNING_BIT (0x0200) - -/* - * @brief ID of the Kia Soul's OBD brake pressure CAN frame. - * - */ -#define KIA_SOUL_OBD_BRAKE_PRESSURE_CAN_ID (0x220) - -/* - * @brief Amount of time between brake pressure CAN frames considered to - * be a timeout. - * - */ -#define KIA_SOUL_OBD_BRAKE_PRESSURE_RX_WARN_TIMEOUT_IN_MSEC (50) - -/* - * @brief Bit in heartbeat warning register corresponding to brake pressure. - * - */ -#define KIA_SOUL_OBD_BRAKE_PRESSURE_WARNING_BIT (0x0400) - -/* - * @brief ID of the Kia Soul's OBD turn signal CAN frame. - * - */ -#define KIA_SOUL_OBD_TURN_SIGNAL_CAN_ID (0x18) - -/* - * @brief Amount of time between turn signal CAN frames considered to - * be a timeout. - * - */ -#define KIA_SOUL_OBD_TURN_SIGNAL_RX_WARN_TIMEOUT_IN_MSEC (500) - -/* - * @brief Bit in heartbeat warning register corresponding to turn signal. - * - */ -#define KIA_SOUL_OBD_TURN_SIGNAL_WARNING_BIT (0x0800) - -/* - * @brief Turn signal flag representing a left turn. - * - */ -#define KIA_SOUL_OBD_TURN_SIGNAL_FLAG_LEFT_TURN (0x0C) - -/* - * @brief Turn signal flag representing a right turn. - * - */ -#define KIA_SOUL_OBD_TURN_SIGNAL_FLAG_RIGHT_TURN (0x0A) - - -/** - * @brief Steering wheel angle message data. - * - * Message size (CAN frame DLC): 8 - * - */ -typedef struct -{ - int16_t steering_angle; /* 1/10th of a degree per bit. */ - - uint16_t reserved_0; /* Reserved. */ - - uint16_t reserved_1; /* Reserved. */ - - uint16_t reserved_2; /* Reserved. */ -} kia_soul_obd_steering_wheel_angle_data_s; - - -/** - * @brief Steering wheel angle message. - * - * CAN frame ID: \ref KIA_SOUL_OBD_STEERING_WHEEL_ANGLE_CAN_ID - * - */ -typedef struct -{ - uint32_t timestamp; /* Timestamp when report was received by the firmware. */ - - kia_soul_obd_steering_wheel_angle_data_s data; /* CAN frame data. */ -} kia_soul_obd_steering_wheel_angle_s; - - -/** - * @brief Wheel speed message data. - * - * Message size (CAN frame DLC): 8 - * - */ -typedef struct -{ - int16_t wheel_speed_front_left; /* 1/128 mph per bit */ - - int16_t wheel_speed_front_right; /* 1/128 mph per bit */ - - int16_t wheel_speed_rear_left; /* 1/128 mph per bit */ - - int16_t wheel_speed_rear_right; /* 1/128 mph per bit */ -} kia_soul_obd_wheel_speed_data_s; - - -/** - * @brief Wheel speed message. - * - * CAN frame ID: \ref KIA_SOUL_OBD_WHEEL_SPEED_CAN_ID - * - */ -typedef struct -{ - uint32_t timestamp; /* Timestamp when report was received by the firmware. */ - - kia_soul_obd_wheel_speed_data_s data; /* CAN frame data. */ -} kia_soul_obd_wheel_speed_s; - - -/** - * @brief Brake pressure message data. - * - * Message size (CAN frame DLC): 8 - * - */ -typedef struct -{ - uint16_t reserved_0; /* Reserved. */ - - uint16_t reserved_1; /* Reserved. */ - - int16_t master_cylinder_pressure; /* 1/10th of a bar per bit */ - - uint16_t reserved_2; /* Reserved. */ -} kia_soul_obd_brake_pressure_data_s; - - -/** - * @brief Brake pressure message. - * - * CAN frame ID: \ref KIA_SOUL_OBD_BRAKE_PRESSURE_CAN_ID - * - */ -typedef struct -{ - uint32_t timestamp; /* Timestamp when report was received by the firmware. */ - - kia_soul_obd_brake_pressure_data_s data; /* CAN frame data. */ -} kia_soul_obd_brake_pressure_s; - - -/** - * @brief Turn signal message data. - * - * Message size (CAN frame DLC): 8 - * - */ -typedef struct -{ - uint16_t reserved_0; /* Reserved. */ - - uint16_t reserved_1; /* Reserved. */ - - uint8_t reserved_2; /* Reserved. */ - - uint8_t reserved_3 : 4; /* Reserved. */ - - uint8_t turn_signal_flags : 4; /* Turn signal flags. */ - - uint16_t reserved_4; /* Reserved. */ -} kia_soul_obd_turn_signal_data_s; - - -/** - * @brief Turn signal message. - * - * CAN frame ID: \ref KIA_SOUL_OBD_TURN_SIGNAL_CAN_ID - * - */ -typedef struct -{ - uint32_t timestamp; /* Timestamp when report was received by the firmware. */ - - kia_soul_obd_turn_signal_data_s data; /* CAN frame data. */ -} kia_soul_obd_turn_signal_s; - - -#endif diff --git a/platforms/kia_soul/firmware/can_gateway/src/communications.cpp b/platforms/kia_soul/firmware/can_gateway/src/communications.cpp deleted file mode 100644 index 5c8bd5ce..00000000 --- a/platforms/kia_soul/firmware/can_gateway/src/communications.cpp +++ /dev/null @@ -1,284 +0,0 @@ -/** - * @file communications.cpp - * - */ - - -#include "gateway_can_protocol.h" -#include "chassis_state_can_protocol.h" -#include "mcp_can.h" -#include "oscc_can.h" -#include "oscc_time.h" - -#include "globals.h" -#include "communications.h" -#include "obd_can_protocol.h" - - -static void publish_heartbeat_report( void ); - -static void publish_chassis_state_1_report( void ); - -static void publish_chassis_state_2_report( void ); - -static void process_obd_steering_wheel_angle( - const uint8_t * const data ); - -static void process_obd_wheel_speed( - const uint8_t * const data ); - -static void process_obd_brake_pressure( - const uint8_t * const data ); - -static void process_obd_turn_signal( - const uint8_t * const data ); - -static void process_rx_frame( - const can_frame_s * const rx_frame ); - - -void publish_reports( void ) -{ - uint32_t delta = 0; - - delta = get_time_delta( g_tx_heartbeat.timestamp, GET_TIMESTAMP_MS() ); - if( delta >= OSCC_REPORT_HEARTBEAT_PUBLISH_INTERVAL_IN_MSEC ) - { - publish_heartbeat_report( ); - } - - delta = get_time_delta( g_tx_chassis_state_1.timestamp, GET_TIMESTAMP_MS() ); - if( delta >= OSCC_REPORT_CHASSIS_STATE_1_PUBLISH_INTERVAL_IN_MSEC ) - { - publish_chassis_state_1_report( ); - } - - delta = get_time_delta( g_tx_chassis_state_2.timestamp, GET_TIMESTAMP_MS() ); - if( delta >= OSCC_REPORT_CHASSIS_STATE_2_PUBLISH_INTERVAL_IN_MSEC ) - { - publish_chassis_state_2_report( ); - } -} - - -void check_for_obd_timeout( void ) -{ - bool timeout = false; - - timeout = is_timeout( - g_obd_steering_wheel_angle_rx_timestamp, - GET_TIMESTAMP_MS(), - KIA_SOUL_OBD_STEERING_WHEEL_ANGLE_RX_WARN_TIMEOUT_IN_MSEC); - - if( timeout == true ) - { - SET_HEARTBEAT_WARNING( KIA_SOUL_OBD_STEERING_WHEEL_ANGLE_HEARTBEAT_WARNING_BIT ); - CLEAR_CHASSIS_1_FLAG( OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_STEER_WHEEL_ANGLE_VALID ); - CLEAR_CHASSIS_1_FLAG( OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_STEER_WHEEL_ANGLE_RATE_VALID ); - } - - timeout = is_timeout( - g_obd_wheel_speed_rx_timestamp, - GET_TIMESTAMP_MS(), - KIA_SOUL_OBD_WHEEL_SPEED_RX_WARN_TIMEOUT_IN_MSEC); - - if( timeout == true ) - { - SET_HEARTBEAT_WARNING( KIA_SOUL_OBD_WHEEL_SPEED_HEARTBEAT_WARNING_BIT ); - CLEAR_CHASSIS_1_FLAG( OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_WHEEL_SPEED_VALID ); - } - - timeout = is_timeout( - g_obd_brake_pressure_rx_timestamp, - GET_TIMESTAMP_MS(), - KIA_SOUL_OBD_BRAKE_PRESSURE_RX_WARN_TIMEOUT_IN_MSEC); - - if( timeout == true ) - { - SET_HEARTBEAT_WARNING( KIA_SOUL_OBD_BRAKE_PRESSURE_WARNING_BIT ); - CLEAR_CHASSIS_1_FLAG( OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_BRAKE_PRESSURE_VALID ); - } - - timeout = is_timeout( - g_obd_turn_signal_rx_timestamp, - GET_TIMESTAMP_MS(), - KIA_SOUL_OBD_TURN_SIGNAL_RX_WARN_TIMEOUT_IN_MSEC); - - if( timeout == true ) - { - SET_HEARTBEAT_WARNING( KIA_SOUL_OBD_TURN_SIGNAL_WARNING_BIT ); - CLEAR_CHASSIS_1_FLAG( OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_LEFT_TURN_SIGNAL_ON ); - CLEAR_CHASSIS_1_FLAG( OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_RIGHT_TURN_SIGNAL_ON ); - CLEAR_CHASSIS_1_FLAG( OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_BRAKE_SIGNAL_ON ); - } -} - - -void check_for_incoming_message( void ) -{ - can_frame_s rx_frame; - can_status_t ret = check_for_rx_frame( g_obd_can, &rx_frame ); - - if( ret == CAN_RX_FRAME_AVAILABLE ) - { - process_rx_frame( &rx_frame ); - } -} - - -void static publish_heartbeat_report( void ) -{ - g_tx_heartbeat.id = (OSCC_REPORT_HEARTBEAT_CAN_ID + OSCC_MODULE_CAN_GATEWAY_NODE_ID); - g_tx_heartbeat.dlc = OSCC_REPORT_HEARTBEAT_CAN_DLC; - g_tx_heartbeat.data.hardware_version = OSCC_MODULE_CAN_GATEWAY_VERSION_HARDWARE; - g_tx_heartbeat.data.firmware_version = OSCC_MODULE_CAN_GATEWAY_VERSION_FIRMWARE; - - g_control_can.sendMsgBuf( - g_tx_heartbeat.id, - CAN_STANDARD, - g_tx_heartbeat.dlc, - (uint8_t *) &g_tx_heartbeat.data ); - - g_tx_heartbeat.timestamp = GET_TIMESTAMP_MS(); -} - - -static void publish_chassis_state_1_report( void ) -{ - g_tx_chassis_state_1.id = OSCC_REPORT_CHASSIS_STATE_1_CAN_ID; - g_tx_chassis_state_1.dlc = OSCC_REPORT_CHASSIS_STATE_1_CAN_DLC; - - g_control_can.sendMsgBuf( - g_tx_chassis_state_1.id, - CAN_STANDARD, - g_tx_chassis_state_1.dlc, - (uint8_t *) &g_tx_chassis_state_1.data ); - - g_tx_chassis_state_1.timestamp = GET_TIMESTAMP_MS(); -} - - -static void publish_chassis_state_2_report( void ) -{ - g_tx_chassis_state_2.id = OSCC_REPORT_CHASSIS_STATE_2_CAN_ID; - g_tx_chassis_state_2.dlc = OSCC_REPORT_CHASSIS_STATE_2_CAN_DLC; - - g_control_can.sendMsgBuf( - g_tx_chassis_state_2.id, - CAN_STANDARD, - g_tx_chassis_state_2.dlc, - (uint8_t *) &g_tx_chassis_state_2.data ); - - g_tx_chassis_state_2.timestamp = GET_TIMESTAMP_MS(); -} - - -static void process_obd_steering_wheel_angle( - const uint8_t * const data ) -{ - if ( data != NULL ) - { - kia_soul_obd_steering_wheel_angle_data_s * steering_wheel_angle_data = - (kia_soul_obd_steering_wheel_angle_data_s *) data; - - CLEAR_HEARTBEAT_WARNING( KIA_SOUL_OBD_STEERING_WHEEL_ANGLE_HEARTBEAT_WARNING_BIT ); - SET_CHASSIS_1_FLAG( OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_STEER_WHEEL_ANGLE_VALID ); - CLEAR_CHASSIS_1_FLAG( OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_STEER_WHEEL_ANGLE_RATE_VALID ); - - g_tx_chassis_state_1.data.steering_wheel_angle_rate = 0; - g_tx_chassis_state_1.data.steering_wheel_angle = steering_wheel_angle_data->steering_angle; - - g_obd_steering_wheel_angle_rx_timestamp = GET_TIMESTAMP_MS( ); - } -} - - -static void process_obd_wheel_speed( - const uint8_t * const data ) -{ - if ( data != NULL ) - { - kia_soul_obd_wheel_speed_data_s * wheel_speed_data = - (kia_soul_obd_wheel_speed_data_s *) data; - - CLEAR_HEARTBEAT_WARNING( KIA_SOUL_OBD_WHEEL_SPEED_HEARTBEAT_WARNING_BIT ); - SET_CHASSIS_1_FLAG( OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_WHEEL_SPEED_VALID ); - - g_tx_chassis_state_2.data.wheel_speed_front_left = wheel_speed_data->wheel_speed_front_left; - g_tx_chassis_state_2.data.wheel_speed_front_right = wheel_speed_data->wheel_speed_front_right; - g_tx_chassis_state_2.data.wheel_speed_rear_left = wheel_speed_data->wheel_speed_rear_left; - g_tx_chassis_state_2.data.wheel_speed_rear_right = wheel_speed_data->wheel_speed_rear_right; - - g_obd_wheel_speed_rx_timestamp = GET_TIMESTAMP_MS( ); - } -} - - -static void process_obd_brake_pressure( - const uint8_t * const data ) -{ - if ( data != NULL ) - { - kia_soul_obd_brake_pressure_data_s * brake_pressure_data = - (kia_soul_obd_brake_pressure_data_s *) data; - - CLEAR_HEARTBEAT_WARNING( KIA_SOUL_OBD_BRAKE_PRESSURE_WARNING_BIT ); - SET_CHASSIS_1_FLAG( OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_BRAKE_PRESSURE_VALID ); - - g_tx_chassis_state_1.data.brake_pressure = brake_pressure_data->master_cylinder_pressure; - - g_obd_brake_pressure_rx_timestamp = GET_TIMESTAMP_MS( ); - } -} - - -static void process_obd_turn_signal( - const uint8_t * const data ) -{ - if ( data != NULL ) - { - kia_soul_obd_turn_signal_data_s * turn_signal_data = - (kia_soul_obd_turn_signal_data_s *) data; - - CLEAR_HEARTBEAT_WARNING( KIA_SOUL_OBD_TURN_SIGNAL_WARNING_BIT ); - CLEAR_CHASSIS_1_FLAG( OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_LEFT_TURN_SIGNAL_ON ); - CLEAR_CHASSIS_1_FLAG( OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_RIGHT_TURN_SIGNAL_ON ); - CLEAR_CHASSIS_1_FLAG( OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_BRAKE_SIGNAL_ON ); - - if( turn_signal_data->turn_signal_flags == KIA_SOUL_OBD_TURN_SIGNAL_FLAG_LEFT_TURN ) - { - SET_CHASSIS_1_FLAG( OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_LEFT_TURN_SIGNAL_ON ); - } - else if( turn_signal_data->turn_signal_flags == KIA_SOUL_OBD_TURN_SIGNAL_FLAG_RIGHT_TURN ) - { - SET_CHASSIS_1_FLAG( OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_RIGHT_TURN_SIGNAL_ON ); - } - - g_obd_turn_signal_rx_timestamp = GET_TIMESTAMP_MS( ); - } -} - - -static void process_rx_frame( - const can_frame_s * const rx_frame ) -{ - if ( rx_frame != NULL ) - { - if( rx_frame->id == KIA_SOUL_OBD_STEERING_WHEEL_ANGLE_CAN_ID ) - { - process_obd_steering_wheel_angle( rx_frame->data ); - } - else if( rx_frame->id == KIA_SOUL_OBD_WHEEL_SPEED_CAN_ID ) - { - process_obd_wheel_speed( rx_frame->data ); - } - else if( rx_frame->id == KIA_SOUL_OBD_BRAKE_PRESSURE_CAN_ID ) - { - process_obd_brake_pressure( rx_frame->data ); - } - else if( rx_frame->id == KIA_SOUL_OBD_TURN_SIGNAL_CAN_ID ) - { - process_obd_turn_signal( rx_frame->data ); - } - } -} diff --git a/platforms/kia_soul/firmware/can_gateway/src/init.cpp b/platforms/kia_soul/firmware/can_gateway/src/init.cpp deleted file mode 100644 index 0826a766..00000000 --- a/platforms/kia_soul/firmware/can_gateway/src/init.cpp +++ /dev/null @@ -1,59 +0,0 @@ -/** - * @file init.cpp - * - */ - - -#include -#include "oscc_serial.h" -#include "oscc_can.h" -#include "debug.h" -#include "oscc_time.h" - -#include "globals.h" -#include "init.h" - - -void init_globals( void ) -{ - memset( - &g_tx_heartbeat, - 0, - sizeof(g_tx_heartbeat) ); - - memset( - &g_tx_chassis_state_1, - 0, - sizeof(g_tx_chassis_state_1) ); - - memset( - &g_tx_chassis_state_2, - 0, - sizeof(g_tx_chassis_state_2) ); - - // initialize timestamps so that we don't get timeouts on start - g_obd_steering_wheel_angle_rx_timestamp = GET_TIMESTAMP_MS(); - g_obd_wheel_speed_rx_timestamp = GET_TIMESTAMP_MS(); - g_obd_brake_pressure_rx_timestamp = GET_TIMESTAMP_MS(); - g_obd_turn_signal_rx_timestamp = GET_TIMESTAMP_MS(); - - // wait a little between timestamps transmissions are offset - g_tx_chassis_state_1.timestamp = GET_TIMESTAMP_MS(); - SLEEP_MS(5); - g_tx_chassis_state_2.timestamp = GET_TIMESTAMP_MS(); - SLEEP_MS(5); -} - - -void init_communication_interfaces( void ) -{ - #ifdef DEBUG - init_serial(); - #endif - - DEBUG_PRINT( "init OBD CAN - "); - init_can( g_obd_can ); - - DEBUG_PRINT( "init Control CAN - "); - init_can( g_control_can ); -} diff --git a/platforms/kia_soul/firmware/can_gateway/tests/features/receiving_reports.feature b/platforms/kia_soul/firmware/can_gateway/tests/features/receiving_reports.feature deleted file mode 100644 index 2e31ccff..00000000 --- a/platforms/kia_soul/firmware/can_gateway/tests/features/receiving_reports.feature +++ /dev/null @@ -1,72 +0,0 @@ -# language: en - -Feature: Receiving reports - - OBD reports should be received and parsed. - - - Scenario Outline: Steering wheel angle report sent from vehicle. - When a steering wheel angle report is sent from the vehicle with the steering wheel angle - - Then the steering wheel angle heartbeat warning should be cleared - And the steering wheel angle rate valid flag should be cleared - And the steering wheel angle valid flag should be set - And the Chassis State 1 steering wheel angle field should be set to - And the Chassis State 1 steering wheel angle rate field should be set to 0 - And the last received steering wheel angle timestamp should be set - - Examples: - | angle | - | -256 | - | -128 | - | 0 | - | 128 | - | 256 | - - - Scenario Outline: Wheel speed report sent from vehicle. - When a wheel speed report is sent from the vehicle with the wheel speed - - Then the wheel speed heartbeat warning should be cleared - And the wheel speed valid flag should be set - And the Chassis State 2 wheel speed fields should be set to - And the last received wheel speed timestamp should be set - - Examples: - | speed | - | -256 | - | -128 | - | 0 | - | 128 | - | 256 | - - - Scenario Outline: Brake pressure report sent from vehicle. - When a brake pressure report is sent from the vehicle with the brake pressure - - Then the brake pressure heartbeat warning should be cleared - And the brake pressure valid flag should be set - And the Chassis State 1 brake pressure field should be set to - And the last received brake pressure timestamp should be set - - Examples: - | pressure | - | -256 | - | -128 | - | 0 | - | 128 | - | 256 | - - - Scenario Outline: Turn signal report sent from vehicle. - When a turn signal report is sent from the vehicle with the turn signal - - Then the turn signal heartbeat warning should be cleared - And the left turn signal flag should be - And the right turn signal flag should be - And the brake signal flag should be - - Examples: - | signal | left_flag | right_flag | brake_flag | - | left | set | cleared | cleared | - | right | cleared | set | cleared | diff --git a/platforms/kia_soul/firmware/can_gateway/tests/features/sending_reports.feature b/platforms/kia_soul/firmware/can_gateway/tests/features/sending_reports.feature deleted file mode 100644 index e29ac0e1..00000000 --- a/platforms/kia_soul/firmware/can_gateway/tests/features/sending_reports.feature +++ /dev/null @@ -1,23 +0,0 @@ -# language: en - -Feature: Sending reports - - Steering reports should be published to the control CAN bus after an interval. - - - Scenario: Heartbeat report published after an interval - When the time since the last heartbeat report publishing exceeds the interval - - Then a heartbeat report should be published to the control CAN bus - - - Scenario: Chassis State 1 report published after an interval - When the time since the last Chassis State 1 report publishing exceeds the interval - - Then a Chassis State 1 report should be published to the control CAN bus - - - Scenario: Chassis State 2 report published after an interval - When the time since the last Chassis State 2 report publishing exceeds the interval - - Then a Chassis State 2 report should be published to the control CAN bus diff --git a/platforms/kia_soul/firmware/can_gateway/tests/features/step_definitions/common.cpp b/platforms/kia_soul/firmware/can_gateway/tests/features/step_definitions/common.cpp deleted file mode 100644 index e6b5b9ce..00000000 --- a/platforms/kia_soul/firmware/can_gateway/tests/features/step_definitions/common.cpp +++ /dev/null @@ -1,290 +0,0 @@ -#include -#include -#include -#include -#include - -#include "Arduino.h" -#include "communications.h" -#include "oscc_can.h" -#include "mcp_can.h" -#include "obd_can_protocol.h" -#include "chassis_state_can_protocol.h" -#include "globals.h" - -using namespace cgreen; - - -extern unsigned long g_mock_arduino_millis_return; -extern uint8_t g_mock_arduino_digital_write_pin; -extern uint8_t g_mock_arduino_digital_write_val; -extern int g_mock_arduino_analog_read_return; - -extern INT8U g_mock_mcp_can_check_receive_return; - -extern INT32U g_mock_mcp_can_read_msg_buf_id; -extern INT8U g_mock_mcp_can_read_msg_buf_buf[8]; - -extern INT32U g_mock_mcp_can_send_msg_buf_id; -extern INT8U g_mock_mcp_can_send_msg_buf_ext; -extern INT8U g_mock_mcp_can_send_msg_buf_len; -extern INT8U *g_mock_mcp_can_send_msg_buf_buf; - -extern oscc_report_heartbeat_s g_tx_heartbeat; -extern oscc_report_chassis_state_1_s g_tx_chassis_state_1; -extern oscc_report_chassis_state_2_s g_tx_chassis_state_2; - -extern uint32_t g_obd_steering_wheel_angle_rx_timestamp; -extern uint32_t g_obd_wheel_speed_rx_timestamp; -extern uint32_t g_obd_brake_pressure_rx_timestamp; -extern uint32_t g_obd_turn_signal_rx_timestamp; - - -// return to known state before every scenario -BEFORE() -{ - g_mock_mcp_can_check_receive_return = -1; - g_mock_mcp_can_read_msg_buf_id = 0; - g_mock_arduino_millis_return = 555; - - memset( - &g_mock_mcp_can_read_msg_buf_buf, - 0, - sizeof(g_mock_mcp_can_read_msg_buf_buf)); - - g_mock_arduino_digital_write_pin = UINT8_MAX; - g_mock_arduino_digital_write_val = UINT8_MAX; - g_mock_arduino_analog_read_return = INT_MAX; - - memset( - &g_tx_heartbeat, - 0, - sizeof(g_tx_heartbeat)); - - memset( - &g_tx_chassis_state_1, - 0, - sizeof(g_tx_chassis_state_1)); - - memset( - &g_tx_chassis_state_2, - 0, - sizeof(g_tx_chassis_state_2)); - - g_obd_steering_wheel_angle_rx_timestamp = 0; - g_obd_wheel_speed_rx_timestamp = 0; - g_obd_brake_pressure_rx_timestamp = 0; - uint32_t g_obd_turn_signal_rx_timestamp = 0; -} - - -THEN("^the steering wheel angle heartbeat warning should be (.*)$") -{ - REGEX_PARAM(std::string, action); - - if(action == "set") - { - assert_that( - g_tx_heartbeat.data.warning_register & KIA_SOUL_OBD_STEERING_WHEEL_ANGLE_HEARTBEAT_WARNING_BIT, - is_equal_to(KIA_SOUL_OBD_STEERING_WHEEL_ANGLE_HEARTBEAT_WARNING_BIT)); - } - else if(action == "cleared") - { - assert_that( - g_tx_heartbeat.data.warning_register & KIA_SOUL_OBD_STEERING_WHEEL_ANGLE_HEARTBEAT_WARNING_BIT, - is_not_equal_to(KIA_SOUL_OBD_STEERING_WHEEL_ANGLE_HEARTBEAT_WARNING_BIT)); - } -} - - -THEN("^the steering wheel angle valid flag should be (.*)$") -{ - REGEX_PARAM(std::string, action); - - if(action == "set") - { - assert_that( - g_tx_chassis_state_1.data.flags & OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_STEER_WHEEL_ANGLE_VALID, - is_equal_to(OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_STEER_WHEEL_ANGLE_VALID)); - } - else if(action == "cleared") - { - - assert_that( - g_tx_chassis_state_1.data.flags & OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_STEER_WHEEL_ANGLE_VALID, - is_not_equal_to(OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_STEER_WHEEL_ANGLE_VALID)); - } -} - - -THEN("^the steering wheel angle rate valid flag should be (.*)$") -{ - REGEX_PARAM(std::string, action); - - if(action == "set") - { - assert_that( - g_tx_chassis_state_1.data.flags & OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_STEER_WHEEL_ANGLE_RATE_VALID, - is_equal_to(OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_STEER_WHEEL_ANGLE_RATE_VALID)); - } - else if(action == "cleared") - { - assert_that( - g_tx_chassis_state_1.data.flags & OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_STEER_WHEEL_ANGLE_RATE_VALID, - is_not_equal_to(OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_STEER_WHEEL_ANGLE_RATE_VALID)); - } -} - - -THEN("^the wheel speed heartbeat warning should be (.*)$") -{ - REGEX_PARAM(std::string, action); - - if(action == "set") - { - assert_that( - g_tx_heartbeat.data.warning_register & KIA_SOUL_OBD_WHEEL_SPEED_HEARTBEAT_WARNING_BIT, - is_equal_to(KIA_SOUL_OBD_WHEEL_SPEED_HEARTBEAT_WARNING_BIT)); - } - else if(action == "cleared") - { - assert_that( - g_tx_heartbeat.data.warning_register & KIA_SOUL_OBD_WHEEL_SPEED_HEARTBEAT_WARNING_BIT, - is_not_equal_to(KIA_SOUL_OBD_WHEEL_SPEED_HEARTBEAT_WARNING_BIT)); - } -} - - -THEN("^the wheel speed valid flag should be (.*)$") -{ - REGEX_PARAM(std::string, action); - - if(action == "set") - { - assert_that( - g_tx_chassis_state_1.data.flags & OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_WHEEL_SPEED_VALID, - is_equal_to(OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_WHEEL_SPEED_VALID)); - } - else if(action == "cleared") - { - assert_that( - g_tx_chassis_state_1.data.flags & OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_WHEEL_SPEED_VALID, - is_not_equal_to(OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_WHEEL_SPEED_VALID)); - } -} - - -THEN("^the brake pressure heartbeat warning should be (.*)$") -{ - REGEX_PARAM(std::string, action); - - if(action == "set") - { - assert_that( - g_tx_heartbeat.data.warning_register & KIA_SOUL_OBD_BRAKE_PRESSURE_WARNING_BIT, - is_equal_to(KIA_SOUL_OBD_BRAKE_PRESSURE_WARNING_BIT)); - } - else if(action == "cleared") - { - assert_that( - g_tx_heartbeat.data.warning_register & KIA_SOUL_OBD_BRAKE_PRESSURE_WARNING_BIT, - is_not_equal_to(KIA_SOUL_OBD_BRAKE_PRESSURE_WARNING_BIT)); - } -} - - -THEN("^the brake pressure valid flag should be (.*)$") -{ - REGEX_PARAM(std::string, action); - - if(action == "set") - { - assert_that( - g_tx_chassis_state_1.data.flags & OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_BRAKE_PRESSURE_VALID, - is_equal_to(OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_BRAKE_PRESSURE_VALID)); - } - else if(action == "cleared") - { - assert_that( - g_tx_chassis_state_1.data.flags & OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_BRAKE_PRESSURE_VALID, - is_not_equal_to(OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_BRAKE_PRESSURE_VALID)); - } -} - - -THEN("^the turn signal heartbeat warning should be (.*)") -{ - REGEX_PARAM(std::string, action); - - if(action == "set") - { - assert_that( - g_tx_heartbeat.data.warning_register & KIA_SOUL_OBD_TURN_SIGNAL_WARNING_BIT, - is_equal_to(KIA_SOUL_OBD_TURN_SIGNAL_WARNING_BIT)); - } - else if(action == "cleared") - { - assert_that( - g_tx_heartbeat.data.warning_register & KIA_SOUL_OBD_TURN_SIGNAL_WARNING_BIT, - is_not_equal_to(KIA_SOUL_OBD_TURN_SIGNAL_WARNING_BIT)); - } -} - - -THEN("^the left turn signal flag should be (.*)$") -{ - REGEX_PARAM(std::string, action); - - if(action == "set") - { - assert_that( - g_tx_chassis_state_1.data.flags & OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_LEFT_TURN_SIGNAL_ON, - is_equal_to(OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_LEFT_TURN_SIGNAL_ON)); - } - else if(action == "cleared") - { - assert_that( - g_tx_chassis_state_1.data.flags & OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_LEFT_TURN_SIGNAL_ON, - is_not_equal_to(OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_LEFT_TURN_SIGNAL_ON)); - } -} - - -THEN("^the right turn signal flag should be (.*)$") -{ - REGEX_PARAM(std::string, action); - - if(action == "set") - { - assert_that( - g_tx_chassis_state_1.data.flags & OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_RIGHT_TURN_SIGNAL_ON, - is_equal_to(OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_RIGHT_TURN_SIGNAL_ON)); - } - else if(action == "cleared") - { - assert_that( - g_tx_chassis_state_1.data.flags & OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_RIGHT_TURN_SIGNAL_ON, - is_not_equal_to(OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_RIGHT_TURN_SIGNAL_ON)); - } -} - - -THEN("^the brake signal flag should be (.*)$") -{ - REGEX_PARAM(std::string, action); - - if(action == "set") - { - assert_that( - g_tx_chassis_state_1.data.flags & OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_BRAKE_SIGNAL_ON, - is_equal_to(OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_BRAKE_SIGNAL_ON)); - } - else if(action == "cleared") - { - assert_that( - g_tx_chassis_state_1.data.flags & OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_BRAKE_SIGNAL_ON, - is_not_equal_to(OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_BRAKE_SIGNAL_ON)); - } -} - - diff --git a/platforms/kia_soul/firmware/can_gateway/tests/features/step_definitions/receiving_reports.cpp b/platforms/kia_soul/firmware/can_gateway/tests/features/step_definitions/receiving_reports.cpp deleted file mode 100644 index e7a8e7d4..00000000 --- a/platforms/kia_soul/firmware/can_gateway/tests/features/step_definitions/receiving_reports.cpp +++ /dev/null @@ -1,162 +0,0 @@ -WHEN("^a steering wheel angle report is sent from the vehicle with the steering wheel angle (.*)$") -{ - REGEX_PARAM(int, angle); - - kia_soul_obd_steering_wheel_angle_data_s * steering_wheel_angle_data = - (kia_soul_obd_steering_wheel_angle_data_s *) g_mock_mcp_can_read_msg_buf_buf; - - steering_wheel_angle_data->steering_angle = angle; - - g_mock_mcp_can_read_msg_buf_id = KIA_SOUL_OBD_STEERING_WHEEL_ANGLE_CAN_ID; - g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL; - - check_for_incoming_message(); -} - - -THEN("^the Chassis State 1 steering wheel angle field should be set to (.*)$") -{ - REGEX_PARAM(int, angle); - - assert_that( - g_tx_chassis_state_1.data.steering_wheel_angle, - is_equal_to(angle)); -} - - -THEN("^the Chassis State 1 steering wheel angle rate field should be set to (.*)$") -{ - REGEX_PARAM(int, angle); - - assert_that( - g_tx_chassis_state_1.data.steering_wheel_angle_rate, - is_equal_to(angle)); -} - - -THEN("^the last received steering wheel angle timestamp should be set$") -{ - assert_that( - g_obd_steering_wheel_angle_rx_timestamp, - is_equal_to(g_mock_arduino_millis_return)); -} - - - - -WHEN("^a wheel speed report is sent from the vehicle with the wheel speed (.*)$") -{ - REGEX_PARAM(int, speed); - - kia_soul_obd_wheel_speed_data_s * wheel_speed_data = - (kia_soul_obd_wheel_speed_data_s *) g_mock_mcp_can_read_msg_buf_buf; - - wheel_speed_data->wheel_speed_front_left = speed; - wheel_speed_data->wheel_speed_front_right = speed; - wheel_speed_data->wheel_speed_rear_left = speed; - wheel_speed_data->wheel_speed_rear_right = speed; - - g_mock_mcp_can_read_msg_buf_id = KIA_SOUL_OBD_WHEEL_SPEED_CAN_ID; - g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL; - - check_for_incoming_message(); -} - - -THEN("^the Chassis State 2 wheel speed fields should be set to (.*)$") -{ - REGEX_PARAM(int, speed); - - assert_that( - g_tx_chassis_state_2.data.wheel_speed_front_left, - is_equal_to(speed)); - - assert_that( - g_tx_chassis_state_2.data.wheel_speed_front_right, - is_equal_to(speed)); - - assert_that( - g_tx_chassis_state_2.data.wheel_speed_rear_left, - is_equal_to(speed)); - - assert_that( - g_tx_chassis_state_2.data.wheel_speed_rear_right, - is_equal_to(speed)); -} - - -THEN("^the last received wheel speed timestamp should be set$") -{ - assert_that( - g_obd_wheel_speed_rx_timestamp, - is_equal_to(g_mock_arduino_millis_return)); -} - - - - -WHEN("^a brake pressure report is sent from the vehicle with the brake pressure (.*)$") -{ - REGEX_PARAM(int, pressure); - - kia_soul_obd_brake_pressure_data_s * brake_pressure_data = - (kia_soul_obd_brake_pressure_data_s *) g_mock_mcp_can_read_msg_buf_buf; - - brake_pressure_data->master_cylinder_pressure = pressure; - - g_mock_mcp_can_read_msg_buf_id = KIA_SOUL_OBD_BRAKE_PRESSURE_CAN_ID; - g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL; - - check_for_incoming_message(); -} - - -THEN("^the Chassis State 1 brake pressure field should be set to (.*)$") -{ - REGEX_PARAM(int, pressure); - - assert_that( - g_tx_chassis_state_1.data.brake_pressure, - is_equal_to(pressure)); -} - - -THEN("^the last received brake pressure timestamp should be set$") -{ - assert_that( - g_obd_brake_pressure_rx_timestamp, - is_equal_to(g_mock_arduino_millis_return)); -} - - - - -WHEN("^a turn signal report is sent from the vehicle with the turn signal (.*)$") -{ - REGEX_PARAM(std::string, turn_signal); - - kia_soul_obd_turn_signal_data_s * turn_signal_data = - (kia_soul_obd_turn_signal_data_s *) g_mock_mcp_can_read_msg_buf_buf; - - if(turn_signal == "left") - { - turn_signal_data->turn_signal_flags = KIA_SOUL_OBD_TURN_SIGNAL_FLAG_LEFT_TURN; - } - else if(turn_signal == "right") - { - turn_signal_data->turn_signal_flags = KIA_SOUL_OBD_TURN_SIGNAL_FLAG_RIGHT_TURN; - } - - g_mock_mcp_can_read_msg_buf_id = KIA_SOUL_OBD_TURN_SIGNAL_CAN_ID; - g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL; - - check_for_incoming_message(); -} - - -THEN("^the last received turn signal timestamp should be set$") -{ - assert_that( - g_obd_turn_signal_rx_timestamp, - is_equal_to(g_mock_arduino_millis_return)); -} diff --git a/platforms/kia_soul/firmware/can_gateway/tests/features/step_definitions/sending_reports.cpp b/platforms/kia_soul/firmware/can_gateway/tests/features/step_definitions/sending_reports.cpp deleted file mode 100644 index 67d5dda4..00000000 --- a/platforms/kia_soul/firmware/can_gateway/tests/features/step_definitions/sending_reports.cpp +++ /dev/null @@ -1,113 +0,0 @@ -WHEN("^the time since the last heartbeat report publishing exceeds the interval$") -{ - // set the timestamps of the other reports to their respective publish intervals - // so that they aren't seen as being in need of a publishing themselves - g_tx_chassis_state_1.timestamp = OSCC_REPORT_CHASSIS_STATE_1_PUBLISH_INTERVAL_IN_MSEC; - g_tx_chassis_state_2.timestamp = OSCC_REPORT_CHASSIS_STATE_2_PUBLISH_INTERVAL_IN_MSEC; - - g_tx_heartbeat.timestamp = 0; - - g_mock_arduino_millis_return = OSCC_REPORT_HEARTBEAT_PUBLISH_INTERVAL_IN_MSEC; - - publish_reports(); -} - - -THEN("^a heartbeat report should be published to the control CAN bus$") -{ - assert_that( - g_mock_mcp_can_send_msg_buf_id, - is_equal_to(OSCC_REPORT_HEARTBEAT_CAN_ID + OSCC_MODULE_CAN_GATEWAY_NODE_ID)); - - assert_that( - g_mock_mcp_can_send_msg_buf_ext, - is_equal_to(CAN_STANDARD)); - - assert_that( - g_mock_mcp_can_send_msg_buf_len, - is_equal_to(OSCC_REPORT_HEARTBEAT_CAN_DLC)); - - assert_that( - g_tx_heartbeat.data.hardware_version, - is_equal_to(OSCC_MODULE_CAN_GATEWAY_VERSION_HARDWARE)); - - assert_that( - g_tx_heartbeat.data.firmware_version, - is_equal_to(OSCC_MODULE_CAN_GATEWAY_VERSION_FIRMWARE)); - - assert_that( - g_tx_heartbeat.timestamp, - is_equal_to(g_mock_arduino_millis_return)); -} - - -WHEN("^the time since the last Chassis State 1 report publishing exceeds the interval$") -{ - // set the timestamps of the other reports to their respective publish intervals - // so that they aren't seen as being in need of a publishing themselves - g_tx_heartbeat.timestamp = OSCC_REPORT_HEARTBEAT_PUBLISH_INTERVAL_IN_MSEC; - g_tx_chassis_state_2.timestamp = OSCC_REPORT_CHASSIS_STATE_2_PUBLISH_INTERVAL_IN_MSEC; - - g_tx_chassis_state_1.timestamp = 0; - - g_mock_arduino_millis_return = OSCC_REPORT_CHASSIS_STATE_1_PUBLISH_INTERVAL_IN_MSEC; - - publish_reports(); -} - - -THEN("^a Chassis State 1 report should be published to the control CAN bus$") -{ - assert_that( - g_mock_mcp_can_send_msg_buf_id, - is_equal_to(OSCC_REPORT_CHASSIS_STATE_1_CAN_ID)); - - assert_that( - g_mock_mcp_can_send_msg_buf_ext, - is_equal_to(CAN_STANDARD)); - - assert_that( - g_mock_mcp_can_send_msg_buf_len, - is_equal_to(OSCC_REPORT_CHASSIS_STATE_1_CAN_DLC)); - - assert_that( - g_tx_chassis_state_1.timestamp, - is_equal_to(g_mock_arduino_millis_return)); -} - - -WHEN("^the time since the last Chassis State 2 report publishing exceeds the interval$") -{ - // set the timestamps of the other reports to their respective publish intervals - // so that they aren't seen as being in need of a publishing themselves - g_tx_heartbeat.timestamp = OSCC_REPORT_HEARTBEAT_PUBLISH_INTERVAL_IN_MSEC; - g_tx_chassis_state_1.timestamp = OSCC_REPORT_CHASSIS_STATE_1_PUBLISH_INTERVAL_IN_MSEC; - - g_tx_chassis_state_2.timestamp = 0; - - g_mock_arduino_millis_return = OSCC_REPORT_CHASSIS_STATE_2_PUBLISH_INTERVAL_IN_MSEC; - - publish_reports(); -} - - -THEN("^a Chassis State 2 report should be published to the control CAN bus$") -{ - assert_that( - g_mock_mcp_can_send_msg_buf_id, - is_equal_to(OSCC_REPORT_CHASSIS_STATE_2_CAN_ID)); - - assert_that( - g_mock_mcp_can_send_msg_buf_ext, - is_equal_to(CAN_STANDARD)); - - assert_that( - g_mock_mcp_can_send_msg_buf_len, - is_equal_to(OSCC_REPORT_CHASSIS_STATE_2_CAN_DLC)); - - assert_that( - g_tx_chassis_state_2.timestamp, - is_equal_to(g_mock_arduino_millis_return)); -} - - diff --git a/platforms/kia_soul/firmware/can_gateway/tests/features/step_definitions/timeouts.cpp b/platforms/kia_soul/firmware/can_gateway/tests/features/step_definitions/timeouts.cpp deleted file mode 100644 index a6ef0a54..00000000 --- a/platforms/kia_soul/firmware/can_gateway/tests/features/step_definitions/timeouts.cpp +++ /dev/null @@ -1,43 +0,0 @@ -WHEN("^the time since the last received steering wheel angle report exceeds the timeout$") -{ - g_obd_steering_wheel_angle_rx_timestamp = 0; - - g_mock_arduino_millis_return = - KIA_SOUL_OBD_STEERING_WHEEL_ANGLE_RX_WARN_TIMEOUT_IN_MSEC; - - check_for_obd_timeout(); -} - - -WHEN("^the time since the last received wheel speed report exceeds the timeout$") -{ - g_obd_wheel_speed_rx_timestamp = 0; - - g_mock_arduino_millis_return = - KIA_SOUL_OBD_WHEEL_SPEED_RX_WARN_TIMEOUT_IN_MSEC; - - check_for_obd_timeout(); -} - - -WHEN("^the time since the last received brake pressure report exceeds the timeout$") -{ - g_obd_brake_pressure_rx_timestamp = 0; - - g_mock_arduino_millis_return = - KIA_SOUL_OBD_BRAKE_PRESSURE_RX_WARN_TIMEOUT_IN_MSEC; - - check_for_obd_timeout(); -} - - -WHEN("^the time since the last received turn signal report exceeds the timeout$") -{ - g_obd_turn_signal_rx_timestamp = 0; - - g_mock_arduino_millis_return = - KIA_SOUL_OBD_TURN_SIGNAL_RX_WARN_TIMEOUT_IN_MSEC; - - check_for_obd_timeout(); -} - diff --git a/platforms/kia_soul/firmware/can_gateway/tests/features/timeouts.feature b/platforms/kia_soul/firmware/can_gateway/tests/features/timeouts.feature deleted file mode 100644 index 23e400b4..00000000 --- a/platforms/kia_soul/firmware/can_gateway/tests/features/timeouts.feature +++ /dev/null @@ -1,37 +0,0 @@ -# language: en - -Feature: Timeouts - - If the module doesn't receive an OBD report from the vehicle after an amount - of time, warning flags should be set and valid flags cleared. - - - Scenario: Steering wheel angle timeout - When the time since the last received steering wheel angle report exceeds the timeout - - Then the steering wheel angle heartbeat warning should be set - And the steering wheel angle valid flag should be cleared - And the steering wheel angle rate valid flag should be cleared - - - Scenario: Wheel speed timeout - When the time since the last received wheel speed report exceeds the timeout - - Then the wheel speed heartbeat warning should be set - And the wheel speed valid flag should be cleared - - - Scenario: Brake pressure timeout - When the time since the last received brake pressure report exceeds the timeout - - Then the brake pressure heartbeat warning should be set - And the brake pressure valid flag should be cleared - - - Scenario: Turn signal timeout - When the time since the last received turn signal report exceeds the timeout - - Then the turn signal heartbeat warning should be set - And the left turn signal flag should be cleared - And the right turn signal flag should be cleared - And the brake signal flag should be cleared diff --git a/platforms/kia_soul/firmware/steering/include/communications.h b/platforms/kia_soul/firmware/steering/include/communications.h deleted file mode 100644 index e8df1a06..00000000 --- a/platforms/kia_soul/firmware/steering/include/communications.h +++ /dev/null @@ -1,81 +0,0 @@ -/** - * @file communications.h - * @brief Communication functionality. - * - */ - - -#ifndef _OSCC_KIA_SOUL_STEERING_COMMUNICATIONS_H_ -#define _OSCC_KIA_SOUL_STEERING_COMMUNICATIONS_H_ - - -/* - * @brief Scalar value to convert angle reported by OBD to human-readable value. - * - */ -#define RAW_ANGLE_SCALAR ( 0.0076294 ) - -/* - * @brief Scalar value to convert wheel angle (-40 to 40 degrees) to steering - * wheel angle (-470 to 470) degrees. - * - */ -#define WHEEL_ANGLE_TO_STEERING_WHEEL_ANGLE_SCALAR ( 11.7 ) - - -/* - * @brief Amount of time after controller command that is considered a - * timeout. [milliseconds] - * - */ -#define COMMAND_TIMEOUT_IN_MSEC ( 250 ) - - -/* - * @brief Amount of time after a Chassis State 1 report that is considered a - * timeout. [milliseconds] - * - */ -#define CHASSIS_STATE_1_REPORT_TIMEOUT_IN_MSEC ( 500 ) - - -// **************************************************************************** -// Function: publish_reports -// -// Purpose: Publish all valid reports to CAN bus. -// -// Returns: void -// -// Parameters: void -// -// **************************************************************************** -void publish_reports( void ); - - -// **************************************************************************** -// Function: check_for_timeouts -// -// Purpose: Check for command and report timeouts. -// -// Returns: void -// -// Parameters: void -// -// **************************************************************************** -void check_for_timeouts( void ); - - -// **************************************************************************** -// Function: check_for_incoming_message -// -// Purpose: Check CAN bus for incoming messages and process any present. -// -// Returns: void -// -// Parameters: void -// -// **************************************************************************** -void check_for_incoming_message( void ); - - -#endif /* _OSCC_KIA_SOUL_STEERING_COMMUNICATIONS_H_ */ diff --git a/platforms/kia_soul/firmware/steering/include/steering_control.h b/platforms/kia_soul/firmware/steering/include/steering_control.h deleted file mode 100644 index 9aa45319..00000000 --- a/platforms/kia_soul/firmware/steering/include/steering_control.h +++ /dev/null @@ -1,235 +0,0 @@ -/** - * @file steering_control.h - * @brief Control of the steering system. - * - */ - - -#ifndef _OSCC_KIA_SOUL_STEERING_CONTROL_H_ -#define _OSCC_KIA_SOUL_STEERING_CONTROL_H_ - - -#include - - -/******************************************************************************* -* WARNING -* -* The ranges selected to do steering control are carefully tested to -* ensure that a torque is not requested that the vehicles steering motor -* cannot handle. By changing any of this code you risk attempting to actuate -* a torque outside of the vehicles valid range. Actuating a torque outside of -* the vehicles valid range will, at best, cause the vehicle to go into an -* unrecoverable fault state. Clearing this fault state requires one of Kia's -* native diagnostics tools, and someone who knows how to clear DTC codes with -* said tool. -* -* It is NOT recommended to modify any of the existing control ranges, or -* gains, without expert knowledge. -*******************************************************************************/ - -/* - * @brief Minimum steering angle rate. [Newton meters] - * - */ -#define TORQUE_MIN_IN_NEWTON_METERS ( -1500.0 ) - -/* - * @brief Maximum steering angle rate. [Newton meters] - * - */ -#define TORQUE_MAX_IN_NEWTON_METERS ( 1500.0 ) - -/* - * @brief Proportional gain of the PID controller. - * - */ -#define PID_PROPORTIONAL_GAIN ( 0.3 ) - -/* - * @brief Integral gain of the PID controller. - * - */ -#define PID_INTEGRAL_GAIN ( 1.3 ) - -/* - * @brief Derivative gain of the PID controller. - * - */ -#define PID_DERIVATIVE_GAIN ( 0.03 ) - -/* - * @brief Value of the torque sensor that indicates operator override. - * [degrees/microsecond] - * - */ -#define OVERRIDE_WHEEL_THRESHOLD_IN_DEGREES_PER_USEC ( 3000 ) - -/* - * @brief Amount of time between sensor checks. [milliseconds] - * - */ -#define SENSOR_VALIDITY_CHECK_INTERVAL_IN_MSEC ( 250 ) - -/* - * @brief Number of consecutive faults that can occur when reading the - * torque sensor before control is disabled. - * - */ -#define SENSOR_VALIDITY_CHECK_FAULT_COUNT ( 4 ) - -/* - * @brief Alpha term for the exponential filter used to smooth the sensor input. - * - */ -#define TORQUE_SENSOR_EXPONENTIAL_FILTER_ALPHA ( 0.5 ) - -/* - * @brief Number of bits to shift to go from a 10-bit value to a 12-bit value. - * - */ -#define BIT_SHIFT_10BIT_TO_12BIT ( 2 ) - -/* - * @brief Number of steps per volt corresponding to 4096 steps across 5 volts. - * - */ -#define STEPS_PER_VOLT ( 819.2 ) - -/* - * @brief Scalar value for the low spoof signal taken from a calibration curve. - * - */ -#define SPOOF_LOW_SIGNAL_CALIBRATION_CURVE_SCALAR ( 0.0008 ) - -/* - * @brief Offset value for the low spoof signal taken from a calibration curve. - * - */ -#define SPOOF_LOW_SIGNAL_CALIBRATION_CURVE_OFFSET ( 2.26 ) - -/* - * @brief Scalar value for the high spoof signal taken from a calibration curve. - * - */ -#define SPOOF_HIGH_SIGNAL_CALIBRATION_CURVE_SCALAR ( -0.0008 ) - -/* - * @brief Offset value for the high spoof signal taken from a calibration curve. - * - */ -#define SPOOF_HIGH_SIGNAL_CALIBRATION_CURVE_OFFSET ( 2.5 ) - - -/** - * @brief Torque values. - * - * Contains the high and low torque values. - * - */ -typedef struct -{ - uint16_t low; /* Low value of torque. */ - - uint16_t high; /* High value of torque. */ -} steering_torque_s; - - -/** - * @brief Current steering control state. - * - * Current state of the throttle module control system. - * - */ -typedef struct -{ - bool enabled; /* Flag indicating control is currently enabled. */ - - bool operator_override; /* Flag indicating whether steering wheel was - manually turned by operator. */ - - bool invalid_sensor_value; /* Flag indicating a value read from one of the - sensors is invalid. */ - - bool obd_timeout; /* Flag indicating whether an OBD timeout has occurred. */ - - float current_steering_wheel_angle; /* Current steering angle as reported - by the vehicle. */ - - float previous_steering_wheel_angle; /* Last steering angle recorded. */ - - float commanded_steering_wheel_angle; /* Angle of steering wheel commanded - by controller. */ - - float commanded_steering_wheel_angle_rate; /* Rate of the steering wheel - angle commanded by controller. */ -} kia_soul_steering_control_state_s; - - -// **************************************************************************** -// Function: check_for_operator_override -// -// Purpose: Checks to see if the vehicle's operator has manually pressed -// the accelerator and disables control if they have. -// -// Returns: void -// -// Parameters: void -// -// **************************************************************************** -void check_for_operator_override( void ); - - -// **************************************************************************** -// Function: check_for_sensor_faults -// -// Purpose: Checks to see if valid values are being read from the sensors. -// -// Returns: void -// -// Parameters: void -// -// **************************************************************************** -void check_for_sensor_faults( void ); - - -// **************************************************************************** -// Function: update_steering -// -// Purpose: Writes steering spoof values to DAC. -// -// Returns: void -// -// Parameters: void -// -// **************************************************************************** -void update_steering( void ); - - -// ***************************************************** -// Function: enable_control -// -// Purpose: Enable control of the steering system. -// -// Returns: void -// -// Parameters: void -// -// ***************************************************** -void enable_control( void ); - - -// ***************************************************** -// Function: disable_control -// -// Purpose: Disable control of the steering system. -// -// Returns: void -// -// Parameters: void -// -// ***************************************************** -void disable_control( void ); - - -#endif /* _OSCC_KIA_SOUL_STEERING_CONTROL_H_ */ diff --git a/platforms/kia_soul/firmware/steering/src/communications.cpp b/platforms/kia_soul/firmware/steering/src/communications.cpp deleted file mode 100644 index 9f7e1a7f..00000000 --- a/platforms/kia_soul/firmware/steering/src/communications.cpp +++ /dev/null @@ -1,199 +0,0 @@ -/** - * @file communications.cpp - * - */ - - -#include "mcp_can.h" -#include "oscc_can.h" -#include "chassis_state_can_protocol.h" -#include "steering_can_protocol.h" -#include "oscc_time.h" -#include "debug.h" - -#include "globals.h" -#include "communications.h" -#include "steering_control.h" - - -static void publish_steering_report( void ); - -static void process_steering_command( - const uint8_t * const data ); - -static void process_chassis_state_1_report( - const uint8_t * const data ); - -static void process_rx_frame( - can_frame_s * const frame ); - -static void check_for_controller_command_timeout( void ); - -static void check_for_chassis_state_1_report_timeout( void ); - - -void publish_reports( void ) -{ - uint32_t delta = get_time_delta( g_steering_report_last_tx_timestamp, GET_TIMESTAMP_MS() ); - - if ( delta >= OSCC_REPORT_STEERING_PUBLISH_INTERVAL_IN_MSEC ) - { - publish_steering_report( ); - } -} - - -void check_for_incoming_message( void ) -{ - can_frame_s rx_frame; - can_status_t ret = check_for_rx_frame( g_control_can, &rx_frame ); - - if( ret == CAN_RX_FRAME_AVAILABLE ) - { - process_rx_frame( &rx_frame ); - } -} - - -void check_for_timeouts( void ) -{ - check_for_controller_command_timeout( ); - - check_for_chassis_state_1_report_timeout( ); -} - - -static void publish_steering_report( void ) -{ - oscc_report_steering_s steering_report; - - steering_report.id = OSCC_REPORT_STEERING_CAN_ID; - steering_report.dlc = OSCC_REPORT_STEERING_CAN_DLC; - steering_report.data.enabled = (uint8_t) g_steering_control_state.enabled; - steering_report.data.override = (uint8_t) g_steering_control_state.operator_override; - steering_report.data.current_steering_wheel_angle = (int16_t) g_steering_control_state.current_steering_wheel_angle; - steering_report.data.commanded_steering_wheel_angle = (int16_t) g_steering_control_state.commanded_steering_wheel_angle; - steering_report.data.fault_obd_timeout = (uint8_t) g_steering_control_state.obd_timeout; - steering_report.data.spoofed_torque_output = (int8_t) g_spoofed_torque_output_sum; - steering_report.data.fault_invalid_sensor_value = (uint8_t) g_steering_control_state.invalid_sensor_value; - - g_control_can.sendMsgBuf( - steering_report.id, - CAN_STANDARD, - steering_report.dlc, - (uint8_t *) &steering_report.data ); - - g_steering_report_last_tx_timestamp = GET_TIMESTAMP_MS( ); -} - - -static void process_steering_command( - const uint8_t * const data ) -{ - if ( data != NULL ) - { - const oscc_command_steering_data_s * const steering_command_data = - (oscc_command_steering_data_s *) data; - - if ( steering_command_data->enabled == true ) - { - // divisor values found empirically to best match steering output - g_steering_control_state.commanded_steering_wheel_angle = - (steering_command_data->commanded_steering_wheel_angle / 9.0); - - g_steering_control_state.commanded_steering_wheel_angle_rate = - (steering_command_data->commanded_steering_wheel_angle_rate * 9.0); - - DEBUG_PRINT( "controller commanded steering wheel angle: " ); - DEBUG_PRINTLN( g_steering_control_state.commanded_steering_wheel_angle ); - - enable_control( ); - } - else - { - disable_control( ); - } - - g_steering_command_last_rx_timestamp = GET_TIMESTAMP_MS( ); - } -} - - -static void process_chassis_state_1_report( - const uint8_t * const data ) -{ - if ( data != NULL ) - { - const oscc_report_chassis_state_1_data_s * const chassis_state_1_data = - (oscc_report_chassis_state_1_data_s *) data; - - if( chassis_state_1_data->flags - & OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_STEER_WHEEL_ANGLE_VALID ) - { - g_steering_control_state.current_steering_wheel_angle = - chassis_state_1_data->steering_wheel_angle - * RAW_ANGLE_SCALAR - * WHEEL_ANGLE_TO_STEERING_WHEEL_ANGLE_SCALAR; - - g_chassis_state_1_report_last_rx_timestamp = GET_TIMESTAMP_MS( ); - } - } -} - - -static void process_rx_frame( - can_frame_s * const frame ) -{ - if ( frame != NULL ) - { - if ( frame->id == OSCC_COMMAND_STEERING_CAN_ID ) - { - process_steering_command( frame->data ); - } - else if ( frame->id == OSCC_REPORT_CHASSIS_STATE_1_CAN_ID ) - { - process_chassis_state_1_report( frame->data ); - } - } -} - - -static void check_for_controller_command_timeout( void ) -{ - if( g_steering_control_state.enabled == true ) - { - bool timeout = is_timeout( - g_steering_command_last_rx_timestamp, - GET_TIMESTAMP_MS( ), - COMMAND_TIMEOUT_IN_MSEC); - - if( timeout == true ) - { - disable_control( ); - - DEBUG_PRINTLN( "Timeout - controller command" ); - } - } -} - - -static void check_for_chassis_state_1_report_timeout( void ) -{ - bool timeout = is_timeout( - g_chassis_state_1_report_last_rx_timestamp, - GET_TIMESTAMP_MS( ), - CHASSIS_STATE_1_REPORT_TIMEOUT_IN_MSEC); - - if( timeout == true ) - { - disable_control( ); - - g_steering_control_state.obd_timeout = true; - - DEBUG_PRINTLN( "Timeout - Chassis State 1 report" ); - } - else - { - g_steering_control_state.obd_timeout = false; - } -} diff --git a/platforms/kia_soul/firmware/steering/src/main.cpp b/platforms/kia_soul/firmware/steering/src/main.cpp deleted file mode 100644 index 62e9331f..00000000 --- a/platforms/kia_soul/firmware/steering/src/main.cpp +++ /dev/null @@ -1,57 +0,0 @@ -/** - * @file main.cpp - * - */ - - -#include -#include "arduino_init.h" -#include "debug.h" -#include "oscc_time.h" - -#include "init.h" -#include "globals.h" -#include "communications.h" -#include "steering_control.h" - - -int main( void ) -{ - init_arduino( ); - - init_globals( ); - - init_devices( ); - - init_communication_interfaces( ); - - wdt_enable( WDTO_120MS ); - - DEBUG_PRINTLN( "initialization complete" ); - - while( true ) - { - wdt_reset(); - - check_for_incoming_message( ); - - check_for_timeouts( ); - - check_for_sensor_faults( ); - - uint32_t time_since_last_control_loop_in_msec = get_time_delta( - g_last_control_loop_timestamp, - GET_TIMESTAMP_MS()); - - if( time_since_last_control_loop_in_msec > CONTROL_LOOP_INTERVAL_IN_MSEC ) - { - check_for_operator_override( ); - - update_steering( ); - - g_last_control_loop_timestamp = GET_TIMESTAMP_MS(); - } - - publish_reports( ); - } -} diff --git a/platforms/kia_soul/firmware/steering/src/steering_control.cpp b/platforms/kia_soul/firmware/steering/src/steering_control.cpp deleted file mode 100644 index 545d4b9f..00000000 --- a/platforms/kia_soul/firmware/steering/src/steering_control.cpp +++ /dev/null @@ -1,241 +0,0 @@ -/** - * @file steering_control.cpp - * - */ - - -#include -#include -#include -#include "debug.h" -#include "oscc_pid.h" -#include "oscc_dac.h" -#include "oscc_signal_smoothing.h" -#include "oscc_time.h" - -#include "globals.h" -#include "steering_control.h" - - -#define MSEC_TO_SEC(msec) ( (msec) / 1000.0 ) - - -static void calculate_torque_spoof( - const int16_t torque_target, - steering_torque_s * const spoof ); - -static void read_torque_sensor( - steering_torque_s * value ); - - -void check_for_operator_override( void ) -{ - if( g_steering_control_state.enabled == true - || g_steering_control_state.operator_override == true ) - { - steering_torque_s torque; - - read_torque_sensor( &torque ); - - if ( (abs(torque.high) >= OVERRIDE_WHEEL_THRESHOLD_IN_DEGREES_PER_USEC) - || (abs(torque.low) >= OVERRIDE_WHEEL_THRESHOLD_IN_DEGREES_PER_USEC) ) - { - disable_control( ); - - g_steering_control_state.operator_override = true; - - DEBUG_PRINTLN( "Operator override" ); - } - else - { - g_steering_control_state.operator_override = false; - } - } -} - - -void check_for_sensor_faults( void ) -{ - if ( (g_steering_control_state.enabled == true) - || (g_steering_control_state.invalid_sensor_value == true) ) - { - uint32_t current_time = GET_TIMESTAMP_MS(); - - bool timeout = is_timeout( - g_sensor_validity_last_check_timestamp, - current_time, - SENSOR_VALIDITY_CHECK_INTERVAL_IN_MSEC ); - - static int fault_count = 0; - - if( timeout == true ) - { - g_sensor_validity_last_check_timestamp = current_time; - - int sensor_high = analogRead( PIN_TORQUE_SENSOR_HIGH ); - int sensor_low = analogRead( PIN_TORQUE_SENSOR_LOW ); - - // sensor pins tied to ground - a value of zero indicates disconnection - if( (sensor_high == 0) - || (sensor_low == 0) ) - { - ++fault_count; - - if( fault_count >= SENSOR_VALIDITY_CHECK_FAULT_COUNT ) - { - disable_control( ); - - g_steering_control_state.invalid_sensor_value = true; - - DEBUG_PRINTLN( "Bad value read from torque sensor" ); - } - } - else - { - g_steering_control_state.invalid_sensor_value = false; - fault_count = 0; - } - } - } -} - - -void update_steering( void ) -{ - if (g_steering_control_state.enabled == true ) - { - float time_between_loops_in_sec = - MSEC_TO_SEC( CONTROL_LOOP_INTERVAL_IN_MSEC ); - - // Calculate steering angle rates (millidegrees/microsecond) - float steering_wheel_angle_rate = - ( g_steering_control_state.current_steering_wheel_angle - - g_steering_control_state.previous_steering_wheel_angle ) - / time_between_loops_in_sec; - - float steering_wheel_angle_rate_target = - ( g_steering_control_state.commanded_steering_wheel_angle - - g_steering_control_state.current_steering_wheel_angle ) - / time_between_loops_in_sec; - - // Save the angle for next iteration - g_steering_control_state.previous_steering_wheel_angle = - g_steering_control_state.current_steering_wheel_angle; - - steering_wheel_angle_rate_target = - constrain( steering_wheel_angle_rate_target, - -g_steering_control_state.commanded_steering_wheel_angle_rate, - g_steering_control_state.commanded_steering_wheel_angle_rate ); - - pid_update( - &g_pid, - steering_wheel_angle_rate_target, - steering_wheel_angle_rate, - time_between_loops_in_sec ); - - float control = g_pid.control; - - control = constrain( control, - TORQUE_MIN_IN_NEWTON_METERS, - TORQUE_MAX_IN_NEWTON_METERS ); - - steering_torque_s torque_spoof; - - calculate_torque_spoof( control, &torque_spoof ); - - g_spoofed_torque_output_sum = torque_spoof.low + torque_spoof.high; - - g_dac.outputA( torque_spoof.low ); - g_dac.outputB( torque_spoof.high ); - } -} - - -void enable_control( void ) -{ - if( g_steering_control_state.enabled == false - && g_steering_control_state.operator_override == false ) - { - const uint16_t num_samples = 20; - prevent_signal_discontinuity( - g_dac, - num_samples, - PIN_TORQUE_SENSOR_HIGH, - PIN_TORQUE_SENSOR_LOW ); - - // Enable the signal interrupt relays - digitalWrite( PIN_SPOOF_ENABLE, HIGH ); - - g_steering_control_state.enabled = true; - - DEBUG_PRINTLN( "Control enabled" ); - } -} - - -void disable_control( void ) -{ - if( g_steering_control_state.enabled == true ) - { - const uint16_t num_samples = 20; - prevent_signal_discontinuity( - g_dac, - num_samples, - PIN_TORQUE_SENSOR_HIGH, - PIN_TORQUE_SENSOR_LOW ); - - // Disable the signal interrupt relays - digitalWrite( PIN_SPOOF_ENABLE, LOW ); - - g_steering_control_state.enabled = false; - - pid_zeroize( &g_pid, PID_WINDUP_GUARD ); - - DEBUG_PRINTLN( "Control disabled" ); - } -} - -static void read_torque_sensor( - steering_torque_s * value ) -{ - steering_torque_s unfiltered_torque; - - unfiltered_torque.high = analogRead( PIN_TORQUE_SENSOR_HIGH ) << BIT_SHIFT_10BIT_TO_12BIT; - unfiltered_torque.low = analogRead( PIN_TORQUE_SENSOR_LOW ) << BIT_SHIFT_10BIT_TO_12BIT; - - const float filter_alpha = TORQUE_SENSOR_EXPONENTIAL_FILTER_ALPHA; - static float filtered_torque_high = 0.0; - static float filtered_torque_low = 0.0; - - filtered_torque_high = exponential_moving_average( - filter_alpha, - unfiltered_torque.high, - filtered_torque_high); - - filtered_torque_low = exponential_moving_average( - filter_alpha, - unfiltered_torque.low, - filtered_torque_low); - - value->high = filtered_torque_high; - value->low = filtered_torque_low; -} - - -static void calculate_torque_spoof( - const int16_t torque_target, - steering_torque_s * const spoof ) -{ - if( spoof != NULL ) - { - spoof->low = - STEPS_PER_VOLT - * ((SPOOF_LOW_SIGNAL_CALIBRATION_CURVE_SCALAR * torque_target) - + SPOOF_LOW_SIGNAL_CALIBRATION_CURVE_OFFSET); - - spoof->high = - STEPS_PER_VOLT - * ((SPOOF_HIGH_SIGNAL_CALIBRATION_CURVE_SCALAR * torque_target) - + SPOOF_HIGH_SIGNAL_CALIBRATION_CURVE_OFFSET); - } -} diff --git a/platforms/kia_soul/firmware/steering/tests/features/checking_sensor_validity.feature b/platforms/kia_soul/firmware/steering/tests/features/checking_sensor_validity.feature deleted file mode 100644 index 48651bf2..00000000 --- a/platforms/kia_soul/firmware/steering/tests/features/checking_sensor_validity.feature +++ /dev/null @@ -1,21 +0,0 @@ -# language: en - -Feature: Checking sensor validity - - Invalid values read from sensors should cause control to be disabled. - - - Scenario: A sensor becomes temporarily disconnected - Given steering control is enabled - - When a sensor becomes temporarily disconnected - - Then control should remain enabled - - - Scenario: A sensor becomes permanently disconnected - Given steering control is enabled - - When a sensor becomes permanently disconnected - - Then control should be disabled diff --git a/platforms/kia_soul/firmware/steering/tests/features/receiving_commands.feature b/platforms/kia_soul/firmware/steering/tests/features/receiving_commands.feature deleted file mode 100644 index 8a558aaa..00000000 --- a/platforms/kia_soul/firmware/steering/tests/features/receiving_commands.feature +++ /dev/null @@ -1,68 +0,0 @@ -# language: en - -Feature: Receiving commands - - Commands received from a controller should be processed and acted upon. - - - Scenario Outline: Enable steering command sent from controller - Given steering control is disabled - And the torque sensors have a reading of - - When an enable steering command is received - - Then control should be enabled - And the last command timestamp should be set - And should be written to DAC A - And should be written to DAC B - - Examples: - | sensor_val | dac_a_val | dac_b_val | - | 0 | 0 | 0 | - | 256 | 1024 | 1024 | - | 512 | 2048 | 2048 | - | 1024 | 4096 | 4096 | - - - Scenario Outline: Disable steering command sent from controller - Given steering control is enabled - And the torque sensors have a reading of - - When a disable steering command is received - - Then control should be disabled - And the last command timestamp should be set - And should be written to DAC A - And should be written to DAC B - - Examples: - | sensor_val | dac_a_val | dac_b_val | - | 0 | 0 | 0 | - | 256 | 1024 | 1024 | - | 512 | 2048 | 2048 | - | 1024 | 4096 | 4096 | - - - Scenario Outline: Steering wheel angle command sent from controller - Given steering control is enabled - And the current steering wheel angle is - - When the steering wheel angle command with angle rate 111 is received - - Then the steering wheel angle command should be parsed - And should be written to DAC A - And should be written to DAC B - - Examples: - | angle | command | spoof_val_high | spoof_val_low | - | -2048 | -4700 | 1064 | 2834 | - | -1024 | -4000 | 3031 | 868 | - | -512 | -3000 | 1064 | 2834 | - | -256 | -2000 | 1064 | 2834 | - | -128 | -1000 | 1707 | 2191 | - | 0 | 0 | 2889 | 1009 | - | 128 | 1000 | 3031 | 868 | - | 256 | 2000 | 3031 | 868 | - | 512 | 3000 | 3031 | 868 | - | 1024 | 4000 | 3031 | 868 | - | 2048 | 4700 | 3031 | 868 | diff --git a/platforms/kia_soul/firmware/steering/tests/features/receiving_reports.feature b/platforms/kia_soul/firmware/steering/tests/features/receiving_reports.feature deleted file mode 100644 index beb3ebc7..00000000 --- a/platforms/kia_soul/firmware/steering/tests/features/receiving_reports.feature +++ /dev/null @@ -1,32 +0,0 @@ -# language: en - -Feature: Receiving reports - - Chassis state reports should be received and parsed. - - - Scenario Outline: Chassis State 1 report sent from CAN gateway. - When a Chassis State 1 report is received with steering wheel angle - - Then the control state's current_steering_wheel_angle field should be - - Examples: - | raw_angle | scaled_angle | - | -32768 | -2925.00 | - | -16384 | -1462.50 | - | -8192 | -731.250 | - | -4096 | -365.625 | - | -2048 | -182.812 | - | -1024 | -91.4063 | - | -512 | -45.7031 | - | -256 | -22.8515 | - | 0 | 0.0 | - | 256 | 22.8515 | - | 512 | 45.7031 | - | 1024 | 91.4063 | - | 2048 | 182.812 | - | 4096 | 365.625 | - | 8192 | 731.250 | - | 16348 | 1459.28 | - | 32767 | 2924.91 | - diff --git a/platforms/kia_soul/firmware/steering/tests/features/sending_reports.feature b/platforms/kia_soul/firmware/steering/tests/features/sending_reports.feature deleted file mode 100644 index c3f316d1..00000000 --- a/platforms/kia_soul/firmware/steering/tests/features/sending_reports.feature +++ /dev/null @@ -1,25 +0,0 @@ -# language: en - -Feature: Sending reports - - Steering reports should be published to the control CAN bus after an interval. - - - Scenario Outline: Steering report published after an interval - Given the torque sensors have a reading of - And the previous steering wheel angle command was - And the current steering wheel angle is - And the spoofed torque output was - - When the time since the last report publishing exceeds the interval - - Then a steering report should be published to the control CAN bus - And the report's command field should be set to - And the report's steering wheel angle field should be set to - And the report's torque output field should be set to - - Examples: - | command | sensor_val | angle | torque | - | 0 | 0 | -2925 | 32 | - | 50 | 256 | 45 | 64 | - | 100 | 512 | 731 | 127 | diff --git a/platforms/kia_soul/firmware/steering/tests/features/step_definitions/checking_sensor_validity.cpp b/platforms/kia_soul/firmware/steering/tests/features/step_definitions/checking_sensor_validity.cpp deleted file mode 100644 index ea400394..00000000 --- a/platforms/kia_soul/firmware/steering/tests/features/step_definitions/checking_sensor_validity.cpp +++ /dev/null @@ -1,40 +0,0 @@ -WHEN("^a sensor becomes temporarily disconnected$") -{ - // first check - error value - one fault - g_mock_arduino_analog_read_return = 0; - g_mock_arduino_millis_return = SENSOR_VALIDITY_CHECK_INTERVAL_IN_MSEC; - check_for_sensor_faults(); - - // second check - error value - two faults - g_mock_arduino_analog_read_return = 0; - g_mock_arduino_millis_return += SENSOR_VALIDITY_CHECK_INTERVAL_IN_MSEC; - check_for_sensor_faults(); - - // third check - valid value - faults reset - g_mock_arduino_analog_read_return = 500; - g_mock_arduino_millis_return += SENSOR_VALIDITY_CHECK_INTERVAL_IN_MSEC; - check_for_sensor_faults(); -} - - -WHEN("^a sensor becomes permanently disconnected$") -{ - g_mock_arduino_analog_read_return = 0; - g_mock_arduino_millis_return = SENSOR_VALIDITY_CHECK_INTERVAL_IN_MSEC; - - // must call function enough times to exceed the fault limit - for( int i = 0; i < SENSOR_VALIDITY_CHECK_FAULT_COUNT; ++i ) - { - // continue timing out - g_mock_arduino_millis_return += SENSOR_VALIDITY_CHECK_INTERVAL_IN_MSEC; - - check_for_sensor_faults(); - } -} - -THEN("^control should remain enabled") -{ - assert_that( - g_steering_control_state.enabled, - is_equal_to(1)); -} diff --git a/platforms/kia_soul/firmware/steering/tests/features/step_definitions/receiving_commands.cpp b/platforms/kia_soul/firmware/steering/tests/features/step_definitions/receiving_commands.cpp deleted file mode 100644 index 308b799f..00000000 --- a/platforms/kia_soul/firmware/steering/tests/features/step_definitions/receiving_commands.cpp +++ /dev/null @@ -1,86 +0,0 @@ -// variables needed to preserve the state of the PID controller between scenarios -float mock_pid_int_error; -float mock_pid_prev_input; - - -WHEN("^an enable steering command is received$") -{ - oscc_command_steering_data_s * steering_command_data = - (oscc_command_steering_data_s *) g_mock_mcp_can_read_msg_buf_buf; - - steering_command_data->enabled = 1; - - g_mock_mcp_can_read_msg_buf_id = OSCC_COMMAND_STEERING_CAN_ID; - g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL; - - check_for_incoming_message(); -} - - -WHEN("^a disable steering command is received$") -{ - oscc_command_steering_data_s * steering_command_data = - (oscc_command_steering_data_s *) g_mock_mcp_can_read_msg_buf_buf; - - steering_command_data->enabled = 0; - - g_mock_mcp_can_read_msg_buf_id = OSCC_COMMAND_STEERING_CAN_ID; - g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL; - - check_for_incoming_message(); -} - - -WHEN("^the steering wheel angle command (.*) with angle rate (.*) is received$") -{ - REGEX_PARAM(int, command); - REGEX_PARAM(int, rate); - - oscc_command_steering_data_s * steering_command_data = - (oscc_command_steering_data_s *) g_mock_mcp_can_read_msg_buf_buf; - - g_mock_mcp_can_read_msg_buf_id = OSCC_COMMAND_STEERING_CAN_ID; - g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL; - - steering_command_data->enabled = 1; - steering_command_data->commanded_steering_wheel_angle = command; - steering_command_data->commanded_steering_wheel_angle_rate = rate; - - pid_zeroize( &g_pid, PID_WINDUP_GUARD ); - - g_pid.proportional_gain = PID_PROPORTIONAL_GAIN; - g_pid.integral_gain = PID_INTEGRAL_GAIN; - g_pid.derivative_gain = PID_DERIVATIVE_GAIN; - - // restore PID params needed for next scenario - g_pid.prev_input = mock_pid_prev_input; - g_pid.int_error = mock_pid_int_error; - - check_for_incoming_message(); - - update_steering(); -} - - -THEN("^the steering wheel angle command should be parsed$") -{ - oscc_command_steering_data_s * steering_command_data = - (oscc_command_steering_data_s *) g_mock_mcp_can_read_msg_buf_buf; - - // save PID params from last scenario needed for the next scenario - mock_pid_prev_input = g_pid.prev_input; - mock_pid_int_error = g_pid.int_error; - - significant_figures_for_assert_double_are(4); - assert_that_double( - g_steering_control_state.commanded_steering_wheel_angle, - is_equal_to_double(steering_command_data->commanded_steering_wheel_angle/9.0)); -} - - -THEN("^the last command timestamp should be set$") -{ - assert_that( - g_steering_command_last_rx_timestamp, - is_equal_to(g_mock_arduino_millis_return)); -} diff --git a/platforms/kia_soul/firmware/steering/tests/features/step_definitions/receiving_reports.cpp b/platforms/kia_soul/firmware/steering/tests/features/step_definitions/receiving_reports.cpp deleted file mode 100644 index 6fd19fda..00000000 --- a/platforms/kia_soul/firmware/steering/tests/features/step_definitions/receiving_reports.cpp +++ /dev/null @@ -1,26 +0,0 @@ -WHEN("^a Chassis State 1 report is received with steering wheel angle (.*)$") -{ - REGEX_PARAM(int, raw_angle); - - oscc_report_chassis_state_1_data_s * chassis_state_1_data = - (oscc_report_chassis_state_1_data_s *) g_mock_mcp_can_read_msg_buf_buf; - - chassis_state_1_data->steering_wheel_angle = raw_angle; - chassis_state_1_data->flags = OSCC_REPORT_CHASSIS_STATE_1_FLAGS_BIT_STEER_WHEEL_ANGLE_VALID; - - g_mock_mcp_can_read_msg_buf_id = OSCC_REPORT_CHASSIS_STATE_1_CAN_ID; - g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL; - - check_for_incoming_message(); -} - - -THEN("^the control state's current_steering_wheel_angle field should be (.*)$") -{ - REGEX_PARAM(float, scaled_angle); - - significant_figures_for_assert_double_are(6); - assert_that_double( - g_steering_control_state.current_steering_wheel_angle, - is_equal_to_double(scaled_angle)); -} diff --git a/platforms/kia_soul/firmware/steering/tests/features/step_definitions/sending_reports.cpp b/platforms/kia_soul/firmware/steering/tests/features/step_definitions/sending_reports.cpp deleted file mode 100644 index 1f1006e6..00000000 --- a/platforms/kia_soul/firmware/steering/tests/features/step_definitions/sending_reports.cpp +++ /dev/null @@ -1,98 +0,0 @@ -GIVEN("^the current steering wheel angle is (.*)$") -{ - REGEX_PARAM(int, angle); - - g_steering_control_state.current_steering_wheel_angle = angle; -} - - -GIVEN("^the spoofed torque output was (.*)$") -{ - REGEX_PARAM(int, torque); - - g_spoofed_torque_output_sum = torque; -} - - -WHEN("^the time since the last report publishing exceeds (.*)$") -{ - REGEX_PARAM(std::string, interval); - - g_steering_report_last_tx_timestamp = 0; - - g_mock_arduino_millis_return = - OSCC_REPORT_STEERING_PUBLISH_INTERVAL_IN_MSEC; - - publish_reports(); -} - - -THEN("^a steering report should be published to the control CAN bus$") -{ - assert_that( - g_mock_mcp_can_send_msg_buf_id, - is_equal_to(OSCC_REPORT_STEERING_CAN_ID)); - - assert_that( - g_mock_mcp_can_send_msg_buf_ext, - is_equal_to(CAN_STANDARD)); - - assert_that( - g_mock_mcp_can_send_msg_buf_len, - is_equal_to(OSCC_REPORT_STEERING_CAN_DLC)); - - - oscc_report_steering_data_s * steering_report_data = - (oscc_report_steering_data_s *) g_mock_mcp_can_send_msg_buf_buf; - - assert_that( - steering_report_data->enabled, - is_equal_to(g_steering_control_state.enabled)); - - assert_that( - steering_report_data->override, - is_equal_to(g_steering_control_state.operator_override)); - - assert_that( - g_steering_report_last_tx_timestamp, - is_equal_to(g_mock_arduino_millis_return)); -} - - -THEN("^the report's command field should be set to (.*)$") -{ - REGEX_PARAM(int, command); - - oscc_report_steering_data_s * steering_report_data = - (oscc_report_steering_data_s *) g_mock_mcp_can_send_msg_buf_buf; - - assert_that( - steering_report_data->commanded_steering_wheel_angle, - is_equal_to(command)); -} - - -THEN("^the report's steering wheel angle field should be set to (.*)$") -{ - REGEX_PARAM(int, scaled_angle); - - oscc_report_steering_data_s * steering_report_data = - (oscc_report_steering_data_s *) g_mock_mcp_can_send_msg_buf_buf; - - assert_that( - steering_report_data->current_steering_wheel_angle, - is_equal_to(scaled_angle)); -} - - -THEN("^the report's torque output field should be set to (.*)$") -{ - REGEX_PARAM(int, torque); - - oscc_report_steering_data_s * steering_report_data = - (oscc_report_steering_data_s *) g_mock_mcp_can_send_msg_buf_buf; - - assert_that( - steering_report_data->spoofed_torque_output, - is_equal_to(torque)); -} diff --git a/platforms/kia_soul/firmware/steering/tests/features/step_definitions/test.cpp b/platforms/kia_soul/firmware/steering/tests/features/step_definitions/test.cpp deleted file mode 100644 index 3c55bedd..00000000 --- a/platforms/kia_soul/firmware/steering/tests/features/step_definitions/test.cpp +++ /dev/null @@ -1,7 +0,0 @@ -// include source files to prevent step files from conflicting with each other -#include "common.cpp" -#include "checking_sensor_validity.cpp" -#include "receiving_commands.cpp" -#include "receiving_reports.cpp" -#include "sending_reports.cpp" -#include "timeouts_and_overrides.cpp" diff --git a/platforms/kia_soul/firmware/steering/tests/features/step_definitions/timeouts_and_overrides.cpp b/platforms/kia_soul/firmware/steering/tests/features/step_definitions/timeouts_and_overrides.cpp deleted file mode 100644 index 9e3b1347..00000000 --- a/platforms/kia_soul/firmware/steering/tests/features/step_definitions/timeouts_and_overrides.cpp +++ /dev/null @@ -1,45 +0,0 @@ -WHEN("^the time since the last received controller command exceeds the timeout$") -{ - g_steering_command_last_rx_timestamp = 0; - - g_mock_arduino_millis_return = - COMMAND_TIMEOUT_IN_MSEC; - - check_for_timeouts(); -} - - -WHEN("^the time since the last received Chassis State 1 report exceeds the timeout$") -{ - g_chassis_state_1_report_last_rx_timestamp = 0; - - g_mock_arduino_millis_return = - CHASSIS_STATE_1_REPORT_TIMEOUT_IN_MSEC; - - check_for_timeouts(); -} - - -WHEN("^the operator applies (.*) to the steering wheel$") -{ - REGEX_PARAM(int, torque_sensor_val); - - g_mock_arduino_analog_read_return = torque_sensor_val; - - // The exponential filter used requires multiple passes for it to recognize - // an override - for( int i = 0; i < 5; ++i ) - { - check_for_operator_override(); - } -} - - -THEN("^override flag should be set$") -{ - assert_that( - g_steering_control_state.operator_override, - is_equal_to(true)); -} - - diff --git a/platforms/kia_soul/firmware/steering/tests/property/build.rs b/platforms/kia_soul/firmware/steering/tests/property/build.rs deleted file mode 100644 index 1f44eb54..00000000 --- a/platforms/kia_soul/firmware/steering/tests/property/build.rs +++ /dev/null @@ -1,75 +0,0 @@ -extern crate gcc; -extern crate bindgen; - -use std::env; -use std::path::Path; - -fn main() { - println!("cargo:rerun-if-changed=../../include"); - println!("cargo:rerun-if-changed=../../include/*"); - println!("cargo:rerun-if-changed=../src"); - println!("cargo:rerun-if-changed=../../src/*"); - println!("cargo:rerun-if-changed=../../../../../common/testing/mocks"); - println!("cargo:rerun-if-changed=../../../../../common/testing/mocks/*"); - - gcc::Config::new() - .flag("-w") - .define("__STDC_LIMIT_MACROS", None) - .include("include") - .include("../../include") - .include("../../../../../common/testing/mocks") - .include("../../../../../common/include") - .include("../../../../../common/libs/can") - .include("../../../../../common/libs/time") - .include("../../../../../common/libs/dac") - .include("../../../../../common/libs/pid") - .include("../../../../../common/libs/signal_smoothing") - .include("/usr/lib/avr/include") - .file("../../../../../common/testing/mocks/Arduino_mock.cpp") - .file("../../../../../common/testing/mocks/mcp_can_mock.cpp") - .file("../../../../../common/testing/mocks/DAC_MCP49xx_mock.cpp") - .file("../../src/communications.cpp") - .file("../../src/steering_control.cpp") - .file("../../src/globals.cpp") - .file("../../../../../common/libs/pid/oscc_pid.cpp") - .file("../../../../../common/libs/time/oscc_time.cpp") - .file("../../../../../common/libs/can/oscc_can.cpp") - .file("../../../../../common/libs/dac/oscc_dac.cpp") - .file("../../../../../common/libs/signal_smoothing/oscc_signal_smoothing.cpp") - .cpp(true) - .compile("libsteering_test.a"); - - let out_dir = env::var("OUT_DIR").unwrap(); - - let _ = bindgen::builder() - .header("include/wrapper.hpp") - .generate_comments(false) - .clang_arg("-I../../include") - .clang_arg("-I/usr/lib/avr/include") - .clang_arg("-I../../../../../common/include") - .clang_arg("-I../../../../../common/libs/pid") - .clang_arg("-I../../../../../common/libs/can") - .clang_arg("-I../../../../../common/testing/mocks") - .whitelisted_function("publish_reports") - .whitelisted_function("check_for_incoming_message") - .whitelisted_function("check_for_operator_override") - .whitelisted_var("g_steering_control_state") - .whitelisted_var("OSCC_REPORT_STEERING_CAN_ID") - .whitelisted_var("OSCC_REPORT_STEERING_CAN_DLC") - .whitelisted_var("OSCC_COMMAND_STEERING_CAN_ID") - .whitelisted_var("OSCC_COMMAND_STEERING_CAN_DLC") - .whitelisted_var("OVERRIDE_WHEEL_THRESHOLD_IN_DEGREES_PER_USEC") - .whitelisted_var("OSCC_REPORT_STEERING_PUBLISH_INTERVAL_IN_MSEC") - .whitelisted_var("CAN_STANDARD") - .whitelisted_var("CAN_MSGAVAIL") - .whitelisted_var("g_control_can") - .whitelisted_type("oscc_report_steering_data_s") - .whitelisted_type("oscc_report_steering_s") - .whitelisted_type("oscc_command_steering_data_s") - .whitelisted_type("oscc_command_steering_s") - .whitelisted_type("can_frame_s") - .generate() - .unwrap() - .write_to_file(Path::new(&out_dir).join("steering_test.rs")) - .expect("Unable to generate bindings"); -} diff --git a/platforms/kia_soul/firmware/steering/tests/property/include/wrapper.hpp b/platforms/kia_soul/firmware/steering/tests/property/include/wrapper.hpp deleted file mode 100644 index ca5074ae..00000000 --- a/platforms/kia_soul/firmware/steering/tests/property/include/wrapper.hpp +++ /dev/null @@ -1,4 +0,0 @@ -#include "globals.h" -#include "communications.h" -#include "oscc_can.h" -#include "steering_can_protocol.h" diff --git a/platforms/kia_soul/firmware/steering/tests/property/src/tests.rs b/platforms/kia_soul/firmware/steering/tests/property/src/tests.rs deleted file mode 100644 index 609cc144..00000000 --- a/platforms/kia_soul/firmware/steering/tests/property/src/tests.rs +++ /dev/null @@ -1,296 +0,0 @@ -#![allow(non_upper_case_globals)] -#![allow(non_camel_case_types)] -#![allow(non_snake_case)] -#![allow(dead_code)] -#![allow(unused_variables)] -#![allow(unused_imports)] -include!(concat!(env!("OUT_DIR"), "/steering_test.rs")); - -extern crate quickcheck; -extern crate rand; - -use quickcheck::{QuickCheck, TestResult, Arbitrary, Gen, StdGen}; - -extern "C" { - #[link_name = "g_mock_mcp_can_check_receive_return"] - pub static mut g_mock_mcp_can_check_receive_return: u8; - #[link_name = "g_mock_mcp_can_read_msg_buf_id"] - pub static mut g_mock_mcp_can_read_msg_buf_id: ::std::os::raw::c_ulong; - #[link_name = "g_mock_mcp_can_read_msg_buf_buf"] - pub static mut g_mock_mcp_can_read_msg_buf_buf: [u8; 8usize]; - #[link_name = "g_mock_mcp_can_send_msg_buf_id"] - pub static mut g_mock_mcp_can_send_msg_buf_id: ::std::os::raw::c_ulong; - #[link_name = "g_mock_mcp_can_send_msg_buf_ext"] - pub static mut g_mock_mcp_can_send_msg_buf_ext: u8; - #[link_name = "g_mock_mcp_can_send_msg_buf_len"] - pub static mut g_mock_mcp_can_send_msg_buf_len: u8; - #[link_name = "g_mock_mcp_can_send_msg_buf_buf"] - pub static mut g_mock_mcp_can_send_msg_buf_buf: *mut u8; - #[link_name = "g_mock_arduino_millis_return"] - pub static mut g_mock_arduino_millis_return: ::std::os::raw::c_ulong; - #[link_name = "g_mock_arduino_analog_read_return"] - pub static mut g_mock_arduino_analog_read_return: u16; -} - -impl Arbitrary for oscc_report_steering_data_s { - fn arbitrary(g: &mut G) -> oscc_report_steering_data_s { - oscc_report_steering_data_s { - current_steering_wheel_angle: i16::arbitrary(g), - commanded_steering_wheel_angle: i16::arbitrary(g), - reserved_0: u16::arbitrary(g), - spoofed_torque_output: i8::arbitrary(g), - _bitfield_1: u8::arbitrary(g), - } - } -} - -impl Arbitrary for oscc_report_steering_s { - fn arbitrary(g: &mut G) -> oscc_report_steering_s { - oscc_report_steering_s { - id: u32::arbitrary(g), - dlc: u8::arbitrary(g), - timestamp: u32::arbitrary(g), - data: oscc_report_steering_data_s::arbitrary(g), - } - } -} - -impl Arbitrary for oscc_command_steering_data_s { - fn arbitrary(g: &mut G) -> oscc_command_steering_data_s { - oscc_command_steering_data_s { - commanded_steering_wheel_angle: i16::arbitrary(g), - commanded_steering_wheel_angle_rate: u8::arbitrary(g), - _bitfield_1: u8::arbitrary(g), - reserved_3: u8::arbitrary(g), - reserved_4: u8::arbitrary(g), - reserved_5: u8::arbitrary(g), - reserved_6: u8::arbitrary(g), - } - } -} - -impl Arbitrary for oscc_command_steering_s { - fn arbitrary(g: &mut G) -> oscc_command_steering_s { - oscc_command_steering_s { - timestamp: u32::arbitrary(g), - data: oscc_command_steering_data_s::arbitrary(g), - } - } -} - - -impl Arbitrary for can_frame_s { - fn arbitrary(g: &mut G) -> can_frame_s { - can_frame_s { - id: u32::arbitrary(g), - dlc: u8::arbitrary(g), - timestamp: u32::arbitrary(g), - data: [u8::arbitrary(g), - u8::arbitrary(g), - u8::arbitrary(g), - u8::arbitrary(g), - u8::arbitrary(g), - u8::arbitrary(g), - u8::arbitrary(g), - u8::arbitrary(g)], - } - } -} - -/// the steering firmware should not attempt processing any messages -/// that are not steering commands -fn prop_only_process_valid_messages(rx_can_msg: can_frame_s, current_target: f32) -> TestResult { - // if we generate a steering can message, ignore the result - if rx_can_msg.id == OSCC_COMMAND_STEERING_CAN_ID { - return TestResult::discard(); - } - unsafe { - g_steering_control_state.commanded_steering_wheel_angle = current_target; - - g_mock_mcp_can_read_msg_buf_id = rx_can_msg.id as u64; - g_mock_mcp_can_read_msg_buf_buf = rx_can_msg.data; - g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL as u8; - - check_for_incoming_message(); - - TestResult::from_bool(g_steering_control_state.commanded_steering_wheel_angle == - current_target) - } -} - -#[test] -fn check_message_type_validity() { - QuickCheck::new() - .tests(1000) - .gen(StdGen::new(rand::thread_rng(), i16::max_value() as usize)) - .quickcheck(prop_only_process_valid_messages as fn(can_frame_s, f32) -> TestResult) -} - -/// the steering firmware should set the commanded accelerator position -/// upon reciept of a valid command steering message -fn prop_no_invalid_targets(mut command_steering_msg: oscc_command_steering_s) -> TestResult { - command_steering_msg.data.set_enabled(1); - unsafe { - g_mock_mcp_can_read_msg_buf_id = OSCC_COMMAND_STEERING_CAN_ID as u64; - g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL as u8; - g_mock_mcp_can_read_msg_buf_buf = std::mem::transmute(command_steering_msg.data); - - check_for_incoming_message(); - - TestResult::from_bool(g_steering_control_state.commanded_steering_wheel_angle == - (command_steering_msg.data.commanded_steering_wheel_angle as - f32 / 9.0) && - g_steering_control_state.commanded_steering_wheel_angle_rate == - (command_steering_msg - .data - .commanded_steering_wheel_angle_rate as - f32 * 9.0)) - } -} - -#[test] -fn check_wheel_angle_validity() { - QuickCheck::new() - .tests(1000) - .quickcheck(prop_no_invalid_targets as fn(oscc_command_steering_s) -> TestResult) -} - -/// the steering firmware should set the control state as enabled -/// upon reciept of a valid command steering message telling it to enable -fn prop_process_enable_command(mut command_steering_msg: oscc_command_steering_s) -> TestResult { - unsafe { - command_steering_msg.data.set_enabled(1); - - g_steering_control_state.enabled = false; - g_steering_control_state.operator_override = false; - - g_mock_mcp_can_read_msg_buf_id = OSCC_COMMAND_STEERING_CAN_ID as u64; - g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL as u8; - g_mock_mcp_can_read_msg_buf_buf = std::mem::transmute(command_steering_msg.data); - - check_for_incoming_message(); - - TestResult::from_bool(g_steering_control_state.enabled == true) - } -} - -#[test] -fn check_process_enable_command() { - QuickCheck::new() - .tests(1000) - .quickcheck(prop_process_enable_command as fn(oscc_command_steering_s) -> TestResult) -} - -/// the steering firmware should set the control state as disabled -/// upon reciept of a valid command steering message telling it to disable -fn prop_process_disable_command(mut command_steering_msg: oscc_command_steering_s) -> TestResult { - unsafe { - command_steering_msg.data.set_enabled(0); - - g_mock_mcp_can_read_msg_buf_id = OSCC_COMMAND_STEERING_CAN_ID as u64; - g_mock_mcp_can_read_msg_buf_buf = std::mem::transmute(command_steering_msg.data); - g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL as u8; - - check_for_incoming_message(); - - TestResult::from_bool(g_steering_control_state.enabled == false) - } -} - -#[test] -fn check_process_disable_command() { - QuickCheck::new() - .tests(1000) - .quickcheck(prop_process_disable_command as fn(oscc_command_steering_s) -> TestResult) -} - -/// the steering firmware should create only valid CAN frames -fn prop_send_valid_can_fields(operator_override: bool, - current_steering_wheel_angle: f32, - commanded_steering_wheel_angle: f32) - -> TestResult { - static mut time: u64 = 0; - unsafe { - g_steering_control_state.operator_override = operator_override; - g_steering_control_state.commanded_steering_wheel_angle = commanded_steering_wheel_angle; - g_steering_control_state.current_steering_wheel_angle = current_steering_wheel_angle; - - time = time + OSCC_REPORT_STEERING_PUBLISH_INTERVAL_IN_MSEC as u64; - - g_mock_arduino_millis_return = time; - - publish_reports(); - - let steering_data = oscc_report_steering_data_s { - current_steering_wheel_angle: std::mem::transmute([*g_mock_mcp_can_send_msg_buf_buf, - *g_mock_mcp_can_send_msg_buf_buf - .offset(1)]), - commanded_steering_wheel_angle: std::mem::transmute([*g_mock_mcp_can_send_msg_buf_buf - .offset(2), - *g_mock_mcp_can_send_msg_buf_buf - .offset(3)]), - reserved_0: std::mem::transmute([*g_mock_mcp_can_send_msg_buf_buf.offset(4), - *g_mock_mcp_can_send_msg_buf_buf.offset(5)]), - spoofed_torque_output: std::mem::transmute(*g_mock_mcp_can_send_msg_buf_buf.offset(6)), - _bitfield_1: std::mem::transmute(*g_mock_mcp_can_send_msg_buf_buf.offset(7)), - }; - - TestResult::from_bool((g_mock_mcp_can_send_msg_buf_id == - OSCC_REPORT_STEERING_CAN_ID as u64) && - (g_mock_mcp_can_send_msg_buf_ext == (CAN_STANDARD as u8)) && - (g_mock_mcp_can_send_msg_buf_len == - (OSCC_REPORT_STEERING_CAN_DLC as u8)) && - (steering_data.current_steering_wheel_angle == - current_steering_wheel_angle as i16) && - (steering_data.commanded_steering_wheel_angle == - commanded_steering_wheel_angle as i16) && - (steering_data.enabled() == - (g_steering_control_state.enabled as u8)) && - (steering_data.override_() == (operator_override as u8))) - } -} - -#[test] -fn check_valid_can_frame() { - QuickCheck::new() - .tests(1000) - .gen(StdGen::new(rand::thread_rng(), i16::max_value() as usize)) - .quickcheck(prop_send_valid_can_fields as fn(bool, f32, f32) -> TestResult) -} - -// the steering firmware should be able to correctly and consistently -// detect operator overrides -fn prop_check_operator_override(analog_read_spoof: u16) -> TestResult { - unsafe { - static mut filtered_torque_a: f32 = 0.0; - static mut filtered_torque_b: f32 = 0.0; - const torque_filter_alpha: f32 = 0.5; - - g_steering_control_state.enabled = true; - g_mock_arduino_analog_read_return = analog_read_spoof; - - check_for_operator_override(); - - filtered_torque_a = (torque_filter_alpha * (analog_read_spoof << 2) as f32) + - ((1.0 - torque_filter_alpha) * filtered_torque_a); - - filtered_torque_b = (torque_filter_alpha * (analog_read_spoof << 2) as f32) + - (1.0 - torque_filter_alpha * filtered_torque_b); - - if filtered_torque_a.abs() >= OVERRIDE_WHEEL_THRESHOLD_IN_DEGREES_PER_USEC as f32 || - filtered_torque_b.abs() >= OVERRIDE_WHEEL_THRESHOLD_IN_DEGREES_PER_USEC as f32 { - TestResult::from_bool(g_steering_control_state.operator_override == true && - g_steering_control_state.enabled == false) - } else { - TestResult::from_bool(g_steering_control_state.operator_override == false) - } - } -} - -#[test] -fn check_operator_override() { - QuickCheck::new() - .tests(1000) - .gen(StdGen::new(rand::thread_rng(), u16::max_value() as usize)) - .quickcheck(prop_check_operator_override as fn(u16) -> TestResult) -} diff --git a/platforms/kia_soul/firmware/throttle/include/throttle_control.h b/platforms/kia_soul/firmware/throttle/include/throttle_control.h deleted file mode 100644 index 8544e0a3..00000000 --- a/platforms/kia_soul/firmware/throttle/include/throttle_control.h +++ /dev/null @@ -1,218 +0,0 @@ -/** - * @file throttle_control.h - * @brief Control of the throttle system. - * - */ - - -#ifndef _OSCC_KIA_SOUL_THROTTLE_CONTROL_H_ -#define _OSCC_KIA_SOUL_THROTTLE_CONTROL_H_ - - -#include - - -/* - * @brief Number of bits to shift to go from a 10-bit value to a 12-bit value. - * - */ -#define BIT_SHIFT_10BIT_TO_12BIT (2) - -/* - * @brief Number of steps per volt corresponding to 4096 steps across 5 volts. - * - */ -#define STEPS_PER_VOLT (819.2) - -/* - * @brief Scalar value for the low spoof signal taken from a calibration curve. - * - */ -#define SPOOF_LOW_SIGNAL_CALIBRATION_CURVE_SCALE (0.0004) - -/* - * @brief Offset value for the low spoof signal taken from a calibration curve. - * - */ -#define SPOOF_LOW_SIGNAL_CALIBRATION_CURVE_OFFSET (0.366) - -/* - * @brief Scalar value for the high spoof signal taken from a calibration curve. - * - */ -#define SPOOF_HIGH_SIGNAL_CALIBRATION_CURVE_SCALE (0.0008) - -/* - * @brief Offset value for the high spoof signal taken from a calibration curve. - * - */ -#define SPOOF_HIGH_SIGNAL_CALIBRATION_CURVE_OFFSET (0.732) - -/* - * @brief Minimum allowed value for the low spoof signal value. - * - */ -#define SPOOF_LOW_SIGNAL_RANGE_MIN (0) - -/* - * @brief Maximum allowed value for the low spoof signal value. - * - */ -#define SPOOF_LOW_SIGNAL_RANGE_MAX (1800) - -/* - * @brief Minimum allowed value for the high spoof signal value. - * - */ -#define SPOOF_HIGH_SIGNAL_RANGE_MIN (0) - -/* - * @brief Maximum allowed value for the high spoof signal value. - * - */ -#define SPOOF_HIGH_SIGNAL_RANGE_MAX (3500) - -/* - * @brief Value of the accelerator position that indicates operator override. - * - */ - -#define ACCELERATOR_OVERRIDE_THRESHOLD ( 750.0 ) - -/* - * @brief Amount of time between sensor checks. [milliseconds] - * - */ -#define SENSOR_VALIDITY_CHECK_INTERVAL_IN_MSEC ( 250 ) - -/* - * @brief Number of consecutive faults that can occur when reading the - * sensors before control is disabled. - * - */ -#define SENSOR_VALIDITY_CHECK_FAULT_COUNT ( 4 ) - -/* - * @brief Alpha term for the exponential filter used to smooth the sensor input. - * - */ -#define ACCELERATOR_SENSOR_EXPONENTIAL_FILTER_ALPHA ( 0.25 ) - - -/** - * @brief Accelerator position values. - * - * Contains high and low accelerator values. - * - */ -typedef struct -{ - uint16_t low; /* Low value of accelerator position. */ - - uint16_t high; /* High value of accelerator position. */ -} accelerator_position_s; - - -/** - * @brief Current throttle control state. - * - * Current state of the throttle module control system. - * - */ -typedef struct -{ - bool enabled; /* Flag indicating whether control is currently enabled. */ - - bool operator_override; /* Flag indicating whether accelerator was manually - pressed by operator. */ - - bool invalid_sensor_value; /* Flag indicating a value read from one of the - sensors is invalid. */ - - uint16_t commanded_accelerator_position; /* Position of accelerator commanded - by the controller. */ -} kia_soul_throttle_control_state_s; - - -// **************************************************************************** -// Function: check_for_operator_override -// -// Purpose: Checks to see if the vehicle's operator has manually pressed -// the accelerator and disables control if they have. -// -// Returns: void -// -// Parameters: void -// -// **************************************************************************** -void check_for_operator_override( void ); - - -// **************************************************************************** -// Function: check_for_sensor_faults -// -// Purpose: Checks to see if valid values are being read from the sensors. -// -// Returns: void -// -// Parameters: void -// -// **************************************************************************** -void check_for_sensor_faults( void ); - - -// **************************************************************************** -// Function: read_accelerator_position_sensor -// -// Purpose: Reads current value from the accelerator position sensor. -// -// Returns: void -// -// Parameters: [out] value - pointer to \ref accelerator_position_s that will receive -// the sensor values. -// -// **************************************************************************** -void read_accelerator_position_sensor( - accelerator_position_s * const value ); - - -// **************************************************************************** -// Function: update_throttle -// -// Purpose: Writes throttle spoof values to DAC. -// -// Returns: void -// -// Parameters: void -// -// **************************************************************************** -void update_throttle( void ); - - -// **************************************************************************** -// Function: enable_control -// -// Purpose: Enable control of the throttle system. -// -// Returns: void -// -// Parameters: void -// -// **************************************************************************** -void enable_control( void ); - - -// **************************************************************************** -// Function: disable_control -// -// Purpose: Disable control of the throttle system. -// -// Returns: void -// -// Parameters: void -// -// **************************************************************************** -void disable_control( void ); - - -#endif /* _OSCC_KIA_SOUL_THROTTLE_CONTROL_H_ */ diff --git a/platforms/kia_soul/firmware/throttle/src/communications.cpp b/platforms/kia_soul/firmware/throttle/src/communications.cpp deleted file mode 100644 index a929e163..00000000 --- a/platforms/kia_soul/firmware/throttle/src/communications.cpp +++ /dev/null @@ -1,137 +0,0 @@ -/** - * @file communications.cpp - * - */ - - -#include "mcp_can.h" -#include "oscc_can.h" -#include "throttle_can_protocol.h" -#include "oscc_time.h" -#include "debug.h" - -#include "globals.h" -#include "communications.h" -#include "throttle_control.h" - - -static void publish_throttle_report( void ); - -static void process_throttle_command( - const uint8_t * const data ); - -static void process_rx_frame( - can_frame_s * const frame ); - - -void publish_reports( void ) -{ - uint32_t delta = get_time_delta( g_throttle_report_last_tx_timestamp, GET_TIMESTAMP_MS() ); - - if( delta >= OSCC_REPORT_THROTTLE_PUBLISH_INTERVAL_IN_MSEC ) - { - publish_throttle_report( ); - } -} - - -void check_for_controller_command_timeout( void ) -{ - if( g_throttle_control_state.enabled == true ) - { - bool timeout = is_timeout( - g_throttle_command_last_rx_timestamp, - GET_TIMESTAMP_MS( ), - COMMAND_TIMEOUT_IN_MSEC ); - - if( timeout == true ) - { - disable_control( ); - - DEBUG_PRINTLN( "Timeout waiting for controller command" ); - } - } -} - - -void check_for_incoming_message( void ) -{ - can_frame_s rx_frame; - can_status_t ret = check_for_rx_frame( g_control_can, &rx_frame ); - - if( ret == CAN_RX_FRAME_AVAILABLE ) - { - process_rx_frame( &rx_frame ); - } -} - - -static void publish_throttle_report( void ) -{ - oscc_report_throttle_s throttle_report; - - accelerator_position_s current_accelerator_position; - read_accelerator_position_sensor( ¤t_accelerator_position ); - - throttle_report.id = OSCC_REPORT_THROTTLE_CAN_ID; - throttle_report.dlc = OSCC_REPORT_THROTTLE_CAN_DLC; - throttle_report.data.enabled = (uint8_t) g_throttle_control_state.enabled; - throttle_report.data.override = (uint8_t) g_throttle_control_state.operator_override; - throttle_report.data.current_accelerator_position = - current_accelerator_position.low + current_accelerator_position.high; - throttle_report.data.commanded_accelerator_position = - g_throttle_control_state.commanded_accelerator_position; - throttle_report.data.spoofed_accelerator_output = g_accelerator_spoof_output_sum; - throttle_report.data.fault_invalid_sensor_value = - g_throttle_control_state.invalid_sensor_value; - - g_control_can.sendMsgBuf( - throttle_report.id, - CAN_STANDARD, - throttle_report.dlc, - (uint8_t*) &throttle_report.data ); - - g_throttle_report_last_tx_timestamp = GET_TIMESTAMP_MS(); -} - - -static void process_throttle_command( - const uint8_t * const data ) -{ - if ( data != NULL ) - { - const oscc_command_throttle_data_s * const throttle_command_data = - (oscc_command_throttle_data_s *) data; - - if( throttle_command_data->enabled == true ) - { - enable_control( ); - } - else - { - disable_control( ); - } - - // divisor value found empirically to best match throttle output - g_throttle_control_state.commanded_accelerator_position = - throttle_command_data->commanded_accelerator_position / 24; - - DEBUG_PRINT( "controller commanded accelerator position: " ); - DEBUG_PRINTLN( g_throttle_control_state.commanded_accelerator_position ); - - g_throttle_command_last_rx_timestamp = GET_TIMESTAMP_MS( ); - } -} - - -static void process_rx_frame( - can_frame_s * const frame ) -{ - if ( frame != NULL ) - { - if( frame->id == OSCC_COMMAND_THROTTLE_CAN_ID ) - { - process_throttle_command( frame->data ); - } - } -} diff --git a/platforms/kia_soul/firmware/throttle/src/throttle_control.cpp b/platforms/kia_soul/firmware/throttle/src/throttle_control.cpp deleted file mode 100644 index ef10f4e6..00000000 --- a/platforms/kia_soul/firmware/throttle/src/throttle_control.cpp +++ /dev/null @@ -1,213 +0,0 @@ -/** - * @file throttle_control.cpp - * - */ - - -#include -#include -#include "debug.h" -#include "DAC_MCP49xx.h" -#include "oscc_dac.h" -#include "oscc_signal_smoothing.h" -#include "oscc_time.h" - -#include "throttle_control.h" -#include "globals.h" - - -static void calculate_accelerator_spoof( - const uint16_t accelerator_target, - accelerator_position_s * const spoof ); - - -void check_for_operator_override( void ) -{ - if ( g_throttle_control_state.enabled == true - || g_throttle_control_state.operator_override == true ) - { - accelerator_position_s accelerator_position; - - read_accelerator_position_sensor( &accelerator_position ); - - uint32_t accelerator_position_average = - (accelerator_position.low + accelerator_position.high) / 2; - - if ( accelerator_position_average >= ACCELERATOR_OVERRIDE_THRESHOLD ) - { - disable_control( ); - - g_throttle_control_state.operator_override = true; - - DEBUG_PRINTLN( "Operator override" ); - } - else - { - g_throttle_control_state.operator_override = false; - } - } -} - - -void check_for_sensor_faults( void ) -{ - if ( (g_throttle_control_state.enabled == true) - || (g_throttle_control_state.invalid_sensor_value == true) ) - { - uint32_t current_time = GET_TIMESTAMP_MS(); - - bool timeout = is_timeout( - g_sensor_validity_last_check_timestamp, - current_time, - SENSOR_VALIDITY_CHECK_INTERVAL_IN_MSEC ); - - static int fault_count = 0; - - if( timeout == true ) - { - g_sensor_validity_last_check_timestamp = current_time; - - int sensor_high = analogRead( PIN_ACCELERATOR_POSITION_SENSOR_HIGH ); - int sensor_low = analogRead( PIN_ACCELERATOR_POSITION_SENSOR_LOW ); - - // sensor pins tied to ground - a value of zero indicates disconnection - if( (sensor_high == 0) - || (sensor_low == 0) ) - { - ++fault_count; - - if( fault_count >= SENSOR_VALIDITY_CHECK_FAULT_COUNT ) - { - disable_control( ); - - g_throttle_control_state.invalid_sensor_value = true; - - DEBUG_PRINTLN( "Bad value read from accelerator position sensor" ); - } - } - else - { - g_throttle_control_state.invalid_sensor_value = false; - fault_count = 0; - } - } - } -} - - -void update_throttle( void ) -{ - if ( g_throttle_control_state.enabled == true ) - { - accelerator_position_s accelerator_spoof; - - calculate_accelerator_spoof( - g_throttle_control_state.commanded_accelerator_position, - &accelerator_spoof ); - - g_accelerator_spoof_output_sum = - accelerator_spoof.high + accelerator_spoof.low; - - g_dac.outputA( accelerator_spoof.high ); - g_dac.outputB( accelerator_spoof.low ); - } -} - - -void enable_control( void ) -{ - if( g_throttle_control_state.enabled == false - && g_throttle_control_state.operator_override == false ) - { - const uint16_t num_samples = 20; - prevent_signal_discontinuity( - g_dac, - num_samples, - PIN_ACCELERATOR_POSITION_SENSOR_HIGH, - PIN_ACCELERATOR_POSITION_SENSOR_LOW ); - - digitalWrite( PIN_SPOOF_ENABLE, HIGH ); - - g_throttle_control_state.enabled = true; - - DEBUG_PRINTLN( "Control enabled" ); - } -} - - -void disable_control( void ) -{ - if( g_throttle_control_state.enabled == true ) - { - const uint16_t num_samples = 20; - prevent_signal_discontinuity( - g_dac, - num_samples, - PIN_ACCELERATOR_POSITION_SENSOR_HIGH, - PIN_ACCELERATOR_POSITION_SENSOR_LOW ); - - digitalWrite( PIN_SPOOF_ENABLE, LOW ); - - g_throttle_control_state.enabled = false; - - DEBUG_PRINTLN( "Control disabled" ); - } -} - - -void read_accelerator_position_sensor( - accelerator_position_s * const value ) -{ - accelerator_position_s unfiltered_position; - - unfiltered_position.high = analogRead( PIN_ACCELERATOR_POSITION_SENSOR_HIGH ) << BIT_SHIFT_10BIT_TO_12BIT; - unfiltered_position.low = analogRead( PIN_ACCELERATOR_POSITION_SENSOR_LOW ) << BIT_SHIFT_10BIT_TO_12BIT; - - const float filter_alpha = ACCELERATOR_SENSOR_EXPONENTIAL_FILTER_ALPHA; - static float filtered_position_high = 0.0; - static float filtered_position_low = 0.0; - - filtered_position_high = exponential_moving_average( - filter_alpha, - unfiltered_position.high, - filtered_position_high); - - filtered_position_low = exponential_moving_average( - filter_alpha, - unfiltered_position.low, - filtered_position_low); - - value->high = filtered_position_high; - value->low = filtered_position_low; -} - - -static void calculate_accelerator_spoof( - const uint16_t accelerator_target, - accelerator_position_s * const spoof ) -{ - if ( spoof != NULL ) - { - uint16_t spoof_low = - STEPS_PER_VOLT - * ((SPOOF_LOW_SIGNAL_CALIBRATION_CURVE_SCALE * accelerator_target) - + SPOOF_LOW_SIGNAL_CALIBRATION_CURVE_OFFSET); - - uint16_t spoof_high = - STEPS_PER_VOLT - * ((SPOOF_HIGH_SIGNAL_CALIBRATION_CURVE_SCALE * accelerator_target) - + SPOOF_HIGH_SIGNAL_CALIBRATION_CURVE_OFFSET); - - spoof->low = - constrain( - spoof_low, - SPOOF_LOW_SIGNAL_RANGE_MIN, - SPOOF_LOW_SIGNAL_RANGE_MAX ); - - spoof->high = - constrain( - spoof_high, - SPOOF_HIGH_SIGNAL_RANGE_MIN, - SPOOF_HIGH_SIGNAL_RANGE_MAX ); - } -} diff --git a/platforms/kia_soul/firmware/throttle/tests/features/checking_sensor_validity.feature b/platforms/kia_soul/firmware/throttle/tests/features/checking_sensor_validity.feature deleted file mode 100644 index 905d5356..00000000 --- a/platforms/kia_soul/firmware/throttle/tests/features/checking_sensor_validity.feature +++ /dev/null @@ -1,21 +0,0 @@ -# language: en - -Feature: Checking sensor validity - - Invalid values read from sensors should cause control to be disabled. - - - Scenario: A sensor becomes temporarily disconnected - Given throttle control is enabled - - When a sensor becomes temporarily disconnected - - Then control should remain enabled - - - Scenario: A sensor becomes permanently disconnected - Given throttle control is enabled - - When a sensor becomes permanently disconnected - - Then control should be disabled diff --git a/platforms/kia_soul/firmware/throttle/tests/features/receiving_commands.feature b/platforms/kia_soul/firmware/throttle/tests/features/receiving_commands.feature deleted file mode 100644 index ea513ee7..00000000 --- a/platforms/kia_soul/firmware/throttle/tests/features/receiving_commands.feature +++ /dev/null @@ -1,65 +0,0 @@ -# language: en - -Feature: Receiving commands - - Commands received from a controller should be processed and acted upon. - - - Scenario Outline: Enable throttle command sent from controller - Given throttle control is disabled - And the accelerator position sensors have a reading of - - When an enable throttle command is received - - Then control should be enabled - And the last command timestamp should be set - And should be written to DAC A - And should be written to DAC B - - Examples: - | sensor_val | dac_a_val | dac_b_val | - | 0 | 0 | 0 | - | 256 | 1024 | 1024 | - | 512 | 2048 | 2048 | - | 1024 | 4096 | 4096 | - - - Scenario Outline: Disable throttle command sent from controller - Given throttle control is enabled - And the accelerator position sensors have a reading of - - When a disable throttle command is received - - Then control should be disabled - And the last command timestamp should be set - And should be written to DAC A - And should be written to DAC B - - Examples: - | sensor_val | dac_a_val | dac_b_val | - | 0 | 0 | 0 | - | 256 | 1024 | 1024 | - | 512 | 2048 | 2048 | - | 1024 | 4096 | 4096 | - - - Scenario Outline: Accelerator position command sent from controller - Given throttle control is enabled - - When the accelerator position command is received - - Then the accelerator position command should be parsed - And should be written to DAC A - And should be written to DAC B - - Examples: - | command | spoof_val_high | spoof_val_low | - | 0 | 599 | 299 | - | 1000 | 626 | 313 | - | 2000 | 654 | 327 | - | 3000 | 681 | 340 | - | 4000 | 708 | 354 | - | 5000 | 735 | 367 | - | 10000 | 872 | 436 | - | 15000 | 1009 | 504 | - | 19660 | 1136 | 568 | diff --git a/platforms/kia_soul/firmware/throttle/tests/features/sending_reports.feature b/platforms/kia_soul/firmware/throttle/tests/features/sending_reports.feature deleted file mode 100644 index 46c58916..00000000 --- a/platforms/kia_soul/firmware/throttle/tests/features/sending_reports.feature +++ /dev/null @@ -1,22 +0,0 @@ -# language: en - -Feature: Sending reports - - Throttle reports should be published to the control CAN bus after an interval. - - - Scenario Outline: Throttle report published after an interval - Given the accelerator position sensors have a reading of - And the previous accelerator position command was - - When the time since the last report publishing exceeds the interval - - Then a throttle report should be published to the control CAN bus - And the report's command field should be set to - And the report's current_accelerator_position field should be set to - - Examples: - | command | sensor_val | current_accelerator_position | - | 0 | 0 | 0 | - | 50 | 256 | 512 | - | 100 | 512 | 1408 | diff --git a/platforms/kia_soul/firmware/throttle/tests/features/step_definitions/checking_sensor_validity.cpp b/platforms/kia_soul/firmware/throttle/tests/features/step_definitions/checking_sensor_validity.cpp deleted file mode 100644 index 8ea80549..00000000 --- a/platforms/kia_soul/firmware/throttle/tests/features/step_definitions/checking_sensor_validity.cpp +++ /dev/null @@ -1,40 +0,0 @@ -WHEN("^a sensor becomes temporarily disconnected$") -{ - // first check - error value - one fault - g_mock_arduino_analog_read_return = 0; - g_mock_arduino_millis_return = SENSOR_VALIDITY_CHECK_INTERVAL_IN_MSEC; - check_for_sensor_faults(); - - // second check - error value - two faults - g_mock_arduino_analog_read_return = 0; - g_mock_arduino_millis_return += SENSOR_VALIDITY_CHECK_INTERVAL_IN_MSEC; - check_for_sensor_faults(); - - // third check - valid value - faults reset - g_mock_arduino_analog_read_return = 500; - g_mock_arduino_millis_return += SENSOR_VALIDITY_CHECK_INTERVAL_IN_MSEC; - check_for_sensor_faults(); -} - - -WHEN("^a sensor becomes permanently disconnected$") -{ - g_mock_arduino_analog_read_return = 0; - g_mock_arduino_millis_return = SENSOR_VALIDITY_CHECK_INTERVAL_IN_MSEC; - - // must call function enough times to exceed the fault limit - for( int i = 0; i < SENSOR_VALIDITY_CHECK_FAULT_COUNT; ++i ) - { - // continue timing out - g_mock_arduino_millis_return += SENSOR_VALIDITY_CHECK_INTERVAL_IN_MSEC; - - check_for_sensor_faults(); - } -} - -THEN("^control should remain enabled") -{ - assert_that( - g_throttle_control_state.enabled, - is_equal_to(1)); -} diff --git a/platforms/kia_soul/firmware/throttle/tests/features/step_definitions/receiving_commands.cpp b/platforms/kia_soul/firmware/throttle/tests/features/step_definitions/receiving_commands.cpp deleted file mode 100644 index e1b6abc1..00000000 --- a/platforms/kia_soul/firmware/throttle/tests/features/step_definitions/receiving_commands.cpp +++ /dev/null @@ -1,55 +0,0 @@ -WHEN("^an enable throttle command is received$") -{ - oscc_command_throttle_data_s * throttle_command_data = - (oscc_command_throttle_data_s *) g_mock_mcp_can_read_msg_buf_buf; - - throttle_command_data->enabled = 1; - - check_for_incoming_message(); -} - - -WHEN("^a disable throttle command is received$") -{ - oscc_command_throttle_data_s * throttle_command_data = - (oscc_command_throttle_data_s *) g_mock_mcp_can_read_msg_buf_buf; - - throttle_command_data->enabled = 0; - - check_for_incoming_message(); -} - - -WHEN("^the accelerator position command (.*) is received$") -{ - REGEX_PARAM(int, commanded_accelerator_position); - - oscc_command_throttle_data_s * throttle_command_data = - (oscc_command_throttle_data_s *) g_mock_mcp_can_read_msg_buf_buf; - - throttle_command_data->enabled = 1; - throttle_command_data->commanded_accelerator_position = commanded_accelerator_position; - - check_for_incoming_message(); - - update_throttle(); -} - - -THEN("^the accelerator position command should be parsed$") -{ - oscc_command_throttle_data_s * throttle_command_data = - (oscc_command_throttle_data_s *) g_mock_mcp_can_read_msg_buf_buf; - - assert_that( - g_throttle_control_state.commanded_accelerator_position, - is_equal_to(throttle_command_data->commanded_accelerator_position/24)); -} - - -THEN("^the last command timestamp should be set$") -{ - assert_that( - g_throttle_command_last_rx_timestamp, - is_equal_to(g_mock_arduino_millis_return)); -} diff --git a/platforms/kia_soul/firmware/throttle/tests/features/step_definitions/sending_reports.cpp b/platforms/kia_soul/firmware/throttle/tests/features/step_definitions/sending_reports.cpp deleted file mode 100644 index 3f0f324d..00000000 --- a/platforms/kia_soul/firmware/throttle/tests/features/step_definitions/sending_reports.cpp +++ /dev/null @@ -1,49 +0,0 @@ -WHEN("^the time since the last report publishing exceeds (.*)$") -{ - REGEX_PARAM(std::string, interval); - - g_throttle_report_last_tx_timestamp = 0; - - g_mock_arduino_millis_return = - OSCC_REPORT_THROTTLE_PUBLISH_INTERVAL_IN_MSEC; - - publish_reports(); -} - - -THEN("^a throttle report should be published to the control CAN bus$") -{ - assert_that(g_mock_mcp_can_send_msg_buf_id, is_equal_to(OSCC_REPORT_THROTTLE_CAN_ID)); - assert_that(g_mock_mcp_can_send_msg_buf_ext, is_equal_to(CAN_STANDARD)); - assert_that(g_mock_mcp_can_send_msg_buf_len, is_equal_to(OSCC_REPORT_THROTTLE_CAN_DLC)); - - oscc_report_throttle_data_s * throttle_report_data = - (oscc_report_throttle_data_s *) g_mock_mcp_can_send_msg_buf_buf; - - assert_that(throttle_report_data->enabled, is_equal_to(g_throttle_control_state.enabled)); - assert_that(throttle_report_data->override, is_equal_to(g_throttle_control_state.operator_override)); - - assert_that(g_throttle_report_last_tx_timestamp, is_equal_to(g_mock_arduino_millis_return)); -} - - -THEN("^the report's command field should be set to (.*)$") -{ - REGEX_PARAM(int, commanded_accelerator_position); - - oscc_report_throttle_data_s * throttle_report_data = - (oscc_report_throttle_data_s *) g_mock_mcp_can_send_msg_buf_buf; - - assert_that(throttle_report_data->commanded_accelerator_position, is_equal_to(commanded_accelerator_position)); -} - - -THEN("^the report's current_accelerator_position field should be set to (.*)$") -{ - REGEX_PARAM(int, current_accelerator_position); - - oscc_report_throttle_data_s * throttle_report_data = - (oscc_report_throttle_data_s *) g_mock_mcp_can_send_msg_buf_buf; - - assert_that(throttle_report_data->current_accelerator_position, is_equal_to(current_accelerator_position)); -} diff --git a/platforms/kia_soul/firmware/throttle/tests/features/step_definitions/timeouts_and_overrides.cpp b/platforms/kia_soul/firmware/throttle/tests/features/step_definitions/timeouts_and_overrides.cpp deleted file mode 100644 index 761bc33e..00000000 --- a/platforms/kia_soul/firmware/throttle/tests/features/step_definitions/timeouts_and_overrides.cpp +++ /dev/null @@ -1,20 +0,0 @@ -WHEN("^the time since the last received controller command exceeds the timeout$") -{ - g_throttle_command_last_rx_timestamp = 0; - - g_mock_arduino_millis_return = - COMMAND_TIMEOUT_IN_MSEC; - - check_for_controller_command_timeout(); -} - - -WHEN("^the operator applies (.*) to the accelerator$") -{ - - REGEX_PARAM(int, throttle_sensor_val); - - g_mock_arduino_analog_read_return = throttle_sensor_val; - - check_for_operator_override(); -} diff --git a/platforms/kia_soul/firmware/throttle/tests/features/timeouts_and_overrides.feature b/platforms/kia_soul/firmware/throttle/tests/features/timeouts_and_overrides.feature deleted file mode 100644 index a2493e95..00000000 --- a/platforms/kia_soul/firmware/throttle/tests/features/timeouts_and_overrides.feature +++ /dev/null @@ -1,29 +0,0 @@ -# language: en - -Feature: Timeouts and overrides - - If the module doesn't hear from the controller after an amount of time, - or the operator manually actuates the accelerator pedal, control should be - disabled. - - - Scenario: Controller command timeout - Given throttle control is enabled - - When the time since the last received controller command exceeds the timeout - - Then control should be disabled - - - Scenario Outline: Operator override - Given throttle control is enabled - - When the operator applies to the accelerator - - Then control should be disabled - - Examples: - | sensor_val | - | 250 | - | 500 | - | 1000 | diff --git a/platforms/kia_soul/firmware/throttle/tests/property/build.rs b/platforms/kia_soul/firmware/throttle/tests/property/build.rs deleted file mode 100644 index 2deaf774..00000000 --- a/platforms/kia_soul/firmware/throttle/tests/property/build.rs +++ /dev/null @@ -1,72 +0,0 @@ -extern crate gcc; -extern crate bindgen; - -use std::env; -use std::path::Path; - -fn main() { - gcc::Config::new() - .flag("-w") - .define("__STDC_LIMIT_MACROS", None) - .include("include") - .include("../../include") - .include("../../../../../common/testing/mocks") - .include("../../../../../common/include") - .include("../../../../../common/libs/arduino_init") - .include("../../../../../common/libs/serial") - .include("../../../../../common/libs/can") - .include("../../../../../common/libs/time") - .include("../../../../../common/libs/dac") - .include("../../../../../common/libs/signal_smoothing") - .include("/usr/lib/avr/include") - .file("../../../../../common/testing/mocks/Arduino_mock.cpp") - .file("../../../../../common/testing/mocks/mcp_can_mock.cpp") - .file("../../../../../common/testing/mocks/DAC_MCP49xx_mock.cpp") - .file("../../../../../common/libs/time/oscc_time.cpp") - .file("../../../../../common/libs/can/oscc_can.cpp") - .file("../../../../../common/libs/dac/oscc_dac.cpp") - .file("../../../../../common/libs/signal_smoothing/oscc_signal_smoothing.cpp") - .file("../../src/communications.cpp") - .file("../../src/throttle_control.cpp") - .file("../../src/globals.cpp") - .cpp(true) - .compiler("/usr/bin/g++") - .compile("libthrottle_test.a"); - - let out_dir = env::var("OUT_DIR").unwrap(); - - let _ = bindgen::builder() - .header("include/wrapper.hpp") - .generate_comments(false) - .clang_arg("-I/usr/lib/avr/include") - .clang_arg("-I../../include") - .clang_arg("-I../../../../../common/include") - .clang_arg("-I../../../../../common/libs/can") - .clang_arg("-I../../../../../common/libs/dac") - .clang_arg("-I../../../../../common/libs/time") - .clang_arg("-I../../../../../common/testing/mocks") - .whitelisted_function("publish_reports") - .whitelisted_function("check_for_incoming_message") - .whitelisted_function("check_for_operator_override") - .whitelisted_function("read_accelerator_position_sensor") - .whitelisted_var("g_throttle_control_state") - .whitelisted_var("OSCC_REPORT_THROTTLE_CAN_ID") - .whitelisted_var("OSCC_REPORT_THROTTLE_CAN_DLC") - .whitelisted_var("OSCC_COMMAND_THROTTLE_CAN_ID") - .whitelisted_var("OSCC_COMMAND_THROTTLE_CAN_DLC") - .whitelisted_var("ACCELERATOR_OVERRIDE_THRESHOLD") - .whitelisted_var("OSCC_REPORT_THROTTLE_PUBLISH_INTERVAL_IN_MSEC") - .whitelisted_var("CAN_MSGAVAIL") - .whitelisted_var("CAN_STANDARD") - .whitelisted_var("g_control_can") - .whitelisted_type("oscc_report_throttle_data_s") - .whitelisted_type("oscc_report_throttle_s") - .whitelisted_type("oscc_command_throttle_data_s") - .whitelisted_type("oscc_command_throttle_s") - .whitelisted_type("accelerator_position_s") - .whitelisted_type("can_frame_s") - .generate() - .unwrap() - .write_to_file(Path::new(&out_dir).join("throttle_test.rs")) - .expect("Unable to generate bindings"); -} diff --git a/platforms/kia_soul/firmware/throttle/tests/property/include/wrapper.hpp b/platforms/kia_soul/firmware/throttle/tests/property/include/wrapper.hpp deleted file mode 100644 index 667d6f34..00000000 --- a/platforms/kia_soul/firmware/throttle/tests/property/include/wrapper.hpp +++ /dev/null @@ -1,6 +0,0 @@ -#include "globals.h" -#include "communications.h" -#include "oscc_can.h" -#include "oscc_time.h" -#include "throttle_can_protocol.h" -#include "Arduino.h" diff --git a/platforms/kia_soul/firmware/throttle/tests/property/src/tests.rs b/platforms/kia_soul/firmware/throttle/tests/property/src/tests.rs deleted file mode 100644 index a55a9933..00000000 --- a/platforms/kia_soul/firmware/throttle/tests/property/src/tests.rs +++ /dev/null @@ -1,285 +0,0 @@ -#![allow(non_upper_case_globals)] -#![allow(non_camel_case_types)] -#![allow(non_snake_case)] -#![allow(dead_code)] -#![allow(unused_variables)] -#![allow(unused_imports)] -include!(concat!(env!("OUT_DIR"), "/throttle_test.rs")); - -extern crate quickcheck; -extern crate rand; - -use quickcheck::{QuickCheck, TestResult, Arbitrary, StdGen, Gen}; - -extern "C" { - #[link_name = "g_mock_mcp_can_check_receive_return"] - pub static mut g_mock_mcp_can_check_receive_return: u8; - #[link_name = "g_mock_mcp_can_read_msg_buf_id"] - pub static mut g_mock_mcp_can_read_msg_buf_id: ::std::os::raw::c_ulong; - #[link_name = "g_mock_mcp_can_read_msg_buf_buf"] - pub static mut g_mock_mcp_can_read_msg_buf_buf: [u8; 8usize]; - #[link_name = "g_mock_mcp_can_send_msg_buf_id"] - pub static mut g_mock_mcp_can_send_msg_buf_id: ::std::os::raw::c_ulong; - #[link_name = "g_mock_mcp_can_send_msg_buf_ext"] - pub static mut g_mock_mcp_can_send_msg_buf_ext: u8; - #[link_name = "g_mock_mcp_can_send_msg_buf_len"] - pub static mut g_mock_mcp_can_send_msg_buf_len: u8; - #[link_name = "g_mock_mcp_can_send_msg_buf_buf"] - pub static mut g_mock_mcp_can_send_msg_buf_buf: *mut u8; - #[link_name = "g_mock_arduino_millis_return"] - pub static mut g_mock_arduino_millis_return: ::std::os::raw::c_ulong; - #[link_name = "g_mock_arduino_analog_read_return"] - pub static mut g_mock_arduino_analog_read_return: u16; -} - -impl Arbitrary for oscc_report_throttle_data_s { - fn arbitrary(g: &mut G) -> oscc_report_throttle_data_s { - oscc_report_throttle_data_s { - current_accelerator_position: u16::arbitrary(g), - commanded_accelerator_position: u16::arbitrary(g), - spoofed_accelerator_output: u16::arbitrary(g), - _bitfield_1: u16::arbitrary(g), - } - } -} - -impl Arbitrary for oscc_report_throttle_s { - fn arbitrary(g: &mut G) -> oscc_report_throttle_s { - oscc_report_throttle_s { - id: u32::arbitrary(g), - dlc: u8::arbitrary(g), - timestamp: u32::arbitrary(g), - data: oscc_report_throttle_data_s::arbitrary(g), - } - } -} - -impl Arbitrary for oscc_command_throttle_data_s { - fn arbitrary(g: &mut G) -> oscc_command_throttle_data_s { - oscc_command_throttle_data_s { - commanded_accelerator_position: u16::arbitrary(g), - reserved_0: u8::arbitrary(g), - _bitfield_1: u8::arbitrary(g), - reserved_4: u8::arbitrary(g), - reserved_5: u8::arbitrary(g), - reserved_6: u8::arbitrary(g), - reserved_7: u8::arbitrary(g), - } - } -} - -impl Arbitrary for oscc_command_throttle_s { - fn arbitrary(g: &mut G) -> oscc_command_throttle_s { - oscc_command_throttle_s { - timestamp: u32::arbitrary(g), - data: oscc_command_throttle_data_s::arbitrary(g), - } - } -} - - -impl Arbitrary for can_frame_s { - fn arbitrary(g: &mut G) -> can_frame_s { - can_frame_s { - id: u32::arbitrary(g), - dlc: u8::arbitrary(g), - timestamp: u32::arbitrary(g), - data: [u8::arbitrary(g), - u8::arbitrary(g), - u8::arbitrary(g), - u8::arbitrary(g), - u8::arbitrary(g), - u8::arbitrary(g), - u8::arbitrary(g), - u8::arbitrary(g)], - } - } -} - -/// the throttle firmware should not attempt processing any messages -/// that are not throttle commands -fn prop_only_process_valid_messages(rx_can_msg: can_frame_s, current_target: u16) -> TestResult { - // if we generate a throttle can message, ignore the result - if rx_can_msg.id == OSCC_COMMAND_THROTTLE_CAN_ID { - return TestResult::discard(); - } - unsafe { - g_throttle_control_state.commanded_accelerator_position = current_target; - - g_mock_mcp_can_read_msg_buf_id = rx_can_msg.id as u64; - g_mock_mcp_can_read_msg_buf_buf = rx_can_msg.data; - g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL as u8; - - check_for_incoming_message(); - - TestResult::from_bool(g_throttle_control_state.commanded_accelerator_position == - current_target) - } -} - -#[test] -fn check_message_type_validity() { - QuickCheck::new() - .tests(1000) - .gen(StdGen::new(rand::thread_rng(), u16::max_value() as usize)) - .quickcheck(prop_only_process_valid_messages as fn(can_frame_s, u16) -> TestResult) -} - -/// the throttle firmware should set the commanded accelerator position -/// upon reciept of a valid command throttle message -fn prop_no_invalid_targets(command_throttle_msg: oscc_command_throttle_s) -> TestResult { - unsafe { - g_mock_mcp_can_read_msg_buf_id = OSCC_COMMAND_THROTTLE_CAN_ID as u64; - g_mock_mcp_can_read_msg_buf_buf = std::mem::transmute(command_throttle_msg.data); - g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL as u8; - - check_for_incoming_message(); - - TestResult::from_bool(g_throttle_control_state.commanded_accelerator_position == - (command_throttle_msg.data.commanded_accelerator_position / 24)) - } -} - -#[test] -fn check_accel_pos_validity() { - QuickCheck::new() - .tests(1000) - .quickcheck(prop_no_invalid_targets as fn(oscc_command_throttle_s) -> TestResult) -} - -/// the throttle firmware should set the control state as enabled -/// upon reciept of a valid command throttle message telling it to enable -fn prop_process_enable_command(mut command_throttle_msg: oscc_command_throttle_s) -> TestResult { - unsafe { - command_throttle_msg.data.set_enabled(1); - - g_throttle_control_state.enabled = false; - g_throttle_control_state.operator_override = false; - - g_mock_mcp_can_read_msg_buf_id = OSCC_COMMAND_THROTTLE_CAN_ID as u64; - g_mock_mcp_can_read_msg_buf_buf = std::mem::transmute(command_throttle_msg.data); - g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL as u8; - - check_for_incoming_message(); - - TestResult::from_bool(g_throttle_control_state.enabled == true) - } -} - -#[test] -fn check_process_enable_command() { - QuickCheck::new() - .tests(1000) - .quickcheck(prop_process_enable_command as fn(oscc_command_throttle_s) -> TestResult) -} - -/// the throttle firmware should set the control state as disabled -/// upon reciept of a valid command throttle message telling it to disable -fn prop_process_disable_command(mut command_throttle_msg: oscc_command_throttle_s) -> TestResult { - unsafe { - command_throttle_msg.data.set_enabled(0); - - g_mock_mcp_can_read_msg_buf_id = OSCC_COMMAND_THROTTLE_CAN_ID as u64; - g_mock_mcp_can_read_msg_buf_buf = std::mem::transmute(command_throttle_msg.data); - g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL as u8; - - check_for_incoming_message(); - - TestResult::from_bool(g_throttle_control_state.enabled == false) - } -} - -#[test] -fn check_process_disable_command() { - QuickCheck::new() - .tests(1000) - .quickcheck(prop_process_disable_command as fn(oscc_command_throttle_s) -> TestResult) -} - -/// the throttle firmware should create only valid CAN frames -fn prop_send_valid_can_fields(operator_override: bool, - commanded_accelerator_position: u16) - -> TestResult { - static mut time: u64 = 0; - unsafe { - g_throttle_control_state.operator_override = operator_override; - g_throttle_control_state.commanded_accelerator_position = commanded_accelerator_position; - - time = time + OSCC_REPORT_THROTTLE_PUBLISH_INTERVAL_IN_MSEC as u64; - - g_mock_arduino_millis_return = time; - - publish_reports(); - - let throttle_data = oscc_report_throttle_data_s { - current_accelerator_position: std::mem::transmute([*g_mock_mcp_can_send_msg_buf_buf, - *g_mock_mcp_can_send_msg_buf_buf - .offset(1)]), - commanded_accelerator_position: std::mem::transmute([*g_mock_mcp_can_send_msg_buf_buf - .offset(2), - *g_mock_mcp_can_send_msg_buf_buf - .offset(3)]), - spoofed_accelerator_output: std::mem::transmute([*g_mock_mcp_can_send_msg_buf_buf - .offset(4), - *g_mock_mcp_can_send_msg_buf_buf - .offset(5)]), - _bitfield_1: std::mem::transmute([*g_mock_mcp_can_send_msg_buf_buf.offset(6), - *g_mock_mcp_can_send_msg_buf_buf.offset(7)]), - }; - - TestResult::from_bool((g_mock_mcp_can_send_msg_buf_id == - OSCC_REPORT_THROTTLE_CAN_ID as u64) && - (g_mock_mcp_can_send_msg_buf_ext == (CAN_STANDARD as u8)) && - (g_mock_mcp_can_send_msg_buf_len == - (OSCC_REPORT_THROTTLE_CAN_DLC as u8)) && - (throttle_data.commanded_accelerator_position == - commanded_accelerator_position) && - (throttle_data.enabled() == - (g_throttle_control_state.enabled as u8)) && - (throttle_data.override_() == (operator_override as u8))) - } -} - -#[test] -fn check_valid_can_frame() { - QuickCheck::new() - .tests(1000) - .gen(StdGen::new(rand::thread_rng(), u16::max_value() as usize)) - .quickcheck(prop_send_valid_can_fields as fn(bool, u16) -> TestResult) -} - -// the throttle firmware should be able to correctly and consistently -// detect operator overrides -fn prop_check_operator_override(analog_read_spoof: u16) -> TestResult { - unsafe { - g_throttle_control_state.enabled = true; - g_mock_arduino_analog_read_return = analog_read_spoof; - - let mut accelerator_position = accelerator_position_s { - low: 0, - high: 0 - }; - - read_accelerator_position_sensor(&mut accelerator_position); - - let accelerator_position_average: u32 = - (accelerator_position.low as u32 + accelerator_position.high as u32) / 2; - - check_for_operator_override(); - - if accelerator_position_average >= ACCELERATOR_OVERRIDE_THRESHOLD as u32 { - TestResult::from_bool(g_throttle_control_state.operator_override == true && - g_throttle_control_state.enabled == false) - } else { - TestResult::from_bool(g_throttle_control_state.operator_override == false) - } - } -} - -#[test] -fn check_operator_override() { - QuickCheck::new() - .tests(1000) - .gen(StdGen::new(rand::thread_rng(), u16::max_value() as usize)) - .quickcheck(prop_check_operator_override as fn(u16) -> TestResult) -} diff --git a/utils/diagnostics_tool/CMakeLists.txt b/utils/diagnostics_tool/CMakeLists.txt index b38ce82c..1ba1f4e6 100644 --- a/utils/diagnostics_tool/CMakeLists.txt +++ b/utils/diagnostics_tool/CMakeLists.txt @@ -10,7 +10,6 @@ add_executable( src/brake_module_state.c src/can_monitor.c src/diagnostics.c - src/gateway_module_state.c src/steering_module_state.c src/system_state.c src/terminal_print.c @@ -20,7 +19,7 @@ target_include_directories( diagnostics-tool PRIVATE include - ../../platforms/common/include + ../../api/include ${SDL2_INCLUDE_DIRS}) target_link_libraries( diff --git a/utils/diagnostics_tool/src/brake_module_state.c b/utils/diagnostics_tool/src/brake_module_state.c index 6b168dab..c7ddd12e 100644 --- a/utils/diagnostics_tool/src/brake_module_state.c +++ b/utils/diagnostics_tool/src/brake_module_state.c @@ -9,7 +9,7 @@ #include #include -#include "brake_can_protocol.h" +#include "can_protocols/brake_can_protocol.h" #include "macros.h" #include "can_monitor.h" @@ -27,7 +27,7 @@ // static int analyze_command_frame( brake_module_state_s * const state, - const oscc_command_brake_data_s * const brake_command ) + const oscc_brake_command_s * const brake_command ) { int module_state = STATE_OK; @@ -39,15 +39,15 @@ static int analyze_command_frame( // static int analyze_report_frame( brake_module_state_s * const state, - const oscc_report_brake_data_s * const brake_report ) + const oscc_brake_report_s * const brake_report ) { int module_state = STATE_OK; state->control_state = brake_report->enabled; - state->override_triggered = brake_report->override; + state->override_triggered = brake_report->operator_override; - if( brake_report->fault_obd_timeout == 1 ) + if( brake_report->dtcs == 1 ) { module_state = STATE_FAULT; } @@ -74,12 +74,12 @@ int analyze_brake_state( analyze_command_frame( state, - (oscc_command_brake_data_s*) + (oscc_brake_command_s*) brake_command_frame->frame_contents.buffer ); // TODO : do we need this? state->module_state = analyze_report_frame( state, - (oscc_report_brake_data_s*) + (oscc_brake_report_s*) brake_report_frame->frame_contents.buffer ); return ret; diff --git a/utils/diagnostics_tool/src/gateway_module_state.c b/utils/diagnostics_tool/src/gateway_module_state.c index 45c1ca5a..105175b8 100644 --- a/utils/diagnostics_tool/src/gateway_module_state.c +++ b/utils/diagnostics_tool/src/gateway_module_state.c @@ -9,7 +9,7 @@ #include #include -#include "gateway_can_protocol.h" +#include "can_protocols/gateway_can_protocol.h" #include "chassis_state_can_protocol.h" #include "macros.h" diff --git a/utils/diagnostics_tool/src/steering_module_state.c b/utils/diagnostics_tool/src/steering_module_state.c index b49ab47b..b38055af 100644 --- a/utils/diagnostics_tool/src/steering_module_state.c +++ b/utils/diagnostics_tool/src/steering_module_state.c @@ -9,7 +9,7 @@ #include #include -#include "steering_can_protocol.h" +#include "can_protocols/steering_can_protocol.h" #include "macros.h" #include "can_monitor.h" @@ -27,7 +27,7 @@ // static int analyze_command_frame( steering_module_state_s * const state, - const oscc_command_steering_data_s * const steering_command ) + const oscc_steering_command_s * const steering_command ) { int module_state = STATE_OK; @@ -46,15 +46,15 @@ static int analyze_command_frame( // static int analyze_report_frame( steering_module_state_s * const state, - const oscc_report_steering_data_s * const steering_report ) + const oscc_steering_report_s * const steering_report ) { int module_state = STATE_OK; state->control_state = steering_report->enabled; - state->override_triggered = steering_report->override; + state->override_triggered = steering_report->operator_override; - if( steering_report->fault_obd_timeout == 1 ) + if( steering_report->dtcs == 1 ) { module_state = STATE_FAULT; } @@ -84,12 +84,12 @@ int analyze_steering_state( analyze_command_frame( state, - (oscc_command_steering_data_s*) + (oscc_steering_command_s*) steering_command_frame->frame_contents.buffer ); // TODO : do we need this? state->module_state = analyze_report_frame( state, - (oscc_report_steering_data_s*) + (oscc_steering_report_s*) steering_report_frame->frame_contents.buffer ); return ret; diff --git a/utils/diagnostics_tool/src/system_state.c b/utils/diagnostics_tool/src/system_state.c index aff157fd..98b0792f 100644 --- a/utils/diagnostics_tool/src/system_state.c +++ b/utils/diagnostics_tool/src/system_state.c @@ -14,11 +14,11 @@ #include "can_monitor.h" #include "terminal_print.h" #include "system_state.h" -#include "brake_can_protocol.h" -#include "chassis_state_can_protocol.h" -#include "gateway_can_protocol.h" -#include "steering_can_protocol.h" -#include "throttle_can_protocol.h" +#include "can_protocols/brake_can_protocol.h" +#include "can_protocols/steering_can_protocol.h" +#include "can_protocols/throttle_can_protocol.h" +// #include "chassis_state_can_protocol.h" +// #include "gateway_can_protocol.h" @@ -44,10 +44,10 @@ static int update_steering_state() int ret = ERROR; const can_frame_s * const steering_command_frame = - get_can_msg_array_index_reference( OSCC_COMMAND_STEERING_CAN_ID ); + get_can_msg_array_index_reference( OSCC_STEERING_COMMAND_CAN_ID ); const can_frame_s * const steering_report_frame = - get_can_msg_array_index_reference( OSCC_REPORT_STEERING_CAN_ID ); + get_can_msg_array_index_reference( OSCC_STEERING_REPORT_CAN_ID ); if( steering_command_frame != NULL || steering_report_frame != NULL ) { @@ -67,10 +67,10 @@ static int update_throttle_state() int ret = ERROR; const can_frame_s * const throttle_command_frame = - get_can_msg_array_index_reference( OSCC_COMMAND_THROTTLE_CAN_ID ); + get_can_msg_array_index_reference( OSCC_THROTTLE_COMMAND_CAN_ID ); const can_frame_s * const throttle_report_frame = - get_can_msg_array_index_reference( OSCC_REPORT_THROTTLE_CAN_ID ); + get_can_msg_array_index_reference( OSCC_THROTTLE_REPORT_CAN_ID ); if( throttle_command_frame != NULL || throttle_report_frame != NULL ) { @@ -90,10 +90,10 @@ static int update_brake_state() int ret = ERROR; const can_frame_s * const brake_command_frame = - get_can_msg_array_index_reference( OSCC_COMMAND_BRAKE_CAN_ID ); + get_can_msg_array_index_reference( OSCC_BRAKE_COMMAND_CAN_ID ); const can_frame_s * const brake_report_frame = - get_can_msg_array_index_reference( OSCC_REPORT_BRAKE_CAN_ID ); + get_can_msg_array_index_reference( OSCC_BRAKE_REPORT_CAN_ID ); if( brake_command_frame != NULL || brake_report_frame != NULL ) { @@ -107,33 +107,33 @@ static int update_brake_state() } -// -static int update_gateway_state() -{ - int ret = ERROR; +// // +// static int update_gateway_state() +// { +// int ret = ERROR; - const can_frame_s * const heartbeat_msg_frame = - get_can_msg_array_index_reference( OSCC_REPORT_HEARTBEAT_CAN_ID ); +// const can_frame_s * const heartbeat_msg_frame = +// get_can_msg_array_index_reference( OSCC_REPORT_HEARTBEAT_CAN_ID ); - const can_frame_s * const chassis_state1_frame = - get_can_msg_array_index_reference( OSCC_REPORT_CHASSIS_STATE_1_CAN_ID ); +// const can_frame_s * const chassis_state1_frame = +// get_can_msg_array_index_reference( OSCC_REPORT_CHASSIS_STATE_1_CAN_ID ); - const can_frame_s * const chassis_state2_frame = - get_can_msg_array_index_reference( OSCC_REPORT_CHASSIS_STATE_2_CAN_ID ); +// const can_frame_s * const chassis_state2_frame = +// get_can_msg_array_index_reference( OSCC_REPORT_CHASSIS_STATE_2_CAN_ID ); - if( heartbeat_msg_frame != NULL || - chassis_state1_frame != NULL || - chassis_state2_frame != NULL ) - { - ret = analyze_gateway_state( - &system_state.gateway_module_state, - heartbeat_msg_frame, - chassis_state1_frame, - chassis_state2_frame ); - } +// if( heartbeat_msg_frame != NULL || +// chassis_state1_frame != NULL || +// chassis_state2_frame != NULL ) +// { +// ret = analyze_gateway_state( +// &system_state.gateway_module_state, +// heartbeat_msg_frame, +// chassis_state1_frame, +// chassis_state2_frame ); +// } - return ret; -} +// return ret; +// } @@ -169,12 +169,12 @@ int update_system_state() ret = ERROR; } - if( update_gateway_state() == ERROR ) - { - system_state.gateway_module_state.module_state = STATE_FAULT; +// if( update_gateway_state() == ERROR ) +// { +// system_state.gateway_module_state.module_state = STATE_FAULT; - ret = ERROR; - } +// ret = ERROR; +// } // TODO : define //update_overall_state(); diff --git a/utils/diagnostics_tool/src/throttle_module_state.c b/utils/diagnostics_tool/src/throttle_module_state.c index f380854f..8713b99b 100644 --- a/utils/diagnostics_tool/src/throttle_module_state.c +++ b/utils/diagnostics_tool/src/throttle_module_state.c @@ -9,7 +9,7 @@ #include #include -#include "throttle_can_protocol.h" +#include "can_protocols/throttle_can_protocol.h" #include "macros.h" #include "can_monitor.h" @@ -27,7 +27,7 @@ // static int analyze_command_frame( throttle_module_state_s * const state, - const oscc_command_throttle_data_s * const throttle_command ) + const oscc_throttle_command_s * const throttle_command ) { int module_state = STATE_OK; @@ -39,13 +39,13 @@ static int analyze_command_frame( // static int analyze_report_frame( throttle_module_state_s * const state, - const oscc_report_throttle_data_s * const throttle_report ) + const oscc_throttle_report_s * const throttle_report ) { int module_state = STATE_OK; state->control_state = throttle_report->enabled; - state->override_triggered = throttle_report->override; + state->override_triggered = throttle_report->operator_override; return module_state; @@ -69,12 +69,12 @@ int analyze_throttle_state( analyze_command_frame( state, - (oscc_command_throttle_data_s*) + (oscc_throttle_command_s*) throttle_command_frame->frame_contents.buffer ); // TODO : do we need this? state->module_state = analyze_report_frame( state, - (oscc_report_throttle_data_s*) + (oscc_throttle_report_s*) throttle_report_frame->frame_contents.buffer ); return ret; diff --git a/utils/joystick_commander/CMakeLists.txt b/utils/joystick_commander/CMakeLists.txt deleted file mode 100644 index da2d391d..00000000 --- a/utils/joystick_commander/CMakeLists.txt +++ /dev/null @@ -1,26 +0,0 @@ -cmake_minimum_required(VERSION 2.8) - -project(joystick-commander) - -find_package(PkgConfig REQUIRED) -pkg_check_modules(SDL2 REQUIRED sdl2) - -add_executable( - joystick-commander - src/commander.c - src/joystick.c - src/main.c - src/oscc_interface.c) - -target_include_directories( - joystick-commander - PRIVATE - include - ../../platforms/common/include - ${SDL2_INCLUDE_DIRS}) - -target_link_libraries( - joystick-commander - PRIVATE - ${SDL2_LIBRARIES}) - diff --git a/utils/joystick_commander/include/commander.h b/utils/joystick_commander/include/commander.h deleted file mode 100644 index b03cbcd2..00000000 --- a/utils/joystick_commander/include/commander.h +++ /dev/null @@ -1,65 +0,0 @@ -/** - * @file commander.h - * @brief Commander Interface. - * - */ - - -#ifndef COMMANDER_H -#define COMMANDER_H - - -/** - * @brief Initialize the commander for use - * - * @param [in] channel to use (preferred to deprecate) - * - * @return ERROR code: - * \li \ref NOERR (1) if success. - * \li \ref ERROR (0) if failure. - * - */ -int commander_init( int channel ); - -/** - * @brief Close the commander. Releases and closes all modules - * under the commander also. - * - * @param [void] - * - * @return void - * - */ -void commander_close( ); - -/** - * @brief Commander low-frequency update. Checks the status of the - * joystick and the the OSCC modules and updates the current - * values, including brakes, throttle and steering. Is expected - * to execute every 50ms. - * - * @param [void] - * - * @return ERROR code: - * \li \ref NOERR (1) if success. - * \li \ref ERROR (0) if failure. - * - */ -int commander_low_frequency_update( ); - -/** - * @brief Commander high-frequency update. Checks the state of the - * driver override to disable the OSCC modules. Is expected to - * execute every 1ms - * - * @param [void] - * - * @return ERROR code: - * \li \ref NOERR (1) if success. - * \li \ref ERROR (0) if failure. - * - */ -int commander_high_frequency_update( ); - - -#endif /* COMMANDER_H */ diff --git a/utils/joystick_commander/include/joystick.h b/utils/joystick_commander/include/joystick.h deleted file mode 100644 index b006a004..00000000 --- a/utils/joystick_commander/include/joystick.h +++ /dev/null @@ -1,161 +0,0 @@ -/** - * @file joystick.h - * @brief Joystick Interface. - * - */ - - -#ifndef JOYSTICK_H -#define JOYSTICK_H - - - -/** - * @brief Lowest joystick axis value. - * - */ -#define JOYSTICK_AXIS_POSITION_MIN ( -32768 ) - - -/** - * @brief Highest joystick axis value. - * - */ -#define JOYSTICK_AXIS_POSITION_MAX ( 32767 ) - - -/** - * @brief Lowest joystick axis value. - * - */ -#define JOYSTICK_TRIGGER_POSITION_MIN ( 0 ) - - -/** - * @brief Highest joystick axis value. - * - */ -#define JOYSTICK_TRIGGER_POSITION_MAX ( 32767 ) - - -/** - * @brief Button state not pressed. - * - */ -#define JOYSTICK_BUTTON_STATE_NOT_PRESSED ( 0 ) - - -/** - * @brief Button state pressed. - * - */ -#define JOYSTICK_BUTTON_STATE_PRESSED ( 1 ) - - - - - -/** - * @brief Initialization function for the joystick - * - * @return ERROR code - * \li \ref NOERR (1) if success. - * \li \ref ERROR (0) if failure. - * - */ -int joystick_init( ); - - -/** - * @brief Open joystick device. - * - * @param [in] device_index Device index in the subsystem. - * - * @return ERROR code: - * \li \ref NOERR (1) if success. - * \li \ref ERROR (0) if failure. - * - */ -int joystick_open( unsigned long device_index ); - - -/** - * @brief Close joystick device. - * - * @return void - * - */ -void joystick_close( ); - - -/** - * @brief Update joystick device. - * - * @return ERROR code: - * \li \ref NOERR (1) if success. - * \li \ref ERROR (0) if failure. - * - */ -int joystick_update( ); - - -/** - * @brief Get joystick axis value. - * - * @param [in] axis_index Axis index. - * @param [out] position Current axis value. - * - * @return ERROR code: - * \li \ref NOERR (1) if success. - * \li \ref ERROR (0) if failure. - * - */ -int joystick_get_axis( const unsigned long axis_index, int * const position ); - - -/** - * @brief Get joystick button state. - * - * @param [in] button_index Button index. - * @param [out] state Current button state. - * - * @return ERROR code: - * \li \ref NOERR (1) if success. - * \li \ref ERROR (0) if failure. - * - */ -int joystick_get_button( const unsigned long button_index, - unsigned int * const state ); - - -/** - * @brief Map axis value from one range to another. - * - * @param [in] position Input value to map. - * @param [in] range_min Output minimum range. - * @param [in] range_max Output maximum range. - * - * @return joystick position mapped to the range of min:max. - * - */ -double joystick_normalize_axis_position( const int position, - const double range_min, - const double range_max ); - - -/** - * @brief Map trigger value from one range to another. - * - * @param [in] position Input value to map. - * @param [in] range_min Output minimum range. - * @param [in] range_max Output maximum range. - * - * @return position mapped to the range of min:max. - * - */ -double joystick_normalize_trigger_position( const int position, - const double range_min, - const double range_max ); - - -#endif /* JOYSTICK_H */ \ No newline at end of file diff --git a/utils/joystick_commander/include/macros.h b/utils/joystick_commander/include/macros.h deleted file mode 100644 index 8073c223..00000000 --- a/utils/joystick_commander/include/macros.h +++ /dev/null @@ -1,45 +0,0 @@ -/** - * @file macros.h - * @brief Global macros for joystick project. - * - */ - - -#ifndef MACROS_H -#define MACROS_H - - - - -/** - * @brief Error macro. - * - */ -#define ERROR 0 - - -/** - * @brief Macro indicating no error. - * - */ -#define NOERR 1 - - -/** - * @brief Macro indicating a warning but not an error. - * - */ -#define UNAVAILABLE 2 - - -/** - * @brief Math macro: constrain(amount, low, high). - * - */ -#define m_constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) - - - - -#endif /* MACROS_H */ - diff --git a/utils/joystick_commander/include/oscc_interface.h b/utils/joystick_commander/include/oscc_interface.h deleted file mode 100644 index 276199cd..00000000 --- a/utils/joystick_commander/include/oscc_interface.h +++ /dev/null @@ -1,197 +0,0 @@ -/** - * @file oscc_interface.h - * @brief OSCC interface - The main command* functions and the - * update function should be called on at least a 50ms - * period. The expectation is that if there is not some - * kind of communication from the controller to the OSCC - * modules in that time, then the OSCC modules will - * disable and return control back to the driver. - */ - - -#ifndef OSCC_INTERFACE_H -#define OSCC_INTERFACE_H - -#include - - -typedef struct -{ - bool operator_override; - bool fault_brake_obd_timeout; - bool fault_brake_invalid_sensor_value; - bool fault_brake_actuator_error; - bool fault_brake_pump_motor_error; - bool fault_steering_obd_timeout; - bool fault_steering_invalid_sensor_value; - bool fault_throttle_invalid_sensor_value; -} oscc_status_s; - - -/** - * @brief Initialize the OSCC interface - This call must occur - * first in order to use the interface at all. If this - * call does not come first, all other calls will return - * ERROR. - * - * @param [in] channel - for now, the CAN channel to use when - * communicating with the OSCC modules - * - * @return ERROR or NOERR - * - */ -int oscc_interface_init( int channel ); - - -/** - * @brief Close the OSCC interface - * - * @param [void] - * - * @return ERROR or NOERR - * - */ -void oscc_interface_close( ); - - -/** - * @brief Enable the OSCC interface - This function specifically - * tells the OSCC modules to engage the control - * functionality. After this command goes through, the - * vehicle is being run via remote control. - * - * @param [void] - * - * @return ERROR or NOERR - * - */ -int oscc_interface_enable( ); - - -/** - * @brief Disable the OSCC interface - Disables the remote - * control and returns vehicle control to the driver. - * - * @param [void] - * - * @return ERROR or NOERR - * - */ -int oscc_interface_disable( ); - - -/** - * @brief Set the default values on the OSCC interface - This is - * mostly a preparatory function for transferring control - * between the OSCC modules and the driver - * - * @param [void] - * - * @return ERROR or NOERR - * - */ -int oscc_interface_set_defaults( ); - - -/** - * @brief Disable only the braking module - * - * @param [void] - * - * @return ERROR or NOERR - * - */ -int oscc_interface_disable_brakes( ); - - -/** - * @brief Disable only the throttle module - * - * @param [void] - * - * @return ERROR or NOERR - * - */ -int oscc_interface_disable_throttle( ); - - -/** - * @brief Disable only the steering module - * - * @param [void] - * - * @return ERROR or NOERR - * - */ -int oscc_interface_disable_steering( ); - - -/** - * @brief Send a brake value to the braking module - This - * command sends a value to the OSCC braking module on - * how hard to apply the brakes. - * - * @param [in] brake_setpoint - value to set the brakes to. The - * possible range goes from 0 to 52428 (16 bits) - * - * @return ERROR or NOERR - * - */ -int oscc_interface_command_brakes( unsigned int brake_setpoint ); - - -/** - * @brief Send a throttle value to the throttle module - This - * command sends a value to the OSCC throttle module to - * control how much fuel to send to the engine - * - * @param [in] throttle_setpoint - value to set the throttle - * to. The possible range goes from 0 to 19660 (16 bits) - * - * @return ERROR or NOERR - * - */ -int oscc_interface_command_throttle( unsigned int throttle_setpoint ); - - -/** - * @brief Send steering values to the steering module - This - * command sends an angle and turn rate to the OSCC - * steering module to control how much and how fast to - * turn the steering wheel - * - * @param [in] angle - steering wheel angle to set - - * possible range goes from -4700 to 4700 degrees (signed - * 16 bit value) - * - * [in] rate - how fast to turn the steering wheel - - * possible range goes from 20 to 254 degrees/sec - * (unsigned 16 bit value) - * - * @return ERROR or NOERR - * - */ -int oscc_interface_command_steering( int angle, unsigned int rate ); - -/** - * @brief OSCC status message - the primary status from the - * OSCC modules is whether or not the modules have - * detected a driver override of the OSCC control. If any - * of the modules have been overridded by the driver, - * this status will indicate that - * - * This is a polled call that must be read at 50ms or - * faster. - * - * @param [in/out] override - If any of the OSCC modules have - * detected a driver override, this value will contain a - * 1 after this function returns. Otherwise the return - * is 0. - * - * @return ERROR or NOERR - * - */ -int oscc_interface_update_status( oscc_status_s * status ); - -#endif /* OSCC_INTERFACE_H */ - diff --git a/utils/joystick_commander/src/commander.c b/utils/joystick_commander/src/commander.c deleted file mode 100644 index 39fc2062..00000000 --- a/utils/joystick_commander/src/commander.c +++ /dev/null @@ -1,897 +0,0 @@ -/** - * @file commander.c - * @brief Commander Interface Source - * - */ - - - - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "macros.h" -#include "joystick.h" -#include "oscc_interface.h" - - -// ***************************************************** -// static global types/macros -// ***************************************************** - -/** - * @brief Joystick axis indices - * - */ -#define JOYSTICK_AXIS_THROTTLE (SDL_CONTROLLER_AXIS_TRIGGERRIGHT) - #define JOYSTICK_AXIS_BRAKE (SDL_CONTROLLER_AXIS_TRIGGERLEFT) - #define JOYSTICK_AXIS_STEER (SDL_CONTROLLER_AXIS_LEFTX) - - -/** - * @brief Joystick button indices - * - */ -#define JOYSTICK_BUTTON_ENABLE_CONTROLS (SDL_CONTROLLER_BUTTON_START) -#define JOYSTICK_BUTTON_DISABLE_CONTROLS (SDL_CONTROLLER_BUTTON_BACK) - -/** - * @brief Throttle pedal position values [normalized] - * - */ -#define MIN_THROTTLE_PEDAL (0.0) -#define MAX_THROTTLE_PEDAL (0.3) - - -/** - * @brief Brake pedal position values [normalized] - * - */ -#define MIN_BRAKE_PEDAL (0.0) -#define MAX_BRAKE_PEDAL (0.8) - - -/** - * @brief Minimum brake value to be considered enabled [normalized] - * - * Throttle is disabled when brake value is greater than this value - * - */ -#define BRAKES_ENABLED_MIN (0.05) - - -/** - * @brief Steering wheel angle values [radians] - * - * Negative value means turning to the right - * - */ -#define MIN_STEERING_WHEEL_ANGLE (-M_PI * 2.0) -#define MAX_STEERING_WHEEL_ANGLE (M_PI * 2.0) - - -/** - * @brief Steering command angles [int16_t] - * - */ -#define STEERING_COMMAND_ANGLE_MIN (-4700) -#define STEERING_COMMAND_ANGLE_MAX (4700) - - -/** - * @brief Steering command angle scale factor - * - */ -#define STEERING_COMMAND_ANGLE_FACTOR ( 10.0 ) - - -/** - * @brief Steering command steering wheel velocities [uint8_t] - * - */ -#define STEERING_COMMAND_MAX_VELOCITY_MIN (20) -#define STEERING_COMMAND_MAX_VELOCITY_MAX (254) - - -/** - * @brief Steering command steering wheel velocity scale factor - * - * This factor can be increased to provide smoother, but - * slightly less responsive, steering control. It is recommended - * to smooth at the higher level, with this factor, before - * trying to smooth at the lower level - * - */ -#define STEERING_COMMAND_MAX_VELOCITY_FACTOR (0.25) - - -/** - * @brief Exponential filter factors - * - */ -#define BRAKES_FILTER_FACTOR (0.2) -#define THROTTLE_FILTER_FACTOR (0.2) -#define STEERING_FILTER_FACTOR (0.1) - -/** - * @brief joystick delay interval [microseconds] - * - * Defines the delay to wait for the joystick to update - * - * 50,000 us == 50 ms == 20 Hertz - * - */ -#define JOYSTICK_DELAY_INTERVAL (50000) - - -/** - * @brief Convert radians to degrees - * - */ -#define m_degrees(rad) ( ( rad ) * ( 180.0 / M_PI ) ) - - -// ***************************************************** -// Local Type definitions -// ***************************************************** - -/** - * @brief Commander setpoint - * - * The commander setpoint is a structure that contains all the - * relevant information to retrieve data from an external source - * and range check it for validity. The range-limits for this - * instance represent the values that are typically available - * from a joystick - * - */ -struct commander_setpoint_s -{ - double setpoint; - - const unsigned long axis; - - const double min_position; - - const double max_position; -}; - - -// ***************************************************** -// static global data -// ***************************************************** - -#define COMMANDER_ENABLED ( 1 ) -#define COMMANDER_DISABLED ( 0 ) - -static int commander_enabled = COMMANDER_DISABLED; - -/** - * @brief Setpoint Data - * - * Static definitions for brake, steering and throttle setpoints - * - */ -static struct commander_setpoint_s brake_setpoint = - { 0.0, JOYSTICK_AXIS_BRAKE, MIN_BRAKE_PEDAL, MAX_BRAKE_PEDAL }; - -static struct commander_setpoint_s throttle_setpoint = - { 0.0, JOYSTICK_AXIS_THROTTLE, MIN_THROTTLE_PEDAL, MAX_THROTTLE_PEDAL }; - -static struct commander_setpoint_s steering_setpoint = - { 0.0, JOYSTICK_AXIS_STEER, MIN_STEERING_WHEEL_ANGLE, MAX_STEERING_WHEEL_ANGLE }; - - -// ***************************************************** -// static definitions -// ***************************************************** - - -// ***************************************************** -// Function: get_setpoint -// -// Purpose: Retrieve the data from the joystick based on which axis is -// selected and normalize that value along the scale that is -// provided -// -// Returns: int - ERROR or NOERR -// -// Parameters: setpoint - the setpoint structure containing the range limits -// the joystick axis, and the storage location for the -// requested value -// -// ***************************************************** -static int get_setpoint( struct commander_setpoint_s* setpoint ) -{ - int return_code = ERROR; - - if ( setpoint != NULL ) - { - int axis_position = 0; - - return_code = joystick_get_axis( setpoint->axis, &axis_position ); - - if ( return_code == NOERR ) - { - if ( setpoint->axis == JOYSTICK_AXIS_STEER ) - { - setpoint->setpoint = - joystick_normalize_axis_position( axis_position, - setpoint->min_position, - setpoint->max_position ); - } - else - { - setpoint->setpoint = - joystick_normalize_trigger_position( axis_position, - setpoint->min_position, - setpoint->max_position ); - } - } - } - return ( return_code ); - -} - - -// ***************************************************** -// Function: is_joystick_safe -// -// Purpose: Examine the positions of the brake and throttle to determine -// if they are in a safe position to enable control -// -// Returns: int - ERROR, NOERR or UNAVAILABLE -// -// Parameters: void -// -// ***************************************************** -static int is_joystick_safe( ) -{ - int return_code = ERROR; - - return_code = joystick_update( ); - - if ( return_code == NOERR ) - { - return_code = get_setpoint( &brake_setpoint ); - - if ( return_code == NOERR ) - { - return_code = get_setpoint( &throttle_setpoint ); - - if ( return_code == NOERR ) - { - if ( ( throttle_setpoint.setpoint > 0.0 ) || - ( brake_setpoint.setpoint > 0.0 ) ) - { - return_code = UNAVAILABLE; - } - } - } - } - return return_code; -} - -// ***************************************************** -// Function: commander_set_safe -// -// Purpose: Put the OSCC module in a safe position -// -// Returns: int - ERROR or NOERR -// -// Parameters: void -// -// ***************************************************** -static int commander_set_safe( ) -{ - int return_code = ERROR; - - if ( commander_enabled == COMMANDER_ENABLED ) - { - return_code = oscc_interface_set_defaults(); - } - return ( return_code ); -} - - -// ***************************************************** -// Function: calc_exponential_average -// -// Purpose: Calculate an exponential average based on previous values -// -// Returns: double - the exponentially averaged result -// -// Parameters: average - previous average -// setpoint - new setpoint to incorperate into average -// factor - factor of exponential average -// -// ***************************************************** -static double calc_exponential_average( double average, - double setpoint, - double factor ) -{ - double exponential_average = - ( setpoint * factor ) + ( ( 1 - factor ) * average ); - - return ( exponential_average ); -} - - -// ***************************************************** -// Function: commander_disable_controls -// -// Purpose: Helper function to put the system in a safe state before -// disabling the OSCC module vehicle controls -// -// Returns: int - ERROR or NOERR -// -// Parameters: void -// -// ***************************************************** -static int commander_disable_controls( ) -{ - int return_code = ERROR; - - printf( "Disable controls\n" ); - - if ( commander_enabled == COMMANDER_ENABLED ) - { - return_code = commander_set_safe( ); - - if ( return_code == NOERR ) - { - return_code = oscc_interface_disable(); - } - } - return return_code; -} - - -// ***************************************************** -// Function: commander_enable_controls -// -// Purpose: Helper function to put the system in a safe state before -// enabling the OSCC module vehicle controls -// -// Returns: int - ERROR or NOERR -// -// Parameters: void -// -// ***************************************************** -static int commander_enable_controls( ) -{ - int return_code = ERROR; - - printf( "Enable controls\n" ); - - if ( commander_enabled == COMMANDER_ENABLED ) - { - return_code = commander_set_safe( ); - - if ( return_code == NOERR ) - { - return_code = oscc_interface_enable(); - } - } - return ( return_code ); -} - - -// ***************************************************** -// Function: get_button -// -// Purpose: Wrapper function to get the status of a given button on the -// joystick -// -// Returns: int - ERROR or NOERR -// -// Parameters: button - which button on the joystick to check -// state - pointer to an unsigned int to store the state of the -// button -// -// ***************************************************** -static int get_button( unsigned long button, unsigned int* const state ) -{ - int return_code = ERROR; - - if ( state != NULL ) - { - unsigned int button_state; - - return_code = joystick_get_button( button, &button_state ); - - if ( ( return_code == NOERR ) && - ( button_state == JOYSTICK_BUTTON_STATE_PRESSED ) ) - { - ( *state ) = 1; - } - else - { - ( *state ) = 0; - } - } - return ( return_code ); -} - - -// ***************************************************** -// Function: command_brakes -// -// Purpose: Determine the setpoint being commanded by the joystick and -// send that value to the OSCC Module -// -// Returns: int - ERROR or NOERR -// -// Parameters: void -// -// ***************************************************** -static int command_brakes( ) -{ - int return_code = ERROR; - unsigned int constrained_value = 0; - static double brake_average = 0.0; - - if ( commander_enabled == COMMANDER_ENABLED ) - { - return_code = get_setpoint( &brake_setpoint ); - - if ( return_code == NOERR ) - { - brake_average = calc_exponential_average( brake_average, - brake_setpoint.setpoint, - BRAKES_FILTER_FACTOR ); - - const float normalized_value = (float) m_constrain( - (float) brake_average, - 0.0f, - MAX_BRAKE_PEDAL ); - - constrained_value = ( unsigned int ) m_constrain( - (float) ( normalized_value * (float) UINT16_MAX ), - (float) 0.0f, - (float) UINT16_MAX ); - } - - printf( "brake: %d\n", constrained_value ); - - return_code = oscc_interface_command_brakes( constrained_value ); - } - return ( return_code ); -} - - -// ***************************************************** -// Function: command_throttle -// -// Purpose: Determine the setpoint being commanded by the joystick and -// send that value to the OSCC Module -// -// Returns: int - ERROR or NOERR -// -// Parameters: void -// -// ***************************************************** -static int command_throttle( ) -{ - int return_code = ERROR; - - if ( commander_enabled == COMMANDER_ENABLED ) - { - return_code = get_setpoint( &throttle_setpoint ); - - // don't allow throttle if brakes are applied - if ( return_code == NOERR ) - { - return_code = get_setpoint( &brake_setpoint ); - - if ( brake_setpoint.setpoint >= BRAKES_ENABLED_MIN ) - { - throttle_setpoint.setpoint = 0.0; - } - } - - // Redundant, but better safe then sorry - const float normalized_value = (float) m_constrain( - (float) throttle_setpoint.setpoint, - 0.0f, - MAX_THROTTLE_PEDAL ); - - unsigned int constrained_value = ( unsigned int )m_constrain( - (float) (normalized_value * (float) UINT16_MAX), - (float) 0.0f, - (float) UINT16_MAX ); - - printf( "throttle: %d\n", constrained_value ); - - return_code = oscc_interface_command_throttle( constrained_value ); - } - return ( return_code ); -} - - -// ***************************************************** -// Function: command_steering -// -// Purpose: Determine the setpoint being commanded by the joystick and -// send that value to the OSCC Module -// -// Returns: int - ERROR or NOERR -// -// Parameters: void -// -// ***************************************************** -static int command_steering( ) -{ - int return_code = ERROR; - static double steering_average = 0.0; - static double last_steering_rate = 0.0; - - if ( commander_enabled == COMMANDER_ENABLED ) - { - return_code = get_setpoint( &steering_setpoint ); - - steering_average = - calc_exponential_average( steering_average, - steering_setpoint.setpoint, - STEERING_FILTER_FACTOR ); - - const float angle_degrees = - (float) m_degrees( (float) steering_average ); - - const int constrained_angle = ( int ) m_constrain( - (float) (angle_degrees * STEERING_COMMAND_ANGLE_FACTOR), - (float) STEERING_COMMAND_ANGLE_MIN, - (float) STEERING_COMMAND_ANGLE_MAX ); - - float rate_degrees = - (float) fabs( constrained_angle - last_steering_rate ); - - last_steering_rate = constrained_angle; - - unsigned int constrained_rate = ( unsigned int ) m_constrain( - (float) (rate_degrees / (float) STEERING_COMMAND_MAX_VELOCITY_FACTOR), - (float) STEERING_COMMAND_MAX_VELOCITY_MIN + 1.0f, - (float) STEERING_COMMAND_MAX_VELOCITY_MAX ); - - printf( "steering: %d\t%d\n", constrained_angle, constrained_rate ); - - return_code = oscc_interface_command_steering( constrained_angle, - constrained_rate ); - } - return ( return_code ); -} - - -// ***************************************************** -// Function: check_for_brake_faults -// -// Purpose: Checks oscc_status_s struct for brake -// faults -// -// Returns: bool - true if fault occurred -// - false if no fault occurred -// -// Parameters: oscc_status_s - struct containing OSCC status -// -// ***************************************************** -static bool check_for_brake_faults( oscc_status_s * status) -{ - bool fault_occurred = false; - - if( status != NULL ) - { - if ( status->fault_brake_obd_timeout == true ) - { - printf( "Brake - OBD Timeout Detected\n" ); - - fault_occurred = true; - } - - if ( status->fault_brake_invalid_sensor_value == true ) - { - printf( "Brake - Invalid Sensor Value Detected\n" ); - - fault_occurred = true; - } - - if ( status->fault_brake_actuator_error == true ) - { - printf( "Brake - Actuator Error Detected\n" ); - - fault_occurred = true; - } - - if ( status->fault_brake_pump_motor_error == true ) - { - printf( "Brake - Accumulator Pump Error Detected\n" ); - - fault_occurred = true; - } - } - - return fault_occurred; -} - - -// ***************************************************** -// Function: check_for_steering_faults -// -// Purpose: Checks oscc_status_s struct for steering -// faults -// -// Returns: bool - true if fault occurred -// - false if no fault occurred -// -// Parameters: oscc_status_s - struct containing OSCC status -// -// ***************************************************** -static bool check_for_steering_faults( oscc_status_s * status) -{ - bool fault_occurred = false; - - if( status != NULL ) - { - if ( status->fault_steering_obd_timeout == true ) - { - printf( "Steering - OBD Timeout Detected\n" ); - - fault_occurred = true; - } - - if ( status->fault_steering_invalid_sensor_value == true ) - { - printf( "Steering - Invalid Sensor Value Detected\n" ); - - fault_occurred = true; - } - } - - return fault_occurred; -} - - -// ***************************************************** -// Function: check_for_throttle_faults -// -// Purpose: Checks oscc_status_s struct for throttle -// faults -// -// Returns: bool - true if fault occurred -// - false if no fault occurred -// -// Parameters: oscc_status_s - struct containing OSCC status -// -// ***************************************************** -static bool check_for_throttle_faults( oscc_status_s * status) -{ - bool fault_occurred = false; - - if( status != NULL ) - { - if ( status->fault_throttle_invalid_sensor_value == true ) - { - printf( "Throttle - Invalid Sensor Value Detected\n" ); - - fault_occurred = true; - } - } - - return fault_occurred; -} - - - -// ***************************************************** -// public definitions -// ***************************************************** - -// ***************************************************** -// Function: commander_init -// -// Purpose: Externally visible function to initialize the commander object -// -// Returns: int - ERROR or NOERR -// -// Parameters: channel - for now, the CAN channel to use when interacting -// with the OSCC modules -// -// ***************************************************** -int commander_init( int channel ) -{ - int return_code = ERROR; - - if ( commander_enabled == COMMANDER_DISABLED ) - { - commander_enabled = COMMANDER_ENABLED; - - return_code = oscc_interface_init( channel ); - - if ( return_code != ERROR ) - { - return_code = joystick_init( ); - - printf( "waiting for joystick controls to zero\n" ); - - while ( return_code != ERROR ) - { - return_code = is_joystick_safe( ); - - if ( return_code == UNAVAILABLE ) - { - (void) usleep( JOYSTICK_DELAY_INTERVAL ); - } - else if ( return_code == ERROR ) - { - printf( "Failed to wait for joystick to zero the control values\n" ); - } - else - { - break; - } - } - } - } - return ( return_code ); -} - -// ***************************************************** -// Function: command_close -// -// Purpose: Shuts down all of the other modules that the commander uses -// and closes the commander object -// -// Returns: int - ERROR or NOERR -// -// Parameters: void -// -// ***************************************************** -void commander_close( ) -{ - if ( commander_enabled == COMMANDER_ENABLED ) - { - commander_disable_controls( ); - - oscc_interface_disable( ); - - oscc_interface_close( ); - - joystick_close( ); - - commander_enabled = COMMANDER_DISABLED; - } -} - - -// ***************************************************** -// Function: commander_low_frequency_update -// -// Purpose: Should be run every 50ms -// The commander low-frequency update function polls the joystick, -// converts the joystick input into values that reflect what the -// vehicle should do and sends them to the OSCC interface -// -// Returns: int - ERROR or NOERR -// -// Parameters: void -// -// ***************************************************** -int commander_low_frequency_update( ) -{ - unsigned int button_pressed = 0; - - int return_code = joystick_update( ); - - if ( return_code == NOERR ) - { - return_code = get_button( JOYSTICK_BUTTON_DISABLE_CONTROLS, - &button_pressed ); - - if ( return_code == NOERR ) - { - if ( button_pressed != 0 ) - { - printf( "Disabling Controls\n" ); - return_code = commander_disable_controls( ); - } - else - { - button_pressed = 0; - return_code = get_button( JOYSTICK_BUTTON_ENABLE_CONTROLS, - &button_pressed ); - - if ( return_code == NOERR ) - { - if ( button_pressed != 0 ) - { - return_code = commander_enable_controls( ); - } - else - { - return_code = command_brakes( ); - - if ( return_code == NOERR ) - { - return_code = command_throttle( ); - } - - if ( return_code == NOERR ) - { - return_code = command_steering( ); - } - } - } - } - } - } - return return_code; -} - - -// ***************************************************** -// Function: commander_high_frequency_update -// -// Purpose: Should be run every 1ms (one millisecond) -// Run the high-frequency commander tasks -// Checks the vehicle for override information -// -// Returns: int - ERROR or NOERR -// -// Parameters: void -// -// ***************************************************** -int commander_high_frequency_update( ) -{ - int return_code = ERROR; - - int oscc_override = 0; - - oscc_status_s status; - - memset( &status, - 0, - sizeof(status) ); - - return_code = oscc_interface_update_status( &status ); - - if ( return_code == NOERR ) - { - if ( status.operator_override == true ) - { - printf( "Driver Override Detected\n" ); - return_code = commander_disable_controls( ); - } - - - bool brake_fault_occurred = check_for_brake_faults( &status ); - - if ( brake_fault_occurred == true ) - { - return_code = commander_disable_controls( ); - } - - - bool steering_fault_occurred = check_for_steering_faults( &status ); - - if ( steering_fault_occurred == true ) - { - return_code = commander_disable_controls( ); - } - - - bool throttle_fault_occurred = check_for_throttle_faults( &status ); - - if ( throttle_fault_occurred == true ) - { - return_code = commander_disable_controls( ); - } - } - - return return_code; -} diff --git a/utils/joystick_commander/src/joystick.c b/utils/joystick_commander/src/joystick.c deleted file mode 100644 index dbd3dd67..00000000 --- a/utils/joystick_commander/src/joystick.c +++ /dev/null @@ -1,563 +0,0 @@ -/** - * @file joystick.c - * @brief Joystick Interface Source - * - */ - - - - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "macros.h" -#include "joystick.h" - - - - -// ***************************************************** -// static global types/macros -// ***************************************************** - -/** - * @brief Button press debounce delay. [microseconds] - * - */ -#define BUTTON_PRESSED_DELAY (5000) - -/** - * @brief Invalid \ref joystick_device_s.controller value - * - */ -#define JOYSTICK_DEVICE_CONTROLLER_INVALID ( NULL ) - -/** - * @brief Joystick Identifier Data - * - */ -#define JOYSTICK_ID_DATA_SIZE ( 16 ) - -/** - * @brief Joystick Description String - * - */ -#define JOYSTICK_ID_STRING_SIZE ( 64 ) - - - -// ***************************************************** -// type definitions -// ***************************************************** - - -/** - * @brief Joystick GUID - * - * Implementation-dependent GUID - * - */ -typedef struct -{ - unsigned char data[ JOYSTICK_ID_DATA_SIZE ]; - char ascii_string[ JOYSTICK_ID_STRING_SIZE ]; - -} joystick_guid_s; - - -/** - * @brief Joystick device - * - */ -typedef struct -{ - void *controller; - - void *haptic; - - joystick_guid_s* guid; - -} joystick_device_data_s; - - -// ***************************************************** -// static global data -// ***************************************************** - - -static joystick_guid_s joystick_guid; -static joystick_device_data_s joystick_data = { NULL, &joystick_guid }; -static joystick_device_data_s* joystick = NULL; - - -// ***************************************************** -// static definitions -// ***************************************************** - - -// ***************************************************** -// Function: joystick_init_subsystem -// -// Purpose: Initialize the joystick and game -// controller subsystems -// -// Returns: int - ERROR or NOERROR -// -// Parameters: None -// -// ***************************************************** -static int joystick_init_subsystem( ) -{ - int return_code = ERROR; - - if ( joystick == NULL ) - { - int init_result = SDL_Init( SDL_INIT_GAMECONTROLLER | SDL_INIT_HAPTIC ); - - return_code = NOERR; - - if ( init_result < 0 ) - { - printf( "ERROR: SDL_Init - %s\n", SDL_GetError() ); - return_code = ERROR; - } - } - return ( return_code ); -} - - -// ***************************************************** -// Function: joystick_get_guid_at_index -// -// Purpose: Return the Globally Unique ID (GUID) for the requested joystick -// -// Returns: int - ERROR or NOERROR -// -// Parameters: device_index - index to the requested device -// guid - pointer to the guid to fill out -// -// ***************************************************** -static int joystick_get_guid_at_index( unsigned long device_index ) -{ - int return_code = ERROR; - - if ( joystick != NULL ) - { - return_code = NOERR; - - const SDL_JoystickGUID m_guid = - SDL_JoystickGetDeviceGUID( (int) device_index ); - - memcpy( joystick_guid.data, m_guid.data, sizeof( m_guid.data ) ); - - memset( joystick_guid.ascii_string, 0, - sizeof( joystick_guid.ascii_string ) ); - - SDL_JoystickGetGUIDString( m_guid, - joystick_guid.ascii_string, - sizeof( joystick_guid.ascii_string ) ); - } - return ( return_code ); -} - - -// ***************************************************** -// Function: joystick_get_num_devices -// -// Purpose: Return the number of joystick devices resident on the system -// -// Returns: int - the number of devices or ERROR -// -// Parameters: None -// -// ***************************************************** -static int joystick_get_num_devices( ) -{ - int num_joysticks = ERROR; - - if ( joystick != NULL ) - { - num_joysticks = SDL_NumJoysticks(); - - if ( num_joysticks < 0 ) - { - printf( "ERROR: SDL_NumJoysticks - %s\n", SDL_GetError() ); - num_joysticks = ERROR; - } - } - return ( num_joysticks ); -} - - -// ***************************************************** -// Function: joystick_curve_fit -// -// Purpose: Calculate the logarithmic range for the joystick position -// -// Create the formula: -// output = ( output_range / ( input_range )^2 ) * ( input )^2 -// -// Which is equal to: -// output = output_range * ( input / input_range )^2 -// -// Returns: double - the curve fit value -// -// Parameters: input_min - input range min -// input_max - input range max -// output_min - output range min -// output_max - output range max -// position - current joystick position -// -// ***************************************************** -static double joystick_curve_fit( double input_min, - double input_max, - double output_min, - double output_max, - double position ) -{ - const double input_range = input_max - input_min; - const double output_range = output_max - output_min; - - double input = position - input_min; - double output = 0.0; - - input /= input_range; // input / input_range - input *= input; // ( input / input_range )^2 - - // output = output_range * ( input / input_range )^2 - output = output_range * input; - - output += output_min; // normalize to output range - - return ( output ); -} - - - -// ***************************************************** -// public definitions -// ***************************************************** - - - -// ***************************************************** -// Function: joystick_init_subsystem -// -// Purpose: Initialize the joystick subsystem -// -// Returns: int - ERROR or NOERROR -// -// Parameters: None -// -// ***************************************************** -int joystick_init( ) -{ - int return_code = NOERR; - - return_code = joystick_init_subsystem(); - - if ( return_code == ERROR ) - { - printf("init subsystem error\n"); - } - else - { - joystick = &joystick_data; - joystick->controller = JOYSTICK_DEVICE_CONTROLLER_INVALID; - - const int num_joysticks = joystick_get_num_devices(); - - if ( num_joysticks > 0 ) - { - unsigned long device_index = 0; - - return_code = joystick_get_guid_at_index( device_index ); - - if ( return_code == NOERR ) - { - printf( "Found %d devices -- connecting to device at system index %lu - GUID: %s\n", - num_joysticks, - device_index, - joystick_guid.ascii_string ); - - return_code = joystick_open( device_index ); - } - } - else - { - printf( "No joystick/devices available on the host\n" ); - } - } - -} - - -// ***************************************************** -// Function: joystick_open -// -// Purpose: Open the requested joystick for use -// -// Returns: int - ERROR or NOERROR -// -// Parameters: device_index - index to the requested device -// -// ***************************************************** -int joystick_open( unsigned long device_index ) -{ - int return_code = ERROR; - - if ( joystick != NULL ) - { - joystick->controller = - (void*) SDL_GameControllerOpen( (int) device_index ); - - if ( joystick->controller == JOYSTICK_DEVICE_CONTROLLER_INVALID ) - { - printf( "ERROR: SDL_JoystickOpen - %s\n", SDL_GetError() ); - } - else - { - return_code = NOERR; - - const SDL_JoystickGUID m_guid = - SDL_JoystickGetGUID( - SDL_GameControllerGetJoystick( joystick->controller ) ); - - memcpy( joystick_guid.data, m_guid.data, sizeof( m_guid.data ) ); - - memset( joystick_guid.ascii_string, - 0, - sizeof( joystick_guid.ascii_string ) ); - - SDL_JoystickGetGUIDString( m_guid, - joystick_guid.ascii_string, - sizeof( joystick_guid.ascii_string ) ); - - joystick->haptic = - (void*) SDL_HapticOpenFromJoystick( - SDL_GameControllerGetJoystick( joystick->controller )); - - if ( SDL_HapticRumbleInit( joystick->haptic ) != 0 ) - { - SDL_HapticClose( joystick->haptic ); - } - } - } - return ( return_code ); -} - - -// ***************************************************** -// Function: jstick_close -// -// Purpose: Close the joystick for use -// -// Returns: void -// -// Parameters: void -// -// ***************************************************** -void joystick_close( ) -{ - if ( joystick != NULL ) - { - if ( joystick->controller != JOYSTICK_DEVICE_CONTROLLER_INVALID ) - { - if ( SDL_GameControllerGetAttached( joystick->controller ) == SDL_TRUE ) - { - if ( joystick->haptic ) - { - SDL_HapticClose( joystick->haptic ); - } - SDL_GameControllerClose( joystick->controller ); - } - - joystick->controller = JOYSTICK_DEVICE_CONTROLLER_INVALID; - } - joystick = NULL; - } - // Release the joystick subsystem - SDL_Quit(); -} - - -// ***************************************************** -// Function: joystick_update -// -// Purpose: Update the requested joystick for use -// -// Returns: int - ERROR or NOERROR -// -// Parameters: device_index - index to the requested device -// -// ***************************************************** -int joystick_update( ) -{ - int return_code = ERROR; - - if ( joystick != NULL ) - { - if ( joystick->controller != JOYSTICK_DEVICE_CONTROLLER_INVALID ) - { - SDL_GameControllerUpdate(); - - if ( SDL_GameControllerGetAttached( joystick->controller ) == SDL_FALSE ) - { - printf("SDL_GameControllerGetAttached - device not attached\n"); - } - else - { - return_code = NOERR; - } - } - } - return ( return_code ); -} - - -// ***************************************************** -// Function: joystick_get_axis -// -// Purpose: Get the axis state -// -// Returns: int - ERROR or NOERROR -// -// Parameters: axis_index - index to the axis to use -// position - pointer to the position to update -// -// ***************************************************** -int joystick_get_axis( unsigned long axis_index, int * const position ) -{ - int return_code = ERROR; - - if ( ( joystick != NULL ) && ( position != NULL ) ) - { - return_code = NOERR; - - const Sint16 pos = SDL_GameControllerGetAxis( joystick->controller, - axis_index ); - ( *position ) = (int) pos; - } - - return return_code; -} - - -// ***************************************************** -// Function: joystick_get_button -// -// Purpose: Get which button was pressed for the requested joystick -// -// Returns: int - ERROR or NOERROR -// -// Parameters: button_index - index to the button to use -// button_state - pointer to the button state to update -// -// ***************************************************** -int joystick_get_button( unsigned long button_index, - unsigned int * const button_state ) -{ - int return_code = ERROR; - - if ( ( joystick != NULL ) && ( button_state != NULL ) ) - { - return_code = NOERR; - - const Uint8 m_state = SDL_GameControllerGetButton( joystick->controller, - button_index ); - - if ( m_state == 1 ) - { - ( *button_state ) = JOYSTICK_BUTTON_STATE_PRESSED; - - if ( joystick->haptic ) - { - SDL_HapticRumblePlay( joystick->haptic, 1.0f, 100 ); - } - - ( void ) usleep( BUTTON_PRESSED_DELAY ); - } - else - { - ( *button_state ) = JOYSTICK_BUTTON_STATE_NOT_PRESSED; - } - } - - return return_code; -} - - -// ***************************************************** -// Function: joystick_normalize_axis_position -// -// Purpose: Convert the integer current joystick input position into a -// scaled value for the variable that the joystick input -// represents -// -// Returns: double - the normalized axis position -// -// Parameters: position - current input position on the joystick -// range_min - minimum scaled value for the variable -// range_max - maximum scaled value for the variable -// -// ***************************************************** -double joystick_normalize_axis_position( const int position, - const double range_min, - const double range_max ) -{ - const double input_min = 0.0; - const double output_min = 0.0; - - double input_max = ( double )JOYSTICK_AXIS_POSITION_MAX; - double output_max = range_min; - - if ( position < 0 ) - { - input_max = ( double )JOYSTICK_AXIS_POSITION_MIN; - output_max = range_max; - } - - const double output = joystick_curve_fit( input_min, - input_max, - output_min, - output_max, - ( double )position ); - - return ( output ); -} - - -// ***************************************************** -// Function: joystick_normalize_trigger_position -// -// Purpose: Convert the integer current joystick trigger position -// to a scaled value -// -// Returns: double - the normalized trigger position -// -// Parameters: position - current trigger position on the joystick -// range_min - minimum scaled value for the variable -// range_max - maximum scaled value for the variable -// -// ***************************************************** -double joystick_normalize_trigger_position( const int position, - const double range_min, - const double range_max ) -{ - const double output = joystick_curve_fit( - ( double )JOYSTICK_TRIGGER_POSITION_MIN, - ( double )JOYSTICK_TRIGGER_POSITION_MAX, - range_min, - range_max, - ( double )position ); - - return ( output ); -} - - diff --git a/utils/joystick_commander/src/main.c b/utils/joystick_commander/src/main.c deleted file mode 100644 index e61d6a6e..00000000 --- a/utils/joystick_commander/src/main.c +++ /dev/null @@ -1,204 +0,0 @@ -/** - * @file main.c - * @brief Entry point for the joystick commander application - * - * Joystick device: Logitech Gamepad F310 - * Mode switch (on back of controller): set to mode X - * Front mode button: set to off (LED is off) - * Brake controls: left trigger - * Throttle controls: right trigger - * Steering controls: right stick - * Left turn signal: left trigger button - * Right turn signal: right trigger button - * Shift to drive: 'A' button - * Shift to park: 'Y' button - * Shift to neutral: 'X' button - * Shift to reverse: 'B' button - * Enable controls: 'start' button - * Disable controls: 'back' button - * - */ - - - - -#include -#include -#include -#include -#include -#include - -#include "macros.h" -#include "commander.h" - - - - -// ***************************************************** -// static global types/macros -// ***************************************************** - -/** - * @brief update interval. [microseconds] - * - * Defines the update rate of the low frequency commander tasks - * - * 50,000 us == 50 ms == 20 Hertz - * - */ -#define COMMANDER_UPDATE_INTERVAL (50000) - - -/** - * @brief sleep interval. [microseconds] - * - * Specifies the amount of time to sleep for during each wait/sleep cycle - * - * Defines the update rate for the high frequency commander tasks - * - * Prevents overloading the host CPU - * - */ -#define SLEEP_TICK_INTERVAL (1000) - - - -// ***************************************************** -// static global data -// ***************************************************** - -/** - * @brief Error thrown from SIGINT - * - */ -static int error_thrown = NOERR; - - -// ***************************************************** -// static declarations -// ***************************************************** - - -// ***************************************************** -// static definitions -// ***************************************************** - - -// ***************************************************** -// Function: get_timestamp -// -// Purpose: Get the current timestamp from the system time -// Value is returned in microseconds -// -// Returns: unsigned long long - current time in microseconds -// -// Parameters: void -// -// ***************************************************** -static unsigned long long get_timestamp( ) -{ - unsigned long long microseconds = 0; - struct timespec timespec; - - clock_gettime( CLOCK_REALTIME, ×pec ); - - // convert to microseconds - microseconds = (unsigned long long) - ( 1000000 * ( (unsigned long long) timespec.tv_sec ) ); - - // convert nanosecond remainder to micro seconds - microseconds += (unsigned long long) - ( ( (unsigned long long) timespec.tv_nsec ) / 1000 ); - - return ( microseconds ); -} - - -// ***************************************************** -// Function: get_elapsed_time -// -// Purpose: Determine the elapsed time since the last check -// -// Returns: unsigned long long - the elapsed time in microseconds -// -// Parameters: timestamp - pointer to a timestamp value -// -// ***************************************************** -static unsigned long long get_elapsed_time( unsigned long long timestamp ) -{ - unsigned long long now = get_timestamp( ); - unsigned long long elapsed_time = now - timestamp; - - return elapsed_time; -} - - -// ***************************************************** -// Function: signal_handler -// -// Purpose: Function to catch and handle SIGINT operations -// -// Returns: void -// -// Parameters: signal_number - number of the signal sent -// -// ***************************************************** -void signal_handler( int signal_number ) -{ - if ( signal_number == SIGINT ) - { - error_thrown = ERROR; - } -} - - -// ***************************************************** -// public definitions -// ***************************************************** - - -int main( int argc, char **argv ) -{ - int return_code = NOERR; - unsigned long long update_timestamp = get_timestamp(); - unsigned long long elapsed_time = 0; - - int channel; - - errno = 0; - - if ( argc != 2 || ( channel = atoi( argv[1] ), errno ) != 0 ) - { - printf( "usage %s channel\n", argv[0] ); - exit( 1 ); - } - - signal( SIGINT, signal_handler ); - - return_code = commander_init( channel ); - - if ( return_code == NOERR ) - { - while ( return_code == NOERR && error_thrown == NOERR ) - { - return_code = commander_high_frequency_update( ); - - elapsed_time = get_elapsed_time( update_timestamp ); - - if ( elapsed_time > COMMANDER_UPDATE_INTERVAL ) - { - update_timestamp = get_timestamp(); - return_code = commander_low_frequency_update( ); - } - - // Delay 1 ms to avoid loading the CPU and to time calls - // to commander_high_frequency_update - (void) usleep( SLEEP_TICK_INTERVAL ); - } - commander_close( ); - } - - return 0; -} - diff --git a/utils/joystick_commander/src/oscc_interface.c b/utils/joystick_commander/src/oscc_interface.c deleted file mode 100644 index 16dfbcaa..00000000 --- a/utils/joystick_commander/src/oscc_interface.c +++ /dev/null @@ -1,661 +0,0 @@ -/************************************************************************/ -/* The MIT License (MIT) */ -/* ===================== */ - -/* Copyright (c) 2017 PolySync Technologies, Inc. All Rights Reserved. */ - -/* Permission is hereby granted, free of charge, to any person */ -/* obtaining a copy of this software and associated documentation */ -/* files (the “Software”), to deal in the Software without */ -/* restriction, including without limitation the rights to use, */ -/* copy, modify, merge, publish, distribute, sublicense, and/or sell */ -/* copies of the Software, and to permit persons to whom the */ -/* Software is furnished to do so, subject to the following */ -/* conditions: */ - -/* The above copyright notice and this permission notice shall be */ -/* included in all copies or substantial portions of the Software. */ - -/* THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, */ -/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES */ -/* OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND */ -/* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT */ -/* HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, */ -/* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING */ -/* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR */ -/* OTHER DEALINGS IN THE SOFTWARE. */ -/************************************************************************/ - -/** - * @file oscc_socketcan.c - * @brief OSCC socketcan source- The main command functions and the - * update function should be called on at least a 50ms period. - * The expectation is that if there is not some kind of - * communication from the controller to the OSCC modules in that - * time, then the OSCC modules will disable and return control - * back to the driver. - */ - - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "macros.h" -#include "brake_can_protocol.h" -#include "steering_can_protocol.h" -#include "throttle_can_protocol.h" -#include "gateway_can_protocol.h" - - -// ***************************************************** -// static global types/macros -// ***************************************************** - -/** - * @brief OSCC interface data - container for the various CAN messages - * that are used to control the brakes, steering and throttle. - * In addition, there are additional variables to store the CAN - * parameters, handle and channel. - * - * The entire structure is packed at the single byte level - * because of the need to send it on the wire to a receiver that - * is expecting a specific layout. - */ - -#pragma pack(push) -#pragma pack(1) - -struct oscc_interface_data_s -{ - oscc_command_brake_data_s brake_cmd; - oscc_command_throttle_data_s throttle_cmd; - oscc_command_steering_data_s steering_cmd; - - int can_socket; - int can_port; -}; - -// restore alignment -#pragma pack(pop) - - -// ***************************************************** -// static global data -// ***************************************************** - -static struct oscc_interface_data_s oscc_interface_data; -static struct oscc_interface_data_s* oscc = NULL; - - -// ***************************************************** -// static definitions -// ***************************************************** - -// ***************************************************** -// Function: oscc_can_write -// -// Purpose: Wrapper around the canWrite routine from the CAN library -// -// Returns: int - ERROR or NOERR -// -// Parameters: id - ID of the CAN message ot send -// msg - pointer to the buffer to send -// dlc - size of the buffer -// -// ***************************************************** -static int oscc_can_write( long id, void* msg, unsigned int dlc ) -{ - int return_code = ERROR; - - if ( oscc != NULL ) - { - struct can_frame write_frame; - - write_frame.can_id = id; - write_frame.can_dlc = dlc; - memcpy( write_frame.data, msg, dlc ); - - int result = - write( oscc->can_socket, &write_frame, sizeof( write_frame ) ); - - if ( result > 0 ) - { - return_code = NOERR; - } - } - return return_code; -} - - -// ***************************************************** -// Function: oscc_nonblock_disable -// -// Purpose: Disables the non-blocking option on the socket CAN interface -// -// Returns: int - ERROR or NOERR -// -// Parameters: int handle - handle to use -// -// ***************************************************** -static int oscc_nonblock_disable( int handle ) -{ - int return_code = ERROR; - - if ( handle >= 0 ) - { - int state = fcntl( handle, F_GETFL, 0 ); - - if ( state >= 0 ) - { - state &= ~O_NONBLOCK; - - int result = fcntl( handle, F_SETFL, state ); - - if ( result >= 0 ) - { - return_code = NOERR; - } - } - } - - return return_code; -} - -// ***************************************************** -// Function: oscc_nonblock_enable -// -// Purpose: Enables the non-blocking option on the socket CAN interface -// -// Returns: int - ERROR or NOERR -// -// Parameters: int handle - handle to use -// -// ***************************************************** -static int oscc_nonblock_enable( int handle ) -{ - int return_code = ERROR; - - if ( handle >= 0 ) - { - int state = fcntl( handle, F_GETFL, 0 ); - - if ( state >= 0 ) - { - state |= O_NONBLOCK; - - int result = fcntl( handle, F_SETFL, state ); - - if ( result >= 0 ) - { - return_code = NOERR; - } - } - } - - return return_code; -} - - -// ***************************************************** -// Function: oscc_interface_init_can -// -// Purpose: Initialize the OSCC communication layer with known values -// -// Returns: int - ERROR or NOERR -// -// Parameters: channel - for now, the CAN channel to use when interacting -// with the OSCC modules -// -// ***************************************************** -static int oscc_init_can( const char* can_channel ) -{ - int return_code = ERROR; - - int can_socket = socket( PF_CAN, SOCK_RAW, CAN_RAW ); - - if ( can_socket >= 0 ) - { - struct ifreq ifr; - - strncpy( ifr.ifr_name, can_channel, IFNAMSIZ ); - - int result = ioctl( can_socket, SIOCGIFINDEX, &ifr ); - - if ( result >= 0 ) - { - struct sockaddr_can can_address; - - can_address.can_family = AF_CAN; - can_address.can_ifindex = ifr.ifr_ifindex; - - result = bind( can_socket, - ( struct sockaddr * )&can_address, - sizeof( can_address )); - - if ( result >= 0 ) - { - result = oscc_nonblock_enable( can_socket ); - - if ( result == NOERR ) - { - oscc_interface_data.can_socket = can_socket; - return_code = NOERR; - } - else - { - printf( "Failed to enable nonblocking option\n" ); - } - } - else - { - printf( "Failed to bind to the socket\n" ); - } - } - else - { - printf( "Failed to find the CAN index\n" ); - } - } - else - { - printf( "Failed to open the CAN socket\n" ); - } - - return return_code; -} - -// ***************************************************** -// public definitions -// ***************************************************** - -// ***************************************************** -// Function: oscc_interface_set_defaults -// -// Purpose: Initialize the OSCC communication layer with known values -// -// Returns: int - ERROR or NOERR -// -// Parameters: void -// -// ***************************************************** -int oscc_interface_set_defaults( ) -{ - int return_code = NOERR; - - oscc_command_brake_data_s* brake = &oscc_interface_data.brake_cmd; - - brake->enabled = 0; - brake->pedal_command = 0; - - oscc_command_throttle_data_s* throttle = &oscc_interface_data.throttle_cmd; - - throttle->enabled = 0; - throttle->commanded_accelerator_position = 0; - - oscc_command_steering_data_s* steering = &oscc_interface_data.steering_cmd; - - steering->enabled = 0; - steering->commanded_steering_wheel_angle = 0; - steering->commanded_steering_wheel_angle_rate = 0; - - return ( return_code ); -} - - -// ***************************************************** -// Function: oscc_interface_init -// -// Purpose: Initialize the OSCC interface - CAN communication -// -// Returns: int - ERROR or NOERR -// -// Parameters: channel - integer value containing the CAN channel to openk -// -// ***************************************************** -int oscc_interface_init( int channel ) -{ - int return_code = ERROR; - char buffer[16]; - - oscc_interface_set_defaults(); - - snprintf( buffer, 16, "can%1d", channel ); - - printf( "Opening CAN channel: %s\n", buffer ); - - return_code = oscc_init_can( buffer ); - - if ( return_code == NOERR ) - { - oscc = &oscc_interface_data; - } - return ( return_code ); -} - -// ***************************************************** -// Function: oscc_interface_close -// -// Purpose: Release resources and close the interface -// -// Returns: int - ERROR or NOERR -// -// Parameters: void -// -// ***************************************************** -void oscc_interface_close( ) -{ - if ( oscc != NULL ) - { - int result = close( oscc->can_socket ); - } - - oscc = NULL; -} - -// ***************************************************** -// Function: oscc_interface_enable -// -// Purpose: Cause the initialized interface to enable control of the -// vehicle using the OSCC modules -// -// Returns: int - ERROR or NOERR -// -// Parameters: void -// -// ***************************************************** -int oscc_interface_enable( ) -{ - int return_code = ERROR; - - if ( oscc != NULL ) - { - return_code = NOERR; - - oscc->brake_cmd.enabled = 1; - oscc->throttle_cmd.enabled = 1; - oscc->steering_cmd.enabled = 1; - } - - return ( return_code ); -} - - -// ***************************************************** -// Function: oscc_interface_command_brakes -// -// Purpose: Send a CAN message to set the brakes to a commanded value -// -// Returns: int - ERROR or NOERR -// -// Parameters: brake_setpoint - unsigned value -// The value is range limited between 0 and 52428 -// -// ***************************************************** -int oscc_interface_command_brakes( unsigned int brake_setpoint ) -{ - int return_code = ERROR; - - if ( oscc != NULL ) - { - oscc->brake_cmd.pedal_command = ( uint16_t )brake_setpoint; - - return_code = oscc_can_write( OSCC_COMMAND_BRAKE_CAN_ID, - (void *) &oscc->brake_cmd, - sizeof( oscc_command_brake_data_s ) ); - } - return ( return_code ); -} - - -// ***************************************************** -// Function: oscc_interface_command_throttle -// -// Purpose: Send a CAN message to set the throttle to a commanded value -// -// Returns: int - ERROR or NOERR -// -// Parameters: throttle_setpoint - unsigned value -// The value is range limited between 0 and 19660 -// -// ***************************************************** -int oscc_interface_command_throttle( unsigned int throttle_setpoint ) -{ - int return_code = ERROR; - - if ( oscc != NULL ) - { - oscc->throttle_cmd.commanded_accelerator_position = ( uint16_t )throttle_setpoint; - - return_code = oscc_can_write( OSCC_COMMAND_THROTTLE_CAN_ID, - (void *) &oscc->throttle_cmd, - sizeof( oscc_command_throttle_data_s ) ); - } - - return ( return_code ); -} - - -// ***************************************************** -// Function: oscc_interface_command_steering -// -// Purpose: Send a CAN message to set the steering to a commanded value -// -// Returns: int - ERROR or NOERR -// -// Parameters: angle - signed value: the steering angle in degrees -// rate - unsigned value; the steering rate in degrees/sec -// -// angle is range limited between -4700 to 4700 -// rate is range limited between 20 to 254 -// -// ***************************************************** -int oscc_interface_command_steering( int angle, unsigned int rate ) -{ - int return_code = ERROR; - - if ( oscc != NULL ) - { - oscc->steering_cmd.commanded_steering_wheel_angle = ( int16_t )angle; - oscc->steering_cmd.commanded_steering_wheel_angle_rate = ( uint16_t )rate; - - return_code = oscc_can_write( OSCC_COMMAND_STEERING_CAN_ID, - (void *) &oscc->steering_cmd, - sizeof( oscc_command_steering_data_s ) ); - } - return ( return_code ); -} - - -// ***************************************************** -// Function: oscc_interface_disable_brakes -// -// Purpose: Send a specific CAN message to set the brake enable value -// to 0. Included with this is a safe brake setting -// -// Returns: int - ERROR or NOERR -// -// Parameters: void -// -// ***************************************************** -int oscc_interface_disable_brakes( ) -{ - int return_code = ERROR; - - if ( oscc != NULL ) - { - oscc->brake_cmd.enabled = 0; - - printf( "brake: %d %d\n", oscc->brake_cmd.enabled, - oscc->brake_cmd.pedal_command ); - - return_code = oscc_interface_command_brakes( 0 ); - } - return ( return_code ); -} - - -// ***************************************************** -// Function: oscc_interface_disable_throttle -// -// Purpose: Send a specific CAN message to set the throttle enable value -// to 0. Included with this is a safe throttle setting -// -// Returns: int - ERROR or NOERR -// -// Parameters: void -// -// ***************************************************** -int oscc_interface_disable_throttle( ) -{ - int return_code = ERROR; - - if ( oscc != NULL ) - { - oscc->throttle_cmd.enabled = 0; - - printf( "throttle: %d %d\n", oscc->throttle_cmd.enabled, - oscc->throttle_cmd.commanded_accelerator_position ); - - return_code = oscc_interface_command_throttle( 0 ); - } - return ( return_code ); -} - - -// ***************************************************** -// Function: oscc_interface_disable_steering -// -// Purpose: Send a specific CAN message to set the steering enable value -// to 0. Included with this is a safe steering angle and rate -// -// Returns: int - ERROR or NOERR -// -// Parameters: void -// -// ***************************************************** -int oscc_interface_disable_steering( ) -{ - int return_code = ERROR; - - if ( oscc != NULL ) - { - oscc->steering_cmd.enabled = 0; - - printf( "steering: %d %d %d\n", - oscc->steering_cmd.enabled, - oscc->steering_cmd.commanded_steering_wheel_angle, - oscc->steering_cmd.commanded_steering_wheel_angle_rate ); - - return_code = oscc_interface_command_steering( 0, 0 ); - } - return ( return_code ); -} - - -// ***************************************************** -// Function: oscc_interface_disable -// -// Purpose: Send a series of CAN messages to disable all of the OSCC -// modules. Mostly a wrapper around the existing specific -// disable functions -// -// Returns: int - ERROR or NOERR -// -// Parameters: void -// -// ***************************************************** -int oscc_interface_disable( ) -{ - int return_code = oscc_interface_disable_brakes( ); - - if ( return_code == NOERR ) - { - return_code = oscc_interface_disable_throttle( ); - - if ( return_code == NOERR ) - { - return_code = oscc_interface_disable_steering( ); - } - } - return ( return_code ); -} - - -// ***************************************************** -// Function: oscc_interface_update_status -// -// Purpose: Read CAN messages from the OSCC modules and check for any -// driver overrides -// -// Returns: int - ERROR or NOERR -// -// Parameters: override - pointer to an integer value that is filled out if -// the OSCC modules indicate any override status -// -// ***************************************************** -int oscc_interface_update_status( int* override ) -{ - int return_code = ERROR; - - if ( oscc != NULL ) - { - struct can_frame read_frame; - - int result = read( oscc->can_socket, &read_frame, CAN_MTU ); - - if ( result > 0 ) - { - return_code = NOERR; - - int local_override = 0; - - if ( read_frame.can_id == OSCC_REPORT_BRAKE_CAN_ID ) - { - oscc_report_brake_data_s* report = - ( oscc_report_brake_data_s* )read_frame.data; - - local_override = (int) report->override; - } - else if ( read_frame.can_id == OSCC_REPORT_THROTTLE_CAN_ID ) - { - oscc_report_throttle_data_s* report = - ( oscc_report_throttle_data_s* )read_frame.data; - - local_override = (int) report->override; - } - else if ( read_frame.can_id == OSCC_REPORT_STEERING_CAN_ID ) - { - oscc_report_steering_data_s* report = - ( oscc_report_steering_data_s* )read_frame.data; - - local_override = (int) report->override; - } - - if ( ( *override ) == 0 ) - { - *override = local_override; - } - } - else if ( result == 0 ) - { - // Do Nothing - return_code = NOERR; - } - else if ( errno == EAGAIN || errno == EWOULDBLOCK ) - { - // Do Nothing - return_code = NOERR; - } - } - return return_code; -} -