diff --git a/.github/ISSUE_TEMPLATE.md b/.github/ISSUE_TEMPLATE.md new file mode 100644 index 00000000..2d752171 --- /dev/null +++ b/.github/ISSUE_TEMPLATE.md @@ -0,0 +1,71 @@ + + + + + + + + +### Expected behavior + + +### Actual behavior + + +### Steps to reproduce + + +### Version info +- _What's the output of running `git describe --tags` in your OSCC directory?_ +- _Is this the same version flashed onto the hardware?_ + +### Hardware info + + + + + + +### Proposed feature or changes + + +### Use case + + + + + + +### Documentation expected + + +### Documentation available + + + + diff --git a/Jenkinsfile b/Jenkinsfile index 4e566ff0..265ffe2a 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -22,8 +22,10 @@ node('arduino') { sh 'cd firmware && mkdir build_kia_soul_petrol_unit_tests && cd build_kia_soul_petrol_unit_tests && cmake .. -DKIA_SOUL=ON -DTESTS=ON -DCMAKE_BUILD_TYPE=Release && make run-unit-tests' echo 'Kia Soul Petrol Unit Tests Complete!' }, 'kia soul petrol property-based tests': { - sh 'cd firmware && mkdir build_kia_soul_petrol_property_tests && cd build_kia_soul_petrol_property_tests && cmake .. -DKIA_SOUL=ON -DTESTS=ON -DCMAKE_BUILD_TYPE=Release && make run-property-tests' - echo 'Kia Soul Petrol Property-Based Tests Complete!' + withEnv(["PATH+CARGO=$HOME/.cargo/bin"]) { + sh 'cd firmware && mkdir build_kia_soul_petrol_property_tests && cd build_kia_soul_petrol_property_tests && cmake .. -DKIA_SOUL=ON -DTESTS=ON -DCMAKE_BUILD_TYPE=Release && make run-property-tests' + echo 'Kia Soul Petrol Property-Based Tests Complete!' + } } echo 'Kia Soul Petrol Tests Complete!' } @@ -32,8 +34,10 @@ node('arduino') { sh 'cd firmware && mkdir build_kia_soul_ev_unit_tests && cd build_kia_soul_ev_unit_tests && cmake .. -DKIA_SOUL_EV=ON -DTESTS=ON -DCMAKE_BUILD_TYPE=Release && make run-unit-tests' echo 'Kia Soul EV Unit Tests Complete!' }, 'kia soul ev property-based tests': { - sh 'cd firmware && mkdir build_kia_soul_ev_property_tests && cd build_kia_soul_ev_property_tests && cmake .. -DKIA_SOUL_EV=ON -DTESTS=ON -DCMAKE_BUILD_TYPE=Release && make run-property-tests' - echo 'Kia Soul EV Property-Based Tests Complete!' + withEnv(["PATH+CARGO=$HOME/.cargo/bin"]) { + sh 'cd firmware && mkdir build_kia_soul_ev_property_tests && cd build_kia_soul_ev_property_tests && cmake .. -DKIA_SOUL_EV=ON -DTESTS=ON -DCMAKE_BUILD_TYPE=Release && make run-property-tests' + echo 'Kia Soul EV Property-Based Tests Complete!' + } } echo 'Kia Soul EV Tests Complete!' } @@ -44,4 +48,4 @@ node('arduino') { finally { deleteDir() } -} +} \ No newline at end of file diff --git a/README.md b/README.md index 978412e0..ba4d4431 100644 --- a/README.md +++ b/README.md @@ -82,8 +82,16 @@ mkdir build cd build ``` -To generate Makefiles, tell `cmake` which platform to build firmware for. For example, if you want to build -firmware for the Kia Soul: +To generate Makefiles, tell `cmake` which vehicle to build for by supplying the +appropriate build flag: + +| Vehicle | Flag | +| --------------- | ---------------- | +| Kia Soul Petrol | -DKIA_SOUL=ON | +| Kia Soul EV | -DKIA_SOUL_EV=ON | + + +For example, if you want to build firmware for the petrol Kia Soul: ``` cmake .. -DKIA_SOUL=ON @@ -95,7 +103,7 @@ cmake .. -DKIA_SOUL=ON cmake .. -DKIA_SOUL=ON -DSTEERING_OVERRIDE=OFF ``` -If steering operator overrides remain enabled, the sensitivity can be adjusted by changing the value of the `TORQUE_DIFFERENCE_OVERRIDE_THRESHOLD` in the corresponding vehicle's header file. +If steering operator overrides remain enabled, the sensitivity can be adjusted by changing the value of the `TORQUE_DIFFERENCE_OVERRIDE_THRESHOLD` in the corresponding vehicle's header file. * Lowering this value will make the steering module more sensitive to operator override, but will result in false positives around high-torque areas, such as the mechanical limits of the steering rack or when quickly and rapidly changing direction. * Increasing this value will result in fewer false positives, but will make it more difficult to manually override the wheel. @@ -209,10 +217,15 @@ strange behavior while printing that does not occur otherwise. # Controlling Your Vehicle - an Example Application Now that all your Arduino modules are properly setup, it is time to start sending control commands. + 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. [OSCC Joystick Commander](https://github.com/PolySync/oscc-joystick-commander) +We've also created a ROS node, that uses the OSCC API to interface with the firmware from ROS messages, allowing you to send commands and receive reports in ROS. + +[ROSCCO](https://github.com/PolySync/roscco) + # OSCC API **Open and close CAN channel to OSCC Control CAN.** @@ -273,6 +286,13 @@ In order to receive reports from the modules, your application will need to regi 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. +Each module's reports are described in their respective wiki sections: + +* [Brake (EV)](https://github.com/PolySync/oscc/wiki/Firmware-Brake-%28EV%29#brake-report) +* [Brake (Petrol)](https://github.com/PolySync/oscc/wiki/Firmware-Brake-%28Petrol%29#brake-report) +* [Steering](https://github.com/PolySync/oscc/wiki/Firmware-Steering#steering-report) +* [Throttle](https://github.com/PolySync/oscc/wiki/Firmware-Throttle#throttle-report) + 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. @@ -420,7 +440,7 @@ make run-all-tests # Additional Vehicles & Contributing -OSCC currently has information regarding the Kia Soul PS (2014-2016), but we want to grow! The +OSCC currently has information regarding the Kia Soul PS (2014-2018), 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 diff --git a/api/CMakeLists.txt b/api/CMakeLists.txt new file mode 100644 index 00000000..17de9626 --- /dev/null +++ b/api/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 2.8) + +project(osccapi) + +include(${CMAKE_SOURCE_DIR}/OsccConfig.cmake) + +set(INCLUDES ${CMAKE_SOURCE_DIR}/include ${CMAKE_SOURCE_DIR}/src) +set(SOURCES ${CMAKE_SOURCE_DIR}/src/oscc.c) +set_source_files_properties(SOURCES PROPERTIES LANGUAGE C) + +set(OBJECTS ${PROJECT_NAME}_objects) +set(SHARED_LIB ${PROJECT_NAME}_shared_lib) +set(STATIC_LIB ${PROJECT_NAME}_static_lib) + +# Reuse object files for both shared and static libraries +# rather than recompiling for both +add_library(${OBJECTS} OBJECT ${SOURCES}) +target_include_directories(${OBJECTS} PUBLIC ${INCLUDES}) +set_target_properties(${OBJECTS} PROPERTIES POSITION_INDEPENDENT_CODE 1) + +add_library(${SHARED_LIB} SHARED $) +set_target_properties(${SHARED_LIB} PROPERTIES OUTPUT_NAME ${PROJECT_NAME}) +target_include_directories(${SHARED_LIB} PUBLIC ${INCLUDES}) + +add_library(${STATIC_LIB} STATIC $) +target_include_directories(${STATIC_LIB} PUBLIC ${INCLUDES}) +set_target_properties(${STATIC_LIB} PROPERTIES OUTPUT_NAME ${PROJECT_NAME}) \ No newline at end of file diff --git a/api/README.md b/api/README.md new file mode 100644 index 00000000..222b3381 --- /dev/null +++ b/api/README.md @@ -0,0 +1,218 @@ + +# Getting started with the OSCC API + +There are a few options on how to get started with your own application that +uses the OSCC API. + +## The library approach + +To build both `libosccapi.so` and `libosccapi.a` for the Kia Soul EV, +you can run the following commands on Linux from this directory. +``` +mkdir build && cd build +cmake -DKIA_SOUL_EV=ON .. +make +``` +Substitute `-DKIA_SOUL_EV` for another supported vehicle if the EV isn't your +target. For example `-DKIA_SOUL=ON` will build the libraries with support +for the Kia Soul Petrol instead. + +### Using `libosccapi.so` +Using the shared library for a project that uses the OSCC API is easy! +The following is an example `CMakeLists.txt` for doing just that. This example +is all you need for a project with it's own source file, `main.c`. +``` +cmake_minimum_required(VERSION 2.8) +project(an_example) +set(OSCC_API_INSTALL /path_to/oscc_directory/api) +link_directories(${OSCC_API_INSTALL}/build) +set(OSCC_INCLUDES ${OSCC_API_INSTALL}/include) +set(SOURCES ${CMAKE_SOURCE_DIR}/src/main.c) +add_executable(${PROJECT_NAME} ${SOURCES}) +target_include_directories(${PROJECT_NAME} PUBLIC ${OSCC_INCLUDES}) +target_link_libraries(${PROJECT_NAME} PUBLIC osccapi) +``` + +### Using `libosccapi.a` +Using the static library isn't much different than the shared one, +the CMakeLists.txt just requires the full path to the library as it doesn't +intuit it from a provided link_directory. +``` +cmake_minimum_required(VERSION 2.8) +project(an_example) +set(OSCC_API_INSTALL /path_to/oscc_directory/api) +set(OSCC_INCLUDES ${OSCC_API_INSTALL}/include) +set(SOURCES ${CMAKE_SOURCE_DIR}/src/main.c) +add_executable(${PROJECT_NAME} ${SOURCES}) +target_include_directories(${PROJECT_NAME} PUBLIC ${OSCC_INCLUDES}) +target_link_libraries(${PROJECT_NAME} PUBLIC ${OSCC_API_INSTALL}/build/libosccapi.a) +``` + +## Straight from the sources +Another approach is to just include the OSCC API source code in your project. +Since the OSCC API has no dependencies beyond a Linux system, this is easy too! +Here's an example CMakeLists.txt that takes the straight from the sources +approach. Note that it makes the assumption that we're building for the Kia +Soul Petrol with the `"-DKIA_SOUL=ON"`. + +``` +cmake_minimum_required(VERSION 2.8) +project(an_example) +set(OSCC_API_INSTALL /path_to/oscc_directory/api) +set(OSCC_INCLUDES ${OSCC_API_INSTALL}/include ${OSCC_API_INSTALL}/src) +set(SOURCES ${CMAKE_SOURCE_DIR}/src/main.c ${OSCC_API_INSTALL}/src/oscc.c) +add_executable(${PROJECT_NAME} ${SOURCES}) +target_compile_definitions(${PROJECT_NAME} PUBLIC "-DKIA_SOUL=ON") +target_include_directories(${PROJECT_NAME} PUBLIC ${OSCC_INCLUDES}) +``` + +## Using the API + +Let's walk through actually writing some code that uses the OSCC API, an example +program that listens to all the information that OSCC reports. Feel free to +experiment with this code, if it's all put into a `main.c` you can use +any of the CMakeLists.txt options above to build it. + +First, we'll do some generic setup. This isn't a requriement of the OSCC API +but might make our example a little more useful. + +``` +#include // printf +#include // SIGINT +#include // usleep +``` +Okay, now for the actual OSCC API include that allows us to initiate, +listen to each message type and then properly release OSCC. `oscc.h` is +all you need! + +``` +#include "oscc.h" // oscc_open, oscc_disable, oscc_close and each subscribe. +``` +In order for our example to trigger the proper cleanup when we press `Ctrl+C` +to quit the program, we'll setup a signal handler to let us know. + +``` +// Global indication of Ctrl+C. +static int EXIT_SIGNALED = OSCC_OK; + +// Help exiting gracefully. +void signal_handler(int signal_number) +{ + if (signal_number == SIGINT) + { + EXIT_SIGNALED = OSCC_ERROR; + } +} +``` + +Now lets walk through our OSCC message subscribers. When we get a report from +something OSCC provides, let's print a message reporting what we see. + +__Throttle__ +``` +void throttle_callback(oscc_throttle_report_s * report) +{ + printf("Hello from throttle_callback!\n"); +} +``` +__Steering__ +``` +void steering_callback(oscc_steering_report_s * report) +{ + printf("steering_callback reporting!\n"); +} +``` +__Brake__ +``` +void brake_callback(oscc_brake_report_s * report) +{ + printf("'Brake-ing' news from the brake_callback!\n"); +} +``` +__Fault__ +``` +void fault_callback(oscc_fault_report_s * report) +{ + printf("Oh! Something caused a fault and we made it to the fault_callback. We're seeing a "); + + if (report->fault_origin_id == FAULT_ORIGIN_BRAKE) + { + printf("brake fault!\n"); + } + else if (report->fault_origin_id == FAULT_ORIGIN_STEERING) + { + printf("steering fault!\n"); + } + else if (report->fault_origin_id == FAULT_ORIGIN_THROTTLE) + { + printf("throttle fault!\n"); + } +} +``` +__OBD__ +``` +static void obd_callback(struct can_frame * frame) +{ + printf("From the obd_callback, CAN ID: %x\n", frame->can_id); +} +``` +Okay, we're all setup! Let get to work! Our code base is pretty small so +we'll just stick to the `main` function +``` +int main +{ +``` +Now let's put that 'Ctrl+C' setup we did earlier to use. +``` + struct sigaction sig; + sig.sa_handler = signal_handler; + sigaction(SIGINT, &sig, NULL); +``` +Next we need to tell OSCC where to to send it's messages. +``` + oscc_subscribe_to_obd_messages(obd_callback); + oscc_subscribe_to_brake_reports(brake_callback); + oscc_subscribe_to_steering_reports(steering_callback); + oscc_subscribe_to_throttle_reports(throttle_callback); + oscc_subscribe_to_fault_reports(fault_callback); +``` +In order to verify that initalizing OSCC succeeds and clean things up +if we fail, we need a way to capture results. +``` + oscc_result_t return_value; +``` +For brevity, we'll assume that we're opening CAN channel 0 to connect to OSCC +and spin it up! +``` + int channel = 0; + return_value = oscc_open(channel); +``` +Since we captured the result of out call to oscc_open, we can verify it +succeded before moving on or signal that we need to clean things up. +``` + if(return_value != OSCC_OK) + { + printf("Error in oscc_open!\n"); + EXIT_SIGNALED = OSCC_ERROR; + } +``` + +Now we wait, just let the callbacks to their work. To avoid loading the +CPU with this example, we'll sleep for a bit (50ms) on each iteration. +``` + while(EXIT_SIGNALED == OSCC_OK) + { + (void) usleep(50000); + } +``` + +Once we're through, we can clean things up. +``` + oscc_disable(); + oscc_close(channel); +``` + +Don't forget to close up the `main` function! +``` +} +``` \ No newline at end of file diff --git a/api/include/can_protocols/brake_can_protocol.h b/api/include/can_protocols/brake_can_protocol.h index 78815aa2..8c055619 100644 --- a/api/include/can_protocols/brake_can_protocol.h +++ b/api/include/can_protocols/brake_can_protocol.h @@ -13,29 +13,36 @@ #include "magic.h" +/* + * @brief CAN ID representing the range of brake messages. + * + */ +#define OSCC_BRAKE_CAN_ID_INDEX (0x70) + + /* * @brief Brake enable message (CAN frame) ID. * */ -#define OSCC_BRAKE_ENABLE_CAN_ID (0x50) +#define OSCC_BRAKE_ENABLE_CAN_ID (0x70) /* * @brief Brake disable message (CAN frame) ID. * */ -#define OSCC_BRAKE_DISABLE_CAN_ID (0x51) +#define OSCC_BRAKE_DISABLE_CAN_ID (0x71) /* * @brief Brake command message (CAN frame) ID. * */ -#define OSCC_BRAKE_COMMAND_CAN_ID (0x60) +#define OSCC_BRAKE_COMMAND_CAN_ID (0x72) /* * @brief Brake report message (CAN frame) ID. * */ -#define OSCC_BRAKE_REPORT_CAN_ID (0x61) +#define OSCC_BRAKE_REPORT_CAN_ID (0x73) /* * @brief Brake report message (CAN frame) length. @@ -50,16 +57,20 @@ #define OSCC_BRAKE_REPORT_PUBLISH_FREQ_IN_HZ (50) /* - * @brief Brake DTC bitfield position indicating an invalid sensor value. + * @brief Enumeration of all possible brake DTCs. * */ -#define OSCC_BRAKE_DTC_INVALID_SENSOR_VAL (0x0) +enum +{ + /* DTC bitfield position indicating an invalid sensor value. */ + OSCC_BRAKE_DTC_INVALID_SENSOR_VAL = 0, -/* - * @brief Brake DTC bitfield position indicating an operator override. - * - */ -#define OSCC_BRAKE_DTC_OPERATOR_OVERRIDE (0x1) + /* DTC bitfield position indicating an operator override. */ + OSCC_BRAKE_DTC_OPERATOR_OVERRIDE, + + /* Number of possible brake DTCs. */ + OSCC_BRAKE_DTC_COUNT +}; #pragma pack(push) @@ -109,17 +120,10 @@ typedef struct * Byte 0 should be \ref OSCC_MAGIC_BYTE_0. * Byte 1 should be \ref OSCC_MAGIC_BYTE_1. */ -#if defined(KIA_SOUL) - uint16_t pedal_command; /*!< Pedal command. [65535 == 100%] */ - - uint8_t reserved[4]; /*!< Reserved. */ -#elif defined(KIA_SOUL_EV) - 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. */ + float pedal_command; /* Brake Request 0.0 to 1.0 where 1.0 is 100% */ uint8_t reserved[2]; /*!< Reserved. */ -#endif + } oscc_brake_command_s; diff --git a/api/include/can_protocols/fault_can_protocol.h b/api/include/can_protocols/fault_can_protocol.h index f8922174..f6f3e282 100644 --- a/api/include/can_protocols/fault_can_protocol.h +++ b/api/include/can_protocols/fault_can_protocol.h @@ -12,11 +12,18 @@ #include #include "magic.h" + +/* + * @brief CAN ID representing the range of global messages. + * + */ +#define OSCC_FAULT_CAN_ID_INDEX (0xA0) + /* * @brief Fault report message (CAN frame) ID. * */ -#define OSCC_FAULT_REPORT_CAN_ID (0x99) +#define OSCC_FAULT_REPORT_CAN_ID (0xAF) /* * @brief Fault report message (CAN frame) length. diff --git a/api/include/can_protocols/steering_can_protocol.h b/api/include/can_protocols/steering_can_protocol.h index 34dde26c..27a374d2 100644 --- a/api/include/can_protocols/steering_can_protocol.h +++ b/api/include/can_protocols/steering_can_protocol.h @@ -13,29 +13,36 @@ #include "magic.h" +/* + * @brief CAN ID representing the range of steering messages. + * + */ +#define OSCC_STEERING_CAN_ID_INDEX (0x80) + + /* * @brief Steering enable message (CAN frame) ID. * */ -#define OSCC_STEERING_ENABLE_CAN_ID (0x54) +#define OSCC_STEERING_ENABLE_CAN_ID (0x80) /* * @brief Steering disable message (CAN frame) ID. * */ -#define OSCC_STEERING_DISABLE_CAN_ID (0x55) +#define OSCC_STEERING_DISABLE_CAN_ID (0x81) /* * @brief Steering command message (CAN frame) ID. * */ -#define OSCC_STEERING_COMMAND_CAN_ID (0x64) +#define OSCC_STEERING_COMMAND_CAN_ID (0x82) /* * @brief Steering report message (CAN frame) ID. * */ -#define OSCC_STEERING_REPORT_CAN_ID (0x65) +#define OSCC_STEERING_REPORT_CAN_ID (0x83) /* * @brief Steering report message (CAN frame) length. @@ -50,16 +57,20 @@ #define OSCC_REPORT_STEERING_PUBLISH_FREQ_IN_HZ (50) /* - * @brief Steering DTC bitfield position indicating an invalid sensor value. + * @brief Enumeration of all possible steering DTCs. * */ -#define OSCC_STEERING_DTC_INVALID_SENSOR_VAL (0x0) +enum +{ + /* DTC bitfield position indicating an invalid sensor value. */ + OSCC_STEERING_DTC_INVALID_SENSOR_VAL = 0, -/* - * @brief Steering DTC bitfield position indicating an operator override. - * - */ -#define OSCC_STEERING_DTC_OPERATOR_OVERRIDE (0x1) + /* DTC bitfield position indicating an operator override. */ + OSCC_STEERING_DTC_OPERATOR_OVERRIDE, + + /* Number of possible steering DTCs. */ + OSCC_STEERING_DTC_COUNT +}; #pragma pack(push) @@ -109,11 +120,13 @@ typedef struct * 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. */ + float torque_command; /* Steering torque request [-1.0, 1.0] where -1.0 is + * max torque in the counterclockwise direction and 1 + * is max torque in the clockwise direction. + * (Note: this is inverse to standard torque direction) + */ - uint8_t reserved[2]; /*!< Reserved. */ + uint8_t reserved[2]; /*!< Reserved. */ } oscc_steering_command_s; diff --git a/api/include/can_protocols/throttle_can_protocol.h b/api/include/can_protocols/throttle_can_protocol.h index 4eb82200..8319f116 100644 --- a/api/include/can_protocols/throttle_can_protocol.h +++ b/api/include/can_protocols/throttle_can_protocol.h @@ -13,29 +13,36 @@ #include "magic.h" +/* + * @brief CAN ID representing the range of throttle messages. + * + */ +#define OSCC_THROTTLE_CAN_ID_INDEX (0x90) + + /* * @brief Throttle enable message (CAN frame) ID. * */ -#define OSCC_THROTTLE_ENABLE_CAN_ID (0x52) +#define OSCC_THROTTLE_ENABLE_CAN_ID (0x90) /* * @brief Throttle disable message (CAN frame) ID. * */ -#define OSCC_THROTTLE_DISABLE_CAN_ID (0x53) +#define OSCC_THROTTLE_DISABLE_CAN_ID (0x91) /* * @brief Throttle command message (CAN frame) ID. * */ -#define OSCC_THROTTLE_COMMAND_CAN_ID (0x62) +#define OSCC_THROTTLE_COMMAND_CAN_ID (0x92) /* * @brief Throttle report message (CAN frame) ID. * */ -#define OSCC_THROTTLE_REPORT_CAN_ID (0x63) +#define OSCC_THROTTLE_REPORT_CAN_ID (0x93) /* * @brief Throttle report message (CAN frame) length. @@ -50,16 +57,20 @@ #define OSCC_REPORT_THROTTLE_PUBLISH_FREQ_IN_HZ (50) /* - * @brief Throttle DTC bitfield position indicating an invalid sensor value. + * @brief Enumeration of all possible throttle DTCs. * */ -#define OSCC_THROTTLE_DTC_INVALID_SENSOR_VAL (0x0) +enum +{ + /* DTC bitfield position indicating an invalid sensor value. */ + OSCC_THROTTLE_DTC_INVALID_SENSOR_VAL = 0, -/* - * @brief Throttle DTC bitfield position indicating an operator override. - * - */ - #define OSCC_THROTTLE_DTC_OPERATOR_OVERRIDE (0x1) + /* DTC bitfield position indicating an operator override. */ + OSCC_THROTTLE_DTC_OPERATOR_OVERRIDE, + + /* Number of possible throttle DTCs. */ + OSCC_THROTTLE_DTC_COUNT +}; #pragma pack(push) @@ -109,9 +120,7 @@ typedef struct * 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. */ + float torque_request; /* Torque request from 0.0 to 1.0 where 1.0 is 100% */ uint8_t reserved[2]; /*!< Reserved. */ } oscc_throttle_command_s; diff --git a/api/include/oscc.h b/api/include/oscc.h index 39c06314..a9126bdb 100644 --- a/api/include/oscc.h +++ b/api/include/oscc.h @@ -177,4 +177,3 @@ oscc_result_t oscc_subscribe_to_obd_messages( void( *callback )( struct can_fram #endif /* _OSCC_H */ - diff --git a/api/include/vehicles.h b/api/include/vehicles.h index 32439604..1fca1dbf 100644 --- a/api/include/vehicles.h +++ b/api/include/vehicles.h @@ -15,5 +15,7 @@ #include "vehicles/kia_soul_ev.h" #endif +#define CONSTRAIN(amt, low, high) ((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt))) + #endif /* _OSCC_VEHICLES_H_ */ diff --git a/api/include/vehicles/kia_soul_ev.h b/api/include/vehicles/kia_soul_ev.h index 675c95e9..3a1cc774 100644 --- a/api/include/vehicles/kia_soul_ev.h +++ b/api/include/vehicles/kia_soul_ev.h @@ -93,12 +93,25 @@ typedef struct } kia_soul_obd_brake_pressure_data_s; + + +// **************************************************************************** +// VEHICLE AND BOARD PARAMETERS +// **************************************************************************** + /* * @brief Number of steps per volt corresponding to 4096 steps (2^12) across 5 volts. * */ #define STEPS_PER_VOLT ( 819.2 ) +/* + * @brief Length of time in ms for delay of signal reads to ensure fault is + * outside the range of noise in the signal. + * + */ +#define FAULT_HYSTERESIS ( 100 ) + diff --git a/api/include/vehicles/kia_soul_petrol.dbc b/api/include/vehicles/kia_soul_petrol.dbc deleted file mode 100644 index b033effd..00000000 --- a/api/include/vehicles/kia_soul_petrol.dbc +++ /dev/null @@ -1,108 +0,0 @@ -VERSION "" - -NS_ : - BA_ - BA_DEF_ - BA_DEF_DEF_ - BA_DEF_DEF_REL_ - BA_DEF_REL_ - BA_DEF_SGTYPE_ - BA_REL_ - BA_SGTYPE_ - BO_TX_BU_ - BU_BO_REL_ - BU_EV_REL_ - BU_SG_REL_ - CAT_ - CAT_DEF_ - CM_ - ENVVAR_DATA_ - EV_DATA_ - FILTER - NS_DESC_ - SGTYPE_ - SGTYPE_VAL_ - SG_MUL_VAL_ - SIGTYPE_VALTYPE_ - SIG_GROUP_ - SIG_TYPE_REF_ - SIG_VALTYPE_ - VAL_ - VAL_TABLE_ - -BS_: - -BU_: BRAKE STEERING THROTTLE FAULT - -BO_ 80 BRAKE_ENABLE: 8 BRAKE - SG_ BRAKE_ENABLE_magic : 0|16@1+ (1,0) [0|0] "" BRAKE - SG_ BRAKE_ENABLE_reserved : 16|48@1+ (1,0) [0|0] "" BRAKE - -BO_ 81 BRAKE_DISABLE: 8 BRAKE - SG_ BRAKE_DISABLE_magic : 0|16@1+ (1,0) [0|0] "" BRAKE - SG_ BRAKE_DISABLE_reserved : 16|48@1+ (1,0) [0|0] "" BRAKE - -BO_ 96 BRAKE_COMMAND: 8 BRAKE - SG_ BRAKE_COMMAND_magic : 0|16@1+ (1,0) [0|0] "" BRAKE - SG_ BRAKE_COMMAND_pedal_command : 16|16@1+ (1,0) [0|0] "" BRAKE - SG_ BRAKE_COMMAND_reserved : 32|32@1+ (1,0) [0|0] "" BRAKE - -BO_ 97 BRAKE_REPORT: 8 BRAKE - SG_ BRAKE_REPORT_magic : 0|16@1+ (1,0) [0|0] "" BRAKE - SG_ BRAKE_REPORT_enabled : 16|8@1+ (1,0) [0|0] "" BRAKE - SG_ BRAKE_REPORT_operator_override : 24|8@1+ (1,0) [0|0] "" BRAKE - SG_ BRAKE_REPORT_dtcs : 32|8@1+ (1,0) [0|0] "" BRAKE - SG_ BRAKE_REPORT_reserved : 40|24@1+ (1,0) [0|0] "" BRAKE - -BO_ 84 STEERING_ENABLE: 8 STEERING - SG_ STEERING_ENABLE_magic : 0|16@1+ (1,0) [0|0] "" STEERING - SG_ STEERING_ENABLE_reserved : 16|48@1+ (1,0) [0|0] "" STEERING - -BO_ 85 STEERING_DISABLE: 8 STEERING - SG_ STEERING_DISABLE_magic : 0|16@1+ (1,0) [0|0] "" STEERING - SG_ STEERING_DISABLE_reserved : 16|48@1+ (1,0) [0|0] "" STEERING - -BO_ 100 STEERING_COMMAND: 8 STEERING - SG_ STEERING_COMMAND_magic : 0|16@1+ (1,0) [0|0] "" STEERING - SG_ STEERING_COMMAND_spoof_low : 16|16@1+ (1,0) [0|0] "" STEERING - SG_ STEERING_COMMAND_spoof_high : 32|16@1+ (1,0) [0|0] "" STEERING - SG_ STEERING_COMMAND_reserved : 48|16@1+ (1,0) [0|0] "" STEERING - -BO_ 101 STEERING_REPORT: 8 STEERING - SG_ STEERING_REPORT_magic : 0|16@1+ (1,0) [0|0] "" STEERING - SG_ STEERING_REPORT_enabled : 16|8@1+ (1,0) [0|0] "" STEERING - SG_ STEERING_REPORT_operator_override : 24|8@1+ (1,0) [0|0] "" STEERING - SG_ STEERING_REPORT_dtcs : 32|8@1+ (1,0) [0|0] "" STEERING - SG_ STEERING_REPORT_reserved : 40|24@1+ (1,0) [0|0] "" STEERING - -BO_ 82 THROTTLE_ENABLE: 8 THROTTLE - SG_ THROTTLE_ENABLE_magic : 0|16@1+ (1,0) [0|0] "" THROTTLE - SG_ THROTTLE_ENABLE_reserved : 16|48@1+ (1,0) [0|0] "" THROTTLE - -BO_ 83 THROTTLE_DISABLE: 8 THROTTLE - SG_ THROTTLE_DISABLE_magic : 0|16@1+ (1,0) [0|0] "" THROTTLE - SG_ THROTTLE_DISABLE_reserved : 16|48@1+ (1,0) [0|0] "" THROTTLE - -BO_ 98 THROTTLE_COMMAND: 8 THROTTLE - SG_ THROTTLE_COMMAND_magic : 0|16@1+ (1,0) [0|0] "" THROTTLE - SG_ THROTTLE_COMMAND_spoof_low : 16|16@1+ (1,0) [0|0] "" THROTTLE - SG_ THROTTLE_COMMAND_spoof_high : 32|16@1+ (1,0) [0|0] "" THROTTLE - SG_ THROTTLE_COMMAND_reserved : 48|16@1+ (1,0) [0|0] "" THROTTLE - -BO_ 99 THROTTLE_REPORT: 8 THROTTLE - SG_ THROTTLE_REPORT_magic : 0|16@1+ (1,0) [0|0] "" THROTTLE - SG_ THROTTLE_REPORT_enabled : 16|8@1+ (1,0) [0|0] "" THROTTLE - SG_ THROTTLE_REPORT_operator_override : 24|8@1+ (1,0) [0|0] "" THROTTLE - SG_ THROTTLE_REPORT_dtcs : 32|8@1+ (1,0) [0|0] "" THROTTLE - SG_ THROTTLE_REPORT_reserved : 40|24@1+ (1,0) [0|0] "" THROTTLE - -BO_ 153 FAULT_REPORT: 8 FAULT - SG_ FAULT_REPORT_magic : 0|16@1+ (1,0) [0|0] "" FAULT - SG_ FAULT_REPORT_fault_origin_id : 16|32@1+ (1,0) [0|0] "" FAULT - SG_ FAULT_REPORT_dtcs : 48|8@1+ (1,0) [0|0] "" FAULT - SG_ FAULT_REPORT_reserved : 56|8@1+ (1,0) [0|0] "" FAULT - -CM_ BU_ BRAKE "The OSCC brake module"; -CM_ BU_ STEERING "The OSCC steering module"; -CM_ BU_ THROTTLE "The OSCC throttle module"; -CM_ BU_ FAULT "The OSCC fault report"; diff --git a/api/include/vehicles/kia_soul_petrol.h b/api/include/vehicles/kia_soul_petrol.h index fb75180b..5a411b58 100644 --- a/api/include/vehicles/kia_soul_petrol.h +++ b/api/include/vehicles/kia_soul_petrol.h @@ -93,12 +93,27 @@ typedef struct } kia_soul_obd_brake_pressure_data_s; + + +// **************************************************************************** +// VEHICLE AND BOARD PARAMETERS +// **************************************************************************** + /* * @brief Number of steps per volt corresponding to 4096 steps (2^12) across 5 volts. * */ #define STEPS_PER_VOLT ( 819.2 ) +/* + * @brief Length of time in ms for delay of signal reads to ensure fault is + * outside the range of noise in the signal. + * + */ +#define FAULT_HYSTERESIS ( 100 ) + + + // **************************************************************************** // BRAKE MODULE @@ -108,13 +123,13 @@ typedef struct * @brief Minimum allowable brake value. * */ -#define MINIMUM_BRAKE_COMMAND ( 0 ) +#define MINIMUM_BRAKE_COMMAND ( 0.0 ) /* * @brief Maximum allowable brake value. * */ -#define MAXIMUM_BRAKE_COMMAND ( 52428 ) +#define MAXIMUM_BRAKE_COMMAND ( 1.0 ) /* * @brief Calculation to convert a brake position to a pedal position. diff --git a/api/include/vehicles/kia_soul_ev.dbc b/api/include/vehicles/oscc.dbc similarity index 75% rename from api/include/vehicles/kia_soul_ev.dbc rename to api/include/vehicles/oscc.dbc index 87304cf1..b5e94403 100644 --- a/api/include/vehicles/kia_soul_ev.dbc +++ b/api/include/vehicles/oscc.dbc @@ -34,70 +34,67 @@ BS_: BU_: BRAKE STEERING THROTTLE FAULT -BO_ 80 BRAKE_ENABLE: 8 BRAKE +BO_ 112 BRAKE_ENABLE: 8 BRAKE SG_ BRAKE_ENABLE_magic : 0|16@1+ (1,0) [0|0] "" BRAKE SG_ BRAKE_ENABLE_reserved : 16|48@1+ (1,0) [0|0] "" BRAKE -BO_ 81 BRAKE_DISABLE: 8 BRAKE +BO_ 113 BRAKE_DISABLE: 8 BRAKE SG_ BRAKE_DISABLE_magic : 0|16@1+ (1,0) [0|0] "" BRAKE SG_ BRAKE_DISABLE_reserved : 16|48@1+ (1,0) [0|0] "" BRAKE -BO_ 96 BRAKE_COMMAND: 8 BRAKE +BO_ 114 BRAKE_COMMAND: 8 BRAKE SG_ BRAKE_COMMAND_magic : 0|16@1+ (1,0) [0|0] "" BRAKE - SG_ BRAKE_COMMAND_spoof_low : 16|16@1+ (1,0) [0|0] "" BRAKE - SG_ BRAKE_COMMAND_spoof_high : 32|16@1+ (1,0) [0|0] "" BRAKE + SG_ BRAKE_COMMAND_pedal_command : 16|32@1+ (1,0) [0|0] "" BRAKE SG_ BRAKE_COMMAND_reserved : 48|16@1+ (1,0) [0|0] "" BRAKE -BO_ 97 BRAKE_REPORT: 8 BRAKE +BO_ 115 BRAKE_REPORT: 8 BRAKE SG_ BRAKE_REPORT_magic : 0|16@1+ (1,0) [0|0] "" BRAKE SG_ BRAKE_REPORT_enabled : 16|8@1+ (1,0) [0|0] "" BRAKE SG_ BRAKE_REPORT_operator_override : 24|8@1+ (1,0) [0|0] "" BRAKE SG_ BRAKE_REPORT_dtcs : 32|8@1+ (1,0) [0|0] "" BRAKE SG_ BRAKE_REPORT_reserved : 40|24@1+ (1,0) [0|0] "" BRAKE -BO_ 84 STEERING_ENABLE: 8 STEERING +BO_ 128 STEERING_ENABLE: 8 STEERING SG_ STEERING_ENABLE_magic : 0|16@1+ (1,0) [0|0] "" STEERING SG_ STEERING_ENABLE_reserved : 16|48@1+ (1,0) [0|0] "" STEERING -BO_ 85 STEERING_DISABLE: 8 STEERING +BO_ 129 STEERING_DISABLE: 8 STEERING SG_ STEERING_DISABLE_magic : 0|16@1+ (1,0) [0|0] "" STEERING SG_ STEERING_DISABLE_reserved : 16|48@1+ (1,0) [0|0] "" STEERING -BO_ 100 STEERING_COMMAND: 8 STEERING +BO_ 130 STEERING_COMMAND: 8 STEERING SG_ STEERING_COMMAND_magic : 0|16@1+ (1,0) [0|0] "" STEERING - SG_ STEERING_COMMAND_spoof_low : 16|16@1+ (1,0) [0|0] "" STEERING - SG_ STEERING_COMMAND_spoof_high : 32|16@1+ (1,0) [0|0] "" STEERING + SG_ STEERING_COMMAND_pedal_command : 16|32@1+ (1,0) [0|0] "" STEERING SG_ STEERING_COMMAND_reserved : 48|16@1+ (1,0) [0|0] "" STEERING -BO_ 101 STEERING_REPORT: 8 STEERING +BO_ 131 STEERING_REPORT: 8 STEERING SG_ STEERING_REPORT_magic : 0|16@1+ (1,0) [0|0] "" STEERING SG_ STEERING_REPORT_enabled : 16|8@1+ (1,0) [0|0] "" STEERING SG_ STEERING_REPORT_operator_override : 24|8@1+ (1,0) [0|0] "" STEERING SG_ STEERING_REPORT_dtcs : 32|8@1+ (1,0) [0|0] "" STEERING SG_ STEERING_REPORT_reserved : 40|24@1+ (1,0) [0|0] "" STEERING -BO_ 82 THROTTLE_ENABLE: 8 THROTTLE +BO_ 144 THROTTLE_ENABLE: 8 THROTTLE SG_ THROTTLE_ENABLE_magic : 0|16@1+ (1,0) [0|0] "" THROTTLE SG_ THROTTLE_ENABLE_reserved : 16|48@1+ (1,0) [0|0] "" THROTTLE -BO_ 83 THROTTLE_DISABLE: 8 THROTTLE +BO_ 145 THROTTLE_DISABLE: 8 THROTTLE SG_ THROTTLE_DISABLE_magic : 0|16@1+ (1,0) [0|0] "" THROTTLE SG_ THROTTLE_DISABLE_reserved : 16|48@1+ (1,0) [0|0] "" THROTTLE -BO_ 98 THROTTLE_COMMAND: 8 THROTTLE +BO_ 146 THROTTLE_COMMAND: 8 THROTTLE SG_ THROTTLE_COMMAND_magic : 0|16@1+ (1,0) [0|0] "" THROTTLE - SG_ THROTTLE_COMMAND_spoof_low : 16|16@1+ (1,0) [0|0] "" THROTTLE - SG_ THROTTLE_COMMAND_spoof_high : 32|16@1+ (1,0) [0|0] "" THROTTLE + SG_ THROTTLE_COMMAND_pedal_command : 16|32@1+ (1,0) [0|0] "" THROTTLE SG_ THROTTLE_COMMAND_reserved : 48|16@1+ (1,0) [0|0] "" THROTTLE -BO_ 99 THROTTLE_REPORT: 8 THROTTLE +BO_ 147 THROTTLE_REPORT: 8 THROTTLE SG_ THROTTLE_REPORT_magic : 0|16@1+ (1,0) [0|0] "" THROTTLE SG_ THROTTLE_REPORT_enabled : 16|8@1+ (1,0) [0|0] "" THROTTLE SG_ THROTTLE_REPORT_operator_override : 24|8@1+ (1,0) [0|0] "" THROTTLE SG_ THROTTLE_REPORT_dtcs : 32|8@1+ (1,0) [0|0] "" THROTTLE SG_ THROTTLE_REPORT_reserved : 40|24@1+ (1,0) [0|0] "" THROTTLE -BO_ 153 FAULT_REPORT: 8 FAULT +BO_ 175 FAULT_REPORT: 8 FAULT SG_ FAULT_REPORT_magic : 0|16@1+ (1,0) [0|0] "" FAULT SG_ FAULT_REPORT_fault_origin_id : 16|32@1+ (1,0) [0|0] "" FAULT SG_ FAULT_REPORT_dtcs : 48|8@1+ (1,0) [0|0] "" FAULT diff --git a/api/src/oscc.c b/api/src/oscc.c index c700ebbc..e39a557d 100644 --- a/api/src/oscc.c +++ b/api/src/oscc.c @@ -107,43 +107,7 @@ oscc_result_t oscc_publish_brake_position( double brake_position ) .magic[1] = ( uint8_t ) OSCC_MAGIC_BYTE_1 }; -#if defined(KIA_SOUL) - const double clamped_position = ( double ) CONSTRAIN ( - brake_position * MAXIMUM_BRAKE_COMMAND, - MINIMUM_BRAKE_COMMAND, - MAXIMUM_BRAKE_COMMAND ); - - brake_cmd.pedal_command = ( uint16_t ) BRAKE_POSITION_TO_PEDAL( clamped_position ); - -#elif defined(KIA_SOUL_EV) - const double clamped_position = CONSTRAIN( - brake_position, - MINIMUM_BRAKE_COMMAND, - MAXIMUM_BRAKE_COMMAND); - - - double spoof_voltage_low = BRAKE_POSITION_TO_VOLTS_LOW( clamped_position ); - - spoof_voltage_low = CONSTRAIN( - spoof_voltage_low, - BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN, - BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX); - - - double spoof_voltage_high = BRAKE_POSITION_TO_VOLTS_HIGH( clamped_position ); - - spoof_voltage_high = CONSTRAIN( - spoof_voltage_high, - BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN, - BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX); - - - const uint16_t spoof_value_low = STEPS_PER_VOLT * spoof_voltage_low; - const uint16_t spoof_value_high = STEPS_PER_VOLT * spoof_voltage_high; - - brake_cmd.spoof_value_low = spoof_value_low; - brake_cmd.spoof_value_high = spoof_value_high; -#endif + brake_cmd.pedal_command = (float) brake_position; result = oscc_can_write( OSCC_BRAKE_COMMAND_CAN_ID, @@ -165,34 +129,7 @@ oscc_result_t oscc_publish_throttle_position( double throttle_position ) .magic[1] = ( uint8_t ) OSCC_MAGIC_BYTE_1 }; - - const double clamped_position = CONSTRAIN( - throttle_position, - MINIMUM_THROTTLE_COMMAND, - MAXIMUM_THROTTLE_COMMAND); - - - double spoof_voltage_low = THROTTLE_POSITION_TO_VOLTS_LOW( clamped_position ); - - spoof_voltage_low = CONSTRAIN( - spoof_voltage_low, - THROTTLE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN, - THROTTLE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX); - - - double spoof_voltage_high = THROTTLE_POSITION_TO_VOLTS_HIGH( clamped_position ); - - spoof_voltage_high = CONSTRAIN( - spoof_voltage_high, - THROTTLE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN, - THROTTLE_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX); - - - const uint16_t spoof_value_low = STEPS_PER_VOLT * spoof_voltage_low; - const uint16_t spoof_value_high = STEPS_PER_VOLT * spoof_voltage_high; - - throttle_cmd.spoof_value_low = spoof_value_low; - throttle_cmd.spoof_value_high = spoof_value_high; + throttle_cmd.torque_request = (float) throttle_position; result = oscc_can_write( OSCC_THROTTLE_COMMAND_CAN_ID, @@ -214,34 +151,7 @@ oscc_result_t oscc_publish_steering_torque( double torque ) .magic[1] = ( uint8_t ) OSCC_MAGIC_BYTE_1 }; - - const double clamped_torque = CONSTRAIN( - torque * MAXIMUM_TORQUE_COMMAND, - MINIMUM_TORQUE_COMMAND, - MAXIMUM_TORQUE_COMMAND); - - - double spoof_voltage_low = STEERING_TORQUE_TO_VOLTS_LOW( clamped_torque ); - - spoof_voltage_low = CONSTRAIN( - spoof_voltage_low, - STEERING_SPOOF_LOW_SIGNAL_VOLTAGE_MIN, - STEERING_SPOOF_LOW_SIGNAL_VOLTAGE_MAX); - - - double spoof_voltage_high = STEERING_TORQUE_TO_VOLTS_HIGH( clamped_torque ); - - spoof_voltage_high = CONSTRAIN( - spoof_voltage_high, - STEERING_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN, - STEERING_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX); - - - const uint16_t spoof_value_low = STEPS_PER_VOLT * spoof_voltage_low; - const uint16_t spoof_value_high = STEPS_PER_VOLT * spoof_voltage_high; - - steering_cmd.spoof_value_low = spoof_value_low; - steering_cmd.spoof_value_high = spoof_value_high; + steering_cmd.torque_command = (float) torque; result = oscc_can_write( OSCC_STEERING_COMMAND_CAN_ID, diff --git a/firmware/CMakeLists.txt b/firmware/CMakeLists.txt index 8c6d6c7b..649a5c6d 100644 --- a/firmware/CMakeLists.txt +++ b/firmware/CMakeLists.txt @@ -3,6 +3,10 @@ if(TESTS) project(tests) + if(TESTS) + add_definitions(-DTESTS) + endif() + if(DEBUG) add_definitions(-DDEBUG) endif() diff --git a/firmware/brake/kia_soul_ev/CMakeLists.txt b/firmware/brake/kia_soul_ev/CMakeLists.txt index ee6cdaa7..891758d9 100644 --- a/firmware/brake/kia_soul_ev/CMakeLists.txt +++ b/firmware/brake/kia_soul_ev/CMakeLists.txt @@ -5,6 +5,7 @@ generate_arduino_firmware( 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/fault_check/oscc_check.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 @@ -14,8 +15,7 @@ generate_arduino_firmware( src/globals.cpp src/brake_control.cpp src/communications.cpp - src/init.cpp - src/timers.cpp) + src/init.cpp) target_include_directories( brake @@ -24,6 +24,7 @@ target_include_directories( ${CMAKE_SOURCE_DIR}/common/include ${CMAKE_SOURCE_DIR}/common/libs/arduino_init ${CMAKE_SOURCE_DIR}/common/libs/DAC_MCP49xx + ${CMAKE_SOURCE_DIR}/common/libs/fault_check ${CMAKE_SOURCE_DIR}/common/libs/mcp_can ${CMAKE_SOURCE_DIR}/common/libs/serial ${CMAKE_SOURCE_DIR}/common/libs/can diff --git a/firmware/brake/kia_soul_ev/include/brake_control.h b/firmware/brake/kia_soul_ev/include/brake_control.h index fa7468af..3bd4c180 100644 --- a/firmware/brake/kia_soul_ev/include/brake_control.h +++ b/firmware/brake/kia_soul_ev/include/brake_control.h @@ -43,31 +43,19 @@ typedef struct } brake_control_state_s; -// **************************************************************************** -// Function: check_for_operator_override -// -// Purpose: Checks to see if the vehicle's operator has manually pressed -// the brake pedal 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. +// Purpose: Checks to see if valid values are being read from the sensors +// and if the vehicle's operator has manually pressed the brake +// pedal to disable if they have. // // Returns: void // // Parameters: void // // **************************************************************************** -void check_for_sensor_faults( void ); +void check_for_faults( void ); // **************************************************************************** diff --git a/firmware/brake/kia_soul_ev/include/communications.h b/firmware/brake/kia_soul_ev/include/communications.h index f672488c..3f703a39 100644 --- a/firmware/brake/kia_soul_ev/include/communications.h +++ b/firmware/brake/kia_soul_ev/include/communications.h @@ -35,20 +35,6 @@ void publish_brake_report( 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 // diff --git a/firmware/brake/kia_soul_ev/include/globals.h b/firmware/brake/kia_soul_ev/include/globals.h index 3b863e62..50391486 100644 --- a/firmware/brake/kia_soul_ev/include/globals.h +++ b/firmware/brake/kia_soul_ev/include/globals.h @@ -76,8 +76,6 @@ #endif -EXTERN volatile bool g_brake_command_timeout; - EXTERN volatile brake_control_state_s g_brake_control_state; diff --git a/firmware/brake/kia_soul_ev/include/init.h b/firmware/brake/kia_soul_ev/include/init.h index df8f22d1..678a6cb3 100644 --- a/firmware/brake/kia_soul_ev/include/init.h +++ b/firmware/brake/kia_soul_ev/include/init.h @@ -48,4 +48,17 @@ void init_devices( void ); void init_communication_interfaces( void ); +// **************************************************************************** +// Function: start_timers +// +// Purpose: Start timers for report publishing and fault checking. +// +// Returns: void +// +// Parameters: void +// +// **************************************************************************** +void start_timers( void ); + + #endif /* _OSCC_BRAKE_INIT_H_ */ diff --git a/firmware/brake/kia_soul_ev/src/brake_control.cpp b/firmware/brake/kia_soul_ev/src/brake_control.cpp index 0ea61035..f92486a3 100644 --- a/firmware/brake/kia_soul_ev/src/brake_control.cpp +++ b/firmware/brake/kia_soul_ev/src/brake_control.cpp @@ -14,96 +14,61 @@ #include "dtc.h" #include "globals.h" #include "oscc_dac.h" +#include "oscc_check.h" #include "vehicles.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_brake_pedal_position_sensor( brake_pedal_position_s * const value ); -void check_for_operator_override( void ) +void check_for_faults( void ) { - if ( g_brake_control_state.enabled == true - || g_brake_control_state.operator_override == true ) - { - brake_pedal_position_s brake_pedal_position; + brake_pedal_position_s brake_pedal_position; + if ( (g_brake_control_state.enabled == true) + || (g_brake_control_state.dtcs > 0) ) + { read_brake_pedal_position_sensor( &brake_pedal_position ); uint32_t brake_pedal_position_average = (brake_pedal_position.low + brake_pedal_position.high) / 2; - if ( brake_pedal_position_average >= BRAKE_PEDAL_OVERRIDE_THRESHOLD ) + // sensor pins tied to ground - a value of zero indicates disconnection + if( check_voltage_grounded( brake_pedal_position.high, + brake_pedal_position.low ) ) { disable_control( ); DTC_SET( g_brake_control_state.dtcs, - OSCC_BRAKE_DTC_OPERATOR_OVERRIDE ); + OSCC_BRAKE_DTC_INVALID_SENSOR_VAL ); publish_fault_report( ); - g_brake_control_state.operator_override = true; - - DEBUG_PRINTLN( "Operator override" ); + DEBUG_PRINTLN( "Bad value read from brake pedal position sensor" ); } - else + else if ( brake_pedal_position_average >= BRAKE_PEDAL_OVERRIDE_THRESHOLD ) { - DTC_CLEAR( + disable_control( ); + + DTC_SET( g_brake_control_state.dtcs, OSCC_BRAKE_DTC_OPERATOR_OVERRIDE ); - g_brake_control_state.operator_override = false; - } - } -} - - -void check_for_sensor_faults( void ) -{ - if ( (g_brake_control_state.enabled == true) - || DTC_CHECK(g_brake_control_state.dtcs, OSCC_BRAKE_DTC_INVALID_SENSOR_VAL) ) - { - static int fault_count = 0; - - brake_pedal_position_s brake_pedal_position; - - read_brake_pedal_position_sensor( &brake_pedal_position ); - - // sensor pins tied to ground - a value of zero indicates disconnection - if( (brake_pedal_position.high == 0) - || (brake_pedal_position.low == 0) ) - { - ++fault_count; - - if( fault_count >= SENSOR_VALIDITY_CHECK_FAULT_COUNT ) - { - disable_control( ); - - DTC_SET( - g_brake_control_state.dtcs, - OSCC_BRAKE_DTC_INVALID_SENSOR_VAL ); + publish_fault_report( ); - publish_fault_report( ); + g_brake_control_state.operator_override = true; - DEBUG_PRINTLN( "Bad value read from brake pedal position sensor" ); - } + DEBUG_PRINTLN( "Operator override" ); } else { - DTC_CLEAR( - g_brake_control_state.dtcs, - OSCC_BRAKE_DTC_INVALID_SENSOR_VAL ); + g_brake_control_state.dtcs = 0; - fault_count = 0; + g_brake_control_state.operator_override = false; } } } @@ -158,14 +123,13 @@ void enable_control( void ) prevent_signal_discontinuity( g_dac, num_samples, - PIN_BRAKE_PEDAL_POSITION_SENSOR_HIGH, - PIN_BRAKE_PEDAL_POSITION_SENSOR_LOW ); + PIN_BRAKE_PEDAL_POSITION_SENSOR_LOW, + PIN_BRAKE_PEDAL_POSITION_SENSOR_HIGH ); cli(); digitalWrite( PIN_SPOOF_ENABLE, HIGH ); sei(); - g_brake_command_timeout = false; g_brake_control_state.enabled = true; DEBUG_PRINTLN( "Control enabled" ); @@ -181,14 +145,14 @@ void disable_control( void ) prevent_signal_discontinuity( g_dac, num_samples, - PIN_BRAKE_PEDAL_POSITION_SENSOR_HIGH, - PIN_BRAKE_PEDAL_POSITION_SENSOR_LOW ); + PIN_BRAKE_PEDAL_POSITION_SENSOR_LOW, + PIN_BRAKE_PEDAL_POSITION_SENSOR_HIGH ); cli(); digitalWrite( PIN_SPOOF_ENABLE, LOW ); + digitalWrite( PIN_BRAKE_LIGHT_ENABLE, LOW ); sei(); - g_brake_command_timeout = false; g_brake_control_state.enabled = false; DEBUG_PRINTLN( "Control disabled" ); diff --git a/firmware/brake/kia_soul_ev/src/communications.cpp b/firmware/brake/kia_soul_ev/src/communications.cpp index bcaf61ea..3c7093a5 100644 --- a/firmware/brake/kia_soul_ev/src/communications.cpp +++ b/firmware/brake/kia_soul_ev/src/communications.cpp @@ -14,6 +14,7 @@ #include "globals.h" #include "mcp_can.h" #include "oscc_can.h" +#include "vehicles.h" static void process_rx_frame( @@ -65,22 +66,6 @@ void publish_fault_report( void ) } -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 waiting for controller command" ); - } - } -} - - void check_for_incoming_message( void ) { can_frame_s rx_frame; @@ -130,11 +115,29 @@ static void process_brake_command( const oscc_brake_command_s * const brake_command = (oscc_brake_command_s *) data; - update_brake( - brake_command->spoof_value_high, - brake_command->spoof_value_low ); + const float clamped_position = CONSTRAIN( + brake_command->pedal_command, + MINIMUM_BRAKE_COMMAND, + MAXIMUM_BRAKE_COMMAND); + + float spoof_voltage_low = BRAKE_POSITION_TO_VOLTS_LOW( clamped_position ); + + spoof_voltage_low = CONSTRAIN( + spoof_voltage_low, + BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN, + BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX); + + float spoof_voltage_high = BRAKE_POSITION_TO_VOLTS_HIGH( clamped_position ); + + spoof_voltage_high = CONSTRAIN( + spoof_voltage_high, + BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN, + BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX); + + const uint16_t spoof_value_low = STEPS_PER_VOLT * spoof_voltage_low; + const uint16_t spoof_value_high = STEPS_PER_VOLT * spoof_voltage_high; - g_brake_command_timeout = false; + update_brake( spoof_value_high, spoof_value_low ); } } diff --git a/firmware/brake/kia_soul_ev/src/init.cpp b/firmware/brake/kia_soul_ev/src/init.cpp index 39a0401e..2bb7f061 100644 --- a/firmware/brake/kia_soul_ev/src/init.cpp +++ b/firmware/brake/kia_soul_ev/src/init.cpp @@ -7,12 +7,14 @@ #include #include "can_protocols/brake_can_protocol.h" +#include "can_protocols/fault_can_protocol.h" #include "communications.h" #include "debug.h" #include "globals.h" #include "init.h" #include "oscc_can.h" #include "oscc_serial.h" +#include "oscc_timer.h" void init_globals( void ) @@ -20,8 +22,6 @@ void init_globals( void ) g_brake_control_state.enabled = false; g_brake_control_state.operator_override = false; g_brake_control_state.dtcs = 0; - - g_brake_command_timeout = false; } @@ -51,4 +51,18 @@ void init_communication_interfaces( void ) DEBUG_PRINT( "init Control CAN - " ); init_can( g_control_can ); + + // Filter CAN messages - accept if (CAN_ID & mask) == (filter & mask) + // Set buffer 0 to filter only brake module and global messages + g_control_can.init_Mask( 0, 0, 0x7F0 ); // Filter for 0x0N0 to 0x0NF + g_control_can.init_Filt( 0, 0, OSCC_BRAKE_CAN_ID_INDEX ); + g_control_can.init_Filt( 1, 0, OSCC_FAULT_CAN_ID_INDEX ); + // Accept only CAN Disable when buffer overflow occurs in buffer 0 + g_control_can.init_Mask( 1, 0, 0x7FF ); // Filter for one CAN ID + g_control_can.init_Filt( 2, 1, OSCC_BRAKE_DISABLE_CAN_ID ); +} + +void start_timers( void ) +{ + timer1_init( OSCC_BRAKE_REPORT_PUBLISH_FREQ_IN_HZ, publish_brake_report ); } diff --git a/firmware/brake/kia_soul_ev/src/main.cpp b/firmware/brake/kia_soul_ev/src/main.cpp index 7b800405..da440eb8 100644 --- a/firmware/brake/kia_soul_ev/src/main.cpp +++ b/firmware/brake/kia_soul_ev/src/main.cpp @@ -9,7 +9,6 @@ #include "communications.h" #include "debug.h" #include "init.h" -#include "timers.h" int main( void ) @@ -30,6 +29,6 @@ int main( void ) { check_for_incoming_message( ); - check_for_operator_override( ); + check_for_faults( ); } } diff --git a/firmware/brake/kia_soul_ev/src/timers.cpp b/firmware/brake/kia_soul_ev/src/timers.cpp deleted file mode 100644 index 3179348e..00000000 --- a/firmware/brake/kia_soul_ev/src/timers.cpp +++ /dev/null @@ -1,45 +0,0 @@ -/** - * @file timers.cpp - * - */ - - -#include - -#include "brake_control.h" -#include "can_protocols/brake_can_protocol.h" -#include "communications.h" -#include "globals.h" -#include "oscc_timer.h" -#include "timers.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_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/firmware/brake/kia_soul_ev/tests/CMakeLists.txt b/firmware/brake/kia_soul_ev/tests/CMakeLists.txt index 900e260e..ead50d80 100644 --- a/firmware/brake/kia_soul_ev/tests/CMakeLists.txt +++ b/firmware/brake/kia_soul_ev/tests/CMakeLists.txt @@ -7,6 +7,7 @@ add_library( ../src/globals.cpp ../src/brake_control.cpp ${CMAKE_SOURCE_DIR}/common/libs/can/oscc_can.cpp + ${CMAKE_SOURCE_DIR}/common/libs/fault_check/oscc_check.cpp ${CMAKE_SOURCE_DIR}/common/libs/dac/oscc_dac.cpp ${CMAKE_SOURCE_DIR}/common/testing/mocks/ ${CMAKE_SOURCE_DIR}/common/testing/mocks/Arduino_mock.cpp @@ -19,6 +20,7 @@ target_include_directories( ../include ${CMAKE_SOURCE_DIR}/common/include ${CMAKE_SOURCE_DIR}/common/libs/can + ${CMAKE_SOURCE_DIR}/common/libs/fault_check ${CMAKE_SOURCE_DIR}/common/libs/dac ${CMAKE_SOURCE_DIR}/common/libs/signal_smoothing ${CMAKE_SOURCE_DIR}/common/testing/mocks @@ -41,6 +43,7 @@ target_include_directories( ../include ${CMAKE_SOURCE_DIR}/common/include ${CMAKE_SOURCE_DIR}/common/libs/can + ${CMAKE_SOURCE_DIR}/common/libs/fault_check ${CMAKE_SOURCE_DIR}/common/testing/mocks ${CMAKE_SOURCE_DIR}/common/testing/framework/cucumber-cpp/include ${CMAKE_SOURCE_DIR}/common/testing/framework/cgreen/include @@ -56,5 +59,5 @@ add_custom_target( add_custom_target( run-brake-property-tests COMMAND - cargo test -- --test-threads=1 + cargo +1.20.0 test -- --test-threads=1 WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/property) diff --git a/firmware/brake/kia_soul_ev/tests/features/checking_faults.feature b/firmware/brake/kia_soul_ev/tests/features/checking_faults.feature index adc6533c..3dd374c0 100644 --- a/firmware/brake/kia_soul_ev/tests/features/checking_faults.feature +++ b/firmware/brake/kia_soul_ev/tests/features/checking_faults.feature @@ -5,13 +5,6 @@ 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 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 @@ -22,15 +15,6 @@ Feature: Checking for faults And a fault report should be published - Scenario: Controller command timeout - Given brake 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 brake control is enabled diff --git a/firmware/brake/kia_soul_ev/tests/features/receiving_messages.feature b/firmware/brake/kia_soul_ev/tests/features/receiving_messages.feature index e84d358c..098b2a1b 100644 --- a/firmware/brake/kia_soul_ev/tests/features/receiving_messages.feature +++ b/firmware/brake/kia_soul_ev/tests/features/receiving_messages.feature @@ -32,32 +32,32 @@ Feature: Receiving commands Scenario Outline: Spoof value sent from application Given brake control is enabled - When a command is received with spoof values and + When a command is received with request value Then should be sent to DAC A And should be sent to DAC B Examples: - | high | low | - | 1876 | 273 | - | 1800 | 300 | - | 1500 | 500 | - | 1000 | 750 | - | 750 | 900 | - | 572 | 917 | + | value | high | low | + | 1 | 1875 | 917 | + | 0.942 | 1800 | 880 | + | 0.712 | 1500 | 731 | + | 0.329 | 1000 | 484 | + | 0.137 | 750 | 361 | + | 0 | 572 | 273 | Scenario Outline: Spoof value sent from application outside valid range Given brake control is enabled - When a command is received with spoof values and + When a command is received with request value Then should be sent to DAC A And should be sent to DAC B Examples: - | high | low | high_clamped | low_clamped | - | 4000 | 0 | 1876 | 273 | - | 3500 | 500 | 1876 | 500 | - | 500 | 3500 | 572 | 917 | - | 0 | 4000 | 572 | 917 | + | value | high_clamped | low_clamped | + | 5 | 1875 | 917 | + | 2 | 1875 | 917 | + | -1 | 572 | 273 | + | -2 | 572 | 273 | diff --git a/firmware/brake/kia_soul_ev/tests/features/step_definitions/checking_faults.cpp b/firmware/brake/kia_soul_ev/tests/features/step_definitions/checking_faults.cpp index b9bb0a51..aef811fe 100644 --- a/firmware/brake/kia_soul_ev/tests/features/step_definitions/checking_faults.cpp +++ b/firmware/brake/kia_soul_ev/tests/features/step_definitions/checking_faults.cpp @@ -1,33 +1,14 @@ -WHEN("^a sensor becomes temporarily disconnected$") -{ - g_mock_arduino_analog_read_return[0] = 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] = 0; g_mock_arduino_analog_read_return[1] = 0; - // must call function enough times to exceed the fault limit - for( int i = 0; i < 100; ++i ) - { - check_for_sensor_faults(); - } -} - + check_for_faults(); -WHEN("^the time since the last received controller command exceeds the timeout$") -{ - g_brake_command_timeout = true; + // must call function enough times to exceed the fault limit + g_mock_arduino_millis_return = 105; - check_for_controller_command_timeout(); + check_for_faults(); } @@ -39,15 +20,7 @@ WHEN("^the operator applies (.*) to the accelerator$") g_mock_arduino_analog_read_return[1] = brake_sensor_val; - check_for_operator_override(); -} - - -THEN("^control should remain enabled") -{ - assert_that( - g_brake_control_state.enabled, - is_equal_to(1)); + check_for_faults(); } diff --git a/firmware/brake/kia_soul_ev/tests/features/step_definitions/common.cpp b/firmware/brake/kia_soul_ev/tests/features/step_definitions/common.cpp index 035acca1..2e1ec6e7 100644 --- a/firmware/brake/kia_soul_ev/tests/features/step_definitions/common.cpp +++ b/firmware/brake/kia_soul_ev/tests/features/step_definitions/common.cpp @@ -34,7 +34,7 @@ extern unsigned short g_mock_dac_output_a; extern unsigned short g_mock_dac_output_b; extern volatile brake_control_state_s g_brake_control_state; - +extern volatile unsigned long g_mock_arduino_millis_return; // return to known state before every scenario BEFORE() @@ -59,6 +59,9 @@ BEFORE() g_brake_control_state.enabled = false; g_brake_control_state.operator_override = false; g_brake_control_state.dtcs = 0; + + // A small amount of time elapsed after boot + g_mock_arduino_millis_return = 1; } @@ -89,7 +92,7 @@ GIVEN("^the operator has applied (.*) to the accelerator$") g_mock_arduino_analog_read_return[0] = brake_sensor_val; - check_for_operator_override(); + check_for_faults(); } diff --git a/firmware/brake/kia_soul_ev/tests/features/step_definitions/receiving_messages.cpp b/firmware/brake/kia_soul_ev/tests/features/step_definitions/receiving_messages.cpp index b298615c..84aba687 100644 --- a/firmware/brake/kia_soul_ev/tests/features/step_definitions/receiving_messages.cpp +++ b/firmware/brake/kia_soul_ev/tests/features/step_definitions/receiving_messages.cpp @@ -43,10 +43,9 @@ WHEN("^a fault report is received$") } -WHEN("^a command is received with spoof values (.*) and (.*)$") +WHEN("^a command is received with request value (.*)$") { - REGEX_PARAM(uint16_t, high); - REGEX_PARAM(uint16_t, low); + REGEX_PARAM(float, value); g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL; g_mock_mcp_can_read_msg_buf_id = OSCC_BRAKE_COMMAND_CAN_ID; @@ -56,12 +55,7 @@ WHEN("^a command is received with spoof values (.*) and (.*)$") brake_command->magic[0] = OSCC_MAGIC_BYTE_0; brake_command->magic[1] = OSCC_MAGIC_BYTE_1; - brake_command->spoof_value_high = high; - brake_command->spoof_value_low = low; + brake_command->pedal_command = value; check_for_incoming_message(); - - update_brake( - brake_command->spoof_value_high, - brake_command->spoof_value_low); } diff --git a/firmware/brake/kia_soul_ev/tests/property/Cargo.toml b/firmware/brake/kia_soul_ev/tests/property/Cargo.toml index ffe54b1b..fd4e028a 100644 --- a/firmware/brake/kia_soul_ev/tests/property/Cargo.toml +++ b/firmware/brake/kia_soul_ev/tests/property/Cargo.toml @@ -10,7 +10,7 @@ quickcheck = "0.4.1" rand = "0.3.15" [build-dependencies] -bindgen = "0.25.0" +bindgen = "0.32.1" gcc = "0.3.51" [lib] diff --git a/firmware/brake/kia_soul_ev/tests/property/build.rs b/firmware/brake/kia_soul_ev/tests/property/build.rs index 5a008efb..edcb4eaf 100644 --- a/firmware/brake/kia_soul_ev/tests/property/build.rs +++ b/firmware/brake/kia_soul_ev/tests/property/build.rs @@ -13,12 +13,14 @@ fn main() { .include("../../../../common/include") .include("../../../../common/testing/mocks/") .include("../../../../common/libs/can") + .include("../../../../common/libs/fault_check") .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/fault_check/oscc_check.cpp") .file("../../../../common/libs/dac/oscc_dac.cpp") .file("../../src/communications.cpp") .file("../../src/brake_control.cpp") @@ -36,11 +38,12 @@ fn main() { .clang_arg("-I../../include") .clang_arg("-I../../../../common/include") .clang_arg("-I../../../../common/libs/can") + .clang_arg("-I../../../../common/libs/fault_check") .clang_arg("-I../../../../common/testing/mocks") .clang_arg("-I../../../../../api/include") .whitelisted_function("publish_brake_report") .whitelisted_function("check_for_incoming_message") - .whitelisted_function("check_for_operator_override") + .whitelisted_function("check_for_faults") .whitelisted_var("OSCC_MAGIC_BYTE_0") .whitelisted_var("OSCC_MAGIC_BYTE_1") .whitelisted_var("OSCC_BRAKE_ENABLE_CAN_ID") @@ -58,6 +61,8 @@ fn main() { .whitelisted_var("BRAKE_SPOOF_LOW_SIGNAL_RANGE_MAX") .whitelisted_var("BRAKE_SPOOF_HIGH_SIGNAL_RANGE_MIN") .whitelisted_var("BRAKE_SPOOF_HIGH_SIGNAL_RANGE_MAX") + .whitelisted_var("MINIMUM_BRAKE_COMMAND") + .whitelisted_var("MAXIMUM_BRAKE_COMMAND") .whitelisted_type("oscc_brake_enable_s") .whitelisted_type("oscc_brake_disable_s") .whitelisted_type("oscc_brake_report_s") diff --git a/firmware/brake/kia_soul_ev/tests/property/src/tests.rs b/firmware/brake/kia_soul_ev/tests/property/src/tests.rs index 12b9b9f3..177a76f0 100644 --- a/firmware/brake/kia_soul_ev/tests/property/src/tests.rs +++ b/firmware/brake/kia_soul_ev/tests/property/src/tests.rs @@ -60,8 +60,7 @@ 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], - spoof_value_low: u16::arbitrary(g), - spoof_value_high: u16::arbitrary(g), + pedal_command: f32::arbitrary(g), reserved: [u8::arbitrary(g); 2] } } @@ -168,12 +167,8 @@ fn check_process_disable_command() { /// the brake firmware should send requested spoof values /// upon receiving a brake command message fn prop_output_accurate_spoofs(mut brake_command_msg: oscc_brake_command_s) -> TestResult { - brake_command_msg.spoof_value_low = - rand::thread_rng().gen_range(BRAKE_SPOOF_LOW_SIGNAL_RANGE_MIN as u16, - BRAKE_SPOOF_LOW_SIGNAL_RANGE_MAX as u16); - brake_command_msg.spoof_value_high = - rand::thread_rng().gen_range(BRAKE_SPOOF_HIGH_SIGNAL_RANGE_MIN as u16, - BRAKE_SPOOF_HIGH_SIGNAL_RANGE_MAX as u16); + brake_command_msg.pedal_command = rand::thread_rng().gen_range(0f32, 1f32); + unsafe { g_brake_control_state.enabled = true; @@ -183,8 +178,10 @@ fn prop_output_accurate_spoofs(mut brake_command_msg: oscc_brake_command_s) -> T check_for_incoming_message(); - TestResult::from_bool(g_mock_dac_output_b == brake_command_msg.spoof_value_low && - g_mock_dac_output_a == brake_command_msg.spoof_value_high) + TestResult::from_bool(g_mock_dac_output_b >= BRAKE_SPOOF_LOW_SIGNAL_RANGE_MIN as u16 && + g_mock_dac_output_b <= BRAKE_SPOOF_LOW_SIGNAL_RANGE_MAX as u16 && + g_mock_dac_output_a >= BRAKE_SPOOF_HIGH_SIGNAL_RANGE_MIN as u16 && + g_mock_dac_output_a <= BRAKE_SPOOF_HIGH_SIGNAL_RANGE_MAX as u16) } } @@ -200,10 +197,8 @@ fn check_output_accurate_spoofs() { /// upon receiving a brake command message fn prop_output_constrained_spoofs(brake_command_msg: oscc_brake_command_s) -> TestResult { unsafe { - if (brake_command_msg.spoof_value_low >= BRAKE_SPOOF_LOW_SIGNAL_RANGE_MIN as u16 && - brake_command_msg.spoof_value_low <= BRAKE_SPOOF_LOW_SIGNAL_RANGE_MAX as u16) || - (brake_command_msg.spoof_value_high >= BRAKE_SPOOF_HIGH_SIGNAL_RANGE_MIN as u16 && - brake_command_msg.spoof_value_high <= BRAKE_SPOOF_HIGH_SIGNAL_RANGE_MAX as u16) { + if brake_command_msg.pedal_command >= MINIMUM_BRAKE_COMMAND as f32 && + brake_command_msg.pedal_command <= MAXIMUM_BRAKE_COMMAND as f32 { return TestResult::discard(); } @@ -264,7 +259,7 @@ fn prop_check_operator_override(analog_read_spoof: u16) -> TestResult { g_mock_arduino_analog_read_return[0] = analog_read_spoof as isize; g_mock_arduino_analog_read_return[1] = analog_read_spoof as isize; - check_for_operator_override(); + check_for_faults(); if analog_read_spoof >= (BRAKE_PEDAL_OVERRIDE_THRESHOLD as u16) { TestResult::from_bool(g_brake_control_state.operator_override == true && diff --git a/firmware/brake/kia_soul_petrol/include/communications.h b/firmware/brake/kia_soul_petrol/include/communications.h index cdfbda44..3f703a39 100644 --- a/firmware/brake/kia_soul_petrol/include/communications.h +++ b/firmware/brake/kia_soul_petrol/include/communications.h @@ -35,19 +35,6 @@ void publish_brake_report( 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 // diff --git a/firmware/brake/kia_soul_petrol/include/globals.h b/firmware/brake/kia_soul_petrol/include/globals.h index 281ea3e8..b475e0ce 100644 --- a/firmware/brake/kia_soul_petrol/include/globals.h +++ b/firmware/brake/kia_soul_petrol/include/globals.h @@ -142,8 +142,6 @@ #endif -EXTERN volatile bool g_brake_command_timeout; - EXTERN volatile brake_control_state_s g_brake_control_state; EXTERN pid_s g_pid; diff --git a/firmware/brake/kia_soul_petrol/src/brake_control.cpp b/firmware/brake/kia_soul_petrol/src/brake_control.cpp index 0779fd9f..ca361054 100644 --- a/firmware/brake/kia_soul_petrol/src/brake_control.cpp +++ b/firmware/brake/kia_soul_petrol/src/brake_control.cpp @@ -68,7 +68,6 @@ void enable_control( void ) set_release_solenoid_duty_cycle( SOLENOID_PWM_OFF ); - g_brake_command_timeout = false; g_brake_control_state.enabled = true; DEBUG_PRINTLN( "Control enabled" ); @@ -92,7 +91,6 @@ void disable_control( void ) set_release_solenoid_duty_cycle( SOLENOID_PWM_OFF ); - g_brake_command_timeout = false; g_brake_control_state.enabled = false; DEBUG_PRINTLN( "Control disabled" ); @@ -243,7 +241,7 @@ void update_brake( void ) time_between_loops /= 1000.0; static interpolate_range_s pressure_ranges = - { 0, UINT16_MAX, BRAKE_PRESSURE_MIN_IN_DECIBARS, BRAKE_PRESSURE_MAX_IN_DECIBARS }; + { 0.0, 1.0, BRAKE_PRESSURE_MIN_IN_DECIBARS, BRAKE_PRESSURE_MAX_IN_DECIBARS }; pressure_at_wheels_target = interpolate( g_brake_control_state.commanded_pedal_position, @@ -513,4 +511,3 @@ static void pump_startup_check( void ) accumulator_turn_pump_off(); } - diff --git a/firmware/brake/kia_soul_petrol/src/communications.cpp b/firmware/brake/kia_soul_petrol/src/communications.cpp index 08a3cc7a..adcf5b12 100644 --- a/firmware/brake/kia_soul_petrol/src/communications.cpp +++ b/firmware/brake/kia_soul_petrol/src/communications.cpp @@ -66,22 +66,6 @@ void publish_fault_report( void ) } -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; @@ -133,8 +117,6 @@ static void process_brake_command( g_brake_control_state.commanded_pedal_position = brake_command->pedal_command; - - g_brake_command_timeout = false; } } diff --git a/firmware/brake/kia_soul_petrol/src/init.cpp b/firmware/brake/kia_soul_petrol/src/init.cpp index 20352326..87555f6f 100644 --- a/firmware/brake/kia_soul_petrol/src/init.cpp +++ b/firmware/brake/kia_soul_petrol/src/init.cpp @@ -9,6 +9,7 @@ #include "accumulator.h" #include "brake_control.h" #include "can_protocols/brake_can_protocol.h" +#include "can_protocols/fault_can_protocol.h" #include "communications.h" #include "debug.h" #include "globals.h" @@ -26,8 +27,6 @@ void init_globals( void ) g_brake_control_state.operator_override = false; g_brake_control_state.dtcs = 0; - g_brake_command_timeout = false; - pid_zeroize( &g_pid, BRAKE_PID_WINDUP_GUARD ); g_pid.proportional_gain = BRAKE_PID_PROPORTIONAL_GAIN; g_pid.integral_gain = BRAKE_PID_INTEGRAL_GAIN; @@ -61,4 +60,13 @@ void init_communication_interfaces( void ) DEBUG_PRINT( "init Control CAN - " ); init_can( g_control_can ); + + // Filter CAN messages - accept if (CAN_ID & mask) == (filter & mask) + // Set buffer 0 to filter only brake module and global messages + g_control_can.init_Mask( 0, 0, 0x7F0 ); // Filter for 0x0N0 to 0x0NF + g_control_can.init_Filt( 0, 0, OSCC_BRAKE_CAN_ID_INDEX ); + g_control_can.init_Filt( 1, 0, OSCC_FAULT_CAN_ID_INDEX ); + // Accept only CAN Disable when buffer overflow occurs in buffer 0 + g_control_can.init_Mask( 1, 0, 0x7FF ); // Filter for one CAN ID + g_control_can.init_Filt( 2, 1, OSCC_BRAKE_DISABLE_CAN_ID ); } diff --git a/firmware/brake/kia_soul_petrol/src/timers.cpp b/firmware/brake/kia_soul_petrol/src/timers.cpp index bec06cac..7a6d8b81 100644 --- a/firmware/brake/kia_soul_petrol/src/timers.cpp +++ b/firmware/brake/kia_soul_petrol/src/timers.cpp @@ -35,11 +35,7 @@ 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/firmware/brake/kia_soul_petrol/tests/CMakeLists.txt b/firmware/brake/kia_soul_petrol/tests/CMakeLists.txt index 8a25a99d..a65710c0 100644 --- a/firmware/brake/kia_soul_petrol/tests/CMakeLists.txt +++ b/firmware/brake/kia_soul_petrol/tests/CMakeLists.txt @@ -59,5 +59,5 @@ add_custom_target( add_custom_target( run-brake-property-tests COMMAND - cargo test -- --test-threads=1 + cargo +1.20.0 test -- --test-threads=1 WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/property) diff --git a/firmware/brake/kia_soul_petrol/tests/features/checking_faults.feature b/firmware/brake/kia_soul_petrol/tests/features/checking_faults.feature index 5230dd8f..3c036eeb 100644 --- a/firmware/brake/kia_soul_petrol/tests/features/checking_faults.feature +++ b/firmware/brake/kia_soul_petrol/tests/features/checking_faults.feature @@ -24,15 +24,6 @@ Feature: Timeouts and overrides And a fault report should be published - Scenario: Controller command timeout - Given brake 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 brake control is enabled diff --git a/firmware/brake/kia_soul_petrol/tests/features/receiving_messages.feature b/firmware/brake/kia_soul_petrol/tests/features/receiving_messages.feature index 83e0be6d..66353574 100644 --- a/firmware/brake/kia_soul_petrol/tests/features/receiving_messages.feature +++ b/firmware/brake/kia_soul_petrol/tests/features/receiving_messages.feature @@ -41,11 +41,11 @@ Feature: Receiving commands Examples: | left_pressure | right_pressure | command | solenoid | duty_cycle | - | 120 | 120 | 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 | + | 120 | 120 | 0.305 | ACCUMULATOR | 105 | + | 160 | 160 | 0.305 | ACCUMULATOR | 104 | + | 190 | 190 | 0.305 | ACCUMULATOR | 93 | + | 230 | 230 | 0.305 | NONE | 0 | + | 200 | 200 | 0.305 | NONE | 0 | + | 220 | 220 | 0.305 | NONE | 0 | + | 205 | 205 | 0.305 | NONE | 0 | + | 215 | 215 | 0.305 | NONE | 0 | diff --git a/firmware/brake/kia_soul_petrol/tests/features/step_definitions/checking_faults.cpp b/firmware/brake/kia_soul_petrol/tests/features/step_definitions/checking_faults.cpp index 9581dbfa..f0b078c2 100644 --- a/firmware/brake/kia_soul_petrol/tests/features/step_definitions/checking_faults.cpp +++ b/firmware/brake/kia_soul_petrol/tests/features/step_definitions/checking_faults.cpp @@ -24,14 +24,6 @@ WHEN("^a sensor becomes permanently disconnected$") } -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); diff --git a/firmware/brake/kia_soul_petrol/tests/features/step_definitions/receiving_messages.cpp b/firmware/brake/kia_soul_petrol/tests/features/step_definitions/receiving_messages.cpp index 73b92ec3..0a3e1b65 100644 --- a/firmware/brake/kia_soul_petrol/tests/features/step_definitions/receiving_messages.cpp +++ b/firmware/brake/kia_soul_petrol/tests/features/step_definitions/receiving_messages.cpp @@ -70,7 +70,7 @@ WHEN("^a fault report is received$") WHEN("^the brake pedal command (.*) is received$") { - REGEX_PARAM(int, command); + REGEX_PARAM(float, command); oscc_brake_command_s * brake_command = (oscc_brake_command_s *) g_mock_mcp_can_read_msg_buf_buf; diff --git a/firmware/brake/kia_soul_petrol/tests/property/Cargo.toml b/firmware/brake/kia_soul_petrol/tests/property/Cargo.toml index 3ec63b3d..77e94e89 100644 --- a/firmware/brake/kia_soul_petrol/tests/property/Cargo.toml +++ b/firmware/brake/kia_soul_petrol/tests/property/Cargo.toml @@ -11,7 +11,7 @@ lazy_static = "0.2.8" rand = "0.3.15" [build-dependencies] -bindgen = "0.20" +bindgen = "0.32.1" gcc = "0.3" [lib] diff --git a/firmware/brake/kia_soul_petrol/tests/property/build.rs b/firmware/brake/kia_soul_petrol/tests/property/build.rs index 5246878a..1ab0e684 100644 --- a/firmware/brake/kia_soul_petrol/tests/property/build.rs +++ b/firmware/brake/kia_soul_petrol/tests/property/build.rs @@ -13,6 +13,7 @@ fn main() { .include("../../../../common/testing/mocks") .include("../../../../common/include") .include("../../../../common/libs/can") + .include("../../../../common/libs/fault_check") .include("../../../../common/libs/time") .include("../../../../common/libs/pid") .include("../../../../../api/include") @@ -24,6 +25,7 @@ fn main() { .file("../../src/master_cylinder.cpp") .file("../../src/helper.cpp") .file("../../../../common/libs/can/oscc_can.cpp") + .file("../../../../common/libs/fault_check/oscc_check.cpp") .cpp(true) .compile("libbrake_test.a"); @@ -37,11 +39,12 @@ fn main() { .clang_arg("-I../../../../common/testing/mocks") .clang_arg("-I../../../../common/include") .clang_arg("-I../../../../common/libs/can") + .clang_arg("-I../../../../common/libs/fault_check") .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_function("check_for_faults") .whitelisted_var("OSCC_MAGIC_BYTE_0") .whitelisted_var("OSCC_MAGIC_BYTE_1") .whitelisted_var("OSCC_BRAKE_ENABLE_CAN_ID") diff --git a/firmware/brake/kia_soul_petrol/tests/property/src/tests.rs b/firmware/brake/kia_soul_petrol/tests/property/src/tests.rs index 3b67a164..b125b229 100644 --- a/firmware/brake/kia_soul_petrol/tests/property/src/tests.rs +++ b/firmware/brake/kia_soul_petrol/tests/property/src/tests.rs @@ -56,8 +56,8 @@ 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), - reserved: [u8::arbitrary(g); 4], + pedal_command: f32::arbitrary(g), + reserved: [u8::arbitrary(g); 2usize], } } } diff --git a/firmware/can_gateway/CMakeLists.txt b/firmware/can_gateway/CMakeLists.txt index 5cd24efe..b5a76658 100644 --- a/firmware/can_gateway/CMakeLists.txt +++ b/firmware/can_gateway/CMakeLists.txt @@ -21,10 +21,15 @@ generate_arduino_firmware( ${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/timer/oscc_timer.cpp + ${CMAKE_SOURCE_DIR}/common/libs/ssd1325/ssd1325.cpp + ${CMAKE_SOURCE_DIR}/common/libs/ssd1325/gfx/gfx.cpp src/main.cpp src/globals.cpp src/init.cpp - src/communications.cpp) + src/communications.cpp + src/display.cpp + src/timer.cpp) target_include_directories( can-gateway @@ -35,4 +40,7 @@ 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/timer + ${CMAKE_SOURCE_DIR}/common/libs/ssd1325 + ${CMAKE_SOURCE_DIR}/common/libs/ssd1325/gfx ${CMAKE_SOURCE_DIR}/../api/include) diff --git a/firmware/can_gateway/include/communications.h b/firmware/can_gateway/include/communications.h index 69da2fe4..f9d0931e 100644 --- a/firmware/can_gateway/include/communications.h +++ b/firmware/can_gateway/include/communications.h @@ -12,6 +12,19 @@ #include "globals.h" +// **************************************************************************** +// Function: check_for_module_reports +// +// Purpose: Checks Control CAN bus for reports from the modules. +// +// Returns: void +// +// Parameters: void +// +// **************************************************************************** +void check_for_module_reports( void ); + + // **************************************************************************** // Function: republish_obd_frames_to_control_can_bus // diff --git a/firmware/can_gateway/include/display.h b/firmware/can_gateway/include/display.h new file mode 100644 index 00000000..078f6709 --- /dev/null +++ b/firmware/can_gateway/include/display.h @@ -0,0 +1,111 @@ +/** + * @file display.h + * @brief Display functionality. + * + */ + + +#ifndef _OSCC_KIA_SOUL_CAN_GATEWAY_DISPLAY_H_ +#define _OSCC_KIA_SOUL_CAN_GATEWAY_DISPLAY_H_ + + +#include "can_protocols/brake_can_protocol.h" +#include "can_protocols/steering_can_protocol.h" +#include "can_protocols/throttle_can_protocol.h" + + +/* +* @brief Frequency of updates of display content. [Hz] +* +*/ +#define DISPLAY_UPDATE_FREQUENCY_IN_HZ ( 4 ) + + +/** + * @brief Enumeration of possible screens. + * + */ +typedef enum +{ + STATUS_SCREEN, + DTC_SCREEN, + SCREEN_COUNT +} screen_t; + + +/** + * @brief Enumeration of possible module statuses. + * + */ +typedef enum +{ + MODULE_STATUS_UNKNOWN, + MODULE_STATUS_ENABLED, + MODULE_STATUS_DISABLED, + MODULE_STATUS_ERROR, +} module_status_t; + + +/** + * @brief Current status screen state. + * + */ +typedef struct +{ + module_status_t brakes; + module_status_t steering; + module_status_t throttle; +} status_screen_s; + + +/** + * @brief Current DTC screen state. + * + */ +typedef struct +{ + bool brakes[OSCC_BRAKE_DTC_COUNT]; + bool steering[OSCC_STEERING_DTC_COUNT]; + bool throttle[OSCC_THROTTLE_DTC_COUNT]; +} dtc_screen_s; + + +/** + * @brief Current display state. + * + */ +typedef struct +{ + screen_t current_screen; + status_screen_s status_screen; + dtc_screen_s dtc_screen; +} kia_soul_gateway_display_state_s; + + +// **************************************************************************** +// Function: init_display +// +// Purpose: Initializes the display. +// +// Returns: void +// +// Parameters: void +// +// **************************************************************************** +void init_display( void ); + + +// **************************************************************************** +// Function: update_display +// +// Purpose: Updates the display with new information. +// +// Returns: void +// +// Parameters: void +// +// **************************************************************************** +void update_display( void ); + + +#endif /* _OSCC_KIA_SOUL_CAN_GATEWAY_DISPLAY_H_ */ diff --git a/firmware/can_gateway/include/globals.h b/firmware/can_gateway/include/globals.h index 21f0c159..a561306f 100644 --- a/firmware/can_gateway/include/globals.h +++ b/firmware/can_gateway/include/globals.h @@ -5,11 +5,14 @@ */ -#ifndef _OSCC_CAN_GATEWAY_GLOBALS_H_ -#define _OSCC_CAN_GATEWAY_GLOBALS_H_ +#ifndef _OSCC_KIA_SOUL_CAN_GATEWAY_GLOBALS_H_ +#define _OSCC_KIA_SOUL_CAN_GATEWAY_GLOBALS_H_ #include "mcp_can.h" +#include "ssd1325.h" + +#include "display.h" /* @@ -24,18 +27,32 @@ */ #define PIN_CONTROL_CAN_CHIP_SELECT ( 10 ) +/* + * @brief SPI CS pin to display. + * + */ +#define PIN_DISPLAY_CHIP_SELECT ( 8 ) + #ifdef GLOBAL_DEFINED MCP_CAN g_obd_can( PIN_OBD_CAN_CHIP_SELECT ); MCP_CAN g_control_can( PIN_CONTROL_CAN_CHIP_SELECT ); + #ifndef TESTS + SSD1325 g_display( PIN_DISPLAY_CHIP_SELECT ); + #endif #define EXTERN #else extern MCP_CAN g_obd_can; extern MCP_CAN g_control_can; + extern SSD1325 g_display; #define EXTERN extern #endif -#endif /* _OSCC_CAN_GATEWAY_GLOBALS_H_ */ +EXTERN kia_soul_gateway_display_state_s g_display_state; + + + +#endif /* _OSCC_KIA_SOUL_CAN_GATEWAY_GLOBALS_H_ */ diff --git a/firmware/can_gateway/include/init.h b/firmware/can_gateway/include/init.h index 231c1c3b..48784053 100644 --- a/firmware/can_gateway/include/init.h +++ b/firmware/can_gateway/include/init.h @@ -9,6 +9,19 @@ #define _OSCC_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/brake/kia_soul_ev/include/timers.h b/firmware/can_gateway/include/timer.h similarity index 52% rename from firmware/brake/kia_soul_ev/include/timers.h rename to firmware/can_gateway/include/timer.h index 7ce05c90..46d78be8 100644 --- a/firmware/brake/kia_soul_ev/include/timers.h +++ b/firmware/can_gateway/include/timer.h @@ -1,25 +1,25 @@ /** - * @file timers.h + * @file timer.h * @brief Timer functionality. * */ -#ifndef _OSCC_BRAKE_TIMERS_H_ -#define _OSCC_BRAKE_TIMERS_H_ +#ifndef _OSCC_CAN_GATEWAY_TIMER_H_ +#define _OSCC_CAN_GATEWAY_TIMER_H_ // **************************************************************************** -// Function: start_timers +// Function: start_timer // -// Purpose: Start timers for report publishing and fault checking. +// Purpose: Start timer for updating the display. // // Returns: void // // Parameters: void // // **************************************************************************** -void start_timers( void ); +void start_timer( void ); -#endif /* _OSCC_BRAKE_TIMERS_H_ */ +#endif /* _OSCC_CAN_GATEWAY_TIMER_H_ */ diff --git a/firmware/can_gateway/src/communications.cpp b/firmware/can_gateway/src/communications.cpp index a7fcfbb7..8589e128 100644 --- a/firmware/can_gateway/src/communications.cpp +++ b/firmware/can_gateway/src/communications.cpp @@ -5,12 +5,41 @@ #include "communications.h" +#include "dtc.h" #include "globals.h" #include "mcp_can.h" #include "oscc_can.h" #include "vehicles.h" +static void parse_brake_report( uint8_t *data ); +static void parse_steering_report( uint8_t *data ); +static void parse_throttle_report( uint8_t *data ); + + +void check_for_module_reports( 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 ) + { + if ( rx_frame.id == OSCC_BRAKE_REPORT_CAN_ID ) + { + parse_brake_report( rx_frame.data ); + } + else if ( rx_frame.id == OSCC_STEERING_REPORT_CAN_ID ) + { + parse_steering_report( rx_frame.data ); + } + else if ( rx_frame.id == OSCC_THROTTLE_REPORT_CAN_ID ) + { + parse_throttle_report( rx_frame.data ); + } + } +} + + void republish_obd_frames_to_control_can_bus( void ) { can_frame_s rx_frame; @@ -22,11 +51,136 @@ void republish_obd_frames_to_control_can_bus( void ) || (rx_frame.id == KIA_SOUL_OBD_WHEEL_SPEED_CAN_ID) || (rx_frame.id == KIA_SOUL_OBD_BRAKE_PRESSURE_CAN_ID) ) { + cli(); g_control_can.sendMsgBuf( rx_frame.id, CAN_STANDARD, sizeof(rx_frame), (uint8_t *) &rx_frame.data ); + sei(); } } } + + +static void parse_brake_report( uint8_t *data ) +{ + oscc_brake_report_s *report = (oscc_brake_report_s *) data; + + + if ( report->enabled == 1 ) + { + g_display_state.status_screen.brakes = MODULE_STATUS_ENABLED; + } + else + { + g_display_state.status_screen.brakes = MODULE_STATUS_DISABLED; + } + + + if ( report->dtcs != 0 ) + { + g_display_state.status_screen.brakes = MODULE_STATUS_ERROR; + } + + + if ( DTC_CHECK(report->dtcs, OSCC_BRAKE_DTC_INVALID_SENSOR_VAL) != 0 ) + { + g_display_state.dtc_screen.brakes[OSCC_BRAKE_DTC_INVALID_SENSOR_VAL] = true; + } + else + { + g_display_state.dtc_screen.brakes[OSCC_BRAKE_DTC_INVALID_SENSOR_VAL] = false; + } + + if ( DTC_CHECK(report->dtcs, OSCC_BRAKE_DTC_OPERATOR_OVERRIDE) != 0 ) + { + g_display_state.dtc_screen.brakes[OSCC_BRAKE_DTC_OPERATOR_OVERRIDE] = true; + } + else + { + g_display_state.dtc_screen.brakes[OSCC_BRAKE_DTC_OPERATOR_OVERRIDE] = false; + } +} + + +static void parse_steering_report( uint8_t *data ) +{ + oscc_steering_report_s *report = (oscc_steering_report_s *) data; + + + if ( report->enabled == 1 ) + { + g_display_state.status_screen.steering = MODULE_STATUS_ENABLED; + } + else + { + g_display_state.status_screen.steering = MODULE_STATUS_DISABLED; + } + + + if ( report->dtcs != 0 ) + { + g_display_state.status_screen.steering = MODULE_STATUS_ERROR; + } + + + if ( DTC_CHECK(report->dtcs, OSCC_STEERING_DTC_INVALID_SENSOR_VAL) != 0 ) + { + g_display_state.dtc_screen.steering[OSCC_STEERING_DTC_INVALID_SENSOR_VAL] = true; + } + else + { + g_display_state.dtc_screen.steering[OSCC_STEERING_DTC_INVALID_SENSOR_VAL] = false; + } + + if ( DTC_CHECK(report->dtcs, OSCC_STEERING_DTC_OPERATOR_OVERRIDE) != 0 ) + { + g_display_state.dtc_screen.steering[OSCC_STEERING_DTC_OPERATOR_OVERRIDE] = true; + } + else + { + g_display_state.dtc_screen.steering[OSCC_STEERING_DTC_OPERATOR_OVERRIDE] = false; + } +} + + +static void parse_throttle_report( uint8_t *data ) +{ + oscc_throttle_report_s *report = (oscc_throttle_report_s *) data; + + + if ( report->enabled == 1 ) + { + g_display_state.status_screen.throttle = MODULE_STATUS_ENABLED; + } + else + { + g_display_state.status_screen.throttle = MODULE_STATUS_DISABLED; + } + + + if ( report->dtcs != 0 ) + { + g_display_state.status_screen.throttle = MODULE_STATUS_ERROR; + } + + + if ( DTC_CHECK(report->dtcs, OSCC_THROTTLE_DTC_INVALID_SENSOR_VAL) != 0 ) + { + g_display_state.dtc_screen.throttle[OSCC_THROTTLE_DTC_INVALID_SENSOR_VAL] = true; + } + else + { + g_display_state.dtc_screen.throttle[OSCC_THROTTLE_DTC_INVALID_SENSOR_VAL] = false; + } + + if ( DTC_CHECK(report->dtcs, OSCC_THROTTLE_DTC_OPERATOR_OVERRIDE) != 0 ) + { + g_display_state.dtc_screen.throttle[OSCC_THROTTLE_DTC_OPERATOR_OVERRIDE] = true; + } + else + { + g_display_state.dtc_screen.throttle[OSCC_THROTTLE_DTC_OPERATOR_OVERRIDE] = false; + } +} diff --git a/firmware/can_gateway/src/display.cpp b/firmware/can_gateway/src/display.cpp new file mode 100644 index 00000000..7eb7bc40 --- /dev/null +++ b/firmware/can_gateway/src/display.cpp @@ -0,0 +1,388 @@ +/** + * @file display.cpp + * + */ + +#include "globals.h" +#include "debug.h" +#include "can_protocols/brake_can_protocol.h" +#include "can_protocols/steering_can_protocol.h" +#include "can_protocols/throttle_can_protocol.h" + +#include "display.h" + + +/* + * @brief X pixel position of where display drawing begins. + * + */ +#define ORIGIN_X ( 4 ) + +/* + * @brief Y pixel position of where display drawing begins. + * + */ +#define ORIGIN_Y ( 4 ) + +/* + * @brief Y pixel position of where the horizontal header line is drawn. + * + */ +#define HEADER_LINE_Y ( 14 ) + +/* + * @brief X pixel position of where the horizontal header line starts being drawn. + * + */ +#define HEADER_LINE_X_START ( 4 ) + +/* + * @brief X pixel position of where the horizontal header line stops being drawn. + * + */ +#define HEADER_LINE_X_STOP ( 125 ) + +/* + * @brief Y pixel position of where the brake status line is drawn. + * + */ +#define BRAKE_STATUS_Y ( 18 ) + +/* + * @brief Y pixel position of where the steering status line is drawn. + * + */ +#define STEERING_STATUS_Y ( 30 ) + +/* + * @brief Y pixel position of where the throttle status line is drawn. + * + */ +#define THROTTLE_STATUS_Y ( 42 ) + +/* + * @brief Y pixel position of the the first row on the DTC screen. + * + */ +#define DTC_Y_START ( 18 ) + +/* + * @brief X pixel position of the DTC screen's brake column. + * + */ +#define BRAKE_DTC_X ( 4 ) + +/* + * @brief X pixel position of the DTC screen's steering column. + * + */ +#define STEERING_DTC_X ( 46 ) + +/* + * @brief X pixel position of the DTC screen's throttle column. + * + */ +#define THROTTLE_DTC_X ( 88 ) + + +/* + * @brief Number of pixels between top of character in one row and top of character + * in the next row. + * + */ +#define ROW_SPACING ( 12 ) + + +static const char * module_status_strings[] = +{ + "UNKNOWN", + "ENABLED", + "DISABLED", + "ERROR" +}; + + +static void display_status_screen( void ); +static void display_dtc_screen( void ); +static void draw_header_line( void ); +static void update_leds( ); +static void print_module_status( module_status_t status ); +static void print_brake_dtcs( void ); +static void print_steering_dtcs( void ); +static void print_throttle_dtcs( void ); +static void print_dtc( const char *type, int num ); +static void print_padded_number( const unsigned int number ); +static void read_button( void ); + + +void init_display( void ) +{ + cli(); + + g_display.begin(); + + g_display.eraseBuffer(); + g_display.sendBuffer(); + + sei(); +} + + +void update_display( void ) +{ + cli(); + + update_leds( ); + + g_display.setCursor( ORIGIN_X, ORIGIN_Y ); + g_display.setTextColor( WHITE, BLACK ); + + read_button( ); + + if( g_display_state.current_screen == STATUS_SCREEN ) + { + display_status_screen( ); + } + else if( g_display_state.current_screen == DTC_SCREEN ) + { + display_dtc_screen( ); + } + + sei(); +} + + +static void update_leds( void ) +{ + static uint32_t lastToggle = 0; + static uint8_t toggleState = 0; + + if( (g_display_state.status_screen.brakes == MODULE_STATUS_ERROR) + || (g_display_state.status_screen.steering == MODULE_STATUS_ERROR) + || (g_display_state.status_screen.throttle == MODULE_STATUS_ERROR) ) + { + g_display.enableRedLed( ); + } + else if( (g_display_state.status_screen.brakes == MODULE_STATUS_DISABLED) + && (g_display_state.status_screen.steering == MODULE_STATUS_DISABLED) + && (g_display_state.status_screen.throttle == MODULE_STATUS_DISABLED) ) + { + if(toggleState) + { + if((millis() - lastToggle) > 500) + { + g_display.enableYellowLed( ); + lastToggle = millis(); + toggleState = 0; + } + } + else + { + if((millis() - lastToggle) > 1000) + { + g_display.disableLeds( ); + lastToggle = millis(); + toggleState = 1; + } + } + } + else if( (g_display_state.status_screen.brakes == MODULE_STATUS_ENABLED) + && (g_display_state.status_screen.steering == MODULE_STATUS_ENABLED) + && (g_display_state.status_screen.throttle == MODULE_STATUS_ENABLED) ) + { + g_display.enableGreenLed( ); + } + else + { + g_display.enableRedLed( ); + } + +} + + +static void display_status_screen( void ) +{ + g_display.eraseBuffer(); + + g_display.setCursor( ORIGIN_X, ORIGIN_Y ); + g_display.print( "STATUS" ); + + draw_header_line( ); + + g_display.setCursor( ORIGIN_X, BRAKE_STATUS_Y ); + + g_display.print( "BRAKES: " ); + print_module_status( g_display_state.status_screen.brakes ); + + g_display.setCursor( ORIGIN_X, STEERING_STATUS_Y ); + + g_display.print( "STEERING: " ); + print_module_status( g_display_state.status_screen.steering ); + + g_display.setCursor( ORIGIN_X, THROTTLE_STATUS_Y ); + + g_display.print( "THROTTLE: " ); + print_module_status( g_display_state.status_screen.throttle ); + + g_display.sendBuffer(); +} + + +static void display_dtc_screen( void ) +{ + g_display.eraseBuffer( ); + + g_display.setCursor( ORIGIN_X, ORIGIN_Y ); + g_display.print( "DTCS" ); + + draw_header_line( ); + + if( g_display_state.status_screen.brakes == MODULE_STATUS_ERROR ) + { + print_brake_dtcs( ); + } + + if( g_display_state.status_screen.steering == MODULE_STATUS_ERROR ) + { + print_steering_dtcs( ); + } + + if( g_display_state.status_screen.throttle == MODULE_STATUS_ERROR ) + { + print_throttle_dtcs( ); + } + + g_display.sendBuffer( ); +} + + +static void draw_header_line( void ) +{ + g_display.setCursor( ORIGIN_X, HEADER_LINE_Y ); + + int pixel; + for( pixel = HEADER_LINE_X_START; pixel <= HEADER_LINE_X_STOP; ++pixel ) + { + g_display.drawPixel(pixel, HEADER_LINE_Y, WHITE); + } +} + + +static void print_module_status( module_status_t status ) +{ + const char * status_string = module_status_strings[status]; + + if( status == MODULE_STATUS_ERROR ) + { + g_display.setTextColor( BLACK, WHITE ); + g_display.print( status_string ); + g_display.setTextColor( WHITE, BLACK ); + } + else + { + g_display.print( status_string ); + } + + g_display.print( "\n\n" ); +} + + +static void print_brake_dtcs( void ) +{ + g_display.setCursor( BRAKE_DTC_X, DTC_Y_START ); + + int row_position = DTC_Y_START; + + for( int dtc = 0; dtc < OSCC_BRAKE_DTC_COUNT; ++dtc ) + { + if( g_display_state.dtc_screen.brakes[dtc] == true ) + { + g_display.setCursor( + BRAKE_DTC_X, + row_position ); + + print_dtc( "P1B", dtc ); + + row_position += ROW_SPACING; + } + } +} + + +static void print_steering_dtcs( void ) +{ + g_display.setCursor( STEERING_DTC_X, DTC_Y_START ) ; + + int row_position = DTC_Y_START; + + for( int dtc = 0; dtc < OSCC_STEERING_DTC_COUNT; ++dtc ) + { + if( g_display_state.dtc_screen.steering[dtc] == true ) + { + g_display.setCursor( + STEERING_DTC_X, + row_position ); + + print_dtc( "P1S", dtc ); + + row_position += ROW_SPACING; + } + } +} + + +static void print_throttle_dtcs( void ) +{ + g_display.setCursor( THROTTLE_DTC_X, DTC_Y_START ); + + int row_position = DTC_Y_START; + + for( int dtc = 0; dtc < OSCC_THROTTLE_DTC_COUNT; ++dtc ) + { + if( g_display_state.dtc_screen.throttle[dtc] == true ) + { + g_display.setCursor( + THROTTLE_DTC_X, + row_position ); + + print_dtc( "P1T", dtc ); + + row_position += ROW_SPACING; + } + } +} + + +static void print_dtc( const char *type, int num ) +{ + g_display.print( type ); + print_padded_number( num ); +} + + +static void print_padded_number( const unsigned int number ) +{ + if( number < 10 ) + { + g_display.print( "0" ); + } + + g_display.print( number ); +} + + +static void read_button( void ) +{ + static bool button_val_last = false; + + bool button_val_current = g_display.readButton(); + + if( (button_val_current != button_val_last) // prevent multiple button presses + && (button_val_current == true) ) + { + g_display_state.current_screen = + (screen_t)((g_display_state.current_screen + 1) % SCREEN_COUNT); + } + + button_val_last = button_val_current; +} diff --git a/firmware/can_gateway/src/init.cpp b/firmware/can_gateway/src/init.cpp index f8182239..b58e1579 100644 --- a/firmware/can_gateway/src/init.cpp +++ b/firmware/can_gateway/src/init.cpp @@ -11,12 +11,24 @@ #include "oscc_serial.h" +void init_globals( void ) +{ + memset( + &g_display_state, + 0, + sizeof(g_display_state) ); +} + + void init_communication_interfaces( void ) { #ifdef DEBUG init_serial(); #endif + DEBUG_PRINTLN( "init display"); + init_display( ); + DEBUG_PRINT( "init OBD CAN - "); init_can( g_obd_can ); diff --git a/firmware/can_gateway/src/main.cpp b/firmware/can_gateway/src/main.cpp index 70b5bfd8..72d9714a 100644 --- a/firmware/can_gateway/src/main.cpp +++ b/firmware/can_gateway/src/main.cpp @@ -10,15 +10,20 @@ #include "communications.h" #include "debug.h" #include "init.h" +#include "timer.h" int main( void ) { init_arduino( ); + init_globals( ); + init_communication_interfaces( ); - wdt_enable( WDTO_120MS ); + start_timer( ); + + wdt_enable( WDTO_250MS ); DEBUG_PRINTLN( "init complete" ); @@ -26,6 +31,8 @@ int main( void ) { wdt_reset(); + check_for_module_reports( ); + republish_obd_frames_to_control_can_bus( ); } } diff --git a/firmware/can_gateway/src/timer.cpp b/firmware/can_gateway/src/timer.cpp new file mode 100644 index 00000000..df004c6c --- /dev/null +++ b/firmware/can_gateway/src/timer.cpp @@ -0,0 +1,16 @@ +/** + * @file timer.cpp + * + */ + + +#include "oscc_timer.h" + +#include "timer.h" +#include "display.h" + + +void start_timer( void ) +{ + timer1_init( DISPLAY_UPDATE_FREQUENCY_IN_HZ, update_display ); +} diff --git a/firmware/common/libs/fault_check/oscc_check.cpp b/firmware/common/libs/fault_check/oscc_check.cpp new file mode 100644 index 00000000..3a6ab8ea --- /dev/null +++ b/firmware/common/libs/fault_check/oscc_check.cpp @@ -0,0 +1,34 @@ +/** + * @file oscc_check.cpp + * + */ + + +#include + +#include "oscc_check.h" +#include "vehicles.h" + + +bool check_voltage_grounded( uint16_t high, uint16_t low ) { + + static unsigned long elapsed_detection_time = 0; + unsigned long current_time = millis(); + + bool ret = false; + if( (high == 0) || (low == 0) ) + { + if ( elapsed_detection_time == 0 ) + { + elapsed_detection_time = millis(); + } + + ret = ( current_time - elapsed_detection_time ) > FAULT_HYSTERESIS; + } + else + { + elapsed_detection_time = 0; + } + + return ret; +} diff --git a/firmware/common/libs/fault_check/oscc_check.h b/firmware/common/libs/fault_check/oscc_check.h new file mode 100644 index 00000000..a3a82138 --- /dev/null +++ b/firmware/common/libs/fault_check/oscc_check.h @@ -0,0 +1,28 @@ +/** + * @file oscc_check.h + * @brief common fault checking functions. + * + */ + + +#ifndef _OSCC_CHECK_H_ +#define _OSCC_CHECK_H_ + +#include + +// **************************************************************************** +// Function: check_voltage_grounded +// +// Purpose: Check if the voltage is ground for the period of time determined +// by the configuration define of HYSTERESIS. This function is +// assumed to be called periodicly. +// +// Returns: bool where true means there is a fault after the HYSTERESIS time +// +// Parameters: [in] high - the high pin to check for ground +// [in] low - the low pin to check for ground +// +// **************************************************************************** +bool check_voltage_grounded( uint16_t high, uint16_t low ); + +#endif /* _OSCC_CHECK_H_ */ diff --git a/firmware/common/libs/ssd1325/gfx/gfx.cpp b/firmware/common/libs/ssd1325/gfx/gfx.cpp new file mode 100755 index 00000000..ac1bbb01 --- /dev/null +++ b/firmware/common/libs/ssd1325/gfx/gfx.cpp @@ -0,0 +1,213 @@ +/* +This is the core graphics library for all our displays, providing a common +set of graphics primitives (points, lines, circles, etc.). It needs to be +paired with a hardware-specific library for each display device we carry +(to handle the lower-level functions). + +Adafruit invests time and resources providing this open source code, please +support Adafruit & open-source hardware by purchasing products from Adafruit! + +Copyright (c) 2013 Adafruit Industries. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +- Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. +- Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE. + */ + +#include "gfx.h" +#include "glcdfont.c" + + +#ifndef _swap_int16_t +#define _swap_int16_t(a, b) { int16_t t = a; a = b; b = t; } +#endif + + +GFX::GFX(int16_t w, int16_t h): +WIDTH(w), HEIGHT(h) +{ + _width = WIDTH; + _height = HEIGHT; + rotation = 0; + cursor_y = cursor_x = 0; + textsize = 1; + textcolor = textbgcolor = 0xFFFF; + wrap = false; +} + + +void GFX::startWrite(){ + // Overwrite in subclasses if desired! +} + +void GFX::writePixel(int16_t x, int16_t y, uint16_t color){ + // Overwrite in subclasses if startWrite is defined! + drawPixel(x, y, color); +} + +void GFX::endWrite(){ + // Overwrite in subclasses if startWrite is defined! +} + +// TEXT- AND CHARACTER-HANDLING FUNCTIONS ---------------------------------- + +// Draw a character +void GFX::drawChar(int16_t x, int16_t y, unsigned char c, + uint16_t color, uint16_t bg, uint8_t size) { + if((x >= _width) || // Clip right + (y >= _height) || // Clip bottom + ((x + 6 * size - 1) < 0) || // Clip left + ((y + 8 * size - 1) < 0)) // Clip top + return; + + startWrite(); + for(int8_t i=0; i<5; i++ ) { // Char bitmap = 5 columns + uint8_t line = pgm_read_byte(&font[c * 5 + i]); + for(int8_t j=0; j<8; j++, line >>= 1) { + if(line & 1) { + if(size == 1) + writePixel(x+i, y+j, color); + } else if(bg != color) { + if(size == 1) + writePixel(x+i, y+j, bg); + } + } + } + // If opaque, draw vertical line for last column + if(bg != color) { + writeLine(x+5, y, x+5, y+7, bg); + } + endWrite(); +} + +#if ARDUINO >= 100 +size_t GFX::write(uint8_t c) { +#else +void GFX::write(uint8_t c) { +#endif + if(c == '\n') { // Newline? + cursor_x = 0; // Reset x to zero, + cursor_y += textsize * 8; // advance y one line + } else if(c != '\r') { // Ignore carriage returns + if(wrap && ((cursor_x + textsize * 6) > _width)) { // Off right? + cursor_x = 0; // Reset x to zero, + cursor_y += textsize * 8; // advance y one line + } + drawChar(cursor_x, cursor_y, c, textcolor, textbgcolor, textsize); + cursor_x += textsize * 6; // Advance x one char + } +#if ARDUINO >= 100 + return 1; +#endif +} + +void GFX::setCursor(int16_t x, int16_t y) { + cursor_x = x; + cursor_y = y; +} + +int16_t GFX::getCursorX(void) const { + return cursor_x; +} + +int16_t GFX::getCursorY(void) const { + return cursor_y; +} + +void GFX::setTextColor(uint16_t c) { + // For 'transparent' background, we'll set the bg + // to the same as fg instead of using a flag + textcolor = textbgcolor = c; +} + +void GFX::setTextColor(uint16_t c, uint16_t b) { + textcolor = c; + textbgcolor = b; +} + +uint8_t GFX::getRotation(void) const { + return rotation; +} + +void GFX::setRotation(uint8_t x) { + rotation = (x & 3); + switch(rotation) { + case 0: + case 2: + _width = WIDTH; + _height = HEIGHT; + break; + case 1: + case 3: + _width = HEIGHT; + _height = WIDTH; + break; + } +} + +// Return the size of the display (per current rotation) +int16_t GFX::width(void) const { + return _width; +} + +int16_t GFX::height(void) const { + return _height; +} + +// Bresenham's algorithm +void GFX::writeLine(int16_t x0, int16_t y0, int16_t x1, int16_t y1, + uint16_t color) { + int16_t steep = abs(y1 - y0) > abs(x1 - x0); + if (steep) { + _swap_int16_t(x0, y0); + _swap_int16_t(x1, y1); + } + + if (x0 > x1) { + _swap_int16_t(x0, x1); + _swap_int16_t(y0, y1); + } + + int16_t dx, dy; + dx = x1 - x0; + dy = abs(y1 - y0); + + int16_t err = dx / 2; + int16_t ystep; + + if (y0 < y1) { + ystep = 1; + } else { + ystep = -1; + } + + for (; x0<=x1; x0++) { + if (steep) { + writePixel(y0, x0, color); + } else { + writePixel(x0, y0, color); + } + err -= dy; + if (err < 0) { + y0 += ystep; + err += dx; + } + } +} diff --git a/firmware/common/libs/ssd1325/gfx/gfx.h b/firmware/common/libs/ssd1325/gfx/gfx.h new file mode 100755 index 00000000..d970459d --- /dev/null +++ b/firmware/common/libs/ssd1325/gfx/gfx.h @@ -0,0 +1,73 @@ +#ifndef _ADAFRUIT_GFX_H +#define _ADAFRUIT_GFX_H + +#if ARDUINO >= 100 + #include "Arduino.h" + #include "Print.h" +#else + #include "WProgram.h" +#endif + +class GFX : public Print { + + public: + + GFX(int16_t w, int16_t h); // Constructor + + // This MUST be defined by the subclass: + virtual void drawPixel(int16_t x, int16_t y, uint16_t color) = 0; + + // TRANSACTION API / CORE DRAW API + // These MAY be overridden by the subclass to provide device-specific + // optimized code. Otherwise 'generic' versions are used. + virtual void startWrite(void); + virtual void writePixel(int16_t x, int16_t y, uint16_t color); + virtual void endWrite(void); + + // CONTROL API + // These MAY be overridden by the subclass to provide device-specific + // optimized code. Otherwise 'generic' versions are used. + virtual void setRotation(uint8_t r); + + // These exist only with GFX (no subclass overrides) + void + drawChar(int16_t x, int16_t y, unsigned char c, uint16_t color, + uint16_t bg, uint8_t size), + setCursor(int16_t x, int16_t y), + setTextColor(uint16_t c), + setTextColor(uint16_t c, uint16_t bg); + +#if ARDUINO >= 100 + virtual size_t write(uint8_t); +#else + virtual void write(uint8_t); +#endif + + int16_t height(void) const; + int16_t width(void) const; + + uint8_t getRotation(void) const; + + // get current cursor position (get rotation safe maximum values, using: width() for x, height() for y) + int16_t getCursorX(void) const; + int16_t getCursorY(void) const; + + protected: + const int16_t + WIDTH, HEIGHT; // This is the 'raw' display w/h - never changes + int16_t + _width, _height, // Display w/h as modified by current rotation + cursor_x, cursor_y; + uint16_t + textcolor, textbgcolor; + uint8_t + textsize, + rotation; + boolean + wrap; // If set, 'wrap' text at right edge of display + void writeLine(int16_t x0, int16_t y0, int16_t x1, int16_t y1, + uint16_t color); +}; + + +#endif // _ADAFRUIT_GFX_H diff --git a/firmware/common/libs/ssd1325/gfx/glcdfont.c b/firmware/common/libs/ssd1325/gfx/glcdfont.c new file mode 100644 index 00000000..16f26c5d --- /dev/null +++ b/firmware/common/libs/ssd1325/gfx/glcdfont.c @@ -0,0 +1,266 @@ +// This is the 'classic' fixed-space bitmap font for Adafruit_GFX since 1.0. +// See gfxfont.h for newer custom bitmap font info. + +#ifndef FONT5X7_H +#define FONT5X7_H + +// Standard ASCII 5x7 font +static const unsigned char font[] PROGMEM = { + 0x00, 0x00, 0x00, 0x00, 0x00, + 0x3E, 0x5B, 0x4F, 0x5B, 0x3E, + 0x3E, 0x6B, 0x4F, 0x6B, 0x3E, + 0x1C, 0x3E, 0x7C, 0x3E, 0x1C, + 0x18, 0x3C, 0x7E, 0x3C, 0x18, + 0x1C, 0x57, 0x7D, 0x57, 0x1C, + 0x1C, 0x5E, 0x7F, 0x5E, 0x1C, + 0x00, 0x18, 0x3C, 0x18, 0x00, + 0xFF, 0xE7, 0xC3, 0xE7, 0xFF, + 0x00, 0x18, 0x24, 0x18, 0x00, + 0xFF, 0xE7, 0xDB, 0xE7, 0xFF, + 0x30, 0x48, 0x3A, 0x06, 0x0E, + 0x26, 0x29, 0x79, 0x29, 0x26, + 0x40, 0x7F, 0x05, 0x05, 0x07, + 0x40, 0x7F, 0x05, 0x25, 0x3F, + 0x5A, 0x3C, 0xE7, 0x3C, 0x5A, + 0x7F, 0x3E, 0x1C, 0x1C, 0x08, + 0x08, 0x1C, 0x1C, 0x3E, 0x7F, + 0x14, 0x22, 0x7F, 0x22, 0x14, + 0x5F, 0x5F, 0x00, 0x5F, 0x5F, + 0x06, 0x09, 0x7F, 0x01, 0x7F, + 0x00, 0x66, 0x89, 0x95, 0x6A, + 0x60, 0x60, 0x60, 0x60, 0x60, + 0x94, 0xA2, 0xFF, 0xA2, 0x94, + 0x08, 0x04, 0x7E, 0x04, 0x08, + 0x10, 0x20, 0x7E, 0x20, 0x10, + 0x08, 0x08, 0x2A, 0x1C, 0x08, + 0x08, 0x1C, 0x2A, 0x08, 0x08, + 0x1E, 0x10, 0x10, 0x10, 0x10, + 0x0C, 0x1E, 0x0C, 0x1E, 0x0C, + 0x30, 0x38, 0x3E, 0x38, 0x30, + 0x06, 0x0E, 0x3E, 0x0E, 0x06, + 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x5F, 0x00, 0x00, + 0x00, 0x07, 0x00, 0x07, 0x00, + 0x14, 0x7F, 0x14, 0x7F, 0x14, + 0x24, 0x2A, 0x7F, 0x2A, 0x12, + 0x23, 0x13, 0x08, 0x64, 0x62, + 0x36, 0x49, 0x56, 0x20, 0x50, + 0x00, 0x08, 0x07, 0x03, 0x00, + 0x00, 0x1C, 0x22, 0x41, 0x00, + 0x00, 0x41, 0x22, 0x1C, 0x00, + 0x2A, 0x1C, 0x7F, 0x1C, 0x2A, + 0x08, 0x08, 0x3E, 0x08, 0x08, + 0x00, 0x80, 0x70, 0x30, 0x00, + 0x08, 0x08, 0x08, 0x08, 0x08, + 0x00, 0x00, 0x60, 0x60, 0x00, + 0x20, 0x10, 0x08, 0x04, 0x02, + 0x3E, 0x51, 0x49, 0x45, 0x3E, + 0x00, 0x42, 0x7F, 0x40, 0x00, + 0x72, 0x49, 0x49, 0x49, 0x46, + 0x21, 0x41, 0x49, 0x4D, 0x33, + 0x18, 0x14, 0x12, 0x7F, 0x10, + 0x27, 0x45, 0x45, 0x45, 0x39, + 0x3C, 0x4A, 0x49, 0x49, 0x31, + 0x41, 0x21, 0x11, 0x09, 0x07, + 0x36, 0x49, 0x49, 0x49, 0x36, + 0x46, 0x49, 0x49, 0x29, 0x1E, + 0x00, 0x00, 0x14, 0x00, 0x00, + 0x00, 0x40, 0x34, 0x00, 0x00, + 0x00, 0x08, 0x14, 0x22, 0x41, + 0x14, 0x14, 0x14, 0x14, 0x14, + 0x00, 0x41, 0x22, 0x14, 0x08, + 0x02, 0x01, 0x59, 0x09, 0x06, + 0x3E, 0x41, 0x5D, 0x59, 0x4E, + 0x7C, 0x12, 0x11, 0x12, 0x7C, + 0x7F, 0x49, 0x49, 0x49, 0x36, + 0x3E, 0x41, 0x41, 0x41, 0x22, + 0x7F, 0x41, 0x41, 0x41, 0x3E, + 0x7F, 0x49, 0x49, 0x49, 0x41, + 0x7F, 0x09, 0x09, 0x09, 0x01, + 0x3E, 0x41, 0x41, 0x51, 0x73, + 0x7F, 0x08, 0x08, 0x08, 0x7F, + 0x00, 0x41, 0x7F, 0x41, 0x00, + 0x20, 0x40, 0x41, 0x3F, 0x01, + 0x7F, 0x08, 0x14, 0x22, 0x41, + 0x7F, 0x40, 0x40, 0x40, 0x40, + 0x7F, 0x02, 0x1C, 0x02, 0x7F, + 0x7F, 0x04, 0x08, 0x10, 0x7F, + 0x3E, 0x41, 0x41, 0x41, 0x3E, + 0x7F, 0x09, 0x09, 0x09, 0x06, + 0x3E, 0x41, 0x51, 0x21, 0x5E, + 0x7F, 0x09, 0x19, 0x29, 0x46, + 0x26, 0x49, 0x49, 0x49, 0x32, + 0x03, 0x01, 0x7F, 0x01, 0x03, + 0x3F, 0x40, 0x40, 0x40, 0x3F, + 0x1F, 0x20, 0x40, 0x20, 0x1F, + 0x3F, 0x40, 0x38, 0x40, 0x3F, + 0x63, 0x14, 0x08, 0x14, 0x63, + 0x03, 0x04, 0x78, 0x04, 0x03, + 0x61, 0x59, 0x49, 0x4D, 0x43, + 0x00, 0x7F, 0x41, 0x41, 0x41, + 0x02, 0x04, 0x08, 0x10, 0x20, + 0x00, 0x41, 0x41, 0x41, 0x7F, + 0x04, 0x02, 0x01, 0x02, 0x04, + 0x40, 0x40, 0x40, 0x40, 0x40, + 0x00, 0x03, 0x07, 0x08, 0x00, + 0x20, 0x54, 0x54, 0x78, 0x40, + 0x7F, 0x28, 0x44, 0x44, 0x38, + 0x38, 0x44, 0x44, 0x44, 0x28, + 0x38, 0x44, 0x44, 0x28, 0x7F, + 0x38, 0x54, 0x54, 0x54, 0x18, + 0x00, 0x08, 0x7E, 0x09, 0x02, + 0x18, 0xA4, 0xA4, 0x9C, 0x78, + 0x7F, 0x08, 0x04, 0x04, 0x78, + 0x00, 0x44, 0x7D, 0x40, 0x00, + 0x20, 0x40, 0x40, 0x3D, 0x00, + 0x7F, 0x10, 0x28, 0x44, 0x00, + 0x00, 0x41, 0x7F, 0x40, 0x00, + 0x7C, 0x04, 0x78, 0x04, 0x78, + 0x7C, 0x08, 0x04, 0x04, 0x78, + 0x38, 0x44, 0x44, 0x44, 0x38, + 0xFC, 0x18, 0x24, 0x24, 0x18, + 0x18, 0x24, 0x24, 0x18, 0xFC, + 0x7C, 0x08, 0x04, 0x04, 0x08, + 0x48, 0x54, 0x54, 0x54, 0x24, + 0x04, 0x04, 0x3F, 0x44, 0x24, + 0x3C, 0x40, 0x40, 0x20, 0x7C, + 0x1C, 0x20, 0x40, 0x20, 0x1C, + 0x3C, 0x40, 0x30, 0x40, 0x3C, + 0x44, 0x28, 0x10, 0x28, 0x44, + 0x4C, 0x90, 0x90, 0x90, 0x7C, + 0x44, 0x64, 0x54, 0x4C, 0x44, + 0x00, 0x08, 0x36, 0x41, 0x00, + 0x00, 0x00, 0x77, 0x00, 0x00, + 0x00, 0x41, 0x36, 0x08, 0x00, + 0x02, 0x01, 0x02, 0x04, 0x02, + 0x3C, 0x26, 0x23, 0x26, 0x3C, + 0x1E, 0xA1, 0xA1, 0x61, 0x12, + 0x3A, 0x40, 0x40, 0x20, 0x7A, + 0x38, 0x54, 0x54, 0x55, 0x59, + 0x21, 0x55, 0x55, 0x79, 0x41, + 0x22, 0x54, 0x54, 0x78, 0x42, // a-umlaut + 0x21, 0x55, 0x54, 0x78, 0x40, + 0x20, 0x54, 0x55, 0x79, 0x40, + 0x0C, 0x1E, 0x52, 0x72, 0x12, + 0x39, 0x55, 0x55, 0x55, 0x59, + 0x39, 0x54, 0x54, 0x54, 0x59, + 0x39, 0x55, 0x54, 0x54, 0x58, + 0x00, 0x00, 0x45, 0x7C, 0x41, + 0x00, 0x02, 0x45, 0x7D, 0x42, + 0x00, 0x01, 0x45, 0x7C, 0x40, + 0x7D, 0x12, 0x11, 0x12, 0x7D, // A-umlaut + 0xF0, 0x28, 0x25, 0x28, 0xF0, + 0x7C, 0x54, 0x55, 0x45, 0x00, + 0x20, 0x54, 0x54, 0x7C, 0x54, + 0x7C, 0x0A, 0x09, 0x7F, 0x49, + 0x32, 0x49, 0x49, 0x49, 0x32, + 0x3A, 0x44, 0x44, 0x44, 0x3A, // o-umlaut + 0x32, 0x4A, 0x48, 0x48, 0x30, + 0x3A, 0x41, 0x41, 0x21, 0x7A, + 0x3A, 0x42, 0x40, 0x20, 0x78, + 0x00, 0x9D, 0xA0, 0xA0, 0x7D, + 0x3D, 0x42, 0x42, 0x42, 0x3D, // O-umlaut + 0x3D, 0x40, 0x40, 0x40, 0x3D, + 0x3C, 0x24, 0xFF, 0x24, 0x24, + 0x48, 0x7E, 0x49, 0x43, 0x66, + 0x2B, 0x2F, 0xFC, 0x2F, 0x2B, + 0xFF, 0x09, 0x29, 0xF6, 0x20, + 0xC0, 0x88, 0x7E, 0x09, 0x03, + 0x20, 0x54, 0x54, 0x79, 0x41, + 0x00, 0x00, 0x44, 0x7D, 0x41, + 0x30, 0x48, 0x48, 0x4A, 0x32, + 0x38, 0x40, 0x40, 0x22, 0x7A, + 0x00, 0x7A, 0x0A, 0x0A, 0x72, + 0x7D, 0x0D, 0x19, 0x31, 0x7D, + 0x26, 0x29, 0x29, 0x2F, 0x28, + 0x26, 0x29, 0x29, 0x29, 0x26, + 0x30, 0x48, 0x4D, 0x40, 0x20, + 0x38, 0x08, 0x08, 0x08, 0x08, + 0x08, 0x08, 0x08, 0x08, 0x38, + 0x2F, 0x10, 0xC8, 0xAC, 0xBA, + 0x2F, 0x10, 0x28, 0x34, 0xFA, + 0x00, 0x00, 0x7B, 0x00, 0x00, + 0x08, 0x14, 0x2A, 0x14, 0x22, + 0x22, 0x14, 0x2A, 0x14, 0x08, + 0x55, 0x00, 0x55, 0x00, 0x55, // #176 (25% block) missing in old code + 0xAA, 0x55, 0xAA, 0x55, 0xAA, // 50% block + 0xFF, 0x55, 0xFF, 0x55, 0xFF, // 75% block + 0x00, 0x00, 0x00, 0xFF, 0x00, + 0x10, 0x10, 0x10, 0xFF, 0x00, + 0x14, 0x14, 0x14, 0xFF, 0x00, + 0x10, 0x10, 0xFF, 0x00, 0xFF, + 0x10, 0x10, 0xF0, 0x10, 0xF0, + 0x14, 0x14, 0x14, 0xFC, 0x00, + 0x14, 0x14, 0xF7, 0x00, 0xFF, + 0x00, 0x00, 0xFF, 0x00, 0xFF, + 0x14, 0x14, 0xF4, 0x04, 0xFC, + 0x14, 0x14, 0x17, 0x10, 0x1F, + 0x10, 0x10, 0x1F, 0x10, 0x1F, + 0x14, 0x14, 0x14, 0x1F, 0x00, + 0x10, 0x10, 0x10, 0xF0, 0x00, + 0x00, 0x00, 0x00, 0x1F, 0x10, + 0x10, 0x10, 0x10, 0x1F, 0x10, + 0x10, 0x10, 0x10, 0xF0, 0x10, + 0x00, 0x00, 0x00, 0xFF, 0x10, + 0x10, 0x10, 0x10, 0x10, 0x10, + 0x10, 0x10, 0x10, 0xFF, 0x10, + 0x00, 0x00, 0x00, 0xFF, 0x14, + 0x00, 0x00, 0xFF, 0x00, 0xFF, + 0x00, 0x00, 0x1F, 0x10, 0x17, + 0x00, 0x00, 0xFC, 0x04, 0xF4, + 0x14, 0x14, 0x17, 0x10, 0x17, + 0x14, 0x14, 0xF4, 0x04, 0xF4, + 0x00, 0x00, 0xFF, 0x00, 0xF7, + 0x14, 0x14, 0x14, 0x14, 0x14, + 0x14, 0x14, 0xF7, 0x00, 0xF7, + 0x14, 0x14, 0x14, 0x17, 0x14, + 0x10, 0x10, 0x1F, 0x10, 0x1F, + 0x14, 0x14, 0x14, 0xF4, 0x14, + 0x10, 0x10, 0xF0, 0x10, 0xF0, + 0x00, 0x00, 0x1F, 0x10, 0x1F, + 0x00, 0x00, 0x00, 0x1F, 0x14, + 0x00, 0x00, 0x00, 0xFC, 0x14, + 0x00, 0x00, 0xF0, 0x10, 0xF0, + 0x10, 0x10, 0xFF, 0x10, 0xFF, + 0x14, 0x14, 0x14, 0xFF, 0x14, + 0x10, 0x10, 0x10, 0x1F, 0x00, + 0x00, 0x00, 0x00, 0xF0, 0x10, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, + 0xFF, 0xFF, 0xFF, 0x00, 0x00, + 0x00, 0x00, 0x00, 0xFF, 0xFF, + 0x0F, 0x0F, 0x0F, 0x0F, 0x0F, + 0x38, 0x44, 0x44, 0x38, 0x44, + 0xFC, 0x4A, 0x4A, 0x4A, 0x34, // sharp-s or beta + 0x7E, 0x02, 0x02, 0x06, 0x06, + 0x02, 0x7E, 0x02, 0x7E, 0x02, + 0x63, 0x55, 0x49, 0x41, 0x63, + 0x38, 0x44, 0x44, 0x3C, 0x04, + 0x40, 0x7E, 0x20, 0x1E, 0x20, + 0x06, 0x02, 0x7E, 0x02, 0x02, + 0x99, 0xA5, 0xE7, 0xA5, 0x99, + 0x1C, 0x2A, 0x49, 0x2A, 0x1C, + 0x4C, 0x72, 0x01, 0x72, 0x4C, + 0x30, 0x4A, 0x4D, 0x4D, 0x30, + 0x30, 0x48, 0x78, 0x48, 0x30, + 0xBC, 0x62, 0x5A, 0x46, 0x3D, + 0x3E, 0x49, 0x49, 0x49, 0x00, + 0x7E, 0x01, 0x01, 0x01, 0x7E, + 0x2A, 0x2A, 0x2A, 0x2A, 0x2A, + 0x44, 0x44, 0x5F, 0x44, 0x44, + 0x40, 0x51, 0x4A, 0x44, 0x40, + 0x40, 0x44, 0x4A, 0x51, 0x40, + 0x00, 0x00, 0xFF, 0x01, 0x03, + 0xE0, 0x80, 0xFF, 0x00, 0x00, + 0x08, 0x08, 0x6B, 0x6B, 0x08, + 0x36, 0x12, 0x36, 0x24, 0x36, + 0x06, 0x0F, 0x09, 0x0F, 0x06, + 0x00, 0x00, 0x18, 0x18, 0x00, + 0x00, 0x00, 0x10, 0x10, 0x00, + 0x30, 0x40, 0xFF, 0x01, 0x01, + 0x00, 0x1F, 0x01, 0x01, 0x1E, + 0x00, 0x19, 0x1D, 0x17, 0x12, + 0x00, 0x3C, 0x3C, 0x3C, 0x3C, + 0x00, 0x00, 0x00, 0x00, 0x00 // #255 NBSP +}; +#endif // FONT5X7_H diff --git a/firmware/common/libs/ssd1325/gfx/license.txt b/firmware/common/libs/ssd1325/gfx/license.txt new file mode 100644 index 00000000..7492e93a --- /dev/null +++ b/firmware/common/libs/ssd1325/gfx/license.txt @@ -0,0 +1,24 @@ +Software License Agreement (BSD License) + +Copyright (c) 2012 Adafruit Industries. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +- Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. +- Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE. diff --git a/firmware/common/libs/ssd1325/license.txt b/firmware/common/libs/ssd1325/license.txt new file mode 100644 index 00000000..0652a4aa --- /dev/null +++ b/firmware/common/libs/ssd1325/license.txt @@ -0,0 +1,26 @@ +Software License Agreement (BSD License) + +Copyright (c) 2015, Adafruit Industries +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright +notice, this list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright +notice, this list of conditions and the following disclaimer in the +documentation and/or other materials provided with the distribution. +3. Neither the name of the copyright holders nor the +names of its contributors may be used to endorse or promote products +derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY +EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/firmware/common/libs/ssd1325/ssd1325.cpp b/firmware/common/libs/ssd1325/ssd1325.cpp new file mode 100644 index 00000000..bdf99502 --- /dev/null +++ b/firmware/common/libs/ssd1325/ssd1325.cpp @@ -0,0 +1,385 @@ +/********************************************************************* +This is a library for our Monochrome OLEDs based on SSD1325 drivers + + Pick one up today in the adafruit shop! + ------> http://www.adafruit.com/category/63_98 + +These displays use SPI to communicate, 4 or 5 pins are required to +interface + +Adafruit invests time and resources providing this open source code, +please support Adafruit and open-source hardware by purchasing +products from Adafruit! + +Written by Limor Fried/Ladyada for Adafruit Industries. +BSD license, check license.txt for more information +All text above, and the splash screen below must be included in any redistribution +*********************************************************************/ + +#include +#include +#include "gfx.h" +#include "glcdfont.c" +#include "ssd1325.h" + + +// Chip select of first shift register +#define SHIFT_REGISTER_CHIP_SELECT_1 ( 4 ) + +// Chip select of second shift register +#define SHIFT_REGISTER_CHIP_SELECT_2 ( 5 ) + +// a 5x7 font table +extern const uint8_t PROGMEM font[]; + +// buffer at startup contains logo +static uint8_t buffer[SSD1325_LCDHEIGHT * SSD1325_LCDWIDTH / 8] = { +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0xc0, 0xe0, 0xe0, 0xf0, 0xf8, 0xf8, 0xf8, 0xfc, 0xfc, +0xfc, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, +0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, +0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, +0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, +0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, +0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfe, 0xfc, +0xfc, 0xfc, 0xf8, 0xf8, 0xf8, 0xf0, 0xe0, 0xe0, 0xc0, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x80, 0xf0, 0xf8, 0xfe, 0xff, 0xff, 0xff, 0xff, 0xff, 0x7f, 0x3f, 0xdf, 0x6f, 0x27, 0xb3, +0xdb, 0x4b, 0x69, 0x2d, 0x25, 0xa5, 0xa5, 0xa5, 0x2d, 0x6d, 0x49, 0xdb, 0x93, 0x37, 0x67, 0xcf, +0x9f, 0x3f, 0x7f, 0xff, 0xff, 0xff, 0xff, 0xff, 0x3f, 0xcf, 0x67, 0x33, 0x9b, 0xdb, 0x49, 0x6d, +0x2d, 0x25, 0x25, 0x25, 0x25, 0x25, 0x6d, 0x6d, 0x49, 0xdb, 0x9b, 0xb3, 0xa7, 0xef, 0xef, 0xff, +0xff, 0xff, 0xff, 0x7f, 0x3f, 0x9f, 0xcf, 0x67, 0xb3, 0x9b, 0xdb, 0x49, 0x6d, 0x25, 0xa5, 0xa5, +0xa5, 0x25, 0x2d, 0x69, 0x4b, 0xdb, 0xb3, 0x27, 0x4f, 0xdf, 0xbf, 0xff, 0xff, 0xff, 0xff, 0x7f, +0x3f, 0xdf, 0x4f, 0x27, 0xb3, 0xdb, 0x4b, 0x49, 0x2d, 0x25, 0xa5, 0xa5, 0xa5, 0x25, 0x6d, 0x49, +0xdb, 0xdb, 0xb3, 0x27, 0x4f, 0x9f, 0xbf, 0xff, 0xff, 0xff, 0xff, 0xfe, 0xf8, 0xf0, 0xc0, 0x00, +0xfc, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x03, 0xf8, 0x3e, 0x03, 0xf8, 0x0e, 0xe3, 0xf9, +0xfc, 0xfe, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0xfc, 0xf1, 0x07, 0x3c, +0xf1, 0x07, 0xfc, 0xe1, 0x0f, 0xff, 0xff, 0xf0, 0xc0, 0x3f, 0x60, 0xcf, 0x9f, 0xb0, 0x26, 0x2f, +0x6f, 0x6f, 0x6f, 0x6f, 0x6f, 0x6f, 0x6f, 0x4f, 0xcf, 0xde, 0x9e, 0x3f, 0x7f, 0xff, 0xff, 0xff, +0xff, 0x0f, 0xe1, 0xfc, 0x07, 0xf1, 0x1c, 0x07, 0xf1, 0xfc, 0xfe, 0xff, 0xff, 0xff, 0xff, 0xff, +0xff, 0xff, 0xff, 0xff, 0xfe, 0xfc, 0xfd, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x07, 0xe1, 0xfe, +0x07, 0xf9, 0x1c, 0x07, 0xf9, 0xfc, 0xfe, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, +0xfe, 0xfc, 0xfd, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfc, +0x1f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf0, 0xc7, 0x1e, 0x70, 0xc7, 0x9c, 0x71, 0x47, +0x9f, 0xbf, 0x3f, 0x7f, 0x7f, 0x7f, 0x7f, 0x7f, 0x7f, 0x7f, 0x3f, 0x9f, 0xcf, 0x63, 0x30, 0x8e, +0xe3, 0x38, 0x9f, 0xc1, 0xf8, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0xbc, 0xbc, 0x3d, 0x79, 0x79, +0x7b, 0x7b, 0x7b, 0x7b, 0x7b, 0x7b, 0x7b, 0x32, 0x86, 0xfc, 0x39, 0x03, 0xfe, 0x00, 0x87, 0xff, +0xff, 0xf8, 0xc3, 0x9f, 0x70, 0xc3, 0x9e, 0x30, 0x67, 0xcf, 0x9f, 0x3f, 0x7f, 0x7f, 0x7f, 0x7f, +0x7f, 0x7f, 0x7f, 0x3f, 0xbf, 0xdf, 0x5f, 0x3f, 0xbf, 0xff, 0xff, 0xff, 0xff, 0xf0, 0xc3, 0x1f, +0x70, 0xc7, 0x9e, 0x30, 0x67, 0xcf, 0xbf, 0x3f, 0x7f, 0x7f, 0x7f, 0x7f, 0x7f, 0x7f, 0x7f, 0x3f, +0x9f, 0xcf, 0x7f, 0x3f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x1f, +0x00, 0x00, 0x07, 0x0f, 0x3f, 0x7f, 0xff, 0xff, 0xff, 0xff, 0xff, 0xfe, 0xfc, 0xf9, 0xf3, 0xf6, +0xe4, 0xed, 0xed, 0xc9, 0xcb, 0xcb, 0xcb, 0xcb, 0xcb, 0xe9, 0xed, 0xe4, 0xf6, 0xf2, 0xfb, 0xfd, +0xfc, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xf9, 0xf2, 0xf6, 0xe4, 0xed, 0xc9, 0xc9, 0xdb, +0xdb, 0xda, 0xda, 0xda, 0xda, 0xdb, 0xdb, 0xc9, 0xed, 0xe4, 0xf6, 0xf3, 0xf9, 0xfe, 0xff, 0xff, +0xff, 0xff, 0xff, 0xff, 0xfe, 0xfc, 0xf9, 0xfb, 0xf2, 0xe6, 0xec, 0xed, 0xe9, 0xcb, 0xcb, 0xcb, +0xcb, 0xcb, 0xc9, 0xed, 0xed, 0xe4, 0xf2, 0xf3, 0xf9, 0xfc, 0xfe, 0xff, 0xff, 0xff, 0xff, 0xff, +0xfe, 0xfc, 0xf9, 0xfb, 0xf2, 0xe4, 0xed, 0xed, 0xc9, 0xcb, 0xcb, 0xcb, 0xcb, 0xcb, 0xc9, 0xed, +0xed, 0xe4, 0xf2, 0xfb, 0xf9, 0xfc, 0xfe, 0xff, 0xff, 0xff, 0x7f, 0x3f, 0x0f, 0x07, 0x01, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x03, 0x03, 0x07, 0x07, 0x0f, 0x0f, 0x1f, 0x1f, +0x1f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, +0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, +0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, +0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, +0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, +0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x3f, 0x1f, +0x1f, 0x1f, 0x0f, 0x0f, 0x0f, 0x07, 0x03, 0x03, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +}; + + +void SSD1325::begin(void) +{ + SPI.begin(); + SPI.setDataMode(SPI_MODE0); + SPI.setClockDivider(SPI_CLOCK_DIV2); + + pinMode(SHIFT_REGISTER_CHIP_SELECT_1, OUTPUT); + pinMode(SHIFT_REGISTER_CHIP_SELECT_2, OUTPUT); + pinMode(cs, OUTPUT); + + digitalWrite(SHIFT_REGISTER_CHIP_SELECT_1, LOW); + digitalWrite(SHIFT_REGISTER_CHIP_SELECT_2, HIGH); + digitalWrite(cs, HIGH); + + setRes(1); + delay(1); + setRes(0); + delay(10); + setRes(1); + + delay(500); + + startSendCommand(); + SPI.transfer(SSD1325_DISPLAYOFF); /* display off */ + SPI.transfer(SSD1325_SETCLOCK); /* set osc division */ + SPI.transfer(0xF1); /* 145 */ + SPI.transfer(SSD1325_SETMULTIPLEX ); /* multiplex ratio */ + SPI.transfer(0x3f); /* duty = 1/64 */ + SPI.transfer( SSD1325_SETOFFSET); /* set display offset --- */ + SPI.transfer(0x4C); /* 76 */ + SPI.transfer(SSD1325_SETSTARTLINE); /*set start line */ + SPI.transfer(0x00); /* ------ */ + SPI.transfer(SSD1325_MASTERCONFIG); /*Set Master Config DC/DC Converter*/ + SPI.transfer(0x02); + SPI.transfer(SSD1325_SETREMAP); /* set segment remap------ */ + SPI.transfer(0x56); + SPI.transfer(SSD1325_SETCURRENT + 0x1); /* Set Current Range */ + SPI.transfer(SSD1325_SETGRAYTABLE); + SPI.transfer(0x01); + SPI.transfer(0x11); + SPI.transfer(0x22); + SPI.transfer(0x32); + SPI.transfer(0x43); + SPI.transfer(0x54); + SPI.transfer(0x65); + SPI.transfer(0x76); + SPI.transfer(SSD1325_SETCONTRAST); /* set contrast current */ + SPI.transfer(0x7F); // max! + SPI.transfer(SSD1325_SETROWPERIOD); + SPI.transfer(0x51); + SPI.transfer(SSD1325_SETPHASELEN); + SPI.transfer(0x55); + SPI.transfer(SSD1325_SETPRECHARGECOMP); + SPI.transfer(0x02); + SPI.transfer(SSD1325_SETPRECHARGECOMPENABLE); + SPI.transfer(0x28); + SPI.transfer(SSD1325_SETVCOMLEVEL); // Set High Voltage Level of COM Pin + SPI.transfer(0x1C); //? + SPI.transfer(SSD1325_SETVSL); // set Low Voltage Level of SEG Pin + SPI.transfer(0x0D|0x02); + SPI.transfer(SSD1325_NORMALDISPLAY); /* set display mode */ + SPI.transfer(0x15); /* set column address */ + SPI.transfer(0x00); /* set column start address */ + SPI.transfer(0x3f); /* set column end address */ + SPI.transfer(0x75); /* set row address */ + SPI.transfer(0x00); /* set row start address */ + SPI.transfer(0x3f); /* set row end address */ + SPI.transfer(SSD1325_DISPLAYON); /* display ON */ + stopSendCommand(); +} + + +void SSD1325::sendBuffer(void) +{ + startSendData(); + + for (uint16_t x=0; x<128; x+=2) + { + for (uint16_t y=0; y<64; y+=8) + { // we write 8 pixels at once + uint8_t left8 = buffer[y*16+x]; + uint8_t right8 = buffer[y*16+x+1]; + + for (uint8_t p=0; p<8; p++) + { + uint8_t d = 0; + + if (left8 & (1 << p)) + { + d |= 0xF0; + }; + + if (right8 & (1 << p)) + { + d |= 0x0F; + } + + SPI.transfer(d); + } + } + } + + stopSendData(); +} + + +void SSD1325::eraseBuffer(void) +{ + memset(buffer, 0, sizeof(buffer)); +} + + +void SSD1325::drawPixel(int16_t x, int16_t y, uint16_t color) +{ + if ((x >= width()) || (y >= height()) || (x < 0) || (y < 0)) + { + return; + } + + // check rotation, move pixel around if necessary + switch (getRotation()) + { + case 1: + gfx_swap(x, y); + x = WIDTH - x - 1; + break; + case 2: + x = WIDTH - x - 1; + y = HEIGHT - y - 1; + break; + case 3: + gfx_swap(x, y); + y = HEIGHT - y - 1; + break; + } + + // x is which column + if (color == WHITE) + { + buffer[x+ (y/8)*SSD1325_LCDWIDTH] |= _BV((y%8)); + } + else + { + buffer[x+ (y/8)*SSD1325_LCDWIDTH] &= ~_BV((y%8)); + } +} + + +bool SSD1325::readButton() +{ + digitalWrite(SHIFT_REGISTER_CHIP_SELECT_2, HIGH); + delay(1); + digitalWrite(SHIFT_REGISTER_CHIP_SELECT_2, LOW); + delay(1); + digitalWrite(SCK, LOW); + delay(1); + uint8_t data = SPI.transfer(0); + delay(1); + digitalWrite(SCK, HIGH); + delay(1); + digitalWrite(SHIFT_REGISTER_CHIP_SELECT_2, HIGH); + delay(1); + + if (data & 0x01) + { + return false; + } + else + { + return true; + } +} + + +void SSD1325::enableRedLed() +{ + shift_register_data = ((0x01 << 4) & 0x70) | (shift_register_data & ~0x70); + + digitalWrite(SHIFT_REGISTER_CHIP_SELECT_1, LOW); + delay(1); + SPI.transfer(shift_register_data); + delay(1); + digitalWrite(SHIFT_REGISTER_CHIP_SELECT_1, HIGH); + delay(1); + digitalWrite(SHIFT_REGISTER_CHIP_SELECT_1, LOW); +} + + +void SSD1325::enableYellowLed() +{ + shift_register_data = ((0x02 << 4) & 0x70) | (shift_register_data & ~0x70); + + digitalWrite(SHIFT_REGISTER_CHIP_SELECT_1, LOW); + delay(1); + SPI.transfer(shift_register_data); + delay(1); + digitalWrite(SHIFT_REGISTER_CHIP_SELECT_1, HIGH); + delay(1); + digitalWrite(SHIFT_REGISTER_CHIP_SELECT_1, LOW); +} + + +void SSD1325::enableGreenLed() +{ + shift_register_data = ((0x04 << 4) & 0x70) | (shift_register_data & ~0x70); + + digitalWrite(SHIFT_REGISTER_CHIP_SELECT_1, LOW); + delay(1); + SPI.transfer(shift_register_data); + delay(1); + digitalWrite(SHIFT_REGISTER_CHIP_SELECT_1, HIGH); + delay(1); + digitalWrite(SHIFT_REGISTER_CHIP_SELECT_1, LOW); +} + +void SSD1325::disableLeds() +{ + + shift_register_data = (shift_register_data & ~0x70); + + digitalWrite(SHIFT_REGISTER_CHIP_SELECT_1, LOW); + delay(1); + SPI.transfer(shift_register_data); + delay(1); + digitalWrite(SHIFT_REGISTER_CHIP_SELECT_1, HIGH); + delay(1); + digitalWrite(SHIFT_REGISTER_CHIP_SELECT_1, LOW); +} + + +void SSD1325::setDC(uint8_t value){ + shift_register_data = ((value << 7) & 0x80) | (shift_register_data & ~0x80); + + digitalWrite(SHIFT_REGISTER_CHIP_SELECT_1, LOW); + delay(1); + SPI.transfer(shift_register_data); + delay(1); + digitalWrite(SHIFT_REGISTER_CHIP_SELECT_1, HIGH); + delay(1); + digitalWrite(SHIFT_REGISTER_CHIP_SELECT_1, LOW); +} + + +void SSD1325::setRes(uint8_t value){ + shift_register_data = ((value << 3) & 0x08) | (shift_register_data & ~0x08); + + digitalWrite(SHIFT_REGISTER_CHIP_SELECT_1, LOW); + delay(1); + SPI.transfer(shift_register_data); + delay(1); + digitalWrite(SHIFT_REGISTER_CHIP_SELECT_1, HIGH); + delay(1); + digitalWrite(SHIFT_REGISTER_CHIP_SELECT_1, LOW); +} + + +void SSD1325::startSendCommand(void) +{ + setDC(0); + digitalWrite(cs, LOW); +} + + +void SSD1325::stopSendCommand(void) +{ + digitalWrite(cs, HIGH); + setDC(1); +} + + +void SSD1325::startSendData(void) +{ + setDC(1); + digitalWrite(cs, LOW); +} + + +void SSD1325::stopSendData(void) +{ + digitalWrite(cs, HIGH); + setDC(0); +} diff --git a/firmware/common/libs/ssd1325/ssd1325.h b/firmware/common/libs/ssd1325/ssd1325.h new file mode 100644 index 00000000..fc525170 --- /dev/null +++ b/firmware/common/libs/ssd1325/ssd1325.h @@ -0,0 +1,97 @@ +/********************************************************************* +This is a library for our Monochrome OLEDs based on SSD1325 drivers + + Pick one up today in the adafruit shop! + ------> http://www.adafruit.com/category/63_98 + +These sendBuffers use SPI to communicate, 4 or 5 pins are required to +interface + +Adafruit invests time and resources providing this open source code, +please support Adafruit and open-source hardware by purchasing +products from Adafruit! + +Written by Limor Fried/Ladyada for Adafruit Industries. +BSD license, check license.txt for more information +All text above, and the splash screen must be included in any redistribution +*********************************************************************/ + + +#if ARDUINO >= 100 + #include "Arduino.h" +#else + #include "WProgram.h" +#endif + +#ifdef __arm__ +#define _BV(b) (1<<(b)) +#endif + +#include "gfx.h" + +#ifndef gfx_swap +#define gfx_swap(a, b) { uint8_t t = a; a = b; b = t; } +#endif + +#define BLACK 0 +#define WHITE 1 + +#define SSD1325_LCDWIDTH 128 +#define SSD1325_LCDHEIGHT 64 + +#define SSD1325_SETCOLADDR 0x15 +#define SSD1325_SETROWADDR 0x75 +#define SSD1325_SETCONTRAST 0x81 +#define SSD1325_SETCURRENT 0x84 + +#define SSD1325_SETREMAP 0xA0 +#define SSD1325_SETSTARTLINE 0xA1 +#define SSD1325_SETOFFSET 0xA2 +#define SSD1325_NORMALDISPLAY 0xA4 +#define SSD1325_DISPLAYALLON 0xA5 +#define SSD1325_DISPLAYALLOFF 0xA6 +#define SSD1325_INVERTDISPLAY 0xA7 +#define SSD1325_SETMULTIPLEX 0xA8 +#define SSD1325_MASTERCONFIG 0xAD +#define SSD1325_DISPLAYOFF 0xAE +#define SSD1325_DISPLAYON 0xAF + +#define SSD1325_SETPRECHARGECOMPENABLE 0xB0 +#define SSD1325_SETPHASELEN 0xB1 +#define SSD1325_SETROWPERIOD 0xB2 +#define SSD1325_SETCLOCK 0xB3 +#define SSD1325_SETPRECHARGECOMP 0xB4 +#define SSD1325_SETGRAYTABLE 0xB8 +#define SSD1325_SETPRECHARGEVOLTAGE 0xBC +#define SSD1325_SETVCOMLEVEL 0xBE +#define SSD1325_SETVSL 0xBF + +#define SSD1325_GFXACCEL 0x23 +#define SSD1325_DRAWRECT 0x24 +#define SSD1325_COPY 0x25 + +class SSD1325 : public GFX { + public: + SSD1325(int8_t CS) : GFX(128,64), cs(CS) {} + + void begin(void); + void eraseBuffer(void); + void sendBuffer(void); + void drawPixel(int16_t x, int16_t y, uint16_t color); + bool readButton(void); + void enableRedLed(void); + void enableYellowLed(void); + void enableGreenLed(void); + void disableLeds(void); + + private: + int8_t cs; + uint8_t shift_register_data; + + void setDC(uint8_t value); + void setRes(uint8_t value); + void startSendCommand(void); + void stopSendCommand(void); + void startSendData(void); + void stopSendData(void); +}; diff --git a/firmware/common/testing/mocks/ssd1325.h b/firmware/common/testing/mocks/ssd1325.h new file mode 100644 index 00000000..d5a0a599 --- /dev/null +++ b/firmware/common/testing/mocks/ssd1325.h @@ -0,0 +1,24 @@ +#include + +class SSD1325 { +public: + SSD1325(int8_t SID, int8_t SCLK, int8_t DC, int8_t RST, int8_t CS){} + + void begin(void); + void eraseBuffer(void); + void sendBuffer(void); + +private: + int8_t sid; + int8_t sclk; + int8_t dc; + int8_t rst; + int8_t cs; + + void startSendCommand(void); + void stopSendCommand(void); + void startSendData(void); + void stopSendData(void); + + void drawPixel(int16_t x, int16_t y, uint16_t color); +}; diff --git a/firmware/steering/CMakeLists.txt b/firmware/steering/CMakeLists.txt index 4231b51d..cc8ad953 100644 --- a/firmware/steering/CMakeLists.txt +++ b/firmware/steering/CMakeLists.txt @@ -19,6 +19,7 @@ generate_arduino_firmware( 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/fault_check/oscc_check.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 @@ -29,8 +30,7 @@ generate_arduino_firmware( src/globals.cpp src/init.cpp src/communications.cpp - src/steering_control.cpp - src/timers.cpp) + src/steering_control.cpp) target_include_directories( steering @@ -39,6 +39,7 @@ target_include_directories( ${CMAKE_SOURCE_DIR}/common/include ${CMAKE_SOURCE_DIR}/common/libs/arduino_init ${CMAKE_SOURCE_DIR}/common/libs/DAC_MCP49xx + ${CMAKE_SOURCE_DIR}/common/libs/fault_check ${CMAKE_SOURCE_DIR}/common/libs/mcp_can ${CMAKE_SOURCE_DIR}/common/libs/pid ${CMAKE_SOURCE_DIR}/common/libs/serial diff --git a/firmware/steering/include/communications.h b/firmware/steering/include/communications.h index 78e5f6b9..df7d2c0d 100644 --- a/firmware/steering/include/communications.h +++ b/firmware/steering/include/communications.h @@ -35,20 +35,6 @@ void publish_steering_report( 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 // diff --git a/firmware/steering/include/globals.h b/firmware/steering/include/globals.h index 04927c77..a70033d0 100644 --- a/firmware/steering/include/globals.h +++ b/firmware/steering/include/globals.h @@ -70,8 +70,6 @@ #endif -EXTERN volatile bool g_steering_command_timeout; - EXTERN volatile steering_control_state_s g_steering_control_state; diff --git a/firmware/steering/include/init.h b/firmware/steering/include/init.h index 509a466f..cb3016f9 100644 --- a/firmware/steering/include/init.h +++ b/firmware/steering/include/init.h @@ -48,4 +48,17 @@ void init_devices( void ); void init_communication_interfaces( void ); +// **************************************************************************** +// Function: start_timers +// +// Purpose: Start timers for report publishing and fault checking. +// +// Returns: void +// +// Parameters: void +// +// **************************************************************************** +void start_timers( void ); + + #endif diff --git a/firmware/steering/include/steering_control.h b/firmware/steering/include/steering_control.h index 9035fae9..95fc7c21 100644 --- a/firmware/steering/include/steering_control.h +++ b/firmware/steering/include/steering_control.h @@ -43,31 +43,19 @@ typedef struct } 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. +// If operator override for steering is turned on detection of the +// steering wheel being manually turned is also detected. // // Returns: void // // Parameters: void // // **************************************************************************** -void check_for_sensor_faults( void ); +void check_for_faults( void ); // **************************************************************************** diff --git a/firmware/steering/include/timers.h b/firmware/steering/include/timers.h deleted file mode 100644 index f99ee377..00000000 --- a/firmware/steering/include/timers.h +++ /dev/null @@ -1,25 +0,0 @@ -/** - * @file timers.h - * @brief Timer functionality. - * - */ - - -#ifndef _OSCC_STEERING_TIMERS_H_ -#define _OSCC_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_TH_OSCC_STEERING_TIMERS_H_ROTTLE_TIMERS_H_ */ diff --git a/firmware/steering/src/communications.cpp b/firmware/steering/src/communications.cpp index 546ba870..80055b8f 100644 --- a/firmware/steering/src/communications.cpp +++ b/firmware/steering/src/communications.cpp @@ -14,6 +14,7 @@ #include "mcp_can.h" #include "steering_control.h" #include "oscc_can.h" +#include "vehicles.h" static void process_rx_frame( @@ -65,22 +66,6 @@ void publish_fault_report( void ) } -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; @@ -130,11 +115,31 @@ static void process_steering_command( const oscc_steering_command_s * const steering_command = (oscc_steering_command_s *) data; - update_steering( - steering_command->spoof_value_high, - steering_command->spoof_value_low ); + const float clamped_torque = CONSTRAIN( + steering_command->torque_command * MAXIMUM_TORQUE_COMMAND, + MINIMUM_TORQUE_COMMAND, + MAXIMUM_TORQUE_COMMAND); + + float spoof_voltage_low = + STEERING_TORQUE_TO_VOLTS_LOW( clamped_torque ); + + spoof_voltage_low = CONSTRAIN( + spoof_voltage_low, + STEERING_SPOOF_LOW_SIGNAL_VOLTAGE_MIN, + STEERING_SPOOF_LOW_SIGNAL_VOLTAGE_MAX); + + float spoof_voltage_high = + STEERING_TORQUE_TO_VOLTS_HIGH( clamped_torque ); + + spoof_voltage_high = CONSTRAIN( + spoof_voltage_high, + STEERING_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN, + STEERING_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX); + + const uint16_t spoof_value_low = STEPS_PER_VOLT * spoof_voltage_low; + const uint16_t spoof_value_high = STEPS_PER_VOLT * spoof_voltage_high; - g_steering_command_timeout = false; + update_steering( spoof_value_high, spoof_value_low ); } } diff --git a/firmware/steering/src/init.cpp b/firmware/steering/src/init.cpp index 500e0878..032aba5b 100644 --- a/firmware/steering/src/init.cpp +++ b/firmware/steering/src/init.cpp @@ -6,6 +6,7 @@ #include +#include "can_protocols/fault_can_protocol.h" #include "can_protocols/steering_can_protocol.h" #include "communications.h" #include "debug.h" @@ -13,6 +14,7 @@ #include "init.h" #include "oscc_can.h" #include "oscc_serial.h" +#include "oscc_timer.h" void init_globals( void ) @@ -20,8 +22,6 @@ void init_globals( void ) g_steering_control_state.enabled = false; g_steering_control_state.operator_override = false; g_steering_control_state.dtcs = 0; - - g_steering_command_timeout = false; } @@ -49,4 +49,19 @@ void init_communication_interfaces( void ) DEBUG_PRINT( "init Control CAN - " ); init_can( g_control_can ); + + // Filter CAN messages - accept if (CAN_ID & mask) == (filter & mask) + // Set buffer 0 to filter only steering module and global messages + g_control_can.init_Mask( 0, 0, 0x7F0 ); // Filter for 0x0N0 to 0x0NF + g_control_can.init_Filt( 0, 0, OSCC_STEERING_CAN_ID_INDEX ); + g_control_can.init_Filt( 1, 0, OSCC_FAULT_CAN_ID_INDEX ); + // Accept only CAN Disable when buffer overflow occurs in buffer 0 + g_control_can.init_Mask( 1, 0, 0x7FF ); // Filter for one CAN ID + g_control_can.init_Filt( 2, 1, OSCC_STEERING_DISABLE_CAN_ID ); +} + + +void start_timers( void ) +{ + timer1_init( OSCC_REPORT_STEERING_PUBLISH_FREQ_IN_HZ, publish_steering_report ); } diff --git a/firmware/steering/src/main.cpp b/firmware/steering/src/main.cpp index fdadf06b..97cc6dec 100644 --- a/firmware/steering/src/main.cpp +++ b/firmware/steering/src/main.cpp @@ -9,7 +9,6 @@ #include "debug.h" #include "init.h" #include "steering_control.h" -#include "timers.h" int main( void ) @@ -30,8 +29,6 @@ int main( void ) { check_for_incoming_message( ); -#ifdef STEERING_OVERRIDE - check_for_operator_override( ); -#endif + check_for_faults( ); } } diff --git a/firmware/steering/src/steering_control.cpp b/firmware/steering/src/steering_control.cpp index 8690fce6..45b2ebd5 100644 --- a/firmware/steering/src/steering_control.cpp +++ b/firmware/steering/src/steering_control.cpp @@ -14,39 +14,36 @@ #include "dtc.h" #include "globals.h" #include "oscc_dac.h" +#include "oscc_check.h" #include "steering_control.h" #include "vehicles.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 ); + static float exponential_moving_average( const float alpha, const float input, const float average ); - +#ifdef STEERING_OVERRIDE static uint16_t filtered_diff = 0; +#endif - -void check_for_operator_override( void ) +void check_for_faults( void ) { - if( g_steering_control_state.enabled == true - || g_steering_control_state.operator_override == true ) - { - steering_torque_s torque; + steering_torque_s torque; - read_torque_sensor( &torque ); + if ( ( g_steering_control_state.enabled == true ) + || (g_steering_control_state.dtcs > 0) ) + { + read_torque_sensor(&torque); +#ifdef STEERING_OVERRIDE uint16_t unfiltered_diff = abs( ( int )torque.high - ( int )torque.low ); const float filter_alpha = 0.01; @@ -60,70 +57,44 @@ void check_for_operator_override( void ) filter_alpha, unfiltered_diff, filtered_diff); +#endif - if( abs( filtered_diff ) > TORQUE_DIFFERENCE_OVERRIDE_THRESHOLD ) + // sensor pins tied to ground - a value of zero indicates disconnection + if( check_voltage_grounded( torque.high, torque.low ) ) { disable_control( ); DTC_SET( g_steering_control_state.dtcs, - OSCC_STEERING_DTC_OPERATOR_OVERRIDE ); + OSCC_STEERING_DTC_INVALID_SENSOR_VAL ); publish_fault_report( ); - g_steering_control_state.operator_override = true; - - DEBUG_PRINTLN( "Operator override" ); + DEBUG_PRINTLN( "Bad value read from torque sensor" ); } - else +#ifdef STEERING_OVERRIDE + else if( abs( filtered_diff ) > TORQUE_DIFFERENCE_OVERRIDE_THRESHOLD ) { - DTC_CLEAR( + disable_control( ); + + DTC_SET( g_steering_control_state.dtcs, OSCC_STEERING_DTC_OPERATOR_OVERRIDE ); - 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( ); - - DTC_SET( - g_steering_control_state.dtcs, - OSCC_STEERING_DTC_INVALID_SENSOR_VAL ); + publish_fault_report( ); - publish_fault_report( ); + g_steering_control_state.operator_override = true; - DEBUG_PRINTLN( "Bad value read from torque sensor" ); - } + DEBUG_PRINTLN( "Operator override" ); } +#endif else { - DTC_CLEAR( - g_steering_control_state.dtcs, - OSCC_STEERING_DTC_INVALID_SENSOR_VAL ); + g_steering_control_state.dtcs = 0; - fault_count = 0; +#ifdef STEERING_OVERRIDE + g_steering_control_state.operator_override = false; +#endif } } } @@ -171,7 +142,6 @@ void enable_control( void ) digitalWrite( PIN_SPOOF_ENABLE, HIGH ); sei(); - g_steering_command_timeout = false; g_steering_control_state.enabled = true; DEBUG_PRINTLN( "Control enabled" ); @@ -194,10 +164,11 @@ void disable_control( void ) digitalWrite( PIN_SPOOF_ENABLE, LOW ); sei(); - g_steering_command_timeout = false; g_steering_control_state.enabled = false; +#ifdef STEERING_OVERRIDE filtered_diff = 0; +#endif DEBUG_PRINTLN( "Control disabled" ); } @@ -219,4 +190,3 @@ static void read_torque_sensor( value->low = analogRead( PIN_TORQUE_SENSOR_LOW ) << 2; sei(); } - diff --git a/firmware/steering/src/timers.cpp b/firmware/steering/src/timers.cpp deleted file mode 100644 index d169791b..00000000 --- a/firmware/steering/src/timers.cpp +++ /dev/null @@ -1,45 +0,0 @@ -/** - * @file timers.cpp - * - */ - - -#include - -#include "can_protocols/steering_can_protocol.h" -#include "communications.h" -#include "globals.h" -#include "oscc_timer.h" -#include "steering_control.h" -#include "timers.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/firmware/steering/tests/CMakeLists.txt b/firmware/steering/tests/CMakeLists.txt index c88d601d..12701b6e 100644 --- a/firmware/steering/tests/CMakeLists.txt +++ b/firmware/steering/tests/CMakeLists.txt @@ -7,6 +7,7 @@ add_library( ../src/globals.cpp ../src/steering_control.cpp ${CMAKE_SOURCE_DIR}/common/libs/can/oscc_can.cpp + ${CMAKE_SOURCE_DIR}/common/libs/fault_check/oscc_check.cpp ${CMAKE_SOURCE_DIR}/common/libs/dac/oscc_dac.cpp ${CMAKE_SOURCE_DIR}/common/libs/pid/oscc_pid.cpp ${CMAKE_SOURCE_DIR}/common/testing/mocks/ @@ -20,6 +21,7 @@ target_include_directories( ../include ${CMAKE_SOURCE_DIR}/common/include ${CMAKE_SOURCE_DIR}/common/libs/can + ${CMAKE_SOURCE_DIR}/common/libs/fault_check ${CMAKE_SOURCE_DIR}/common/libs/dac ${CMAKE_SOURCE_DIR}/common/libs/pid ${CMAKE_SOURCE_DIR}/common/libs/signal_smoothing @@ -43,6 +45,7 @@ target_include_directories( ../include ${CMAKE_SOURCE_DIR}/common/include ${CMAKE_SOURCE_DIR}/common/libs/can + ${CMAKE_SOURCE_DIR}/common/libs/fault_check ${CMAKE_SOURCE_DIR}/common/libs/pid ${CMAKE_SOURCE_DIR}/common/testing/mocks ${CMAKE_SOURCE_DIR}/common/testing/framework/cucumber-cpp/include @@ -59,5 +62,5 @@ add_custom_target( add_custom_target( run-steering-property-tests COMMAND - cargo test -- --test-threads=1 + cargo +1.20.0 test -- --test-threads=1 WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/property) diff --git a/firmware/steering/tests/features/checking_faults.feature b/firmware/steering/tests/features/checking_faults.feature index ef696630..6cfb190a 100644 --- a/firmware/steering/tests/features/checking_faults.feature +++ b/firmware/steering/tests/features/checking_faults.feature @@ -5,13 +5,6 @@ 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 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 @@ -22,15 +15,6 @@ Feature: Checking for faults And a fault report should be published - Scenario: Controller command timeout - Given steering 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 steering control is enabled diff --git a/firmware/steering/tests/features/receiving_messages.feature b/firmware/steering/tests/features/receiving_messages.feature index 8f567126..71c55f05 100644 --- a/firmware/steering/tests/features/receiving_messages.feature +++ b/firmware/steering/tests/features/receiving_messages.feature @@ -32,31 +32,31 @@ Feature: Receiving commands Scenario Outline: Spoof value sent from application Given steering control is enabled - When a command is received with spoof values and + When a command is received with request value Then should be sent to DAC A And should be sent to DAC B Examples: - | high | low | - | 3440 | 656 | - | 2500 | 1500 | - | 2000 | 2000 | - | 1500 | 2500 | - | 738 | 3358 | + | value | high | low | + | -1 | 3440 | 656 | + | -0.3435 | 2500 | 1475 | + | 0 | 1982 | 1957 | + | 0.3435 | 1464 | 2440 | + | 1 | 738 | 3358 | Scenario Outline: Spoof value sent from application outside valid range Given steering control is enabled - When a command is received with spoof values and + When a command is received with request value Then should be sent to DAC A And should be sent to DAC B Examples: - | high | low | high_clamped | low_clamped | - | 4000 | 0 | 3440 | 656 | - | 3500 | 500 | 3440 | 656 | - | 500 | 3500 | 738 | 3358 | - | 0 | 4000 | 738 | 3358 | + | value | high_clamped | low_clamped | + | -15 | 3440 | 656 | + | -1.1 | 3440 | 656 | + | 1.1 | 738 | 3358 | + | 15 | 738 | 3358 | diff --git a/firmware/steering/tests/features/step_definitions/checking_faults.cpp b/firmware/steering/tests/features/step_definitions/checking_faults.cpp index 2dc4867d..afd44ae3 100644 --- a/firmware/steering/tests/features/step_definitions/checking_faults.cpp +++ b/firmware/steering/tests/features/step_definitions/checking_faults.cpp @@ -1,34 +1,15 @@ -WHEN("^a sensor becomes temporarily disconnected$") -{ - g_mock_arduino_analog_read_return[0] = 0; - g_mock_arduino_analog_read_return[1] = 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] = 0; g_mock_arduino_analog_read_return[1] = 0; - // must call function enough times to exceed the fault limit - for( int i = 0; i < 100; ++i ) - { - check_for_sensor_faults(); - } -} + check_for_faults(); + // must call function enough times to exceed the fault limit + g_mock_arduino_millis_return = 105; -WHEN("^the time since the last received controller command exceeds the timeout$") -{ - g_steering_command_timeout = true; + check_for_faults(); - check_for_controller_command_timeout(); } @@ -43,16 +24,14 @@ WHEN("^the operator applies (.*) to the steering wheel$") // and the average will start at 0.0 for the tests for( int i = 0; i < 200; ++i ) { - check_for_operator_override(); + check_for_faults(); } -} -THEN("^control should remain enabled") -{ - assert_that( - g_steering_control_state.enabled, - is_equal_to(1)); + // set an elapsed time to account for hystoresis compensation + g_mock_arduino_millis_return = 105; + + check_for_faults(); } diff --git a/firmware/steering/tests/features/step_definitions/common.cpp b/firmware/steering/tests/features/step_definitions/common.cpp index 09fcfada..b0775daa 100644 --- a/firmware/steering/tests/features/step_definitions/common.cpp +++ b/firmware/steering/tests/features/step_definitions/common.cpp @@ -34,7 +34,7 @@ extern unsigned short g_mock_dac_output_a; extern unsigned short g_mock_dac_output_b; extern volatile steering_control_state_s g_steering_control_state; - +extern volatile unsigned long g_mock_arduino_millis_return; // return to known state before every scenario BEFORE() @@ -60,6 +60,9 @@ BEFORE() g_steering_control_state.enabled = false; g_steering_control_state.operator_override = false; g_steering_control_state.dtcs = 0; + + // A small amount of time elapsed after boot + g_mock_arduino_millis_return = 1; } @@ -91,7 +94,7 @@ GIVEN("^the operator has applied (.*) to the steering wheel$") g_mock_arduino_analog_read_return[0] = steering_sensor_val; g_mock_arduino_analog_read_return[1] = steering_sensor_val; - check_for_operator_override(); + check_for_faults(); } diff --git a/firmware/steering/tests/features/step_definitions/receiving_messages.cpp b/firmware/steering/tests/features/step_definitions/receiving_messages.cpp index ece3ee4d..6e94a356 100644 --- a/firmware/steering/tests/features/step_definitions/receiving_messages.cpp +++ b/firmware/steering/tests/features/step_definitions/receiving_messages.cpp @@ -43,10 +43,9 @@ WHEN("^a fault report is received$") } -WHEN("^a command is received with spoof values (.*) and (.*)$") +WHEN("^a command is received with request value (.*)$") { - REGEX_PARAM(uint16_t, high); - REGEX_PARAM(uint16_t, low); + REGEX_PARAM(float, request_value); g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL; g_mock_mcp_can_read_msg_buf_id = OSCC_STEERING_COMMAND_CAN_ID; @@ -57,12 +56,7 @@ WHEN("^a command is received with spoof values (.*) and (.*)$") steering_command->magic[0] = OSCC_MAGIC_BYTE_0; steering_command->magic[1] = OSCC_MAGIC_BYTE_1; - steering_command->spoof_value_high = high; - steering_command->spoof_value_low = low; + steering_command->torque_command = request_value; check_for_incoming_message(); - - update_steering( - steering_command->spoof_value_high, - steering_command->spoof_value_low); } diff --git a/firmware/steering/tests/property/Cargo.toml b/firmware/steering/tests/property/Cargo.toml index bd96c490..8ada3db1 100644 --- a/firmware/steering/tests/property/Cargo.toml +++ b/firmware/steering/tests/property/Cargo.toml @@ -10,7 +10,7 @@ quickcheck = "0.4.1" rand = "0.3.15" [build-dependencies] -bindgen = "0.25.0" +bindgen = "0.32.1" gcc = "0.3.51" [lib] diff --git a/firmware/steering/tests/property/build.rs b/firmware/steering/tests/property/build.rs index aab81c39..f729bf48 100644 --- a/firmware/steering/tests/property/build.rs +++ b/firmware/steering/tests/property/build.rs @@ -13,12 +13,14 @@ fn main() { .include("../../../common/include") .include("../../../common/testing/mocks/") .include("../../../common/libs/can") + .include("../../../common/libs/fault_check") .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/fault_check/oscc_check.cpp") .file("../../../common/libs/dac/oscc_dac.cpp") .file("../../src/communications.cpp") .file("../../src/steering_control.cpp") @@ -36,11 +38,12 @@ fn main() { .clang_arg("-I../../include") .clang_arg("-I../../../common/include") .clang_arg("-I../../../common/libs/can") + .clang_arg("-I../../../common/libs/fault_check") .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_function("check_for_faults") .whitelisted_var("OSCC_MAGIC_BYTE_0") .whitelisted_var("OSCC_MAGIC_BYTE_1") .whitelisted_var("OSCC_STEERING_ENABLE_CAN_ID") @@ -57,6 +60,8 @@ fn main() { .whitelisted_var("STEERING_SPOOF_LOW_SIGNAL_RANGE_MAX") .whitelisted_var("STEERING_SPOOF_HIGH_SIGNAL_RANGE_MIN") .whitelisted_var("STEERING_SPOOF_HIGH_SIGNAL_RANGE_MAX") + .whitelisted_var("MINIMUM_TORQUE_COMMAND") + .whitelisted_var("MAXIMUM_TORQUE_COMMAND") .whitelisted_var("CAN_STANDARD") .whitelisted_var("CAN_MSGAVAIL") .whitelisted_type("oscc_steering_enable_s") diff --git a/firmware/steering/tests/property/src/tests.rs b/firmware/steering/tests/property/src/tests.rs index 095d693d..26ffe2ab 100644 --- a/firmware/steering/tests/property/src/tests.rs +++ b/firmware/steering/tests/property/src/tests.rs @@ -60,8 +60,7 @@ 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), + torque_command: f32::arbitrary(g), reserved: [u8::arbitrary(g); 2], } } @@ -171,8 +170,7 @@ fn check_process_disable_command() { /// 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.spoof_value_low = rand::thread_rng().gen_range(STEERING_SPOOF_LOW_SIGNAL_RANGE_MIN as u16, STEERING_SPOOF_LOW_SIGNAL_RANGE_MAX as u16); - steering_command_msg.spoof_value_high = rand::thread_rng().gen_range(STEERING_SPOOF_HIGH_SIGNAL_RANGE_MIN as u16, STEERING_SPOOF_HIGH_SIGNAL_RANGE_MAX as u16); + steering_command_msg.torque_command = rand::thread_rng().gen_range(0f32,1f32); unsafe { g_steering_control_state.enabled = true; @@ -182,9 +180,10 @@ fn prop_output_accurate_spoofs(mut steering_command_msg: oscc_steering_command_s 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 ) + TestResult::from_bool(g_mock_dac_output_b >= STEERING_SPOOF_LOW_SIGNAL_RANGE_MIN as u16 && + g_mock_dac_output_b <= STEERING_SPOOF_LOW_SIGNAL_RANGE_MAX as u16 && + g_mock_dac_output_a >= STEERING_SPOOF_HIGH_SIGNAL_RANGE_MIN as u16 && + g_mock_dac_output_a <= STEERING_SPOOF_HIGH_SIGNAL_RANGE_MAX as u16 ) } } @@ -200,14 +199,8 @@ fn check_output_accurate_spoofs() { /// upon recieving a steering command message fn prop_output_constrained_spoofs(steering_command_msg: oscc_steering_command_s) -> TestResult { unsafe { - if (steering_command_msg.spoof_value_low >= - STEERING_SPOOF_LOW_SIGNAL_RANGE_MIN as u16 && - steering_command_msg.spoof_value_low <= - STEERING_SPOOF_LOW_SIGNAL_RANGE_MAX as u16) || - (steering_command_msg.spoof_value_high >= - STEERING_SPOOF_HIGH_SIGNAL_RANGE_MIN as u16 && - steering_command_msg.spoof_value_high <= - STEERING_SPOOF_HIGH_SIGNAL_RANGE_MAX as u16) + if steering_command_msg.torque_command >= MINIMUM_TORQUE_COMMAND as f32 && + steering_command_msg.torque_command <= MAXIMUM_TORQUE_COMMAND as f32 { return TestResult::discard(); } @@ -272,7 +265,7 @@ fn prop_check_operator_override(analog_read_spoof_high: i16, analog_read_spoof_l g_mock_arduino_analog_read_return[0] = analog_read_spoof_high as isize; g_mock_arduino_analog_read_return[1] = analog_read_spoof_low as isize; - check_for_operator_override(); + check_for_faults(); if (analog_read_spoof_high - analog_read_spoof_low) >= (TORQUE_DIFFERENCE_OVERRIDE_THRESHOLD as i16) { TestResult::from_bool(g_steering_control_state.operator_override == true && g_steering_control_state.enabled == false && diff --git a/firmware/throttle/CMakeLists.txt b/firmware/throttle/CMakeLists.txt index 50c505b8..36b0ba12 100644 --- a/firmware/throttle/CMakeLists.txt +++ b/firmware/throttle/CMakeLists.txt @@ -19,6 +19,7 @@ generate_arduino_firmware( 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/fault_check/oscc_check.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 @@ -28,8 +29,7 @@ generate_arduino_firmware( src/globals.cpp src/init.cpp src/communications.cpp - src/throttle_control.cpp - src/timers.cpp) + src/throttle_control.cpp) target_include_directories( throttle @@ -38,6 +38,7 @@ target_include_directories( ${CMAKE_SOURCE_DIR}/common/include ${CMAKE_SOURCE_DIR}/common/libs/arduino_init ${CMAKE_SOURCE_DIR}/common/libs/DAC_MCP49xx + ${CMAKE_SOURCE_DIR}/common/libs/fault_check ${CMAKE_SOURCE_DIR}/common/libs/mcp_can ${CMAKE_SOURCE_DIR}/common/libs/serial ${CMAKE_SOURCE_DIR}/common/libs/can diff --git a/firmware/throttle/include/communications.h b/firmware/throttle/include/communications.h index 4fd150e8..218551be 100644 --- a/firmware/throttle/include/communications.h +++ b/firmware/throttle/include/communications.h @@ -35,20 +35,6 @@ void publish_throttle_report( 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 // diff --git a/firmware/throttle/include/globals.h b/firmware/throttle/include/globals.h index 1109a35c..6ce7d511 100644 --- a/firmware/throttle/include/globals.h +++ b/firmware/throttle/include/globals.h @@ -70,8 +70,6 @@ #endif -EXTERN volatile bool g_throttle_command_timeout; - EXTERN volatile throttle_control_state_s g_throttle_control_state; diff --git a/firmware/throttle/include/init.h b/firmware/throttle/include/init.h index f71af23c..4027a372 100644 --- a/firmware/throttle/include/init.h +++ b/firmware/throttle/include/init.h @@ -48,4 +48,17 @@ void init_devices( void ); void init_communication_interfaces( void ); +// **************************************************************************** +// Function: start_timers +// +// Purpose: Start timers for report publishing and fault checking. +// +// Returns: void +// +// Parameters: void +// +// **************************************************************************** +void start_timers( void ); + + #endif /* _OSCC_THROTTLE_INIT_H_ */ diff --git a/firmware/throttle/include/throttle_control.h b/firmware/throttle/include/throttle_control.h index 1fe9c3dc..9a84dc78 100644 --- a/firmware/throttle/include/throttle_control.h +++ b/firmware/throttle/include/throttle_control.h @@ -43,31 +43,19 @@ typedef struct } 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. +// Purpose: Checks to see if valid values are being read from the sensors +// and if the vehicle's operator has manually pressed the +// accelerator to disable control if they have. // // Returns: void // // Parameters: void // // **************************************************************************** -void check_for_sensor_faults( void ); +void check_for_faults( void ); // **************************************************************************** diff --git a/firmware/throttle/include/timers.h b/firmware/throttle/include/timers.h deleted file mode 100644 index cefc8cae..00000000 --- a/firmware/throttle/include/timers.h +++ /dev/null @@ -1,25 +0,0 @@ -/** - * @file timers.h - * @brief Timer functionality. - * - */ - - -#ifndef _OSCC_THROTTLE_TIMERS_H_ -#define _OSCC_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_THROTTLE_TIMERS_H_ */ diff --git a/firmware/throttle/src/communications.cpp b/firmware/throttle/src/communications.cpp index 1f0d8645..f7288ba3 100644 --- a/firmware/throttle/src/communications.cpp +++ b/firmware/throttle/src/communications.cpp @@ -14,6 +14,7 @@ #include "mcp_can.h" #include "oscc_can.h" #include "throttle_control.h" +#include "vehicles.h" static void process_rx_frame( @@ -65,22 +66,6 @@ void publish_fault_report( void ) } -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 - controller command" ); - } - } -} - - void check_for_incoming_message( void ) { can_frame_s rx_frame; @@ -130,11 +115,29 @@ static void process_throttle_command( const oscc_throttle_command_s * const throttle_command = (oscc_throttle_command_s *) data; - update_throttle( - throttle_command->spoof_value_high, - throttle_command->spoof_value_low ); + const float clamped_position = CONSTRAIN( + throttle_command->torque_request, + MINIMUM_THROTTLE_COMMAND, + MAXIMUM_THROTTLE_COMMAND); + + float spoof_voltage_low = THROTTLE_POSITION_TO_VOLTS_LOW( clamped_position ); + + spoof_voltage_low = CONSTRAIN( + spoof_voltage_low, + THROTTLE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN, + THROTTLE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX); + + float spoof_voltage_high = THROTTLE_POSITION_TO_VOLTS_HIGH( clamped_position ); + + spoof_voltage_high = CONSTRAIN( + spoof_voltage_high, + THROTTLE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN, + THROTTLE_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX); + + const uint16_t spoof_value_low = STEPS_PER_VOLT * spoof_voltage_low; + const uint16_t spoof_value_high = STEPS_PER_VOLT * spoof_voltage_high; - g_throttle_command_timeout = false; + update_throttle( spoof_value_high, spoof_value_low ); } } diff --git a/firmware/throttle/src/init.cpp b/firmware/throttle/src/init.cpp index f3091ee9..5e6e586b 100644 --- a/firmware/throttle/src/init.cpp +++ b/firmware/throttle/src/init.cpp @@ -6,11 +6,13 @@ #include +#include "can_protocols/fault_can_protocol.h" #include "can_protocols/throttle_can_protocol.h" #include "communications.h" #include "debug.h" #include "globals.h" #include "init.h" +#include "oscc_timer.h" #include "oscc_can.h" #include "oscc_serial.h" @@ -20,8 +22,6 @@ void init_globals( void ) g_throttle_control_state.enabled = false; g_throttle_control_state.operator_override = false; g_throttle_control_state.dtcs = 0; - - g_throttle_command_timeout = false; } @@ -49,4 +49,18 @@ void init_communication_interfaces( void ) DEBUG_PRINT( "init Control CAN - " ); init_can( g_control_can ); + + // Filter CAN messages - accept if (CAN_ID & mask) == (filter & mask) + // Set buffer 0 to filter only throttle module and global messages + g_control_can.init_Mask( 0, 0, 0x7F0 ); // Filter for 0x0N0 to 0x0NF + g_control_can.init_Filt( 0, 0, OSCC_THROTTLE_CAN_ID_INDEX ); + g_control_can.init_Filt( 1, 0, OSCC_FAULT_CAN_ID_INDEX ); + // Accept only CAN Disable when buffer overflow occurs in buffer 0 + g_control_can.init_Mask( 1, 0, 0x7FF ); // Filter for one CAN ID + g_control_can.init_Filt( 2, 1, OSCC_THROTTLE_DISABLE_CAN_ID ); +} + +void start_timers( void ) +{ + timer1_init( OSCC_REPORT_THROTTLE_PUBLISH_FREQ_IN_HZ, publish_throttle_report ); } diff --git a/firmware/throttle/src/main.cpp b/firmware/throttle/src/main.cpp index c399890a..8f754fcf 100644 --- a/firmware/throttle/src/main.cpp +++ b/firmware/throttle/src/main.cpp @@ -5,10 +5,10 @@ #include "arduino_init.h" +#include "can_protocols/throttle_can_protocol.h" #include "communications.h" #include "debug.h" #include "init.h" -#include "timers.h" #include "throttle_control.h" @@ -30,6 +30,6 @@ int main( void ) { check_for_incoming_message( ); - check_for_operator_override( ); + check_for_faults( ); } } diff --git a/firmware/throttle/src/throttle_control.cpp b/firmware/throttle/src/throttle_control.cpp index 414543f8..39c10bae 100644 --- a/firmware/throttle/src/throttle_control.cpp +++ b/firmware/throttle/src/throttle_control.cpp @@ -13,97 +13,64 @@ #include "dtc.h" #include "globals.h" #include "oscc_dac.h" +#include "oscc_check.h" #include "throttle_control.h" #include "vehicles.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 ) +void check_for_faults( void ) { - if ( g_throttle_control_state.enabled == true - || g_throttle_control_state.operator_override == true ) - { - accelerator_position_s accelerator_position; + accelerator_position_s accelerator_position; + if ( ( g_throttle_control_state.enabled == true ) + || (g_throttle_control_state.dtcs > 0) ) + { 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 ) + // sensor pins tied to ground - a value of zero indicates disconnection + if( check_voltage_grounded( accelerator_position.high, + accelerator_position.low ) ) { disable_control( ); DTC_SET( g_throttle_control_state.dtcs, - OSCC_THROTTLE_DTC_OPERATOR_OVERRIDE ); + OSCC_THROTTLE_DTC_INVALID_SENSOR_VAL ); publish_fault_report( ); - g_throttle_control_state.operator_override = true; - - DEBUG_PRINTLN( "Operator override" ); + DEBUG_PRINTLN( "Bad value read from accelerator position sensor" ); } - else + else if ( accelerator_position_average >= ACCELERATOR_OVERRIDE_THRESHOLD + && g_throttle_control_state.operator_override == false ) { - DTC_CLEAR( + disable_control( ); + + DTC_SET( g_throttle_control_state.dtcs, OSCC_THROTTLE_DTC_OPERATOR_OVERRIDE ); - 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( ); - - DTC_SET( - g_throttle_control_state.dtcs, - OSCC_THROTTLE_DTC_INVALID_SENSOR_VAL ); + publish_fault_report( ); - publish_fault_report( ); + g_throttle_control_state.operator_override = true; - DEBUG_PRINTLN( "Bad value read from accelerator position sensor" ); - } + DEBUG_PRINTLN( "Operator override" ); } else { - DTC_CLEAR( - g_throttle_control_state.dtcs, - OSCC_THROTTLE_DTC_INVALID_SENSOR_VAL ); + g_throttle_control_state.dtcs = 0; - fault_count = 0; + if ( g_throttle_control_state.operator_override == true ) + { + g_throttle_control_state.operator_override = false; + } } } } @@ -141,17 +108,17 @@ void enable_control( void ) && 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 ); + PIN_ACCELERATOR_POSITION_SENSOR_LOW, + PIN_ACCELERATOR_POSITION_SENSOR_HIGH ); cli(); digitalWrite( PIN_SPOOF_ENABLE, HIGH ); sei(); - g_throttle_command_timeout = false; g_throttle_control_state.enabled = true; DEBUG_PRINTLN( "Control enabled" ); @@ -164,17 +131,17 @@ 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 ); + PIN_ACCELERATOR_POSITION_SENSOR_LOW, + PIN_ACCELERATOR_POSITION_SENSOR_HIGH ); cli(); digitalWrite( PIN_SPOOF_ENABLE, LOW ); sei(); - g_throttle_command_timeout = false; g_throttle_control_state.enabled = false; DEBUG_PRINTLN( "Control disabled" ); diff --git a/firmware/throttle/src/timers.cpp b/firmware/throttle/src/timers.cpp deleted file mode 100644 index a93cbaba..00000000 --- a/firmware/throttle/src/timers.cpp +++ /dev/null @@ -1,45 +0,0 @@ -/** - * @file timers.cpp - * - */ - - -#include - -#include "can_protocols/throttle_can_protocol.h" -#include "communications.h" -#include "globals.h" -#include "oscc_timer.h" -#include "throttle_control.h" -#include "timers.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/firmware/throttle/tests/CMakeLists.txt b/firmware/throttle/tests/CMakeLists.txt index 71820f71..145f91ab 100644 --- a/firmware/throttle/tests/CMakeLists.txt +++ b/firmware/throttle/tests/CMakeLists.txt @@ -7,6 +7,7 @@ add_library( ../src/globals.cpp ../src/throttle_control.cpp ${CMAKE_SOURCE_DIR}/common/libs/can/oscc_can.cpp + ${CMAKE_SOURCE_DIR}/common/libs/fault_check/oscc_check.cpp ${CMAKE_SOURCE_DIR}/common/libs/dac/oscc_dac.cpp ${CMAKE_SOURCE_DIR}/common/testing/mocks/ ${CMAKE_SOURCE_DIR}/common/testing/mocks/Arduino_mock.cpp @@ -19,6 +20,7 @@ target_include_directories( ../include ${CMAKE_SOURCE_DIR}/common/include ${CMAKE_SOURCE_DIR}/common/libs/can + ${CMAKE_SOURCE_DIR}/common/libs/fault_check ${CMAKE_SOURCE_DIR}/common/libs/dac ${CMAKE_SOURCE_DIR}/common/libs/signal_smoothing ${CMAKE_SOURCE_DIR}/common/testing/mocks @@ -41,6 +43,7 @@ target_include_directories( ../include ${CMAKE_SOURCE_DIR}/common/include ${CMAKE_SOURCE_DIR}/common/libs/can + ${CMAKE_SOURCE_DIR}/common/libs/fault_check ${CMAKE_SOURCE_DIR}/common/testing/mocks ${CMAKE_SOURCE_DIR}/common/testing/framework/cucumber-cpp/include ${CMAKE_SOURCE_DIR}/common/testing/framework/cgreen/include @@ -56,5 +59,5 @@ add_custom_target( add_custom_target( run-throttle-property-tests COMMAND - cargo test -- --test-threads=1 + cargo +1.20.0 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 index 18d6c43a..bc6f480b 100644 --- a/firmware/throttle/tests/features/checking_faults.feature +++ b/firmware/throttle/tests/features/checking_faults.feature @@ -5,14 +5,6 @@ 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 @@ -22,15 +14,6 @@ Feature: Checking for faults 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 diff --git a/firmware/throttle/tests/features/receiving_messages.feature b/firmware/throttle/tests/features/receiving_messages.feature index 91ed16f0..ed16a435 100644 --- a/firmware/throttle/tests/features/receiving_messages.feature +++ b/firmware/throttle/tests/features/receiving_messages.feature @@ -32,34 +32,33 @@ Feature: Receiving commands Scenario Outline: Spoof value sent from application Given throttle control is enabled - When a command is received with spoof values and + When a command is received with request value Then should be sent to DAC A And should be sent to DAC B Examples: - | high | low | - | 3358 | 328 | - | 3000 | 500 | - | 2500 | 1000 | - | 2000 | 1500 | - | 1500 | 1638 | - | 1000 | 1638 | - | 656 | 1638 | - | 573 | 1638 | + | value | high | low | + | 1 | 3358 | 1638 | + | 0.8 | 2801 | 1359 | + | 0.6 | 2244 | 1081 | + | 0.5 | 1966 | 942 | + | 0.4 | 1687 | 802 | + | 0.2 | 1130 | 524 | + | 0 | 573 | 245 | Scenario Outline: Spoof value sent from application outside valid range Given throttle control is enabled - When a command is received with spoof values and + When a command is received with request value Then should be sent to DAC A And should be sent to DAC B Examples: - | high | low | high_clamped | low_clamped | - | 4000 | 0 | 3358 | 245 | - | 3500 | 500 | 3358 | 500 | - | 580 | 3500 | 580 | 1638 | - | 500 | 4000 | 573 | 1638 | + | value | high_clamped | low_clamped | + | 5 | 3358 | 1638 | + | 1.1 | 3358 | 1638 | + | -0.1 | 573 | 245 | + | -5 | 573 | 245 | diff --git a/firmware/throttle/tests/features/step_definitions/checking_faults.cpp b/firmware/throttle/tests/features/step_definitions/checking_faults.cpp index 0a09c09f..34f7f118 100644 --- a/firmware/throttle/tests/features/step_definitions/checking_faults.cpp +++ b/firmware/throttle/tests/features/step_definitions/checking_faults.cpp @@ -1,34 +1,14 @@ -WHEN("^a sensor becomes temporarily disconnected$") -{ - g_mock_arduino_analog_read_return[0] = 0; - g_mock_arduino_analog_read_return[1] = 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] = 0; g_mock_arduino_analog_read_return[1] = 0; - // must call function enough times to exceed the fault limit - for( int i = 0; i < 100; ++i ) - { - check_for_sensor_faults(); - } -} - + check_for_faults(); -WHEN("^the time since the last received controller command exceeds the timeout$") -{ - g_throttle_command_timeout = true; + // must call function enough times to exceed the fault limit + g_mock_arduino_millis_return = 105; - check_for_controller_command_timeout(); + check_for_faults(); } @@ -39,15 +19,7 @@ WHEN("^the operator applies (.*) to the accelerator$") g_mock_arduino_analog_read_return[0] = throttle_sensor_val; g_mock_arduino_analog_read_return[1] = throttle_sensor_val; - check_for_operator_override(); -} - - -THEN("^control should remain enabled") -{ - assert_that( - g_throttle_control_state.enabled, - is_equal_to(1)); + check_for_faults(); } diff --git a/firmware/throttle/tests/features/step_definitions/common.cpp b/firmware/throttle/tests/features/step_definitions/common.cpp index a2636b08..f4166445 100644 --- a/firmware/throttle/tests/features/step_definitions/common.cpp +++ b/firmware/throttle/tests/features/step_definitions/common.cpp @@ -34,6 +34,7 @@ extern unsigned short g_mock_dac_output_a; extern unsigned short g_mock_dac_output_b; extern volatile throttle_control_state_s g_throttle_control_state; +extern volatile unsigned long g_mock_arduino_millis_return; // return to known state before every scenario @@ -60,6 +61,9 @@ BEFORE() g_throttle_control_state.enabled = false; g_throttle_control_state.operator_override = false; g_throttle_control_state.dtcs = 0; + + // A small amount of time elapsed after boot + g_mock_arduino_millis_return = 1; } @@ -92,7 +96,7 @@ GIVEN("^the operator has applied (.*) to the accelerator$") g_mock_arduino_analog_read_return[0] = throttle_sensor_val; g_mock_arduino_analog_read_return[1] = throttle_sensor_val; - check_for_operator_override(); + check_for_faults(); } diff --git a/firmware/throttle/tests/features/step_definitions/receiving_messages.cpp b/firmware/throttle/tests/features/step_definitions/receiving_messages.cpp index 19058750..bbfd980b 100644 --- a/firmware/throttle/tests/features/step_definitions/receiving_messages.cpp +++ b/firmware/throttle/tests/features/step_definitions/receiving_messages.cpp @@ -43,10 +43,9 @@ WHEN("^a fault report is received$") } -WHEN("^a command is received with spoof values (.*) and (.*)$") +WHEN("^a command is received with request value (.*)$") { - REGEX_PARAM(uint16_t, high); - REGEX_PARAM(uint16_t, low); + REGEX_PARAM(float, request_value); g_mock_mcp_can_check_receive_return = CAN_MSGAVAIL; g_mock_mcp_can_read_msg_buf_id = OSCC_THROTTLE_COMMAND_CAN_ID; @@ -56,12 +55,7 @@ WHEN("^a command is received with spoof values (.*) and (.*)$") throttle_command->magic[0] = OSCC_MAGIC_BYTE_0; throttle_command->magic[1] = OSCC_MAGIC_BYTE_1; - throttle_command->spoof_value_high = high; - throttle_command->spoof_value_low = low; + throttle_command->torque_request = request_value; check_for_incoming_message(); - - update_throttle( - throttle_command->spoof_value_high, - throttle_command->spoof_value_low); } diff --git a/firmware/throttle/tests/property/Cargo.toml b/firmware/throttle/tests/property/Cargo.toml index 4e96a03d..25d058b7 100644 --- a/firmware/throttle/tests/property/Cargo.toml +++ b/firmware/throttle/tests/property/Cargo.toml @@ -10,7 +10,7 @@ quickcheck = "0.4.1" rand = "0.3.15" [build-dependencies] -bindgen = "0.25.0" +bindgen = "0.32.1" gcc = "0.3.51" [lib] diff --git a/firmware/throttle/tests/property/build.rs b/firmware/throttle/tests/property/build.rs index 53da6d28..63c4cfa7 100644 --- a/firmware/throttle/tests/property/build.rs +++ b/firmware/throttle/tests/property/build.rs @@ -13,12 +13,14 @@ fn main() { .include("../../../common/include") .include("../../../common/testing/mocks/") .include("../../../common/libs/can") + .include("../../../common/libs/fault_check") .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/fault_check/oscc_check.cpp") .file("../../../common/libs/dac/oscc_dac.cpp") .file("../../src/communications.cpp") .file("../../src/throttle_control.cpp") @@ -36,11 +38,12 @@ fn main() { .clang_arg("-I../../include") .clang_arg("-I../../../common/include") .clang_arg("-I../../../common/libs/can") + .clang_arg("-I../../../common/libs/fault_check") .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_function("check_for_faults") .whitelisted_var("OSCC_MAGIC_BYTE_0") .whitelisted_var("OSCC_MAGIC_BYTE_1") .whitelisted_var("OSCC_THROTTLE_ENABLE_CAN_ID") @@ -54,6 +57,8 @@ fn main() { .whitelisted_var("OSCC_THROTTLE_REPORT_PUBLISH_INTERVAL_IN_MSEC") .whitelisted_var("CAN_MSGAVAIL") .whitelisted_var("CAN_STANDARD") + .whitelisted_var("MAXIMUM_THROTTLE_COMMAND") + .whitelisted_var("MINIMUM_THROTTLE_COMMAND") .whitelisted_var("THROTTLE_SPOOF_LOW_SIGNAL_RANGE_MIN") .whitelisted_var("THROTTLE_SPOOF_LOW_SIGNAL_RANGE_MAX") .whitelisted_var("THROTTLE_SPOOF_HIGH_SIGNAL_RANGE_MIN") diff --git a/firmware/throttle/tests/property/src/tests.rs b/firmware/throttle/tests/property/src/tests.rs index 35e5366f..8c69f4f3 100644 --- a/firmware/throttle/tests/property/src/tests.rs +++ b/firmware/throttle/tests/property/src/tests.rs @@ -58,11 +58,10 @@ impl Arbitrary for oscc_throttle_report_s { impl Arbitrary for oscc_throttle_command_s { fn arbitrary(g: &mut G) -> oscc_throttle_command_s { - 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), - reserved: [u8::arbitrary(g); 2], + torque_request: f32::arbitrary(g), + reserved: [u8::arbitrary(g); 2usize], } } } @@ -171,12 +170,8 @@ fn check_process_disable_command() { /// 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.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); + throttle_command_msg.torque_request = rand::thread_rng().gen_range(0f32, 1f32); + unsafe { g_throttle_control_state.enabled = true; @@ -186,8 +181,10 @@ fn prop_output_accurate_spoofs(mut throttle_command_msg: oscc_throttle_command_s 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) + 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_LOW_SIGNAL_RANGE_MIN as u16 && + g_mock_dac_output_b <= THROTTLE_SPOOF_LOW_SIGNAL_RANGE_MAX as u16) } } @@ -203,10 +200,8 @@ fn check_output_accurate_spoofs() { /// upon recieving a throttle command message fn prop_output_constrained_spoofs(throttle_command_msg: oscc_throttle_command_s) -> TestResult { unsafe { - if (throttle_command_msg.spoof_value_low >= THROTTLE_SPOOF_LOW_SIGNAL_RANGE_MIN as u16 && - throttle_command_msg.spoof_value_low <= THROTTLE_SPOOF_LOW_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) { + if throttle_command_msg.torque_request >= MINIMUM_THROTTLE_COMMAND as f32 && + throttle_command_msg.torque_request <= MAXIMUM_THROTTLE_COMMAND as f32 { return TestResult::discard(); } @@ -267,7 +262,7 @@ fn prop_check_operator_override(analog_read_spoof: u16) -> TestResult { g_mock_arduino_analog_read_return[0] = analog_read_spoof as isize; g_mock_arduino_analog_read_return[1] = analog_read_spoof as isize; - check_for_operator_override(); + check_for_faults(); if analog_read_spoof >= (ACCELERATOR_OVERRIDE_THRESHOLD as u16) { TestResult::from_bool(g_throttle_control_state.operator_override == true && diff --git a/hardware/boards/gateway/gateway_r0.brd b/hardware/boards/gateway/gateway_r0.brd new file mode 100644 index 00000000..7c25452c --- /dev/null +++ b/hardware/boards/gateway/gateway_r0.brd @@ -0,0 +1,15485 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +CANH +CANL + + + + + + + + + + + + + + + + + + + ++12V +GND +PWR +CAN-TX +CAN-RX +INT + + + + + + + + + + + + + + + + + + + + + + + + + + +CANL +CANH +Display +CAN-TX +CAN-RX +INT + + + + + + + + + + + + + + + + + + + +>NAME +>VALUE + + + + + + + + + + + + + + + + + + + + + + + + + + + + +>NAME +>VALUE + + + + +Plastic- encapsulated transistors + + +<b>CRYSTAL</b> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +>NAME +>VALUE + + + + + +>name + + + + + + + + + + + + + +>NAME + + + + + + + + +>NAME + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +>NAME + + +<b>PLASTIC SMALL-OUTLINE PACKAGE SO 20</b> JEDEC MO-153, PW Type<p> +Source: www.ti.com/.. slvs087l.pdf + + + + + + + + + + + + + + + + + + + + + + + + + +>NAME +>VALUE + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +<b>Description:</b> Standard 0402 Package for Resistors<br/> + + + + + + +>NAME + + +<b>Description:</b> Standard 0402 Package for Capacitors<br/> + + + + + + +>NAME + + + + + + + + +>NAME + + + + + + + + +<b>Description:</b> Footprint for Single LEDs in 0603<br/> + + + + + + + + + + + + +>NAME + + + + + + + + + +<b>Jumpers</b><p> +<author>Created by librarian@cadsoft.de</author> + + +<b>JUMPER</b> + + + + + + + + + + + + + + + + +>NAME +>VALUE + + + + + + +Generated from <b>polysync_steering_throttle.sch</b><p> +by exp-lbrs.ulp + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +8 +9 +10 +11 +12 +13 +GND +AREF +SDA +SCL +JP1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +0 +1 +2 +3 +4 +5 +6 +7 +JP2 + + + + + + + + + + + + + + + + + + + + + + + + + + + + +JP3 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +1 +0 +2 +4 +5 +ANALOG +3 +JP4 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +RST +3V3 +5V +GND +Vin +IOREF +NC +JP5 + + + + + + + + + + + + + + +<b>MacroFab, INC. EAGLE Design Rules</b> +<p>This DRC check is for 2-Layer Standard Capability boards to be manufactured through the MacroFab, INC. service. + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/hardware/boards/gateway/gateway_r0.sch b/hardware/boards/gateway/gateway_r0.sch new file mode 100644 index 00000000..3233c5aa --- /dev/null +++ b/hardware/boards/gateway/gateway_r0.sch @@ -0,0 +1,8778 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +>NAME +>VALUE + + + + + + + + + + + + + + + + + + + + + + + + + + + + +>NAME +>VALUE + + + + + + + + +>name +>value + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +>NAME +>VALUE + + + + +High-Speed CAN Transceiver<p/> +http://ww1.microchip.com/downloads/en/devicedoc/21667f.pdf + + + + + + + + + + + + + + + + + + + + + + +Stand-Alone CAN Controller With SPI Interface + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +Plastic- encapsulated transistors + + +<b>CRYSTAL</b> + + + + + + + + + + + + + + + + + + + + +>NAME +>VALUE + + + + + + + +<b>CRYSTAL</b> + + + + + + + + + + + + + + + + + + + + + + + + +>NAME +>VALUE + + + +<b>CRYSTAL</b> + + + + + + + + + + + + + + + + + + + + + + + + +>NAME +>VALUE + + + + + +<b>CRYSTAL</b> + + + + + + + + + + + + + + + + + + + + +>NAME +>VALUE + + + + +<b>CRYSTAL</b> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +>NAME +>VALUE + + + +<b>CRYSTAL</b> + + + + + + + + + + + + + + + + + + + + +>NAME +>VALUE + + + + + + + + + + + +<b>CRYSTAL</b> + + + + + + + + + + + + + + + + + + + + +>NAME +>VALUE + + + + + + + + + + + +<b>CRYSTAL</b> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +>NAME +>VALUE + + + +<b>CRYSTAL</b> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +>NAME +>VALUE + + + +<b>CRYSTAL</b> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +>NAME +>VALUE + + + +<b>CRYSTAL</b> + + + + + + + + + + + + + + + + + + + + + + + + + + + + +>NAME +>VALUE + + + +<b>CRYSTAL</b> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +>NAME +>VALUE + + + + + +<b>CRYSTAL</b> + + + + + + + + + + + + + + + + + + + + +>NAME +>VALUE + + + + + + + + + +<b>CRYSTAL</b> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +>NAME +>VALUE + + + +<b>CRYSTAL</b> + + + + + + + + + + + + + + + + + + + + + + + + + + + +>NAME +>VALUE + + + + + +<b>CRYSTAL</b> + + + + + + + + + + + + + + + + + +>NAME +>VALUE + + +<b>CRYSTAL</b> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +>NAME +>VALUE + + + + + +<b>CRYSTAL RESONATOR</b> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +>NAME +>VALUE + + +<b>CRYSTAL</b> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +>NAME +>VALUE + + +<b>CRYSTAL</b> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +>NAME +>VALUE + + +<b>Model 406 6.0x3.5mm Low Cost Surface Mount Crystal</b><p> +Source: 008-0260-0_E.pdf + + + + + + + + + + + + + + + + + + + + + +>NAME +>VALUE + + +3.2mm*5mm dimension <br> +8MHz available + + + + + + + + + + + + + + + + + + +>name +>value +>name + + + + + +3.6mm*6mm dimension <br> +12MHz available + + + + + + + + + + + + + + + + + + + + + + +>name +>value +>name + + + + + +4.5mm*8mm dimension <br> +12MHz available + + + + + + + + + + + + + + + + + + + + + + +>name +>value +>name + + + + + + + + + + + + +>VALUE +>NAME + + + + + + + + + + + + + + +>name +>value + + + + + + + + + +>name +>value + + + + + + + + + +>NAME +>VALUE + + + + +>name + + + + +>name + + + + + + + +>name +>value + + + + + + + + + + + + + + + + + + +>NAME +>VALUE +1 +2 + + + + +>name + + + + + + + +<b>CRYSTAL</b> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +<b>Frames for Sheet and Layout</b> + + + + + + + + + + + + + + + + + + + + + + + + + + +Date: +>LAST_DATE_TIME +Sheet: +>SHEET +REV: +TITLE: +Document Number: +>DRAWING_NAME + + + + +<b>FRAME</b> A Size , 8 1/2 x 11 INCH, Landscape<p> + + + + + + + + + + + + + + + + + + + + + + + + +>NAME + + + + + + + + +>NAME + + + + + + + + + + + + + + + + + + + + +>NAME + + + + + + + + + + + + + + + + + +>NAME + + +<b>Dual In Line Package</b> + + + + + + + + + + + + + + + + + + + + + + + + + + +>NAME +>VALUE + + +<b>Wide Small Outline package</b> 300 mil + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +>VALUE +>NAME + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +<b>Leadless Chip Carrier</b><p> Ceramic Package + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +>NAME +>VALUE + + +<b>PLASTIC SMALL-OUTLINE PACKAGE SO 20</b> JEDEC MO-153, PW Type<p> +Source: www.ti.com/.. slvs087l.pdf + + + + + + + + + + + + + + + + + + + + + + + + + +>NAME +>VALUE + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +>NAME +>VALUE + + + + + + + + +>NAME + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +>NAME +>VALUE + + + + + + +>NAME +>VALUE + + + + + + + + + + + +>NAME +GND +VCC + + + + + + + + +>NAME +>VALUE + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +Octal <b>BUFFER</b> and <b>LINE DRIVER</b>, 3-state + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +<b>Supply Symbols</b><p> + GND, VCC, 0V, +5V, -5V, etc.<p> + Please keep in mind, that these devices are necessary for the + automatic wiring of the supply signals.<p> + The pin name defined in the symbol is identical to the net which is to be wired automatically.<p> + In this library the device names are the same as the pin names of the symbols, therefore the correct signal names appear next to the supply symbols in the schematic.<p> + <author>Created by librarian@cadsoft.de</author> + + + + + +>VALUE + + + + + +>VALUE + + + + + + +>VALUE + + + + + +>VALUE + + + + + +<b>SUPPLY SYMBOL</b> + + + + + + + + + + + + +<b>SUPPLY SYMBOL</b> + + + + + + + + + + + + +<b>SUPPLY SYMBOL</b> + + + + + + + + + + + + +<b>SUPPLY SYMBOL</b> + + + + + + + + + + + + + + + + +<b>Description:</b> Standard 0402 Package for Resistors<br/> + + + + + + +>NAME + + +<b>Description:</b> Standard 0603 Package for Resistors<br/> + + + + + + +>NAME + + +<b>Description:</b> Standard 0805 Package for Resistors<br/> + + + + + + +>NAME + + +<b>Description:</b> Standard 1206 Package for Resistors<br/> + + + + + + +>NAME + + +<b>Description:</b> Standard 1210 Package for Resistors<br/> + + + + + + +>NAME + + +<b>Description:</b> Standard 0402 Package for Capacitors<br/> + + + + + + +>NAME + + +<b>Description:</b> Standard 0603 Package for Capacitors<br/> + + + + + + +>NAME + + +<b>Description:</b> Standard 0805 Package for Capacitors<br/> + + + + + + +>NAME + + +<b>Description:</b> Standard 1206 Package for Capacitors<br/> + + + + + + +>NAME + + +<b>Description:</b> Standard 1210 Package for Capacitors<br/> + + + + + + +>NAME + + +<b>Description:</b> 4MM Round Package for Electrolytic Capacitors. Based on Panasonic Electrolytic Capacitors.<br> + + + + + + + + + + +>NAME + + + + +<b>Description:</b> 6.3MM Round Package for Electrolytic Capacitors. Based on Panasonic Electrolytic Capacitors.<br> + + + + + + + + + + + + +>NAME + + +<b>Description:</b> 8MM Round Package for Electrolytic Capacitors. Based on Panasonic Electrolytic Capacitors.<br> + + + + + + + + + + + + +>NAME + + +<b>Description:</b> 10MM Round Package for Electrolytic Capacitors. Based on Panasonic Electrolytic Capacitors. +<br/> + + +>NAME + + + + + + + + + + + + +<b>Description:</b> 12.5MM Round Package for Electrolytic Capacitors. Based on Panasonic Electrolytic Capacitors.<br> + + + + + + + + + + + + +>NAME + + + + + + + + +>NAME + + + + + + +<b>Library:</b> MF_Passives<br/> +<b>Description:</b> Symbol for Resistors<br/> + + + + + + + + + + + +>NAME +>VALUE + + +<b>Library:</b> MF_Passives<br/> +<b>Description:</b> Symbol for Non-Polarized Capacitors<br/> + + +>NAME +>VALUE + + + + + + +<b>Library:</b> MF_Passives<br/> +<b>Description:</b> Symbol for Polarized Capacitors<br/> + + + + + + + + + +>NAME +>VALUE + + + + +<b>Library:</b> MF_Passives<br/> +<b>Description:</b> Device for Resistors. Manufacture part number (MPN), Voltage, Tolerance, and Wattage Rating can be added via Attributes. Check https://factory.macrofab.com/parts for the house parts list.<br/> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +<b>Library:</b> MF_Passives<br/> +<b>Description:</b> Device for Non-Polarized Capacitors. Manufacture part number (MPN) can be added via Attributes. Check https://factory.macrofab.com/parts for the house parts list.<br/> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +<b>Library:</b> MF_Passives<br/> +<b>Description:</b> Device for Polarized Capacitors. Manufacture part number (MPN), Voltage Rating, and Tolerance can be added via Attributes. Check https://factory.macrofab.com/parts for the house parts list.<br/> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +<b>Description:</b> Footprint for Single LEDs in 0603<br/> + + + + + + + + + + + + +>NAME + + + + + + + +<b>Description:</b> Footprint for Single LEDs in 0805<br/> + + + + + + + + + + + + +>NAME + + + + + + + + + +<b>Description:</b> Symbol for Single LEDs<br/> + + + + + + + + + + + + + + + + + + + + + +>NAME +>VALUE + + + + + + +<b>Library:</b> MF_LEDs<br/> +<b>Description:</b> Device for Single LED Packages. Manufacture part number (MFG#) can be added via Attributes.<br/> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +<b>Jumpers</b><p> +<author>Created by librarian@cadsoft.de</author> + + +<b>JUMPER</b> + + + + + + + + + + + + + + + + +>NAME +>VALUE + + + + + + + + + + + + + + +>NAME +>VALUE + + + + + + +<b>JUMPER</b> + + + + + + + + + + + + + + + + + + +Generated from <b>polysync_steering_throttle.sch</b><p> +by exp-lbrs.ulp + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +8 +9 +10 +11 +12 +13 +GND +AREF +SDA +SCL +JP1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +0 +1 +2 +3 +4 +5 +6 +7 +JP2 + + + + + + + + + + + + + + + + + + + + + + + + + + + + +JP3 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +1 +0 +2 +4 +5 +ANALOG +3 +JP4 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +RST +3V3 +5V +GND +Vin +IOREF +NC +JP5 + + + + + + + + + + + + + + + + + + +>NAME +>VALUE +>VALUE + + + + + + + + + + + + + + +>NAME +>VALUE + + + + + + + + + + + + +>NAME +>VALUE + + + + + + + + + + + + +>NAME +>VALUE + + + + + + + + + + + + + + +>NAME +>VALUE + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/utils/diagnostics_tool/src/brake_module_state.c b/utils/diagnostics_tool/src/brake_module_state.c index c7ddd12e..af25b219 100644 --- a/utils/diagnostics_tool/src/brake_module_state.c +++ b/utils/diagnostics_tool/src/brake_module_state.c @@ -47,7 +47,7 @@ static int analyze_report_frame( state->override_triggered = brake_report->operator_override; - if( brake_report->dtcs == 1 ) + if( brake_report->dtcs != 0 ) { module_state = STATE_FAULT; } diff --git a/utils/diagnostics_tool/src/steering_module_state.c b/utils/diagnostics_tool/src/steering_module_state.c index b38055af..57512998 100644 --- a/utils/diagnostics_tool/src/steering_module_state.c +++ b/utils/diagnostics_tool/src/steering_module_state.c @@ -54,7 +54,7 @@ static int analyze_report_frame( state->override_triggered = steering_report->operator_override; - if( steering_report->dtcs == 1 ) + if( steering_report->dtcs != 0 ) { module_state = STATE_FAULT; }