├── .gitignore ├── .rustfmt.toml ├── src ├── brake │ ├── kia_soul_petrol │ │ └── brake_module.rs │ └── kia_soul_ev_niro │ │ └── brake_module.rs ├── can_protocols │ ├── oscc_magic_byte.rs │ ├── fault_can_protocol.rs │ ├── brake_can_protocol.rs │ ├── steering_can_protocol.rs │ └── throttle_can_protocol.rs ├── vehicle.rs ├── oxcc_error.rs ├── dtc.rs ├── dual_signal.rs ├── fault_condition.rs ├── ranges.rs ├── dac_mcp4922.rs ├── types.rs ├── panic_abort.rs ├── config.rs ├── can_gateway_module.rs ├── throttle_module.rs ├── steering_module.rs ├── vehicles │ ├── kial_niro.rs │ ├── kial_soul_ev.rs │ └── kial_soul_petrol.rs ├── main.rs └── board.rs ├── scripts ├── run-openocd └── deploy ├── .cargo └── config ├── .gdbinit ├── memory.x ├── LICENSE-MIT ├── Cargo.toml ├── README.md ├── LICENSE-APACHE └── Cargo.lock /.gitignore: -------------------------------------------------------------------------------- 1 | /target 2 | **/*.rs.bk 3 | .idea 4 | -------------------------------------------------------------------------------- /.rustfmt.toml: -------------------------------------------------------------------------------- 1 | wrap_comments = true 2 | normalize_comments = true 3 | -------------------------------------------------------------------------------- /src/brake/kia_soul_petrol/brake_module.rs: -------------------------------------------------------------------------------- 1 | //! Kia Soul Petrol brake module 2 | 3 | unimplemented!(); 4 | -------------------------------------------------------------------------------- /scripts/run-openocd: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | set -e 4 | 5 | openocd -f board/stm32f7discovery.cfg 6 | 7 | exit 0 8 | -------------------------------------------------------------------------------- /src/can_protocols/oscc_magic_byte.rs: -------------------------------------------------------------------------------- 1 | //! OSCC magic byte constants 2 | 3 | pub const OSCC_MAGIC_BYTE_0: u8 = 0x05; 4 | pub const OSCC_MAGIC_BYTE_1: u8 = 0xCC; 5 | -------------------------------------------------------------------------------- /.cargo/config: -------------------------------------------------------------------------------- 1 | [target.thumbv7em-none-eabihf] 2 | runner = 'arm-none-eabi-gdb' 3 | rustflags = [ 4 | # link with lld 5 | "-C", "link-arg=-Tlink.x", 6 | "-C", "linker=rust-lld", 7 | "-Z", "linker-flavor=ld.lld", 8 | ] 9 | 10 | [build] 11 | target = "thumbv7em-none-eabihf" 12 | -------------------------------------------------------------------------------- /src/vehicle.rs: -------------------------------------------------------------------------------- 1 | //! Vehicle abstraction 2 | //! 3 | //! Pick your vehicle feature set in the `Cargo.toml` 4 | #[cfg(feature = "kia-niro")] 5 | pub use kial_niro::*; 6 | #[cfg(feature = "kia-soul-ev")] 7 | pub use kial_soul_ev::*; 8 | #[cfg(feature = "kia-soul-petrol")] 9 | pub use kial_soul_petrol::*; 10 | -------------------------------------------------------------------------------- /scripts/deploy: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | set -e 4 | 5 | arm-none-eabi-objcopy \ 6 | -O ihex \ 7 | target/thumbv7em-none-eabihf/release/oxcc \ 8 | target/thumbv7em-none-eabihf/release/oxcc.hex 9 | 10 | st-flash --format ihex write target/thumbv7em-none-eabihf/release/oxcc.hex 11 | 12 | exit 0 13 | -------------------------------------------------------------------------------- /src/oxcc_error.rs: -------------------------------------------------------------------------------- 1 | //! OxCC error type 2 | 3 | use nucleo_f767zi::hal::can::CanError; 4 | use nucleo_f767zi::hal::spi; 5 | 6 | #[derive(Copy, Clone, Debug, PartialEq)] 7 | pub enum OxccError { 8 | Spi(spi::Error), 9 | Can(CanError), 10 | } 11 | 12 | impl From for OxccError { 13 | fn from(e: spi::Error) -> Self { 14 | OxccError::Spi(e) 15 | } 16 | } 17 | 18 | impl From for OxccError { 19 | fn from(e: CanError) -> Self { 20 | OxccError::Can(e) 21 | } 22 | } 23 | -------------------------------------------------------------------------------- /.gdbinit: -------------------------------------------------------------------------------- 1 | target remote :3333 2 | 3 | # print demangled symbols by default 4 | set print asm-demangle on 5 | 6 | monitor arm semihosting enable 7 | 8 | # # send captured ITM to the file itm.fifo 9 | # # (the microcontroller SWO pin must be connected to the programmer SWO pin) 10 | # # 8000000 must match the core clock frequency 11 | # monitor tpiu config internal itm.fifo uart off 8000000 12 | 13 | # # OR: make the microcontroller SWO pin output compatible with UART (8N1) 14 | # # 2000000 is the frequency of the SWO pin 15 | # monitor tpiu config external uart off 8000000 2000000 16 | 17 | # # enable ITM port 0 18 | # monitor itm port 0 on 19 | 20 | load 21 | step 22 | -------------------------------------------------------------------------------- /src/dtc.rs: -------------------------------------------------------------------------------- 1 | //! Diagnostic trouble codes 2 | 3 | pub trait DtcBitfield { 4 | fn set(&mut self, dtc: u8); 5 | fn clear(&mut self, dtc: u8); 6 | fn clear_all(&mut self); 7 | fn check(&self, dtc: u8) -> bool; 8 | fn are_any_set(&self) -> bool; 9 | } 10 | 11 | impl DtcBitfield for u8 { 12 | fn set(&mut self, dtc: u8) { 13 | *self |= 1 << dtc; 14 | } 15 | 16 | fn clear(&mut self, dtc: u8) { 17 | *self &= !(1 << dtc); 18 | } 19 | 20 | fn clear_all(&mut self) { 21 | *self = 0; 22 | } 23 | 24 | fn check(&self, dtc: u8) -> bool { 25 | *self & (1 << dtc) != 0 26 | } 27 | 28 | fn are_any_set(&self) -> bool { 29 | *self > 0 30 | } 31 | } 32 | -------------------------------------------------------------------------------- /memory.x: -------------------------------------------------------------------------------- 1 | MEMORY 2 | { 3 | /* NOTE K = KiBi = 1024 bytes */ 4 | 5 | /* STM32F767ZI 2 MByte FLASH, 512 KByte RAM */ 6 | FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 2048K 7 | RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 512K 8 | } 9 | 10 | /* This is where the call stack will be allocated. */ 11 | /* The stack is of the full descending type. */ 12 | /* You may want to use this variable to locate the call stack and static 13 | variables in different memory regions. Below is shown the default value */ 14 | /* _stack_start = ORIGIN(RAM) + LENGTH(RAM); */ 15 | 16 | /* You can use this symbol to customize the location of the .text section */ 17 | /* If omitted the .text section will be placed right after the .vector_table 18 | section */ 19 | /* This is required only on microcontrollers that store some configuration right 20 | after the vector table */ 21 | /* _stext = ORIGIN(FLASH) + 0x400; */ 22 | 23 | /* Size of the heap (in bytes) */ 24 | /* _heap_size = 1024; */ 25 | -------------------------------------------------------------------------------- /LICENSE-MIT: -------------------------------------------------------------------------------- 1 | Copyright (c) 2018 Jonathan Lamb 2 | 3 | Permission is hereby granted, free of charge, to any 4 | person obtaining a copy of this software and associated 5 | documentation files (the "Software"), to deal in the 6 | Software without restriction, including without 7 | limitation the rights to use, copy, modify, merge, 8 | publish, distribute, sublicense, and/or sell copies of 9 | the Software, and to permit persons to whom the Software 10 | is furnished to do so, subject to the following 11 | conditions: 12 | 13 | The above copyright notice and this permission notice 14 | shall be included in all copies or substantial portions 15 | of the Software. 16 | 17 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF 18 | ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED 19 | TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 20 | PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT 21 | SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY 22 | CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 23 | OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR 24 | IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 25 | DEALINGS IN THE SOFTWARE. 26 | -------------------------------------------------------------------------------- /src/can_protocols/fault_can_protocol.rs: -------------------------------------------------------------------------------- 1 | //! Fault CAN protocol 2 | 3 | use nucleo_f767zi::hal::can::{BaseID, CanError, DataFrame, ID}; 4 | 5 | pub const OSCC_FAULT_REPORT_CAN_ID: u16 = 0xAF; 6 | 7 | pub const OSCC_FAULT_REPORT_CAN_DLC: u8 = 8; 8 | 9 | // TODO - enum 10 | pub const FAULT_ORIGIN_BRAKE: u32 = 0; 11 | pub const FAULT_ORIGIN_STEERING: u32 = 1; 12 | pub const FAULT_ORIGIN_THROTTLE: u32 = 2; 13 | 14 | // TODO - fix this organization 15 | pub struct OsccFaultReport { 16 | pub fault_origin_id: u32, 17 | pub dtcs: u8, 18 | } 19 | 20 | impl<'a> From<&'a DataFrame> for OsccFaultReport { 21 | fn from(f: &DataFrame) -> Self { 22 | assert_eq!(u32::from(f.id()), u32::from(OSCC_FAULT_REPORT_CAN_ID)); 23 | let data = f.data(); 24 | 25 | let fault_origin_id: u32 = u32::from(data[2]) 26 | | (u32::from(data[3]) << 8) 27 | | (u32::from(data[4]) << 16) 28 | | (u32::from(data[5]) << 24); 29 | 30 | OsccFaultReport { 31 | fault_origin_id, 32 | dtcs: data[6], 33 | } 34 | } 35 | } 36 | 37 | pub trait FaultReportSupplier { 38 | fn supply_fault_report(&mut self) -> &OsccFaultReport; 39 | } 40 | 41 | pub trait FaultReportPublisher { 42 | fn publish_fault_report(&mut self, fault_report: &OsccFaultReport) -> Result<(), CanError>; 43 | } 44 | 45 | pub fn default_fault_report_data_frame() -> DataFrame { 46 | DataFrame::new(ID::BaseID(BaseID::new(OSCC_FAULT_REPORT_CAN_ID))) 47 | } 48 | -------------------------------------------------------------------------------- /Cargo.toml: -------------------------------------------------------------------------------- 1 | [package] 2 | name = "oxcc" 3 | version = "0.1.0" 4 | authors = [ 5 | "Jon Lamb ", 6 | "Zachary Pierce ", 7 | "Russell Mull " 8 | ] 9 | categories = ["embedded", "hardware-support", "no-std"] 10 | description = "A port of Open Source Car Control written in Rust" 11 | license = "MIT OR Apache-2.0" 12 | readme = "README.md" 13 | repository = "https://github.com/jonlamb-gh/oxcc" 14 | 15 | [dependencies.panic-semihosting] 16 | version = "0.5.0" 17 | optional = true 18 | 19 | [dependencies.cortex-m-semihosting] 20 | version = "0.3.1" 21 | optional = true 22 | 23 | [dependencies.cortex-m] 24 | version = "0.5.7" 25 | features = ["const-fn"] 26 | 27 | [dependencies.cortex-m-rt] 28 | version = "0.6.3" 29 | features = ["device"] 30 | 31 | [dependencies.oxcc-nucleo-f767zi] 32 | version = "0.1.1" 33 | features = ["rt"] 34 | 35 | [dependencies.num] 36 | version = "0.2" 37 | default-features = false 38 | 39 | [dependencies.embedded-hal] 40 | features = ["unproven"] 41 | version = "0.2.1" 42 | 43 | [dependencies.typenum] 44 | version = "1.1.0" 45 | default-features = false 46 | 47 | [profile.release] 48 | codegen-units = 1 # better optimizations 49 | lto = true # better optimizations 50 | 51 | [features] 52 | default = ["kia-soul-ev", "panic-abort"] 53 | kia-soul-ev = [] 54 | kia-niro = [] 55 | # No plans to support the Petrol, however it is stubbed out for use 56 | kia-soul-petrol = [] 57 | # Panic stategies 58 | # Note that panic-over-semihosting requires a debugger to be attached 59 | panic-over-semihosting = ["cortex-m-semihosting", "panic-semihosting"] 60 | # The default OxCC panic implementation will (attempt to) output 61 | # the PanicInfo to Serial3 of the board and disable all control 62 | # related functionality before aborting. 63 | panic-abort = [] 64 | -------------------------------------------------------------------------------- /src/dual_signal.rs: -------------------------------------------------------------------------------- 1 | //! Dual signal 2 | 3 | use board::DAC_SAMPLE_AVERAGE_COUNT; 4 | use num; 5 | 6 | pub struct DualSignal { 7 | high: u16, 8 | low: u16, 9 | reader: T, 10 | } 11 | 12 | impl DualSignal 13 | where 14 | T: HighLowReader, 15 | { 16 | pub fn new(high: u16, low: u16, high_low_reader: T) -> Self { 17 | DualSignal { 18 | high, 19 | low, 20 | reader: high_low_reader, 21 | } 22 | } 23 | 24 | pub fn update(&mut self) { 25 | self.high = self.reader.read_high(); 26 | self.low = self.reader.read_low(); 27 | } 28 | 29 | // not sure if the averaging is needed, we might be able to just use a 30 | // single read with large Cycles480 sample time? 31 | // https://github.com/jonlamb-gh/oscc/blob/devel/firmware/common/libs/dac/oscc_dac.cpp#L17 32 | pub fn prevent_signal_discontinuity(&mut self) { 33 | let mut low: u32 = 0; 34 | let mut high: u32 = 0; 35 | 36 | for _ in 0..DAC_SAMPLE_AVERAGE_COUNT { 37 | low += u32::from(self.reader.read_low()); 38 | } 39 | 40 | for _ in 0..DAC_SAMPLE_AVERAGE_COUNT { 41 | high += u32::from(self.reader.read_high()); 42 | } 43 | 44 | self.low = (low / DAC_SAMPLE_AVERAGE_COUNT) as _; 45 | self.high = (high / DAC_SAMPLE_AVERAGE_COUNT) as _; 46 | } 47 | 48 | pub fn average(&self) -> u32 { 49 | (u32::from(self.low) + u32::from(self.high)) / 2 50 | } 51 | 52 | pub fn diff(&self) -> u16 { 53 | num::abs(i32::from(self.high) - i32::from(self.low)) as u16 54 | } 55 | 56 | pub fn high(&self) -> u16 { 57 | self.high 58 | } 59 | 60 | pub fn low(&self) -> u16 { 61 | self.low 62 | } 63 | } 64 | 65 | pub trait HighLowReader { 66 | fn read_high(&self) -> u16; 67 | fn read_low(&self) -> u16; 68 | } 69 | -------------------------------------------------------------------------------- /src/can_protocols/brake_can_protocol.rs: -------------------------------------------------------------------------------- 1 | //! Brake CAN protocol 2 | 3 | use nucleo_f767zi::hal::can::{BaseID, CanError, DataFrame, ID}; 4 | 5 | pub const OSCC_BRAKE_ENABLE_CAN_ID: u16 = 0x70; 6 | pub const OSCC_BRAKE_DISABLE_CAN_ID: u16 = 0x71; 7 | pub const OSCC_BRAKE_COMMAND_CAN_ID: u16 = 0x72; 8 | pub const OSCC_BRAKE_REPORT_CAN_ID: u16 = 0x73; 9 | 10 | pub const OSCC_BRAKE_REPORT_CAN_DLC: u8 = 8; 11 | 12 | // TODO - enum 13 | pub const OSCC_BRAKE_DTC_INVALID_SENSOR_VAL: u8 = 0; 14 | pub const OSCC_BRAKE_DTC_OPERATOR_OVERRIDE: u8 = 1; 15 | 16 | pub struct OsccBrakeCommand { 17 | pub pedal_command: f32, 18 | } 19 | 20 | impl<'a> From<&'a DataFrame> for OsccBrakeCommand { 21 | fn from(f: &DataFrame) -> Self { 22 | assert_eq!(u32::from(f.id()), u32::from(OSCC_BRAKE_COMMAND_CAN_ID)); 23 | let data = f.data(); 24 | 25 | let raw_brake_request: u32 = u32::from(data[2]) 26 | | (u32::from(data[3]) << 8) 27 | | (u32::from(data[4]) << 16) 28 | | (u32::from(data[5]) << 24); 29 | 30 | OsccBrakeCommand { 31 | pedal_command: f32::from_bits(raw_brake_request), 32 | } 33 | } 34 | } 35 | 36 | pub struct OsccBrakeReport { 37 | pub enabled: bool, 38 | pub operator_override: bool, 39 | pub dtcs: u8, 40 | } 41 | 42 | pub trait BrakeReportSupplier { 43 | fn supply_brake_report(&mut self) -> &OsccBrakeReport; 44 | } 45 | 46 | pub trait BrakeReportPublisher { 47 | fn publish_brake_report(&mut self, brake_report: &OsccBrakeReport) -> Result<(), CanError>; 48 | } 49 | 50 | pub fn default_brake_report_data_frame() -> DataFrame { 51 | DataFrame::new(ID::BaseID(BaseID::new(OSCC_BRAKE_REPORT_CAN_ID))) 52 | } 53 | 54 | impl OsccBrakeReport { 55 | pub fn new() -> Self { 56 | OsccBrakeReport { 57 | enabled: false, 58 | operator_override: false, 59 | dtcs: 0, 60 | } 61 | } 62 | } 63 | -------------------------------------------------------------------------------- /src/fault_condition.rs: -------------------------------------------------------------------------------- 1 | //! Fault condition 2 | 3 | use dual_signal::{DualSignal, HighLowReader}; 4 | use embedded_hal::timer::CountDown; 5 | use nucleo_f767zi::hal::timer::OnePulse; 6 | 7 | pub struct FaultCondition { 8 | monitoring_active: bool, 9 | timer: TIMER, 10 | } 11 | 12 | impl FaultCondition 13 | where 14 | TIMER: CountDown + OnePulse, 15 | { 16 | pub fn new(mut timer: TIMER) -> Self { 17 | timer.reconfigure_one_pulse_mode(); 18 | 19 | FaultCondition { 20 | monitoring_active: false, 21 | timer, 22 | } 23 | } 24 | 25 | pub fn condition_exceeded_duration(&mut self, condition_active: bool) -> bool { 26 | let mut faulted = false; 27 | 28 | if !condition_active { 29 | // If a fault condition is not active, update the state to clear 30 | // the condition active flag and reset the last detection time. 31 | self.monitoring_active = false; 32 | } else { 33 | if !self.monitoring_active { 34 | // We just detected a condition that may lead to a fault. Update 35 | // the state to track that the condition is active and store the 36 | // first time of detection. 37 | self.monitoring_active = true; 38 | self.timer.reset(); 39 | } 40 | 41 | if self.timer.wait().is_ok() { 42 | // The fault condition has been active for longer than the maximum 43 | // acceptable duration. 44 | faulted = true; 45 | self.timer.reset(); 46 | } 47 | } 48 | 49 | faulted 50 | } 51 | 52 | pub fn check_voltage_grounded(&mut self, signal: &DualSignal) -> bool { 53 | let condition_active = (signal.high() == 0) || (signal.low() == 0); 54 | 55 | self.condition_exceeded_duration(condition_active) 56 | } 57 | } 58 | -------------------------------------------------------------------------------- /src/can_protocols/steering_can_protocol.rs: -------------------------------------------------------------------------------- 1 | //! Steering CAN protocol 2 | 3 | use nucleo_f767zi::hal::can::{BaseID, CanError, DataFrame, ID}; 4 | 5 | pub const OSCC_STEERING_ENABLE_CAN_ID: u16 = 0x80; 6 | pub const OSCC_STEERING_DISABLE_CAN_ID: u16 = 0x81; 7 | pub const OSCC_STEERING_COMMAND_CAN_ID: u16 = 0x82; 8 | pub const OSCC_STEERING_REPORT_CAN_ID: u16 = 0x83; 9 | 10 | pub const OSCC_STEERING_REPORT_CAN_DLC: u8 = 8; 11 | 12 | // TODO - enum 13 | pub const OSCC_STEERING_DTC_INVALID_SENSOR_VAL: u8 = 0; 14 | pub const OSCC_STEERING_DTC_OPERATOR_OVERRIDE: u8 = 1; 15 | 16 | pub struct OsccSteeringCommand { 17 | pub torque_request: f32, 18 | } 19 | 20 | impl<'a> From<&'a DataFrame> for OsccSteeringCommand { 21 | fn from(f: &DataFrame) -> Self { 22 | assert_eq!(u32::from(f.id()), u32::from(OSCC_STEERING_COMMAND_CAN_ID)); 23 | let data = f.data(); 24 | 25 | let raw_torque_request: u32 = u32::from(data[2]) 26 | | (u32::from(data[3]) << 8) 27 | | (u32::from(data[4]) << 16) 28 | | (u32::from(data[5]) << 24); 29 | 30 | OsccSteeringCommand { 31 | torque_request: f32::from_bits(raw_torque_request), 32 | } 33 | } 34 | } 35 | 36 | pub struct OsccSteeringReport { 37 | pub enabled: bool, 38 | pub operator_override: bool, 39 | pub dtcs: u8, 40 | } 41 | 42 | pub trait SteeringReportSupplier { 43 | fn supply_steering_report(&mut self) -> &OsccSteeringReport; 44 | } 45 | 46 | pub trait SteeringReportPublisher { 47 | fn publish_steering_report( 48 | &mut self, 49 | brake_report: &OsccSteeringReport, 50 | ) -> Result<(), CanError>; 51 | } 52 | 53 | pub fn default_steering_report_data_frame() -> DataFrame { 54 | DataFrame::new(ID::BaseID(BaseID::new(OSCC_STEERING_REPORT_CAN_ID))) 55 | } 56 | 57 | impl OsccSteeringReport { 58 | pub fn new() -> Self { 59 | OsccSteeringReport { 60 | enabled: false, 61 | operator_override: false, 62 | dtcs: 0, 63 | } 64 | } 65 | } 66 | -------------------------------------------------------------------------------- /src/can_protocols/throttle_can_protocol.rs: -------------------------------------------------------------------------------- 1 | //! Throttle CAN protocol 2 | 3 | use nucleo_f767zi::hal::can::{BaseID, CanError, DataFrame, ID}; 4 | 5 | pub const OSCC_THROTTLE_ENABLE_CAN_ID: u16 = 0x90; 6 | pub const OSCC_THROTTLE_DISABLE_CAN_ID: u16 = 0x91; 7 | pub const OSCC_THROTTLE_COMMAND_CAN_ID: u16 = 0x92; 8 | pub const OSCC_THROTTLE_REPORT_CAN_ID: u16 = 0x93; 9 | 10 | pub const OSCC_THROTTLE_REPORT_CAN_DLC: u8 = 8; 11 | 12 | // TODO - enum 13 | pub const OSCC_THROTTLE_DTC_INVALID_SENSOR_VAL: u8 = 0; 14 | pub const OSCC_THROTTLE_DTC_OPERATOR_OVERRIDE: u8 = 1; 15 | 16 | pub struct OsccThrottleCommand { 17 | pub torque_request: f32, 18 | } 19 | 20 | impl<'a> From<&'a DataFrame> for OsccThrottleCommand { 21 | fn from(f: &DataFrame) -> Self { 22 | assert_eq!(u32::from(f.id()), u32::from(OSCC_THROTTLE_COMMAND_CAN_ID)); 23 | let data = f.data(); 24 | 25 | let raw_torque_request: u32 = u32::from(data[2]) 26 | | (u32::from(data[3]) << 8) 27 | | (u32::from(data[4]) << 16) 28 | | (u32::from(data[5]) << 24); 29 | 30 | OsccThrottleCommand { 31 | torque_request: f32::from_bits(raw_torque_request), 32 | } 33 | } 34 | } 35 | 36 | pub struct OsccThrottleReport { 37 | pub enabled: bool, 38 | pub operator_override: bool, 39 | pub dtcs: u8, 40 | } 41 | 42 | pub trait ThrottleReportSupplier { 43 | fn supply_throttle_report(&mut self) -> &OsccThrottleReport; 44 | } 45 | 46 | pub trait ThrottleReportPublisher { 47 | fn publish_throttle_report( 48 | &mut self, 49 | throttle_report: &OsccThrottleReport, 50 | ) -> Result<(), CanError>; 51 | } 52 | 53 | pub fn default_throttle_report_data_frame() -> DataFrame { 54 | DataFrame::new(ID::BaseID(BaseID::new(OSCC_THROTTLE_REPORT_CAN_ID))) 55 | } 56 | 57 | impl OsccThrottleReport { 58 | pub fn new() -> Self { 59 | OsccThrottleReport { 60 | enabled: false, 61 | operator_override: false, 62 | dtcs: 0, 63 | } 64 | } 65 | } 66 | -------------------------------------------------------------------------------- /src/ranges.rs: -------------------------------------------------------------------------------- 1 | //! OxCC bounded ranges 2 | 3 | use core::marker::PhantomData; 4 | use num; 5 | 6 | use typenum::{Cmp, Same, Unsigned, B1}; 7 | // Although this says private, it's needed to write generic inequality 8 | // constraints for typenums 9 | use typenum::private::IsLessOrEqualPrivate; 10 | 11 | /// Indicates that a type-level number may be converted to a runtime-level 12 | /// number of the type T 13 | pub trait ReifyTo { 14 | fn reify() -> T; 15 | } 16 | 17 | impl ReifyTo for T { 18 | fn reify() -> u8 { 19 | ::to_u8() 20 | } 21 | } 22 | 23 | impl ReifyTo for T { 24 | fn reify() -> u16 { 25 | ::to_u16() 26 | } 27 | } 28 | 29 | #[derive(Debug)] 30 | pub struct Bounded { 31 | val: T, 32 | _lower_inclusive: PhantomData, 33 | _upper_inclusive: PhantomData, 34 | } 35 | 36 | impl, U: ReifyTo> Bounded { 37 | pub fn clamp(val: T) -> Bounded { 38 | Bounded { 39 | val: num::clamp(val, L::reify(), U::reify()), 40 | _lower_inclusive: PhantomData, 41 | _upper_inclusive: PhantomData, 42 | } 43 | } 44 | 45 | pub fn val(&self) -> &T { 46 | &self.val 47 | } 48 | } 49 | 50 | pub fn coerce( 51 | b: Bounded, 52 | ) -> Bounded 53 | where 54 | T: PartialOrd, 55 | Lower1: ReifyTo, 56 | Upper1: ReifyTo, 57 | Lower2: ReifyTo, 58 | Upper2: ReifyTo, 59 | 60 | // Lower2 <= Upper2 61 | Lower2: Cmp, 62 | Lower2: IsLessOrEqualPrivate>::Output>, 63 | >::Output>>::Output: Same, 64 | 65 | // Lower1 <= Lower2 66 | Lower2: Cmp, 67 | Lower2: IsLessOrEqualPrivate>::Output>, 68 | >::Output>>::Output: Same, 69 | 70 | // Lower2 <= Upper1 71 | Lower2: Cmp, 72 | Lower2: IsLessOrEqualPrivate>::Output>, 73 | >::Output>>::Output: Same, 74 | 75 | // Upper1 <= Upper2 76 | Upper1: Cmp, 77 | Upper1: IsLessOrEqualPrivate>::Output>, 78 | >::Output>>::Output: Same, 79 | { 80 | Bounded { 81 | val: b.val, 82 | _lower_inclusive: PhantomData, 83 | _upper_inclusive: PhantomData, 84 | } 85 | } 86 | -------------------------------------------------------------------------------- /src/dac_mcp4922.rs: -------------------------------------------------------------------------------- 1 | //! A driver to interface with the MCP4922 (12-bit, dual channel DAC) 2 | //! 3 | //! This driver was built using [`embedded-hal`] traits. 4 | //! 5 | //! This is a minimal port of the Arduino implementation 6 | //! used in OSCC. You can find the original source here: 7 | //! https://github.com/jonlamb-gh/oscc/blob/master/firmware/common/libs/DAC_MCP49xx/README.txt 8 | //! 9 | //! Features that don't exist: 10 | //! - latching 11 | //! - gain configuration 12 | //! - reference voltage configuration 13 | 14 | use embedded_hal::blocking::spi::Write; 15 | use embedded_hal::digital::OutputPin; 16 | use embedded_hal::spi::{Mode, Phase, Polarity}; 17 | 18 | use ranges::Bounded; 19 | use typenum::{U0, U1, U4096}; 20 | 21 | type U4095 = op! { U4096 - U1 }; 22 | 23 | /// It's a 12 bit dac, so the upper bound is 4095 (2^12 - 1) 24 | pub type DacOutput = Bounded; 25 | 26 | /// SPI mode 27 | pub const MODE: Mode = Mode { 28 | phase: Phase::CaptureOnFirstTransition, 29 | polarity: Polarity::IdleLow, 30 | }; 31 | 32 | /// DAC Channel 33 | #[derive(Copy, Clone, Debug, PartialEq)] 34 | pub enum Channel { 35 | ChannelA, 36 | ChannelB, 37 | } 38 | 39 | /// DAC Errors 40 | #[derive(Debug)] 41 | pub enum Error { 42 | /// SPI error 43 | Spi(E), 44 | } 45 | 46 | impl From for Error { 47 | fn from(e: E) -> Self { 48 | Error::Spi(e) 49 | } 50 | } 51 | 52 | /// MCP4922 driver 53 | pub struct Mcp4922 { 54 | spi: SPI, 55 | cs: CS, 56 | } 57 | 58 | impl Mcp4922 59 | where 60 | SPI: Write, 61 | CS: OutputPin, 62 | { 63 | /// Creates a new driver from a SPI peripheral and a CS pin 64 | pub fn new(spi: SPI, mut cs: CS) -> Self { 65 | // unselect the device 66 | cs.set_high(); 67 | 68 | Mcp4922 { spi, cs } 69 | } 70 | 71 | /// Writes the two output values to the two output channels of the DAC 72 | pub fn output_ab(&mut self, output_a: DacOutput, output_b: DacOutput) -> Result<(), E> { 73 | self.output(output_a, Channel::ChannelA)?; 74 | self.output(output_b, Channel::ChannelB) 75 | } 76 | 77 | /// Writes a bounded 16-bit value `data` to the output `channel` of the DAC 78 | pub fn output(&mut self, data: DacOutput, channel: Channel) -> Result<(), E> { 79 | self.cs.set_low(); 80 | 81 | // NOTE: swapping the bytes here, the HAL should be able to handle such a thing 82 | let mut buffer = [0u8; 2]; 83 | // bits 11 through 0: data 84 | buffer[1] = (data.val() & 0x00FF) as _; 85 | buffer[0] = ((data.val() >> 8) & (0x000F as u16)) as u8 86 | // bit 12: shutdown bit. 1 for active operation 87 | | (1 << 4) 88 | // bit 13: gain bit; 0 for 1x gain, 1 for 2x 89 | // bit 14: buffer VREF? 90 | // bit 15: 0 for DAC A, 1 for DAC B 91 | | u8::from(channel) << 7; 92 | 93 | if let Err(e) = self.spi.write(&buffer) { 94 | self.cs.set_high(); 95 | return Err(e); 96 | } 97 | 98 | self.cs.set_high(); 99 | 100 | Ok(()) 101 | } 102 | } 103 | 104 | impl From for u8 { 105 | fn from(c: Channel) -> u8 { 106 | match c { 107 | Channel::ChannelA => 0b0, 108 | Channel::ChannelB => 0b1, 109 | } 110 | } 111 | } 112 | -------------------------------------------------------------------------------- /src/types.rs: -------------------------------------------------------------------------------- 1 | //! OxCC types 2 | 3 | use dac_mcp4922::Mcp4922; 4 | use nucleo_f767zi::hal::can::Can; 5 | use nucleo_f767zi::hal::gpio::gpioa::{PA15, PA4, PA5, PA6, PA7}; 6 | use nucleo_f767zi::hal::gpio::gpiob::{PB10, PB12, PB13, PB15, PB4}; 7 | use nucleo_f767zi::hal::gpio::gpioc::{PC10, PC11, PC12, PC2}; 8 | use nucleo_f767zi::hal::gpio::gpiod::{PD0, PD1, PD11, PD12, PD13}; 9 | use nucleo_f767zi::hal::gpio::gpioe::PE2; 10 | use nucleo_f767zi::hal::gpio::{Output, PushPull, AF5, AF9}; 11 | use nucleo_f767zi::hal::spi::Spi; 12 | use nucleo_f767zi::hal::stm32f7x7::{ 13 | CAN1, CAN2, SPI1, SPI2, SPI3, TIM2, TIM3, TIM4, TIM5, TIM6, TIM7, 14 | }; 15 | use nucleo_f767zi::hal::timer::Timer; 16 | use nucleo_f767zi::{ 17 | AnalogInput0Pin, AnalogInput1Pin, AnalogInput2Pin, AnalogInput4Pin, AnalogInput5Pin, 18 | AnalogInput6Pin, 19 | }; 20 | 21 | pub type CanPublishTimer = Timer; 22 | pub type BrakeGroundedFaultTimer = Timer; 23 | pub type BrakeOverrideFaultTimer = Timer; 24 | pub type ThrottleGroundedFaultTimer = Timer; 25 | pub type ThrottleOverrideFaultTimer = Timer; 26 | pub type SteeringGroundedFaultTimer = Timer; 27 | 28 | pub type ControlCan = Can, PD0)>; 29 | pub type ObdCan = Can, PB12)>; 30 | 31 | pub type BrakeSpi = Spi, PA6, PA7)>; 32 | pub type ThrottleSpi = Spi, PC2, PB15)>; 33 | pub type SteeringSpi = Spi, PC11, PC12)>; 34 | 35 | pub type BrakeSpoofEnablePin = PD12>; 36 | pub type BrakeLightEnablePin = PD13>; 37 | // AIN pins chosen to allow brake module to own ADC1 38 | pub type BrakePedalPositionSensorHighPin = AnalogInput0Pin; 39 | pub type BrakePedalPositionSensorLowPin = AnalogInput1Pin; 40 | pub type BrakeSpiSckPin = PA5; 41 | pub type BrakeSpiMisoPin = PA6; 42 | pub type BrakeSpiMosiPin = PA7; 43 | pub type BrakeSpiNssPin = PA4>; 44 | 45 | pub type BrakeDac = Mcp4922; 46 | 47 | pub type ThrottleSpoofEnablePin = PE2>; 48 | // AIN pins chosen to allow throttle module to own ADC2 49 | pub type AcceleratorPositionSensorHighPin = AnalogInput2Pin; 50 | pub type AcceleratorPositionSensorLowPin = AnalogInput6Pin; 51 | pub type ThrottleSpiSckPin = PB10; 52 | pub type ThrottleSpiMisoPin = PC2; 53 | pub type ThrottleSpiMosiPin = PB15; 54 | pub type ThrottleSpiNssPin = PB4>; 55 | 56 | pub type ThrottleDac = Mcp4922; 57 | 58 | pub type SteeringSpoofEnablePin = PD11>; 59 | // AIN pins chosen to allow steering module to own ADC3 60 | pub type TorqueSensorHighPin = AnalogInput4Pin; 61 | pub type TorqueSensorLowPin = AnalogInput5Pin; 62 | pub type SteeringSpiSckPin = PC10; 63 | pub type SteeringSpiMisoPin = PC11; 64 | pub type SteeringSpiMosiPin = PC12; 65 | pub type SteeringSpiNssPin = PA15>; 66 | 67 | pub type SteeringDac = Mcp4922; 68 | 69 | pub struct BrakePins { 70 | pub spoof_enable: BrakeSpoofEnablePin, 71 | pub brake_light_enable: BrakeLightEnablePin, 72 | pub pedal_pos_sensor_high: BrakePedalPositionSensorHighPin, 73 | pub pedal_pos_sensor_low: BrakePedalPositionSensorLowPin, 74 | } 75 | 76 | pub struct ThrottlePins { 77 | pub spoof_enable: ThrottleSpoofEnablePin, 78 | pub accel_pos_sensor_high: AcceleratorPositionSensorHighPin, 79 | pub accel_pos_sensor_low: AcceleratorPositionSensorLowPin, 80 | } 81 | 82 | pub struct SteeringPins { 83 | pub spoof_enable: SteeringSpoofEnablePin, 84 | pub torque_sensor_high: TorqueSensorHighPin, 85 | pub torque_sensor_low: TorqueSensorLowPin, 86 | } 87 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # OxCC 2 | 3 | ## Overview 4 | 5 | A port of [Open Source Car Control](https://github.com/jonlamb-gh/oscc) written in Rust. 6 | 7 | `OxCC` runs on the [NUCLEO-F767ZI STM32F767ZI](https://www.st.com/en/evaluation-tools/nucleo-f767zi.html) board. 8 | 9 | It is built around the traits and patterns provided by the [embedded-hal](https://github.com/rust-embedded/embedded-hal) 10 | project and community: 11 | see the [BSP crate](https://github.com/jonlamb-gh/oxcc-nucleo-f767zi), 12 | the [HAL crate](https://github.com/jonlamb-gh/oxcc-stm32f767-hal), 13 | and the [device crate](https://github.com/jonlamb-gh/oxcc-stm32f767). 14 | 15 | ### OSCC Divergence 16 | 17 | Apart from the change in MCU/board, `OxCC` combines all of the OSCC modules (throttle, brake, steering, CAN gateway) into a single application. 18 | 19 | #### Hardware 20 | 21 | A new layout and schematic is currently in the works, check out the rough [pinout](https://jonlamb-gh.github.io/oxcc/nucleo-144-oxcc-pins.png) to get started. 22 | 23 | * CAN 24 | 25 | `OxCC` uses the stm's on-board bxCAN controller. 26 | For a transceiver I've been using the [SN65HVD230](https://www.waveshare.com/sn65hvd230-can-board.htm) from Waveshare. 27 | 28 | ## Getting Started 29 | 30 | ### Dependencies 31 | 32 | * [rust](https://github.com/rust-lang-nursery/rustup.rs) (nightly) 33 | * [svd2rust](https://github.com/rust-embedded/svd2rust) 34 | * [openocd](http://openocd.org/) (for debugging) 35 | * [gdb-arm-none-eabi](https://gcc.gnu.org/) (for debugging) 36 | * [binutils-arm-none-eabi](https://gcc.gnu.org/) (uses `objcopy` for device deployment) 37 | * [stlink](https://github.com/texane/stlink) (for device deployment) 38 | 39 | ### Building 40 | 41 | The default Cargo configuration will build for the `Kia Soul EV` vehicle 42 | with the `panic-over-abort` strategy. 43 | 44 | See the `[features]` section of the [Cargo.toml](Cargo.toml) to change configurations. 45 | 46 | * Install system package dependencies: 47 | ```bash 48 | sudo apt-get install openocd 49 | sudo apt-get install gdb-arm-none-eabi 50 | sudo apt-get install binutils-arm-none-eabi 51 | ``` 52 | * Install `stlink` from source: [guide](https://github.com/texane/stlink/blob/master/doc/compiling.md) 53 | * Install Rust nightly and additional components: 54 | ```bash 55 | curl https://sh.rustup.rs -sSf | sh 56 | rustup install nightly 57 | rustup component add rust-src 58 | rustup component add rustfmt-preview --toolchain nightly 59 | rustup target add thumbv7em-none-eabihf 60 | ``` 61 | * Install `svd2rust`: 62 | ```bash 63 | cargo install svd2rust 64 | ``` 65 | * Build `OxCC` firmware: 66 | ```bash 67 | cargo build 68 | ``` 69 | 70 | ## Deploying 71 | 72 | Deploy the firmware Using `st-flash` (provided by `stlink`): 73 | 74 | ```bash 75 | # Convert ELF to ihex format 76 | arm-none-eabi-objcopy \ 77 | -O ihex \ 78 | target/thumbv7em-none-eabihf/release/oxcc \ 79 | target/thumbv7em-none-eabihf/release/oxcc.hex 80 | 81 | # Upload to flash 82 | st-flash \ 83 | --format ihex \ 84 | write \ 85 | target/thumbv7em-none-eabihf/release/oxcc.hex 86 | ``` 87 | 88 | ## Debugging 89 | 90 | In one terminal, start `openocd`: 91 | 92 | ```bash 93 | openocd -f board/stm32f7discovery.cfg 94 | ``` 95 | 96 | In the `OxCC` terminal, use the [runner](.cargo/config) to start a `gdb` session: 97 | 98 | ```bash 99 | cargo run 100 | ``` 101 | 102 | # License 103 | 104 | Licensed under either of 105 | 106 | * Apache License, Version 2.0 ([LICENSE-APACHE](LICENSE-APACHE) or 107 | http://www.apache.org/licenses/LICENSE-2.0) 108 | * MIT license ([LICENSE-MIT](LICENSE-MIT) or http://opensource.org/licenses/MIT) 109 | 110 | at your option. 111 | -------------------------------------------------------------------------------- /src/panic_abort.rs: -------------------------------------------------------------------------------- 1 | //! Panic behavior implementation for OxCC 2 | //! 3 | //! The implementation performs the following (in order): 4 | //! - disable safety/control related GPIO pins 5 | //! - enable red LED and brake lights as a fault indicator 6 | //! - output the PanicInfo to Serial3 7 | //! - `intrinsics::abort` 8 | //! 9 | //! **NOTE** 10 | //! The watchdog is enable by default, so you might not 11 | //! even notice the fault. 12 | //! It might be worth exposing the `hard_fault_indicator()` 13 | //! function so we can call it during initialization if 14 | //! the `ResetConditions` indicate a watchdog reset. That 15 | //! would persist the indication across panicing/faults. 16 | //! We could also disable the watchdog (and other interrupts) 17 | //! completely to stay in the abort. 18 | 19 | use core::panic::PanicInfo; 20 | use core::{intrinsics, ptr}; 21 | use cortex_m::interrupt::CriticalSection; 22 | 23 | use nucleo_f767zi::hal::stm32f7x7; 24 | use nucleo_f767zi::hal::time::*; 25 | 26 | struct SerialOutputHandle; 27 | 28 | macro_rules! serial_print { 29 | ($($arg:tt)*) => ({ 30 | use core::fmt::Write; 31 | SerialOutputHandle.write_fmt(format_args!($($arg)*)).unwrap(); 32 | }); 33 | } 34 | 35 | macro_rules! serial_println { 36 | ($fmt:expr) => (serial_print!(concat!($fmt, "\n"))); 37 | ($fmt:expr, $($arg:tt)*) => (serial_print!(concat!($fmt, "\n"), $($arg)*)); 38 | } 39 | 40 | #[panic_handler] 41 | fn panic(info: &PanicInfo) -> ! { 42 | cortex_m::interrupt::free(|cs| { 43 | disable_controls_gpio(cs); 44 | hard_fault_indicator(cs); 45 | serial3_panicinfo_dump(cs, info); 46 | }); 47 | 48 | unsafe { intrinsics::abort() } 49 | } 50 | 51 | /// Disable safety/control related GPIO 52 | fn disable_controls_gpio(_cs: &CriticalSection) { 53 | let gpiod = unsafe { &*stm32f7x7::GPIOD::ptr() }; 54 | let gpioe = unsafe { &*stm32f7x7::GPIOE::ptr() }; 55 | 56 | // disable throttle controls spoof-enable pin on PE2 57 | gpioe.moder.modify(|_, w| w.moder2().output()); 58 | gpioe.odr.modify(|_, w| w.odr2().clear_bit()); 59 | 60 | // disable steering controls spoof-enable pin on PD11 61 | gpiod.moder.modify(|_, w| w.moder11().output()); 62 | gpiod.odr.modify(|_, w| w.odr11().clear_bit()); 63 | 64 | // disable brake controls spoof-enable pin on PD12 65 | gpiod.moder.modify(|_, w| w.moder12().output()); 66 | gpiod.odr.modify(|_, w| w.odr12().clear_bit()); 67 | } 68 | 69 | /// Enable the hard fault indication 70 | fn hard_fault_indicator(_cs: &CriticalSection) { 71 | let gpiob = unsafe { &*stm32f7x7::GPIOB::ptr() }; 72 | let gpiod = unsafe { &*stm32f7x7::GPIOD::ptr() }; 73 | 74 | // turn red LED (PB14) on 75 | gpiob.moder.modify(|_, w| w.moder14().output()); 76 | gpiob.odr.modify(|_, w| w.odr14().set_bit()); 77 | 78 | // enable the brake lights on PD13 79 | gpiod.moder.modify(|_, w| w.moder13().output()); 80 | gpiod.odr.modify(|_, w| w.odr13().set_bit()); 81 | } 82 | 83 | /// Output PanicInfo to Serial3 84 | fn serial3_panicinfo_dump(_cs: &CriticalSection, info: &PanicInfo) { 85 | let rcc = unsafe { &*stm32f7x7::RCC::ptr() }; 86 | let usart = unsafe { &*stm32f7x7::USART3::ptr() }; 87 | 88 | // reset and enable 89 | rcc.apb1enr.modify(|_, w| w.usart3en().enabled()); 90 | rcc.apb1rstr.modify(|_, w| w.uart3rst().set_bit()); 91 | rcc.apb1rstr.modify(|_, w| w.uart3rst().clear_bit()); 92 | 93 | let pclk1: Hertz = Hertz(216_000_000 / 4); 94 | let baud_rate: Bps = 115_200.bps(); 95 | let brr = pclk1.0 / baud_rate.0; 96 | usart.brr.write(|w| unsafe { w.bits(brr) }); 97 | 98 | // enable USART, tx 99 | usart.cr1.write(|w| w.ue().set_bit().te().set_bit()); 100 | 101 | serial_println!("\n!!! WARNING !!!\n{}", info); 102 | } 103 | 104 | /// Blocking putchar function to be used by the panic handler only 105 | fn putchar(byte: u8) { 106 | let mut isr = unsafe { (*stm32f7x7::USART3::ptr()).isr.read() }; 107 | 108 | if isr.txe().bit_is_set() { 109 | unsafe { 110 | ptr::write_volatile(&(*stm32f7x7::USART3::ptr()).tdr as *const _ as *mut _, byte); 111 | } 112 | 113 | // wait for completion 114 | while { 115 | isr = unsafe { (*stm32f7x7::USART3::ptr()).isr.read() }; 116 | isr.tc().bit_is_clear() 117 | } {} 118 | } 119 | } 120 | 121 | impl ::core::fmt::Write for SerialOutputHandle { 122 | fn write_str(&mut self, s: &str) -> ::core::fmt::Result { 123 | for &b in s.as_bytes() { 124 | putchar(b); 125 | } 126 | Ok(()) 127 | } 128 | } 129 | -------------------------------------------------------------------------------- /src/config.rs: -------------------------------------------------------------------------------- 1 | //! OxCC configuration data 2 | 3 | use brake_can_protocol::*; 4 | use fault_can_protocol::*; 5 | use nucleo_f767zi::hal::can::{ 6 | CanBitTiming, CanConfig, CanFilterConfig, FilterMode, FilterScale, RxFifo, 7 | }; 8 | use steering_can_protocol::*; 9 | use throttle_can_protocol::*; 10 | use vehicle::*; 11 | 12 | /// Control CAN interface configuration 13 | pub const CONTROL_CAN_CONFIG: CanConfig = CanConfig { 14 | loopback_mode: false, 15 | silent_mode: false, 16 | ttcm: false, 17 | abom: true, 18 | awum: false, 19 | nart: false, 20 | rflm: false, 21 | txfp: false, 22 | // TODO - update CAN impl to calculate these 23 | /// Control CAN bus is configured for 500K 24 | bit_timing: CanBitTiming { 25 | prescaler: 5, // 6 26 | sjw: 0, // CAN_SJW_1TQ 27 | bs1: 14, // CAN_BS1_15TQ 28 | bs2: 1, // CAN_BS2_2TQ 29 | }, 30 | }; 31 | 32 | /// Vehicle OBD CAN interface configuration 33 | pub const OBD_CAN_CONFIG: CanConfig = CanConfig { 34 | loopback_mode: false, 35 | silent_mode: false, 36 | ttcm: false, 37 | abom: true, 38 | awum: false, 39 | nart: false, 40 | rflm: false, 41 | txfp: false, 42 | /// OBD CAN bus is configured for 500K 43 | bit_timing: CanBitTiming { 44 | prescaler: 5, // 6 45 | sjw: 0, // CAN_SJW_1TQ 46 | bs1: 14, // CAN_BS1_15TQ 47 | bs2: 1, // CAN_BS2_2TQ 48 | }, 49 | }; 50 | 51 | /// Gather the Control CAN filter configurations 52 | /// 53 | /// Since we're only interrested in a small number of messages, 54 | /// we can use ID list mode instead of masking. 55 | /// Only the specific message IDs are allowed through the filter. 56 | /// Filter 0 is the highest priority filter, followed by filter 1, etc. 57 | pub fn gather_control_can_filters() -> [CanFilterConfig; 3] { 58 | // Filter 0, bound to FIFO_0 59 | // - disable control IDs for throttle, brake, steering 60 | // - fault report ID 61 | let mut f0 = CanFilterConfig::default(); 62 | f0.filter_number = 0; 63 | f0.enabled = true; 64 | f0.mode = FilterMode::IdList; 65 | f0.fifo_assignment = RxFifo::Fifo0; 66 | f0.scale = FilterScale::Fs16Bit; 67 | f0.filter_mask_id_low = u32::from(OSCC_THROTTLE_DISABLE_CAN_ID << 5); 68 | f0.filter_id_low = u32::from(OSCC_BRAKE_DISABLE_CAN_ID << 5); 69 | f0.filter_mask_id_high = u32::from(OSCC_STEERING_DISABLE_CAN_ID << 5); 70 | f0.filter_id_high = u32::from(OSCC_FAULT_REPORT_CAN_ID << 5); 71 | 72 | // Filter 1, bound to FIFO_1 73 | // - control command IDs for brake, throttle, and steering 74 | let mut f1 = CanFilterConfig::default(); 75 | f1.filter_number = 1; 76 | f1.enabled = true; 77 | f1.mode = FilterMode::IdList; 78 | f1.fifo_assignment = RxFifo::Fifo1; 79 | f1.scale = FilterScale::Fs16Bit; 80 | f1.filter_mask_id_low = u32::from(OSCC_BRAKE_COMMAND_CAN_ID << 5); 81 | f1.filter_id_low = u32::from(OSCC_THROTTLE_COMMAND_CAN_ID << 5); 82 | f1.filter_mask_id_high = u32::from(OSCC_STEERING_COMMAND_CAN_ID << 5); 83 | f1.filter_id_high = 0; 84 | 85 | // filter 2, bound to FIFO_1 86 | // - enable control IDs for brake, throttle, and steering 87 | let mut f2 = CanFilterConfig::default(); 88 | f2.filter_number = 2; 89 | f2.enabled = true; 90 | f2.mode = FilterMode::IdList; 91 | f2.fifo_assignment = RxFifo::Fifo1; 92 | f2.scale = FilterScale::Fs16Bit; 93 | f2.filter_mask_id_low = u32::from(OSCC_BRAKE_ENABLE_CAN_ID << 5); 94 | f2.filter_id_low = u32::from(OSCC_THROTTLE_ENABLE_CAN_ID << 5); 95 | f2.filter_mask_id_high = u32::from(OSCC_STEERING_ENABLE_CAN_ID << 5); 96 | f2.filter_id_high = 0; 97 | 98 | [f0, f1, f2] 99 | } 100 | 101 | /// Gather the vehicle OBD CAN filter configurations 102 | /// 103 | /// **NOTE** 104 | /// Vehicle OBD CAN is on CAN2 which shares a configuration 105 | /// peripheral with CAN1 (see notes on bxCAN). The split is 50/50, so 106 | /// CAN1 gets the first 0:13 filters and CAN2 gets the remaining 14:27 107 | /// filters. This is why the filter numbers here start at 14. 108 | /// 109 | /// Since we're only interrested in a small number of messages, 110 | /// we can use ID list mode instead of masking. 111 | /// Only the specific message IDs are allowed through the filter. 112 | /// Filter 0 is the highest priority filter, followed by filter 1, etc. 113 | pub fn gather_obd_can_filters() -> [CanFilterConfig; 1] { 114 | // filter 14, bound to FIFO_0 115 | // - the 4 OBD IDs 116 | let mut f3 = CanFilterConfig::default(); 117 | f3.filter_number = 14; 118 | f3.bank_number = 14; 119 | f3.enabled = true; 120 | f3.mode = FilterMode::IdList; 121 | f3.fifo_assignment = RxFifo::Fifo0; 122 | f3.scale = FilterScale::Fs16Bit; 123 | f3.filter_mask_id_low = u32::from(KIA_SOUL_OBD_STEERING_WHEEL_ANGLE_CAN_ID << 5); 124 | f3.filter_id_low = u32::from(KIA_SOUL_OBD_WHEEL_SPEED_CAN_ID << 5); 125 | f3.filter_mask_id_high = u32::from(KIA_SOUL_OBD_BRAKE_PRESSURE_CAN_ID << 5); 126 | #[cfg(feature = "kia-soul-ev")] 127 | { 128 | f3.filter_id_high = u32::from(KIA_SOUL_OBD_THROTTLE_PRESSURE_CAN_ID << 5); 129 | } 130 | 131 | [f3] 132 | } 133 | -------------------------------------------------------------------------------- /src/can_gateway_module.rs: -------------------------------------------------------------------------------- 1 | //! CAN gateway module 2 | 3 | use brake_can_protocol::*; 4 | use fault_can_protocol::*; 5 | use nucleo_f767zi::hal::can::{CanError, CanFrame, DataFrame, RxFifo}; 6 | use nucleo_f767zi::hal::prelude::*; 7 | use oscc_magic_byte::*; 8 | use oxcc_error::OxccError; 9 | use steering_can_protocol::*; 10 | use throttle_can_protocol::*; 11 | use types::*; 12 | use vehicle::*; 13 | 14 | pub struct CanGatewayModule { 15 | can_publish_timer: CanPublishTimer, 16 | control_can: ControlCan, 17 | obd_can: ObdCan, 18 | fault_report_can_frame: DataFrame, 19 | throttle_report_can_frame: DataFrame, 20 | brake_report_can_frame: DataFrame, 21 | steering_report_can_frame: DataFrame, 22 | } 23 | 24 | impl CanGatewayModule { 25 | pub fn new( 26 | can_publish_timer: CanPublishTimer, 27 | control_can: ControlCan, 28 | obd_can: ObdCan, 29 | ) -> Self { 30 | CanGatewayModule { 31 | can_publish_timer, 32 | control_can, 33 | obd_can, 34 | fault_report_can_frame: default_fault_report_data_frame(), 35 | brake_report_can_frame: default_brake_report_data_frame(), 36 | throttle_report_can_frame: default_throttle_report_data_frame(), 37 | steering_report_can_frame: default_steering_report_data_frame(), 38 | } 39 | } 40 | 41 | pub fn republish_obd_frames_to_control_can_bus(&mut self) -> Result<(), OxccError> { 42 | // poll both OBD CAN FIFOs 43 | for fifo in &[RxFifo::Fifo0, RxFifo::Fifo1] { 44 | if let Ok(rx_frame) = self.obd_can().receive(fifo) { 45 | self.republish_obd_frame_to_control_can_bus(&rx_frame)?; 46 | } 47 | } 48 | 49 | Ok(()) 50 | } 51 | 52 | fn republish_obd_frame_to_control_can_bus( 53 | &mut self, 54 | frame: &CanFrame, 55 | ) -> Result<(), OxccError> { 56 | let id: u32 = frame.id().into(); 57 | 58 | let mut is_a_match = (id == KIA_SOUL_OBD_STEERING_WHEEL_ANGLE_CAN_ID.into()) 59 | || (id == KIA_SOUL_OBD_WHEEL_SPEED_CAN_ID.into()) 60 | || (id == KIA_SOUL_OBD_BRAKE_PRESSURE_CAN_ID.into()); 61 | 62 | #[cfg(feature = "kia-soul-ev")] 63 | { 64 | if id == KIA_SOUL_OBD_THROTTLE_PRESSURE_CAN_ID.into() { 65 | is_a_match = true; 66 | } 67 | } 68 | 69 | if is_a_match { 70 | self.control_can().transmit(&frame)?; 71 | } 72 | 73 | Ok(()) 74 | } 75 | 76 | // TODO - hide these details, switch to a publisher approach 77 | pub fn control_can(&mut self) -> &mut ControlCan { 78 | &mut self.control_can 79 | } 80 | 81 | pub fn obd_can(&mut self) -> &mut ObdCan { 82 | &mut self.obd_can 83 | } 84 | 85 | pub fn wait_for_publish(&mut self) -> bool { 86 | self.can_publish_timer.wait().is_ok() 87 | } 88 | } 89 | 90 | impl FaultReportPublisher for CanGatewayModule { 91 | fn publish_fault_report(&mut self, fault_report: &OsccFaultReport) -> Result<(), CanError> { 92 | { 93 | self.fault_report_can_frame 94 | .set_data_length(OSCC_FAULT_REPORT_CAN_DLC as _); 95 | 96 | let data = self.fault_report_can_frame.data_as_mut(); 97 | 98 | data[0] = OSCC_MAGIC_BYTE_0; 99 | data[1] = OSCC_MAGIC_BYTE_1; 100 | data[2] = (fault_report.fault_origin_id & 0xFF) as _; 101 | data[3] = ((fault_report.fault_origin_id >> 8) & 0xFF) as _; 102 | data[4] = ((fault_report.fault_origin_id >> 16) & 0xFF) as _; 103 | data[5] = ((fault_report.fault_origin_id >> 24) & 0xFF) as _; 104 | data[6] = fault_report.dtcs; 105 | } 106 | 107 | self.control_can 108 | .transmit(&self.fault_report_can_frame.into()) 109 | } 110 | } 111 | 112 | impl BrakeReportPublisher for CanGatewayModule { 113 | fn publish_brake_report(&mut self, brake_report: &OsccBrakeReport) -> Result<(), CanError> { 114 | { 115 | self.brake_report_can_frame 116 | .set_data_length(OSCC_BRAKE_REPORT_CAN_DLC as _); 117 | 118 | let data = self.brake_report_can_frame.data_as_mut(); 119 | 120 | data[0] = OSCC_MAGIC_BYTE_0; 121 | data[1] = OSCC_MAGIC_BYTE_1; 122 | data[2] = brake_report.enabled as _; 123 | data[3] = brake_report.operator_override as _; 124 | data[4] = brake_report.dtcs; 125 | } 126 | 127 | self.control_can 128 | .transmit(&self.brake_report_can_frame.into()) 129 | } 130 | } 131 | 132 | impl ThrottleReportPublisher for CanGatewayModule { 133 | fn publish_throttle_report( 134 | &mut self, 135 | throttle_report: &OsccThrottleReport, 136 | ) -> Result<(), CanError> { 137 | { 138 | self.throttle_report_can_frame 139 | .set_data_length(OSCC_THROTTLE_REPORT_CAN_DLC as _); 140 | 141 | let data = self.throttle_report_can_frame.data_as_mut(); 142 | 143 | data[0] = OSCC_MAGIC_BYTE_0; 144 | data[1] = OSCC_MAGIC_BYTE_1; 145 | data[2] = throttle_report.enabled as _; 146 | data[3] = throttle_report.operator_override as _; 147 | data[4] = throttle_report.dtcs; 148 | } 149 | 150 | self.control_can 151 | .transmit(&self.throttle_report_can_frame.into()) 152 | } 153 | } 154 | 155 | impl SteeringReportPublisher for CanGatewayModule { 156 | fn publish_steering_report( 157 | &mut self, 158 | steering_report: &OsccSteeringReport, 159 | ) -> Result<(), CanError> { 160 | { 161 | self.steering_report_can_frame 162 | .set_data_length(OSCC_STEERING_REPORT_CAN_DLC as _); 163 | 164 | let data = self.steering_report_can_frame.data_as_mut(); 165 | 166 | data[0] = OSCC_MAGIC_BYTE_0; 167 | data[1] = OSCC_MAGIC_BYTE_1; 168 | data[2] = steering_report.enabled as _; 169 | data[3] = steering_report.operator_override as _; 170 | data[4] = steering_report.dtcs; 171 | } 172 | 173 | self.control_can 174 | .transmit(&self.steering_report_can_frame.into()) 175 | } 176 | } 177 | -------------------------------------------------------------------------------- /src/throttle_module.rs: -------------------------------------------------------------------------------- 1 | //! Throttle module 2 | 3 | use board::AcceleratorPositionSensor; 4 | use core::fmt::Write; 5 | use dac_mcp4922::DacOutput; 6 | use dtc::DtcBitfield; 7 | use dual_signal::DualSignal; 8 | use fault_can_protocol::*; 9 | use fault_condition::FaultCondition; 10 | use nucleo_f767zi::debug_console::DebugConsole; 11 | use nucleo_f767zi::hal::can::CanFrame; 12 | use nucleo_f767zi::hal::prelude::*; 13 | use num; 14 | use oscc_magic_byte::*; 15 | use oxcc_error::OxccError; 16 | use ranges; 17 | use throttle_can_protocol::*; 18 | use types::*; 19 | use vehicle::*; 20 | 21 | struct ThrottleControlState { 22 | enabled: bool, 23 | operator_override: bool, 24 | dtcs: DTCS, 25 | } 26 | 27 | impl ThrottleControlState 28 | where 29 | DTCS: DtcBitfield, 30 | { 31 | pub const fn new(dtcs: DTCS) -> Self { 32 | ThrottleControlState { 33 | enabled: false, 34 | operator_override: false, 35 | dtcs, 36 | } 37 | } 38 | } 39 | 40 | /// Throttle module 41 | pub struct ThrottleModule { 42 | accelerator_position: DualSignal, 43 | control_state: ThrottleControlState, 44 | grounded_fault_state: FaultCondition, 45 | operator_override_state: FaultCondition, 46 | throttle_report: OsccThrottleReport, 47 | fault_report: OsccFaultReport, 48 | throttle_dac: ThrottleDac, 49 | throttle_pins: ThrottlePins, 50 | } 51 | 52 | pub struct UnpreparedThrottleModule { 53 | throttle_module: ThrottleModule, 54 | } 55 | 56 | impl UnpreparedThrottleModule { 57 | pub fn new( 58 | accelerator_position_sensor: AcceleratorPositionSensor, 59 | throttle_dac: ThrottleDac, 60 | throttle_pins: ThrottlePins, 61 | grounded_fault_timer: ThrottleGroundedFaultTimer, 62 | override_timer: ThrottleOverrideFaultTimer, 63 | ) -> UnpreparedThrottleModule { 64 | UnpreparedThrottleModule { 65 | throttle_module: ThrottleModule { 66 | accelerator_position: DualSignal::new(0, 0, accelerator_position_sensor), 67 | control_state: ThrottleControlState::new(u8::default()), 68 | grounded_fault_state: FaultCondition::new(grounded_fault_timer), 69 | operator_override_state: FaultCondition::new(override_timer), 70 | throttle_report: OsccThrottleReport::new(), 71 | fault_report: OsccFaultReport { 72 | fault_origin_id: FAULT_ORIGIN_THROTTLE, 73 | dtcs: 0, 74 | }, 75 | throttle_dac, 76 | throttle_pins, 77 | }, 78 | } 79 | } 80 | 81 | pub fn prepare_module(self) -> ThrottleModule { 82 | let mut throttle_module = self.throttle_module; 83 | throttle_module.throttle_pins.spoof_enable.set_low(); 84 | throttle_module 85 | } 86 | } 87 | 88 | impl ThrottleModule { 89 | pub fn disable_control(&mut self, debug_console: &mut DebugConsole) -> Result<(), OxccError> { 90 | if self.control_state.enabled { 91 | self.accelerator_position.prevent_signal_discontinuity(); 92 | 93 | let result = self.throttle_dac.output_ab( 94 | DacOutput::clamp(self.accelerator_position.low()), 95 | DacOutput::clamp(self.accelerator_position.high()), 96 | ); 97 | 98 | // even if we've encountered an error, we can still disable 99 | self.throttle_pins.spoof_enable.set_low(); 100 | self.control_state.enabled = false; 101 | writeln!(debug_console, "Throttle control disabled"); 102 | 103 | return if let Err(e) = result { 104 | Err(OxccError::from(e)) 105 | } else { 106 | Ok(()) 107 | }; 108 | } 109 | 110 | Ok(()) 111 | } 112 | 113 | fn enable_control(&mut self, debug_console: &mut DebugConsole) -> Result<(), OxccError> { 114 | if !self.control_state.enabled && !self.control_state.operator_override { 115 | self.accelerator_position.prevent_signal_discontinuity(); 116 | 117 | let result = self.throttle_dac.output_ab( 118 | DacOutput::clamp(self.accelerator_position.low()), 119 | DacOutput::clamp(self.accelerator_position.high()), 120 | ); 121 | 122 | return if let Err(e) = result { 123 | Err(OxccError::from(e)) 124 | } else { 125 | self.throttle_pins.spoof_enable.set_high(); 126 | self.control_state.enabled = true; 127 | writeln!(debug_console, "Throttle control enabled"); 128 | Ok(()) 129 | }; 130 | } 131 | 132 | Ok(()) 133 | } 134 | 135 | fn update_throttle( 136 | &mut self, 137 | spoof_command_high: u16, 138 | spoof_command_low: u16, 139 | ) -> Result<(), OxccError> { 140 | if self.control_state.enabled { 141 | // TODO - revisit this, enforce high->A, low->B 142 | self.throttle_dac.output_ab( 143 | ranges::coerce(ThrottleSpoofHighSignal::clamp(spoof_command_high)), 144 | ranges::coerce(ThrottleSpoofHighSignal::clamp(spoof_command_low)), 145 | )?; 146 | } 147 | 148 | Ok(()) 149 | } 150 | 151 | /// Checks for any fresh (previously undetected or unhandled) faults 152 | pub fn check_for_faults( 153 | &mut self, 154 | debug_console: &mut DebugConsole, 155 | ) -> Result, OxccError> { 156 | if !self.control_state.enabled && !self.control_state.dtcs.are_any_set() { 157 | // Assumes this module already went through the proper transition into a faulted 158 | // and disabled state, and we do not want to double-report a possible duplicate 159 | // fault. 160 | return Ok(None); 161 | } 162 | 163 | self.accelerator_position.update(); 164 | 165 | let accelerator_position_average = self.accelerator_position.average(); 166 | 167 | let operator_overridden: bool = self.operator_override_state.condition_exceeded_duration( 168 | accelerator_position_average >= ACCELERATOR_OVERRIDE_THRESHOLD, 169 | ); 170 | 171 | let inputs_grounded: bool = self 172 | .grounded_fault_state 173 | .check_voltage_grounded(&self.accelerator_position); 174 | 175 | // sensor pins tied to ground - a value of zero indicates disconnection 176 | if inputs_grounded { 177 | self.disable_control(debug_console)?; 178 | 179 | self.control_state 180 | .dtcs 181 | .set(OSCC_THROTTLE_DTC_INVALID_SENSOR_VAL); 182 | 183 | self.update_fault_report(); 184 | 185 | writeln!( 186 | debug_console, 187 | "Bad value read from accelerator position sensor" 188 | ); 189 | 190 | Ok(Some(&self.fault_report)) 191 | } else if operator_overridden && !self.control_state.operator_override { 192 | self.disable_control(debug_console)?; 193 | 194 | self.control_state 195 | .dtcs 196 | .set(OSCC_THROTTLE_DTC_OPERATOR_OVERRIDE); 197 | 198 | self.update_fault_report(); 199 | 200 | self.control_state.operator_override = true; 201 | 202 | writeln!(debug_console, "Throttle operator override"); 203 | 204 | Ok(Some(&self.fault_report)) 205 | } else { 206 | self.control_state.dtcs.clear_all(); 207 | self.control_state.operator_override = false; 208 | Ok(None) 209 | } 210 | } 211 | 212 | fn update_fault_report(&mut self) { 213 | self.fault_report.dtcs = self.control_state.dtcs; 214 | } 215 | 216 | pub fn supply_throttle_report(&mut self) -> &OsccThrottleReport { 217 | self.throttle_report.enabled = self.control_state.enabled; 218 | self.throttle_report.operator_override = self.control_state.operator_override; 219 | self.throttle_report.dtcs = self.control_state.dtcs; 220 | &self.throttle_report 221 | } 222 | 223 | pub fn process_rx_frame( 224 | &mut self, 225 | can_frame: &CanFrame, 226 | debug_console: &mut DebugConsole, 227 | ) -> Result<(), OxccError> { 228 | if let CanFrame::DataFrame(ref frame) = can_frame { 229 | let id: u32 = frame.id().into(); 230 | let data = frame.data(); 231 | 232 | if (data[0] == OSCC_MAGIC_BYTE_0) && (data[1] == OSCC_MAGIC_BYTE_1) { 233 | if id == OSCC_THROTTLE_ENABLE_CAN_ID.into() { 234 | self.enable_control(debug_console)?; 235 | } else if id == OSCC_THROTTLE_DISABLE_CAN_ID.into() { 236 | self.disable_control(debug_console)?; 237 | } else if id == OSCC_THROTTLE_COMMAND_CAN_ID.into() { 238 | self.process_throttle_command(&OsccThrottleCommand::from(frame))?; 239 | } else if id == OSCC_FAULT_REPORT_CAN_ID.into() { 240 | self.process_fault_report(&OsccFaultReport::from(frame), debug_console)?; 241 | } 242 | } 243 | } 244 | 245 | Ok(()) 246 | } 247 | 248 | fn process_fault_report( 249 | &mut self, 250 | fault_report: &OsccFaultReport, 251 | debug_console: &mut DebugConsole, 252 | ) -> Result<(), OxccError> { 253 | writeln!( 254 | debug_console, 255 | "Fault report received from: {} DTCs: {}", 256 | fault_report.fault_origin_id, fault_report.dtcs 257 | ); 258 | 259 | self.disable_control(debug_console) 260 | } 261 | 262 | fn process_throttle_command(&mut self, command: &OsccThrottleCommand) -> Result<(), OxccError> { 263 | let clamped_position = num::clamp( 264 | command.torque_request, 265 | MINIMUM_THROTTLE_COMMAND, 266 | MAXIMUM_THROTTLE_COMMAND, 267 | ); 268 | 269 | let spoof_voltage_low: f32 = num::clamp( 270 | throttle_position_to_volts_low(clamped_position), 271 | THROTTLE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN, 272 | THROTTLE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX, 273 | ); 274 | 275 | let spoof_voltage_high: f32 = num::clamp( 276 | throttle_position_to_volts_high(clamped_position), 277 | THROTTLE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN, 278 | THROTTLE_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX, 279 | ); 280 | 281 | let spoof_value_low = (STEPS_PER_VOLT * spoof_voltage_low) as u16; 282 | let spoof_value_high = (STEPS_PER_VOLT * spoof_voltage_high) as u16; 283 | 284 | self.update_throttle(spoof_value_high, spoof_value_low) 285 | } 286 | } 287 | -------------------------------------------------------------------------------- /src/steering_module.rs: -------------------------------------------------------------------------------- 1 | //! Steering module 2 | 3 | use board::TorqueSensor; 4 | use core::fmt::Write; 5 | use dac_mcp4922::DacOutput; 6 | use dtc::DtcBitfield; 7 | use dual_signal::DualSignal; 8 | use fault_can_protocol::*; 9 | use fault_condition::FaultCondition; 10 | use nucleo_f767zi::debug_console::DebugConsole; 11 | use nucleo_f767zi::hal::can::CanFrame; 12 | use nucleo_f767zi::hal::prelude::*; 13 | use num; 14 | use oscc_magic_byte::*; 15 | use oxcc_error::OxccError; 16 | use ranges; 17 | use steering_can_protocol::*; 18 | use types::*; 19 | use vehicle::*; 20 | 21 | const FILTER_ALPHA: f32 = 0.01_f32; 22 | 23 | struct SteeringControlState { 24 | enabled: bool, 25 | operator_override: bool, 26 | dtcs: DTCS, 27 | } 28 | 29 | impl SteeringControlState 30 | where 31 | DTCS: DtcBitfield, 32 | { 33 | pub const fn new(dtcs: DTCS) -> Self { 34 | SteeringControlState { 35 | enabled: false, 36 | operator_override: false, 37 | dtcs, 38 | } 39 | } 40 | } 41 | 42 | /// Steering module 43 | pub struct SteeringModule { 44 | steering_torque: DualSignal, 45 | control_state: SteeringControlState, 46 | grounded_fault_state: FaultCondition, 47 | filtered_diff: u16, 48 | steering_report: OsccSteeringReport, 49 | fault_report: OsccFaultReport, 50 | steering_dac: SteeringDac, 51 | steering_pins: SteeringPins, 52 | } 53 | 54 | pub struct UnpreparedSteeringModule { 55 | steering_module: SteeringModule, 56 | } 57 | 58 | impl UnpreparedSteeringModule { 59 | pub fn new( 60 | torque_sensor: TorqueSensor, 61 | steering_dac: SteeringDac, 62 | steering_pins: SteeringPins, 63 | grounded_fault_timer: SteeringGroundedFaultTimer, 64 | ) -> Self { 65 | UnpreparedSteeringModule { 66 | steering_module: SteeringModule { 67 | steering_torque: DualSignal::new(0, 0, torque_sensor), 68 | control_state: SteeringControlState::new(u8::default()), 69 | grounded_fault_state: FaultCondition::new(grounded_fault_timer), 70 | filtered_diff: 0, 71 | steering_report: OsccSteeringReport::new(), 72 | fault_report: OsccFaultReport { 73 | fault_origin_id: FAULT_ORIGIN_STEERING, 74 | dtcs: 0, 75 | }, 76 | steering_dac, 77 | steering_pins, 78 | }, 79 | } 80 | } 81 | 82 | pub fn prepare_module(self) -> SteeringModule { 83 | let mut steering_module = self.steering_module; 84 | steering_module.steering_pins.spoof_enable.set_low(); 85 | steering_module 86 | } 87 | } 88 | 89 | impl SteeringModule { 90 | pub fn disable_control(&mut self, debug_console: &mut DebugConsole) -> Result<(), OxccError> { 91 | if self.control_state.enabled { 92 | self.steering_torque.prevent_signal_discontinuity(); 93 | 94 | let result = self.steering_dac.output_ab( 95 | DacOutput::clamp(self.steering_torque.low()), 96 | DacOutput::clamp(self.steering_torque.high()), 97 | ); 98 | 99 | // even if we've encountered an error, we can still disable 100 | self.steering_pins.spoof_enable.set_low(); 101 | self.control_state.enabled = false; 102 | writeln!(debug_console, "Steering control disabled"); 103 | 104 | return if let Err(e) = result { 105 | Err(OxccError::from(e)) 106 | } else { 107 | Ok(()) 108 | }; 109 | } 110 | 111 | Ok(()) 112 | } 113 | 114 | pub fn enable_control(&mut self, debug_console: &mut DebugConsole) -> Result<(), OxccError> { 115 | if !self.control_state.enabled && !self.control_state.operator_override { 116 | self.steering_torque.prevent_signal_discontinuity(); 117 | 118 | let result = self.steering_dac.output_ab( 119 | DacOutput::clamp(self.steering_torque.low()), 120 | DacOutput::clamp(self.steering_torque.high()), 121 | ); 122 | 123 | return if let Err(e) = result { 124 | Err(OxccError::from(e)) 125 | } else { 126 | self.steering_pins.spoof_enable.set_high(); 127 | self.control_state.enabled = true; 128 | writeln!(debug_console, "Steering control enabled"); 129 | Ok(()) 130 | }; 131 | } 132 | 133 | Ok(()) 134 | } 135 | 136 | pub fn update_steering( 137 | &mut self, 138 | spoof_command_high: u16, 139 | spoof_command_low: u16, 140 | ) -> Result<(), OxccError> { 141 | if self.control_state.enabled { 142 | // TODO - revisit this, enforce high->A, low->B 143 | self.steering_dac.output_ab( 144 | ranges::coerce(SteeringSpoofHighSignal::clamp(spoof_command_high)), 145 | ranges::coerce(SteeringSpoofLowSignal::clamp(spoof_command_low)), 146 | )?; 147 | } 148 | 149 | Ok(()) 150 | } 151 | 152 | pub fn check_for_faults( 153 | &mut self, 154 | debug_console: &mut DebugConsole, 155 | ) -> Result, OxccError> { 156 | if !self.control_state.enabled && !self.control_state.dtcs.are_any_set() { 157 | // Assumes this module already went through the proper transition into a faulted 158 | // and disabled state, and we do not want to double-report a possible duplicate 159 | // fault. 160 | return Ok(None); 161 | } 162 | 163 | self.steering_torque.update(); 164 | 165 | let unfiltered_diff = self.steering_torque.diff(); 166 | 167 | if self.filtered_diff == 0 { 168 | self.filtered_diff = unfiltered_diff; 169 | } 170 | 171 | // TODO - revist this 172 | // OSCC goes back and forth with u16 and f32 types 173 | self.filtered_diff = self.exponential_moving_average( 174 | FILTER_ALPHA, 175 | f32::from(unfiltered_diff), 176 | f32::from(self.filtered_diff), 177 | ) as _; 178 | 179 | let inputs_grounded: bool = self 180 | .grounded_fault_state 181 | .check_voltage_grounded(&self.steering_torque); 182 | 183 | // sensor pins tied to ground - a value of zero indicates disconnection 184 | if inputs_grounded { 185 | self.disable_control(debug_console)?; 186 | 187 | self.control_state 188 | .dtcs 189 | .set(OSCC_STEERING_DTC_INVALID_SENSOR_VAL); 190 | 191 | self.update_fault_report(); 192 | 193 | writeln!(debug_console, "Bad value read from torque sensor"); 194 | 195 | Ok(Some(&self.fault_report)) 196 | } else if (self.filtered_diff > TORQUE_DIFFERENCE_OVERRIDE_THRESHOLD) 197 | && !self.control_state.operator_override 198 | { 199 | self.disable_control(debug_console)?; 200 | 201 | self.control_state 202 | .dtcs 203 | .set(OSCC_STEERING_DTC_OPERATOR_OVERRIDE); 204 | 205 | self.update_fault_report(); 206 | 207 | self.control_state.operator_override = true; 208 | 209 | writeln!(debug_console, "Steering operator override"); 210 | 211 | Ok(Some(&self.fault_report)) 212 | } else { 213 | self.control_state.dtcs.clear_all(); 214 | self.control_state.operator_override = false; 215 | Ok(None) 216 | } 217 | } 218 | 219 | fn update_fault_report(&mut self) { 220 | self.fault_report.dtcs = self.control_state.dtcs; 221 | } 222 | 223 | fn exponential_moving_average(&self, alpha: f32, input: f32, average: f32) -> f32 { 224 | (alpha * input) + ((1.0 - alpha) * average) 225 | } 226 | 227 | pub fn supply_steering_report(&mut self) -> &OsccSteeringReport { 228 | self.steering_report.enabled = self.control_state.enabled; 229 | self.steering_report.operator_override = self.control_state.operator_override; 230 | self.steering_report.dtcs = self.control_state.dtcs; 231 | &self.steering_report 232 | } 233 | 234 | pub fn process_rx_frame( 235 | &mut self, 236 | can_frame: &CanFrame, 237 | debug_console: &mut DebugConsole, 238 | ) -> Result<(), OxccError> { 239 | if let CanFrame::DataFrame(ref frame) = can_frame { 240 | let id: u32 = frame.id().into(); 241 | let data = frame.data(); 242 | 243 | if (data[0] == OSCC_MAGIC_BYTE_0) && (data[1] == OSCC_MAGIC_BYTE_1) { 244 | if id == OSCC_STEERING_ENABLE_CAN_ID.into() { 245 | self.enable_control(debug_console)?; 246 | } else if id == OSCC_STEERING_DISABLE_CAN_ID.into() { 247 | self.disable_control(debug_console)?; 248 | } else if id == OSCC_STEERING_COMMAND_CAN_ID.into() { 249 | self.process_steering_command(&OsccSteeringCommand::from(frame))?; 250 | } else if id == OSCC_FAULT_REPORT_CAN_ID.into() { 251 | self.process_fault_report(&OsccFaultReport::from(frame), debug_console)?; 252 | } 253 | } 254 | } 255 | 256 | Ok(()) 257 | } 258 | 259 | fn process_fault_report( 260 | &mut self, 261 | fault_report: &OsccFaultReport, 262 | debug_console: &mut DebugConsole, 263 | ) -> Result<(), OxccError> { 264 | writeln!( 265 | debug_console, 266 | "Fault report received from: {} DTCs: {}", 267 | fault_report.fault_origin_id, fault_report.dtcs 268 | ); 269 | 270 | self.disable_control(debug_console) 271 | } 272 | 273 | fn process_steering_command(&mut self, command: &OsccSteeringCommand) -> Result<(), OxccError> { 274 | let clamped_torque = num::clamp( 275 | command.torque_request * MAXIMUM_TORQUE_COMMAND, 276 | MINIMUM_TORQUE_COMMAND, 277 | MAXIMUM_TORQUE_COMMAND, 278 | ); 279 | 280 | let spoof_voltage_low: f32 = num::clamp( 281 | steering_torque_to_volts_low(clamped_torque), 282 | STEERING_SPOOF_LOW_SIGNAL_VOLTAGE_MIN, 283 | STEERING_SPOOF_LOW_SIGNAL_VOLTAGE_MAX, 284 | ); 285 | 286 | let spoof_voltage_high: f32 = num::clamp( 287 | steering_torque_to_volts_high(clamped_torque), 288 | STEERING_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN, 289 | STEERING_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX, 290 | ); 291 | 292 | let spoof_value_low = (STEPS_PER_VOLT * spoof_voltage_low) as u16; 293 | let spoof_value_high = (STEPS_PER_VOLT * spoof_voltage_high) as u16; 294 | 295 | self.update_steering(spoof_value_high, spoof_value_low) 296 | } 297 | } 298 | -------------------------------------------------------------------------------- /src/vehicles/kial_niro.rs: -------------------------------------------------------------------------------- 1 | //! Kia Niro vehicle configuration data 2 | //! 3 | //! **WARNING** 4 | //! 5 | //! The values listed here are carefully tested to ensure that the vehicle's 6 | //! components are not actuated outside of the range of what they can handle. 7 | //! By changing any of these values you risk attempting to actuate outside of 8 | //! the vehicle's valid range. This can cause damage to the hardware and/or a 9 | //! vehicle fault. Clearing this fault state requires additional tools. 10 | //! 11 | //! It is NOT recommended to modify any of these values without expert 12 | //! knowledge. 13 | 14 | #![allow(dead_code)] 15 | 16 | use ranges; 17 | use typenum::consts::*; 18 | type U1723 = op!{U1000 + U723}; 19 | type U3358 = op!{U1000 + U1000 + U1000 + U358}; 20 | type U3440 = op!{U1000 + U1000 + U1000 + U440}; 21 | type U3446 = op!{U1000 + U1000 + U1000 + U446}; 22 | 23 | // **************************************************************************** 24 | // OBD MESSAGES 25 | // **************************************************************************** 26 | 27 | /// ID of the Kia Niro's OBD steering wheel angle CAN frame. 28 | pub const KIA_SOUL_OBD_STEERING_WHEEL_ANGLE_CAN_ID: u16 = 0x2B0; 29 | 30 | /// ID of the Kia Niro's OBD wheel speed CAN frame. 31 | pub const KIA_SOUL_OBD_WHEEL_SPEED_CAN_ID: u16 = 0x386; 32 | 33 | /// ID of the Kia Niro's OBD brake pressure CAN frame. 34 | pub const KIA_SOUL_OBD_BRAKE_PRESSURE_CAN_ID: u16 = 0x220; 35 | 36 | /// ID of the Kia Niro's OBD speed CAN frame. 37 | pub const KIA_SOUL_OBD_SPEED_CAN_ID: u16 = 0x371; 38 | 39 | /// Factor to scale OBD steering angle to degrees 40 | pub const KIA_SOUL_OBD_STEERING_ANGLE_SCALAR: f32 = 0.1; 41 | 42 | // **************************************************************************** 43 | // VEHICLE AND BOARD PARAMETERS 44 | // **************************************************************************** 45 | 46 | /// Number of steps per volt corresponding to 4096 steps (2^12) across 47 | /// 5 volts. 48 | pub const STEPS_PER_VOLT: f32 = 819.2; 49 | 50 | /// Length of time in ms for delay of signal reads to ensure fault is 51 | /// outside the range of noise in the signal. 52 | pub const FAULT_HYSTERESIS: u32 = 150; 53 | 54 | // **************************************************************************** 55 | // BRAKE MODULE 56 | // **************************************************************************** 57 | 58 | /// Minimum allowable brake value. 59 | pub const MINIMUM_BRAKE_COMMAND: f32 = 0.0; 60 | 61 | /// Maximum allowable brake value. 62 | pub const MAXIMUM_BRAKE_COMMAND: f32 = 1.0; 63 | 64 | /// Minimum allowed voltage for the high spoof signal voltage. \[volts\] 65 | pub const BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN: f32 = 0.609; 66 | 67 | /// Maximum allowed voltage for the high spoof signal voltage. \[volts\] 68 | pub const BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX: f32 = 2.880; 69 | 70 | /// Minimum allowed voltage for the low spoof signal voltage. \[volts\] 71 | pub const BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN: f32 = 0.279; 72 | 73 | /// Maximum allowed voltage for the low spoof signal voltage. \[volts\] 74 | pub const BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX: f32 = 1.386; 75 | 76 | /// Minimum allowed value for the high spoof signal value. \[steps\] 77 | /// Equal to BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN * STEPS_PER_VOLT. 78 | pub const BRAKE_SPOOF_LOW_SIGNAL_RANGE_MIN: u16 = 499; 79 | 80 | /// Minimum allowed value for the high spoof signal value. \[steps\] 81 | /// Equal to BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX * STEPS_PER_VOLT. 82 | pub const BRAKE_SPOOF_LOW_SIGNAL_RANGE_MAX: u16 = 2359; 83 | 84 | /// Minimum allowed value for the low spoof signal value. \[steps\] 85 | /// Equal to BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN * STEPS_PER_VOLT. 86 | pub const BRAKE_SPOOF_HIGH_SIGNAL_RANGE_MIN: u16 = 229; 87 | 88 | /// Minimum allowed value for the low spoof signal value. \[steps\] 89 | /// Equal to BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX * STEPS_PER_VOLT. 90 | pub const BRAKE_SPOOF_HIGH_SIGNAL_RANGE_MAX: u16 = 1135; 91 | 92 | /// Calculation to convert a brake position to a low spoof voltage. 93 | pub const fn brake_position_to_volts_low(position: f32) -> f32 { 94 | position * (BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX - BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN) 95 | + BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN 96 | } 97 | 98 | /// Calculation to convert a brake position to a high spoof voltage. 99 | pub const fn brake_position_to_volts_high(position: f32) -> f32 { 100 | position * (BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX - BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN) 101 | + BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN 102 | } 103 | 104 | /// Value of the brake position that indicates operator 105 | /// override. \[steps\] 106 | pub const BRAKE_PEDAL_OVERRIDE_THRESHOLD: u16 = 200; 107 | 108 | /// Minimum value of the high spoof signal that activates the brake 109 | /// lights. \[steps\] 110 | pub const BRAKE_LIGHT_SPOOF_HIGH_THRESHOLD: u16 = 300; 111 | 112 | /// Minimum value of the low spoof signal that activates the brake 113 | /// lights. \[steps\] 114 | pub const BRAKE_LIGHT_SPOOF_LOW_THRESHOLD: u16 = 600; 115 | 116 | // **************************************************************************** 117 | // STEERING MODULE 118 | // **************************************************************************** 119 | 120 | /// Minimum allowable torque value. 121 | pub const MINIMUM_TORQUE_COMMAND: f32 = -12.8; 122 | 123 | /// Maximum allowable torque value. 124 | pub const MAXIMUM_TORQUE_COMMAND: f32 = 12.7; 125 | 126 | /// Minimum allowable steering DAC output. \[volts\] 127 | pub const STEERING_SPOOF_LOW_SIGNAL_VOLTAGE_MIN: f32 = 0.80; 128 | 129 | /// Maximum allowable steering DAC output. \[volts\] 130 | pub const STEERING_SPOOF_LOW_SIGNAL_VOLTAGE_MAX: f32 = 4.10; 131 | 132 | /// Minimum allowable steering DAC output. \[volts\] 133 | pub const STEERING_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN: f32 = 0.90; 134 | 135 | /// Maximum allowable steering DAC output. \[volts\] 136 | pub const STEERING_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX: f32 = 4.20; 137 | 138 | /// Minimum allowed value for the low spoof signal value. \[steps\] 139 | /// Equal to STEERING_SPOOF_LOW_SIGNAL_VOLTAGE_MIN * STEPS_PER_VOLT. 140 | pub const STEERING_SPOOF_LOW_SIGNAL_RANGE_MIN: u16 = 656; 141 | 142 | /// Minimum allowed value for the low spoof signal value. \[steps\] 143 | /// Equal to STEERING_SPOOF_LOW_SIGNAL_VOLTAGE_MAX * STEPS_PER_VOLT. 144 | pub const STEERING_SPOOF_LOW_SIGNAL_RANGE_MAX: u16 = 3358; 145 | 146 | pub type SteeringSpoofLowSignal = ranges::Bounded; 147 | 148 | /// Minimum allowed value for the low spoof signal value. \[steps\] 149 | /// Equal to STEERING_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN * STEPS_PER_VOLT. 150 | pub const STEERING_SPOOF_HIGH_SIGNAL_RANGE_MIN: u16 = 738; 151 | 152 | /// Minimum allowed value for the low spoof signal value. \[steps\] 153 | /// Equal to STEERING_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX * STEPS_PER_VOLT. 154 | pub const STEERING_SPOOF_HIGH_SIGNAL_RANGE_MAX: u16 = 3440; 155 | 156 | pub type SteeringSpoofHighSignal = ranges::Bounded; 157 | 158 | /// Scalar value for the low spoof signal taken from a calibration 159 | /// curve. 160 | pub const TORQUE_SPOOF_LOW_SIGNAL_CALIBRATION_CURVE_SCALE: f32 = 0.135; 161 | 162 | /// Offset value for the low spoof signal taken from a calibration 163 | /// curve. 164 | pub const TORQUE_SPOOF_LOW_SIGNAL_CALIBRATION_CURVE_OFFSET: f32 = 2.39; 165 | 166 | /// Scalar value for the high spoof signal taken from a calibration 167 | /// curve. 168 | pub const TORQUE_SPOOF_HIGH_SIGNAL_CALIBRATION_CURVE_SCALE: f32 = -0.145; 169 | 170 | /// Offset value for the high spoof signal taken from a calibration 171 | /// curve. 172 | pub const TORQUE_SPOOF_HIGH_SIGNAL_CALIBRATION_CURVE_OFFSET: f32 = 2.42; 173 | 174 | /// Minimum allowed value for the high spoof signal value. 175 | pub const fn steering_torque_to_volts_low(torque: f32) -> f32 { 176 | (TORQUE_SPOOF_HIGH_SIGNAL_CALIBRATION_CURVE_SCALE * torque) 177 | + TORQUE_SPOOF_HIGH_SIGNAL_CALIBRATION_CURVE_OFFSET 178 | } 179 | 180 | /// Calculation to convert a steering torque to a low spoof value. 181 | pub const fn steering_torque_to_volts_high(torque: f32) -> f32 { 182 | (TORQUE_SPOOF_LOW_SIGNAL_CALIBRATION_CURVE_SCALE * torque) 183 | + TORQUE_SPOOF_LOW_SIGNAL_CALIBRATION_CURVE_OFFSET 184 | } 185 | 186 | /// Value of torque sensor difference that indicates likely operator 187 | /// override. 188 | pub const TORQUE_DIFFERENCE_OVERRIDE_THRESHOLD: u16 = 1600; 189 | 190 | // **************************************************************************** 191 | // THROTTLE MODULE 192 | // **************************************************************************** 193 | 194 | /// Minimum allowable throttle value. 195 | pub const MINIMUM_THROTTLE_COMMAND: f32 = 0.0; 196 | 197 | /// Maximum allowable throttle value. 198 | pub const MAXIMUM_THROTTLE_COMMAND: f32 = 1.0; 199 | 200 | /// Minimum allowed voltage for the low spoof signal voltage. \[volts\] 201 | pub const THROTTLE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN: f32 = 0.380; 202 | 203 | /// Maximum allowed voltage for the low spoof signal voltage. \[volts\] 204 | pub const THROTTLE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX: f32 = 2.104; 205 | 206 | /// Minimum allowed voltage for the high spoof signal voltage. \[volts\] 207 | pub const THROTTLE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN: f32 = 0.757; 208 | 209 | /// Maximum allowed voltage for the high spoof signal voltage. \[volts\] 210 | pub const THROTTLE_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX: f32 = 4.207; 211 | 212 | /// Minimum allowed value for the low spoof signal value. \[steps\] 213 | /// Equal to THROTTLE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN * STEPS_PER_VOLT. 214 | pub const THROTTLE_SPOOF_LOW_SIGNAL_RANGE_MIN: u16 = 311; 215 | 216 | /// Minimum allowed value for the low spoof signal value. \[steps\] 217 | /// Equal to THROTTLE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX * STEPS_PER_VOLT. 218 | pub const THROTTLE_SPOOF_LOW_SIGNAL_RANGE_MAX: u16 = 1723; 219 | 220 | pub type ThrottleSpoofLowSignal = ranges::Bounded; 221 | 222 | /// Minimum allowed value for the low spoof signal value. \[steps\] 223 | /// Equal to THROTTLE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN * STEPS_PER_VOLT. 224 | pub const THROTTLE_SPOOF_HIGH_SIGNAL_RANGE_MIN: u16 = 620; 225 | 226 | /// Minimum allowed value for the low spoof signal value. \[steps\] 227 | /// Equal to THROTTLE_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX * STEPS_PER_VOLT. 228 | pub const THROTTLE_SPOOF_HIGH_SIGNAL_RANGE_MAX: u16 = 3446; 229 | 230 | pub type ThrottleSpoofHighSignal = ranges::Bounded; 231 | 232 | /// Calculation to convert a throttle position to a low spoof voltage. 233 | pub const fn throttle_position_to_volts_low(position: f32) -> f32 { 234 | position * (THROTTLE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX - THROTTLE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN) 235 | + THROTTLE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN 236 | } 237 | 238 | /// Calculation to convert a throttle position to a high spoof voltage. 239 | pub const fn throttle_position_to_volts_high(position: f32) -> f32 { 240 | position * (THROTTLE_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX - THROTTLE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN) 241 | + THROTTLE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN 242 | } 243 | 244 | /// Value of the accelerator position that indicates operator 245 | /// override. \[steps\] 246 | pub const ACCELERATOR_OVERRIDE_THRESHOLD: u32 = 185; 247 | -------------------------------------------------------------------------------- /src/brake/kia_soul_ev_niro/brake_module.rs: -------------------------------------------------------------------------------- 1 | //! Kia Soul EV/Niro brake module 2 | 3 | use super::types::*; 4 | use board::BrakePedalPositionSensor; 5 | use brake_can_protocol::*; 6 | use core::fmt::Write; 7 | use dac_mcp4922::DacOutput; 8 | use dtc::DtcBitfield; 9 | use dual_signal::DualSignal; 10 | use fault_can_protocol::*; 11 | use fault_condition::FaultCondition; 12 | use nucleo_f767zi::debug_console::DebugConsole; 13 | use nucleo_f767zi::hal::can::CanFrame; 14 | use nucleo_f767zi::hal::prelude::*; 15 | use num; 16 | use oscc_magic_byte::*; 17 | use oxcc_error::OxccError; 18 | use ranges; 19 | use vehicle::*; 20 | 21 | struct BrakeControlState { 22 | enabled: bool, 23 | operator_override: bool, 24 | dtcs: DTCS, 25 | } 26 | 27 | impl BrakeControlState 28 | where 29 | DTCS: DtcBitfield, 30 | { 31 | pub const fn new(dtcs: DTCS) -> Self { 32 | BrakeControlState { 33 | enabled: false, 34 | operator_override: false, 35 | dtcs, 36 | } 37 | } 38 | } 39 | 40 | /// Brake module 41 | pub struct BrakeModule { 42 | brake_pedal_position: DualSignal, 43 | control_state: BrakeControlState, 44 | grounded_fault_state: FaultCondition, 45 | operator_override_state: FaultCondition, 46 | brake_report: OsccBrakeReport, 47 | fault_report: OsccFaultReport, 48 | brake_dac: BrakeDac, 49 | brake_pins: BrakePins, 50 | } 51 | 52 | pub struct UnpreparedBrakeModule { 53 | brake_module: BrakeModule, 54 | } 55 | 56 | impl UnpreparedBrakeModule { 57 | pub fn new( 58 | brake_dac: BrakeDac, 59 | brake_pins: BrakePins, 60 | brake_pedal_position_sensor: BrakePedalPositionSensor, 61 | grounded_fault_timer: BrakeGroundedFaultTimer, 62 | override_timer: BrakeOverrideFaultTimer, 63 | ) -> Self { 64 | UnpreparedBrakeModule { 65 | brake_module: BrakeModule { 66 | brake_pedal_position: DualSignal::new(0, 0, brake_pedal_position_sensor), 67 | control_state: BrakeControlState::new(u8::default()), 68 | grounded_fault_state: FaultCondition::new(grounded_fault_timer), 69 | operator_override_state: FaultCondition::new(override_timer), 70 | brake_report: OsccBrakeReport::new(), 71 | fault_report: OsccFaultReport { 72 | fault_origin_id: FAULT_ORIGIN_BRAKE, 73 | dtcs: 0, 74 | }, 75 | brake_dac, 76 | brake_pins, 77 | }, 78 | } 79 | } 80 | 81 | pub fn prepare_module(self) -> BrakeModule { 82 | let mut brake_module = self.brake_module; 83 | brake_module.brake_pins.spoof_enable.set_low(); 84 | brake_module.brake_pins.brake_light_enable.set_low(); 85 | brake_module 86 | } 87 | } 88 | 89 | impl BrakeModule { 90 | pub fn disable_control(&mut self, debug_console: &mut DebugConsole) -> Result<(), OxccError> { 91 | if self.control_state.enabled { 92 | self.brake_pedal_position.prevent_signal_discontinuity(); 93 | 94 | let result = self.brake_dac.output_ab( 95 | DacOutput::clamp(self.brake_pedal_position.low()), 96 | DacOutput::clamp(self.brake_pedal_position.high()), 97 | ); 98 | 99 | // even if we've encountered an error, we can still disable 100 | self.brake_pins.spoof_enable.set_low(); 101 | self.brake_pins.brake_light_enable.set_low(); 102 | self.control_state.enabled = false; 103 | writeln!(debug_console, "Brake control disabled"); 104 | 105 | return if let Err(e) = result { 106 | Err(OxccError::from(e)) 107 | } else { 108 | Ok(()) 109 | }; 110 | } 111 | 112 | Ok(()) 113 | } 114 | 115 | fn enable_control(&mut self, debug_console: &mut DebugConsole) -> Result<(), OxccError> { 116 | if !self.control_state.enabled && !self.control_state.operator_override { 117 | self.brake_pedal_position.prevent_signal_discontinuity(); 118 | 119 | let result = self.brake_dac.output_ab( 120 | DacOutput::clamp(self.brake_pedal_position.low()), 121 | DacOutput::clamp(self.brake_pedal_position.high()), 122 | ); 123 | 124 | return if let Err(e) = result { 125 | Err(OxccError::from(e)) 126 | } else { 127 | self.brake_pins.spoof_enable.set_high(); 128 | self.control_state.enabled = true; 129 | writeln!(debug_console, "Brake control enabled"); 130 | Ok(()) 131 | }; 132 | } 133 | 134 | Ok(()) 135 | } 136 | 137 | fn update_brake( 138 | &mut self, 139 | spoof_command_high: u16, 140 | spoof_command_low: u16, 141 | ) -> Result<(), OxccError> { 142 | if self.control_state.enabled { 143 | let spoof_high = BrakeSpoofHighSignal::clamp(spoof_command_high); 144 | let spoof_low = BrakeSpoofLowSignal::clamp(spoof_command_low); 145 | 146 | if (spoof_high.val() > &BRAKE_LIGHT_SPOOF_HIGH_THRESHOLD) 147 | || (spoof_low.val() > &BRAKE_LIGHT_SPOOF_LOW_THRESHOLD) 148 | { 149 | self.brake_pins.brake_light_enable.set_high(); 150 | } else { 151 | self.brake_pins.brake_light_enable.set_low(); 152 | } 153 | 154 | // TODO - revisit this, enforce high->A, low->B 155 | self.brake_dac 156 | .output_ab(ranges::coerce(spoof_high), ranges::coerce(spoof_low))?; 157 | } 158 | 159 | Ok(()) 160 | } 161 | 162 | pub fn check_for_faults( 163 | &mut self, 164 | debug_console: &mut DebugConsole, 165 | ) -> Result, OxccError> { 166 | if !self.control_state.enabled && !self.control_state.dtcs.are_any_set() { 167 | // Assumes this module already went through the proper transition into a faulted 168 | // and disabled state, and we do not want to double-report a possible duplicate 169 | // fault. 170 | return Ok(None); 171 | } 172 | 173 | self.brake_pedal_position.update(); 174 | 175 | let brake_pedal_position_average = self.brake_pedal_position.average(); 176 | 177 | let operator_overridden: bool = self.operator_override_state.condition_exceeded_duration( 178 | brake_pedal_position_average >= BRAKE_PEDAL_OVERRIDE_THRESHOLD.into(), 179 | ); 180 | 181 | let inputs_grounded: bool = self 182 | .grounded_fault_state 183 | .check_voltage_grounded(&self.brake_pedal_position); 184 | 185 | // sensor pins tied to ground - a value of zero indicates disconnection 186 | if inputs_grounded { 187 | self.disable_control(debug_console)?; 188 | 189 | self.control_state 190 | .dtcs 191 | .set(OSCC_BRAKE_DTC_INVALID_SENSOR_VAL); 192 | 193 | self.update_fault_report(); 194 | 195 | writeln!( 196 | debug_console, 197 | "Bad value read from brake pedal position sensor" 198 | ); 199 | 200 | Ok(Some(&self.fault_report)) 201 | } else if operator_overridden && !self.control_state.operator_override { 202 | self.disable_control(debug_console)?; 203 | 204 | self.control_state 205 | .dtcs 206 | .set(OSCC_BRAKE_DTC_OPERATOR_OVERRIDE); 207 | 208 | self.update_fault_report(); 209 | 210 | self.control_state.operator_override = true; 211 | 212 | writeln!(debug_console, "Brake operator override"); 213 | 214 | Ok(Some(&self.fault_report)) 215 | } else { 216 | self.control_state.dtcs.clear_all(); 217 | self.control_state.operator_override = false; 218 | Ok(None) 219 | } 220 | } 221 | 222 | fn update_fault_report(&mut self) { 223 | self.fault_report.dtcs = self.control_state.dtcs; 224 | } 225 | 226 | pub fn supply_brake_report(&mut self) -> &OsccBrakeReport { 227 | self.brake_report.enabled = self.control_state.enabled; 228 | self.brake_report.operator_override = self.control_state.operator_override; 229 | self.brake_report.dtcs = self.control_state.dtcs; 230 | &self.brake_report 231 | } 232 | 233 | pub fn process_rx_frame( 234 | &mut self, 235 | can_frame: &CanFrame, 236 | debug_console: &mut DebugConsole, 237 | ) -> Result<(), OxccError> { 238 | if let CanFrame::DataFrame(ref frame) = can_frame { 239 | let id: u32 = frame.id().into(); 240 | let data = frame.data(); 241 | 242 | if (data[0] == OSCC_MAGIC_BYTE_0) && (data[1] == OSCC_MAGIC_BYTE_1) { 243 | if id == OSCC_BRAKE_ENABLE_CAN_ID.into() { 244 | self.enable_control(debug_console)?; 245 | } else if id == OSCC_BRAKE_DISABLE_CAN_ID.into() { 246 | self.disable_control(debug_console)?; 247 | } else if id == OSCC_BRAKE_COMMAND_CAN_ID.into() { 248 | self.process_brake_command(&OsccBrakeCommand::from(frame))?; 249 | } else if id == OSCC_FAULT_REPORT_CAN_ID.into() { 250 | self.process_fault_report(&OsccFaultReport::from(frame), debug_console)?; 251 | } 252 | } 253 | } 254 | 255 | Ok(()) 256 | } 257 | 258 | fn process_fault_report( 259 | &mut self, 260 | fault_report: &OsccFaultReport, 261 | debug_console: &mut DebugConsole, 262 | ) -> Result<(), OxccError> { 263 | writeln!( 264 | debug_console, 265 | "Fault report received from: {} DTCs: {}", 266 | fault_report.fault_origin_id, fault_report.dtcs 267 | ); 268 | 269 | self.disable_control(debug_console) 270 | } 271 | 272 | fn process_brake_command(&mut self, command: &OsccBrakeCommand) -> Result<(), OxccError> { 273 | let clamped_position = num::clamp( 274 | command.pedal_command, 275 | MINIMUM_BRAKE_COMMAND, 276 | MAXIMUM_BRAKE_COMMAND, 277 | ); 278 | 279 | let spoof_voltage_low: f32 = num::clamp( 280 | brake_position_to_volts_low(clamped_position), 281 | BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN, 282 | BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX, 283 | ); 284 | 285 | let spoof_voltage_high: f32 = num::clamp( 286 | brake_position_to_volts_high(clamped_position), 287 | BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN, 288 | BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX, 289 | ); 290 | 291 | let spoof_value_low = (STEPS_PER_VOLT * spoof_voltage_low) as u16; 292 | let spoof_value_high = (STEPS_PER_VOLT * spoof_voltage_high) as u16; 293 | 294 | self.update_brake(spoof_value_high, spoof_value_low) 295 | } 296 | } 297 | 298 | trait HighLowReader { 299 | fn read_high(&self) -> u16; 300 | fn read_low(&self) -> u16; 301 | } 302 | -------------------------------------------------------------------------------- /src/vehicles/kial_soul_ev.rs: -------------------------------------------------------------------------------- 1 | //! Kia Soul EV vehicle configuration data 2 | //! 3 | //! **WARNING** 4 | //! 5 | //! The values listed here are carefully tested to ensure that the vehicle's 6 | //! components are not actuated outside of the range of what they can handle. 7 | //! By changing any of these values you risk attempting to actuate outside of 8 | //! the vehicle's valid range. This can cause damage to the hardware and/or a 9 | //! vehicle fault. Clearing this fault state requires additional tools. 10 | //! 11 | //! It is NOT recommended to modify any of these values without expert 12 | //! knowledge. 13 | 14 | #![allow(dead_code)] 15 | 16 | use ranges; 17 | use typenum::consts::*; 18 | type U1638 = op!{U1000 + U638}; 19 | type U1875 = op!{U1000 + U875}; 20 | type U3358 = op!{U1000 + U1000 + U1000 + U358}; 21 | type U3440 = op!{U1000 + U1000 + U1000 + U440}; 22 | 23 | // **************************************************************************** 24 | // OBD MESSAGES 25 | // **************************************************************************** 26 | 27 | /// ID of the Kia Soul's OBD steering wheel angle CAN frame. 28 | pub const KIA_SOUL_OBD_STEERING_WHEEL_ANGLE_CAN_ID: u16 = 0x2B0; 29 | 30 | /// ID of the Kia Soul's OBD wheel speed CAN frame. 31 | pub const KIA_SOUL_OBD_WHEEL_SPEED_CAN_ID: u16 = 0x4B0; 32 | 33 | /// ID of the Kia Soul's OBD brake pressure CAN frame. 34 | pub const KIA_SOUL_OBD_BRAKE_PRESSURE_CAN_ID: u16 = 0x220; 35 | 36 | /// ID of the Kia Soul's OBD throttle pressure CAN frame. 37 | pub const KIA_SOUL_OBD_THROTTLE_PRESSURE_CAN_ID: u16 = 0x200; 38 | 39 | /// Factor to scale OBD steering angle to degrees 40 | pub const KIA_SOUL_OBD_STEERING_ANGLE_SCALAR: f32 = 0.1; 41 | 42 | // **************************************************************************** 43 | // VEHICLE AND BOARD PARAMETERS 44 | // **************************************************************************** 45 | 46 | /// Number of steps per volt corresponding to 4096 steps (2^12) across 47 | /// 5 volts. 48 | pub const STEPS_PER_VOLT: f32 = 819.2; 49 | 50 | /// Length of time in ms for delay of signal reads to ensure fault is 51 | /// outside the range of noise in the signal. 52 | pub const FAULT_HYSTERESIS: u32 = 100; 53 | 54 | // **************************************************************************** 55 | // BRAKE MODULE 56 | // **************************************************************************** 57 | 58 | /// Minimum allowable brake value. 59 | pub const MINIMUM_BRAKE_COMMAND: f32 = 0.0; 60 | 61 | /// Maximum allowable brake value. 62 | pub const MAXIMUM_BRAKE_COMMAND: f32 = 1.0; 63 | 64 | /// Minimum allowed voltage for the low spoof signal voltage. \[volts\] 65 | pub const BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN: f32 = 0.333; 66 | 67 | /// Maximum allowed voltage for the low spoof signal voltage. \[volts\] 68 | pub const BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX: f32 = 1.12; 69 | 70 | /// Minimum allowed voltage for the high spoof signal voltage. \[volts\] 71 | pub const BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN: f32 = 0.698; 72 | 73 | /// Maximum allowed voltage for the high spoof signal voltage. \[volts\] 74 | pub const BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX: f32 = 2.29; 75 | 76 | /// Minimum allowed value for the low spoof signal value. \[steps\] 77 | /// Equal to BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN * STEPS_PER_VOLT. 78 | pub const BRAKE_SPOOF_LOW_SIGNAL_RANGE_MIN: u16 = 273; 79 | 80 | /// Minimum allowed value for the low spoof signal value. \[steps\] 81 | /// Equal to BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX * STEPS_PER_VOLT. 82 | pub const BRAKE_SPOOF_LOW_SIGNAL_RANGE_MAX: u16 = 917; 83 | 84 | pub type BrakeSpoofLowSignal = ranges::Bounded; 85 | 86 | /// Minimum allowed value for the low spoof signal value. \[steps\] 87 | /// Equal to BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN * STEPS_PER_VOLT. 88 | pub const BRAKE_SPOOF_HIGH_SIGNAL_RANGE_MIN: u16 = 572; 89 | 90 | /// Minimum allowed value for the low spoof signal value. \[steps\] 91 | /// Equal to BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX * STEPS_PER_VOLT. 92 | pub const BRAKE_SPOOF_HIGH_SIGNAL_RANGE_MAX: u16 = 1875; 93 | 94 | pub type BrakeSpoofHighSignal = ranges::Bounded; 95 | 96 | /// Calculation to convert a brake position to a low spoof voltage. 97 | pub const fn brake_position_to_volts_low(position: f32) -> f32 { 98 | position * (BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX - BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN) 99 | + BRAKE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN 100 | } 101 | 102 | /// Calculation to convert a brake position to a high spoof voltage. 103 | pub const fn brake_position_to_volts_high(position: f32) -> f32 { 104 | position * (BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX - BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN) 105 | + BRAKE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN 106 | } 107 | 108 | /// Value of the brake position that indicates operator 109 | /// override. \[steps\] 110 | pub const BRAKE_PEDAL_OVERRIDE_THRESHOLD: u16 = 130; 111 | 112 | /// Minimum value of the low spoof signal that activates the brake 113 | /// lights. \[steps\] 114 | pub const BRAKE_LIGHT_SPOOF_LOW_THRESHOLD: u16 = 300; 115 | 116 | /// Minimum value of the high spoof signal that activates the brake 117 | /// lights. \[steps\] 118 | pub const BRAKE_LIGHT_SPOOF_HIGH_THRESHOLD: u16 = 600; 119 | 120 | // **************************************************************************** 121 | // STEERING MODULE 122 | // **************************************************************************** 123 | 124 | /// Minimum allowable torque value. 125 | pub const MINIMUM_TORQUE_COMMAND: f32 = -12.8; 126 | 127 | /// Maximum allowable torque value. 128 | pub const MAXIMUM_TORQUE_COMMAND: f32 = 12.7; 129 | 130 | /// Minimum allowable steering DAC output. \[volts\] 131 | pub const STEERING_SPOOF_LOW_SIGNAL_VOLTAGE_MIN: f32 = 0.80; 132 | 133 | /// Maximum allowable steering DAC output. \[volts\] 134 | pub const STEERING_SPOOF_LOW_SIGNAL_VOLTAGE_MAX: f32 = 4.10; 135 | 136 | /// Minimum allowable steering DAC output. \[volts\] 137 | pub const STEERING_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN: f32 = 0.90; 138 | 139 | /// Maximum allowable steering DAC output. \[volts\] 140 | pub const STEERING_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX: f32 = 4.20; 141 | 142 | /// Minimum allowed value for the low spoof signal value. \[steps\] 143 | /// Equal to STEERING_SPOOF_LOW_SIGNAL_VOLTAGE_MIN * STEPS_PER_VOLT. 144 | pub const STEERING_SPOOF_LOW_SIGNAL_RANGE_MIN: u16 = 656; 145 | 146 | /// Minimum allowed value for the low spoof signal value. \[steps\] 147 | /// Equal to STEERING_SPOOF_LOW_SIGNAL_VOLTAGE_MAX * STEPS_PER_VOLT. 148 | pub const STEERING_SPOOF_LOW_SIGNAL_RANGE_MAX: u16 = 3358; 149 | 150 | pub type SteeringSpoofLowSignal = ranges::Bounded; 151 | 152 | /// Minimum allowed value for the low spoof signal value. \[steps\] 153 | /// Equal to STEERING_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN * STEPS_PER_VOLT. 154 | pub const STEERING_SPOOF_HIGH_SIGNAL_RANGE_MIN: u16 = 738; 155 | 156 | /// Minimum allowed value for the low spoof signal value. \[steps\] 157 | /// Equal to STEERING_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX * STEPS_PER_VOLT. 158 | pub const STEERING_SPOOF_HIGH_SIGNAL_RANGE_MAX: u16 = 3440; 159 | 160 | pub type SteeringSpoofHighSignal = ranges::Bounded; 161 | 162 | /// Scalar value for the low spoof signal taken from a calibration 163 | /// curve. 164 | pub const TORQUE_SPOOF_LOW_SIGNAL_CALIBRATION_CURVE_SCALE: f32 = 0.135; 165 | 166 | /// Offset value for the low spoof signal taken from a calibration 167 | /// curve. 168 | pub const TORQUE_SPOOF_LOW_SIGNAL_CALIBRATION_CURVE_OFFSET: f32 = 2.39; 169 | 170 | /// Scalar value for the high spoof signal taken from a calibration 171 | /// curve. 172 | pub const TORQUE_SPOOF_HIGH_SIGNAL_CALIBRATION_CURVE_SCALE: f32 = -0.145; 173 | 174 | /// Offset value for the high spoof signal taken from a calibration 175 | /// curve. 176 | pub const TORQUE_SPOOF_HIGH_SIGNAL_CALIBRATION_CURVE_OFFSET: f32 = 2.42; 177 | 178 | /// Minimum allowed value for the high spoof signal value. 179 | pub const fn steering_torque_to_volts_low(torque: f32) -> f32 { 180 | (TORQUE_SPOOF_HIGH_SIGNAL_CALIBRATION_CURVE_SCALE * torque) 181 | + TORQUE_SPOOF_HIGH_SIGNAL_CALIBRATION_CURVE_OFFSET 182 | } 183 | 184 | /// Calculation to convert a steering torque to a low spoof value. 185 | pub const fn steering_torque_to_volts_high(torque: f32) -> f32 { 186 | (TORQUE_SPOOF_LOW_SIGNAL_CALIBRATION_CURVE_SCALE * torque) 187 | + TORQUE_SPOOF_LOW_SIGNAL_CALIBRATION_CURVE_OFFSET 188 | } 189 | 190 | /// Value of torque sensor difference that indicates likely operator 191 | /// override. 192 | pub const TORQUE_DIFFERENCE_OVERRIDE_THRESHOLD: u16 = 1600; 193 | 194 | // **************************************************************************** 195 | // THROTTLE MODULE 196 | // **************************************************************************** 197 | 198 | /// Minimum allowable throttle value. 199 | pub const MINIMUM_THROTTLE_COMMAND: f32 = 0.0; 200 | 201 | /// Maximum allowable throttle value. 202 | pub const MAXIMUM_THROTTLE_COMMAND: f32 = 1.0; 203 | 204 | /// Minimum allowed voltage for the low spoof signal voltage. \[volts\] 205 | pub const THROTTLE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN: f32 = 0.30; 206 | 207 | /// Maximum allowed voltage for the low spoof signal voltage. \[volts\] 208 | pub const THROTTLE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX: f32 = 2.00; 209 | 210 | /// Minimum allowed voltage for the high spoof signal voltage. \[volts\] 211 | pub const THROTTLE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN: f32 = 0.70; 212 | 213 | /// Maximum allowed voltage for the high spoof signal voltage. \[volts\] 214 | pub const THROTTLE_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX: f32 = 4.10; 215 | 216 | /// Minimum allowed value for the low spoof signal value. \[steps\] 217 | /// Equal to THROTTLE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN * STEPS_PER_VOLT. 218 | pub const THROTTLE_SPOOF_LOW_SIGNAL_RANGE_MIN: u16 = 245; 219 | 220 | /// Minimum allowed value for the low spoof signal value. \[steps\] 221 | /// Equal to THROTTLE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX * STEPS_PER_VOLT. 222 | pub const THROTTLE_SPOOF_LOW_SIGNAL_RANGE_MAX: u16 = 1638; 223 | 224 | pub type ThrottleSpoofLowSignal = ranges::Bounded; 225 | 226 | /// Minimum allowed value for the low spoof signal value. \[steps\] 227 | /// Equal to THROTTLE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN * STEPS_PER_VOLT. 228 | pub const THROTTLE_SPOOF_HIGH_SIGNAL_RANGE_MIN: u16 = 573; 229 | 230 | /// Minimum allowed value for the low spoof signal value. \[steps\] 231 | /// Equal to THROTTLE_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX * STEPS_PER_VOLT. 232 | pub const THROTTLE_SPOOF_HIGH_SIGNAL_RANGE_MAX: u16 = 3358; 233 | 234 | pub type ThrottleSpoofHighSignal = ranges::Bounded; 235 | /// Calculation to convert a throttle position to a low spoof voltage. 236 | pub const fn throttle_position_to_volts_low(position: f32) -> f32 { 237 | position * (THROTTLE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX - THROTTLE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN) 238 | + THROTTLE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN 239 | } 240 | 241 | /// Calculation to convert a throttle position to a high spoof voltage. 242 | pub const fn throttle_position_to_volts_high(position: f32) -> f32 { 243 | position * (THROTTLE_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX - THROTTLE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN) 244 | + THROTTLE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN 245 | } 246 | 247 | /// Value of the accelerator position that indicates operator 248 | /// override. \[steps\] 249 | pub const ACCELERATOR_OVERRIDE_THRESHOLD: u32 = 185; 250 | -------------------------------------------------------------------------------- /LICENSE-APACHE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /src/vehicles/kial_soul_petrol.rs: -------------------------------------------------------------------------------- 1 | //! Kia Soul Petrol vehicle configuration data 2 | //! 3 | //! **WARNING** 4 | //! 5 | //! The values listed here are carefully tested to ensure that the vehicle's 6 | //! components are not actuated outside of the range of what they can handle. 7 | //! By changing any of these values you risk attempting to actuate outside of 8 | //! the vehicle's valid range. This can cause damage to the hardware and/or a 9 | //! vehicle fault. Clearing this fault state requires additional tools. 10 | //! 11 | //! It is NOT recommended to modify any of these values without expert 12 | //! knowledge. 13 | 14 | #![allow(dead_code)] 15 | 16 | use ranges; 17 | use typenum::consts::*; 18 | type U1638 = op!{U1000 + U638}; 19 | type U1875 = op!{U1000 + U875}; 20 | type U3358 = op!{U1000 + U1000 + U1000 + U358}; 21 | type U3440 = op!{U1000 + U1000 + U1000 + U440}; 22 | 23 | // **************************************************************************** 24 | // OBD MESSAGES 25 | // **************************************************************************** 26 | 27 | /// ID of the Kia Soul's OBD steering wheel angle CAN frame. 28 | pub const KIA_SOUL_OBD_STEERING_WHEEL_ANGLE_CAN_ID: u16 = 0x2B0; 29 | 30 | /// ID of the Kia Soul's OBD wheel speed CAN frame. 31 | pub const KIA_SOUL_OBD_WHEEL_SPEED_CAN_ID: u16 = 0x4B0; 32 | 33 | /// ID of the Kia Soul's OBD brake pressure CAN frame. 34 | pub const KIA_SOUL_OBD_BRAKE_PRESSURE_CAN_ID: u16 = 0x220; 35 | 36 | /// Factor to scale OBD steering angle to degrees 37 | pub const KIA_SOUL_OBD_STEERING_ANGLE_SCALAR: f32 = 0.1; 38 | 39 | // **************************************************************************** 40 | // VEHICLE AND BOARD PARAMETERS 41 | // **************************************************************************** 42 | 43 | /// Number of steps per volt corresponding to 4096 steps (2^12) across 44 | /// 5 volts. 45 | pub const STEPS_PER_VOLT: f32 = 819.2; 46 | 47 | /// Length of time in ms for delay of signal reads to ensure fault is 48 | /// outside the range of noise in the signal. 49 | pub const FAULT_HYSTERESIS: u32 = 100; 50 | 51 | // **************************************************************************** 52 | // BRAKE MODULE 53 | // **************************************************************************** 54 | 55 | /// Minimum allowable brake value. 56 | pub const MINIMUM_BRAKE_COMMAND: f32 = 0.0; 57 | 58 | /// Maximum allowable brake value. 59 | pub const MAXIMUM_BRAKE_COMMAND: f32 = 1.0; 60 | 61 | /// Calculation to convert a brake position to a pedal position. 62 | pub const fn brake_position_to_pedal(position: f32) -> f32 { 63 | position 64 | } 65 | 66 | /// Calculation to convert a brake pressure to a pedal position. 67 | pub const fn brake_pressure_to_pedal(pressure: f32) -> f32 { 68 | pressure 69 | } 70 | 71 | /// Minimum accumulator presure. \[decibars\] 72 | pub const BRAKE_ACCUMULATOR_PRESSURE_MIN_IN_DECIBARS: f32 = 777.6; 73 | 74 | /// Maximum accumulator pressure. \[decibars\] 75 | pub const BRAKE_ACCUMULATOR_PRESSURE_MAX_IN_DECIBARS: f32 = 878.0; 76 | 77 | /// Value of brake pressure that indicates operator override. 78 | /// \[decibars\] 79 | pub const BRAKE_OVERRIDE_PEDAL_THRESHOLD_IN_DECIBARS: f32 = 43.2; 80 | 81 | /// Brake pressure threshold for when to enable the brake light. 82 | pub const BRAKE_LIGHT_PRESSURE_THRESHOLD_IN_DECIBARS: f32 = 20.0; 83 | 84 | /// Minimum possible pressure of brake system. \[decibars\] 85 | pub const BRAKE_PRESSURE_MIN_IN_DECIBARS: f32 = 12.0; 86 | 87 | /// Maximum possible pressure of brake system. \[decibars\] 88 | pub const BRAKE_PRESSURE_MAX_IN_DECIBARS: f32 = 878.3; 89 | 90 | /// Minimum possible value expected to be read from the brake pressure 91 | /// sensors when the pressure check pins (PCK1/PCK2) are asserted. 92 | pub const BRAKE_PRESSURE_SENSOR_CHECK_VALUE_MIN: u16 = 665; 93 | 94 | /// Maximum possible value expected to be read from the brake pressure 95 | /// sensors when the pressure check pins (PCK1/PCK2) are asserted. 96 | pub const BRAKE_PRESSURE_SENSOR_CHECK_VALUE_MAX: u16 = 680; 97 | 98 | /// Proportional gain of the PID controller. 99 | pub const BRAKE_PID_PROPORTIONAL_GAIN: f32 = 0.65; 100 | 101 | /// Integral gain of the PID controller. 102 | pub const BRAKE_PID_INTEGRAL_GAIN: f32 = 1.75; 103 | 104 | /// Derivative gain of the PID controller. 105 | pub const BRAKE_PID_DERIVATIVE_GAIN: f32 = 0.000; 106 | 107 | /// Windup guard of the PID controller. 108 | pub const BRAKE_PID_WINDUP_GUARD: f32 = 30.0; 109 | 110 | /// Minimum output value of PID to be within a valid pressure range. 111 | pub const BRAKE_PID_OUTPUT_MIN: f32 = -10.0; 112 | 113 | /// Maximum output value of PID to be within a valid pressure range. 114 | pub const BRAKE_PID_OUTPUT_MAX: f32 = 10.0; 115 | 116 | /// Minimum clamped PID value of the actuation solenoid. 117 | pub const BRAKE_PID_ACCUMULATOR_SOLENOID_CLAMPED_MIN: f32 = 10.0; 118 | 119 | /// Maximum clamped PID value of the actuation solenoid. 120 | pub const BRAKE_PID_ACCUMULATOR_SOLENOID_CLAMPED_MAX: f32 = 110.0; 121 | 122 | /// Minimum clamped PID value of the release solenoid. 123 | pub const BRAKE_PID_RELEASE_SOLENOID_CLAMPED_MIN: f32 = 0.0; 124 | 125 | /// Maximum clamped PID value of the release solenoid. 126 | pub const BRAKE_PID_RELEASE_SOLENOID_CLAMPED_MAX: f32 = 60.0; 127 | 128 | /// Minimum duty cycle that begins to actuate the actuation solenoid. 129 | /// 3.921 KHz PWM frequency 130 | pub const BRAKE_ACCUMULATOR_SOLENOID_DUTY_CYCLE_MIN: f32 = 80.0; 131 | 132 | /// Maximum duty cycle where actuation solenoid has reached its stop. 133 | /// 3.921 KHz PWM frequency 134 | pub const BRAKE_ACCUMULATOR_SOLENOID_DUTY_CYCLE_MAX: f32 = 105.0; 135 | 136 | /// Minimum duty cycle that begins to actuate the release solenoid. 137 | /// 3.921 KHz PWM frequency 138 | pub const BRAKE_RELEASE_SOLENOID_DUTY_CYCLE_MIN: f32 = 65.0; 139 | 140 | /// Maximum duty cycle where release solenoid has reached its stop. 141 | /// 3.921 KHz PWM frequency 142 | pub const BRAKE_RELEASE_SOLENOID_DUTY_CYCLE_MAX: f32 = 100.0; 143 | 144 | // **************************************************************************** 145 | // STEERING MODULE 146 | // **************************************************************************** 147 | 148 | /// Minimum allowable torque value. 149 | pub const MINIMUM_TORQUE_COMMAND: f32 = -12.8; 150 | 151 | /// Maximum allowable torque value. 152 | pub const MAXIMUM_TORQUE_COMMAND: f32 = 12.7; 153 | 154 | /// Minimum allowable steering DAC output. \[volts\] 155 | pub const STEERING_SPOOF_LOW_SIGNAL_VOLTAGE_MIN: f32 = 0.80; 156 | 157 | /// Maximum allowable steering DAC output. \[volts\] 158 | pub const STEERING_SPOOF_LOW_SIGNAL_VOLTAGE_MAX: f32 = 4.10; 159 | 160 | /// Minimum allowable steering DAC output. \[volts\] 161 | pub const STEERING_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN: f32 = 0.90; 162 | 163 | /// Maximum allowable steering DAC output. \[volts\] 164 | pub const STEERING_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX: f32 = 4.20; 165 | 166 | /// Minimum allowed value for the low spoof signal value. \[steps\] 167 | /// Equal to STEERING_SPOOF_LOW_SIGNAL_VOLTAGE_MIN * STEPS_PER_VOLT. 168 | pub const STEERING_SPOOF_LOW_SIGNAL_RANGE_MIN: u16 = 656; 169 | 170 | /// Minimum allowed value for the low spoof signal value. \[steps\] 171 | /// Equal to STEERING_SPOOF_LOW_SIGNAL_VOLTAGE_MAX * STEPS_PER_VOLT. 172 | pub const STEERING_SPOOF_LOW_SIGNAL_RANGE_MAX: u16 = 3358; 173 | 174 | pub type SteeringSpoofLowSignal = ranges::Bounded; 175 | 176 | /// Minimum allowed value for the low spoof signal value. \[steps\] 177 | /// Equal to STEERING_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN * STEPS_PER_VOLT. 178 | pub const STEERING_SPOOF_HIGH_SIGNAL_RANGE_MIN: u16 = 738; 179 | 180 | /// Minimum allowed value for the low spoof signal value. \[steps\] 181 | /// Equal to STEERING_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX * STEPS_PER_VOLT. 182 | pub const STEERING_SPOOF_HIGH_SIGNAL_RANGE_MAX: u16 = 3440; 183 | 184 | pub type SteeringSpoofHighSignal = ranges::Bounded; 185 | 186 | /// Scalar value for the low spoof signal taken from a calibration 187 | /// curve. 188 | pub const TORQUE_SPOOF_LOW_SIGNAL_CALIBRATION_CURVE_SCALE: f32 = 0.135; 189 | 190 | /// Offset value for the low spoof signal taken from a calibration 191 | /// curve. 192 | pub const TORQUE_SPOOF_LOW_SIGNAL_CALIBRATION_CURVE_OFFSET: f32 = 2.39; 193 | 194 | /// Scalar value for the high spoof signal taken from a calibration 195 | /// curve. 196 | pub const TORQUE_SPOOF_HIGH_SIGNAL_CALIBRATION_CURVE_SCALE: f32 = -0.145; 197 | 198 | /// Offset value for the high spoof signal taken from a calibration 199 | /// curve. 200 | pub const TORQUE_SPOOF_HIGH_SIGNAL_CALIBRATION_CURVE_OFFSET: f32 = 2.42; 201 | 202 | /// Minimum allowed value for the high spoof signal value. 203 | pub const fn steering_torque_to_volts_high(torque: f32) -> f32 { 204 | (TORQUE_SPOOF_HIGH_SIGNAL_CALIBRATION_CURVE_SCALE * torque) 205 | + TORQUE_SPOOF_HIGH_SIGNAL_CALIBRATION_CURVE_OFFSET 206 | } 207 | 208 | /// Calculation to convert a steering torque to a low spoof value. 209 | pub const fn steering_torque_to_volts_low(torque: f32) -> f32 { 210 | (TORQUE_SPOOF_LOW_SIGNAL_CALIBRATION_CURVE_SCALE * torque) 211 | + TORQUE_SPOOF_LOW_SIGNAL_CALIBRATION_CURVE_OFFSET 212 | } 213 | 214 | /// Value of torque sensor difference that indicates likely operator 215 | /// override. 216 | pub const TORQUE_DIFFERENCE_OVERRIDE_THRESHOLD: u16 = 1600; 217 | 218 | // **************************************************************************** 219 | // THROTTLE MODULE 220 | // **************************************************************************** 221 | 222 | /// Minimum allowable throttle value. 223 | pub const MINIMUM_THROTTLE_COMMAND: f32 = 0.0; 224 | 225 | /// Maximum allowable throttle value. 226 | pub const MAXIMUM_THROTTLE_COMMAND: f32 = 1.0; 227 | 228 | /// Minimum allowed voltage for the low spoof signal voltage. \[volts\] 229 | pub const THROTTLE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN: f32 = 0.30; 230 | 231 | /// Maximum allowed voltage for the low spoof signal voltage. \[volts\] 232 | pub const THROTTLE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX: f32 = 2.00; 233 | 234 | /// Minimum allowed voltage for the high spoof signal voltage. \[volts\] 235 | pub const THROTTLE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN: f32 = 0.70; 236 | 237 | /// Maximum allowed voltage for the high spoof signal voltage. \[volts\] 238 | pub const THROTTLE_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX: f32 = 4.10; 239 | 240 | /// Minimum allowed value for the low spoof signal value. \[steps\] 241 | /// Equal to THROTTLE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN * STEPS_PER_VOLT. 242 | pub const THROTTLE_SPOOF_LOW_SIGNAL_RANGE_MIN: u16 = 245; 243 | 244 | /// Minimum allowed value for the low spoof signal value. \[steps\] 245 | /// Equal to THROTTLE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX * STEPS_PER_VOLT. 246 | pub const THROTTLE_SPOOF_LOW_SIGNAL_RANGE_MAX: u16 = 1638; 247 | 248 | pub type ThrottleSpoofLowSignal = ranges::Bounded; 249 | 250 | /// Minimum allowed value for the low spoof signal value. \[steps\] 251 | /// Equal to THROTTLE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN * STEPS_PER_VOLT. 252 | pub const THROTTLE_SPOOF_HIGH_SIGNAL_RANGE_MIN: u16 = 573; 253 | 254 | /// Minimum allowed value for the low spoof signal value. \[steps\] 255 | /// Equal to THROTTLE_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX * STEPS_PER_VOLT. 256 | pub const THROTTLE_SPOOF_HIGH_SIGNAL_RANGE_MAX: u16 = 3358; 257 | 258 | pub type ThrottleSpoofHighSignal = ranges::Bounded; 259 | 260 | /// Calculation to convert a throttle position to a low spoof voltage. 261 | pub const fn throttle_position_to_volts_low(position: f32) -> f32 { 262 | position * (THROTTLE_SPOOF_LOW_SIGNAL_VOLTAGE_MAX - THROTTLE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN) 263 | + THROTTLE_SPOOF_LOW_SIGNAL_VOLTAGE_MIN 264 | } 265 | 266 | /// Calculation to convert a throttle position to a high spoof voltage. 267 | pub const fn throttle_position_to_volts_high(position: f32) -> f32 { 268 | position * (THROTTLE_SPOOF_HIGH_SIGNAL_VOLTAGE_MAX - THROTTLE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN) 269 | + THROTTLE_SPOOF_HIGH_SIGNAL_VOLTAGE_MIN 270 | } 271 | 272 | /// Value of the brake position that indicates operator 273 | /// override. \[steps\] 274 | pub const ACCELERATOR_OVERRIDE_THRESHOLD: u32 = 185; 275 | -------------------------------------------------------------------------------- /src/main.rs: -------------------------------------------------------------------------------- 1 | //! OxCC 2 | //! 3 | //! A port of Open Source Car Control written in Rust. 4 | 5 | #![no_std] 6 | #![no_main] 7 | #![feature(const_fn)] 8 | #![cfg_attr(feature = "panic-abort", feature(core_intrinsics))] 9 | 10 | extern crate cortex_m; 11 | extern crate cortex_m_rt as rt; 12 | #[cfg(feature = "panic-over-semihosting")] 13 | extern crate cortex_m_semihosting; 14 | extern crate embedded_hal; 15 | extern crate num; 16 | extern crate oxcc_nucleo_f767zi as nucleo_f767zi; 17 | #[cfg(feature = "panic-over-semihosting")] 18 | extern crate panic_semihosting; 19 | #[macro_use] 20 | extern crate typenum; 21 | 22 | mod board; 23 | mod can_gateway_module; 24 | mod config; 25 | mod dac_mcp4922; 26 | mod dtc; 27 | mod dual_signal; 28 | mod fault_condition; 29 | mod oxcc_error; 30 | #[cfg(feature = "panic-abort")] 31 | mod panic_abort; 32 | mod ranges; 33 | mod steering_module; 34 | mod throttle_module; 35 | mod types; 36 | 37 | #[path = "can_protocols/brake_can_protocol.rs"] 38 | mod brake_can_protocol; 39 | #[path = "can_protocols/fault_can_protocol.rs"] 40 | mod fault_can_protocol; 41 | #[path = "can_protocols/oscc_magic_byte.rs"] 42 | mod oscc_magic_byte; 43 | #[path = "can_protocols/steering_can_protocol.rs"] 44 | mod steering_can_protocol; 45 | #[path = "can_protocols/throttle_can_protocol.rs"] 46 | mod throttle_can_protocol; 47 | 48 | #[cfg(feature = "kia-niro")] 49 | #[path = "vehicles/kial_niro.rs"] 50 | mod kial_niro; 51 | #[cfg(feature = "kia-soul-ev")] 52 | #[path = "vehicles/kial_soul_ev.rs"] 53 | mod kial_soul_ev; 54 | #[cfg(feature = "kia-soul-petrol")] 55 | #[path = "vehicles/kial_soul_petrol.rs"] 56 | mod kial_soul_petrol; 57 | mod vehicle; 58 | 59 | #[cfg(any(feature = "kia-soul-ev", feature = "kia-niro"))] 60 | #[path = "brake/kia_soul_ev_niro/brake_module.rs"] 61 | mod brake_module; 62 | #[cfg(feature = "kia-soul-petrol")] 63 | #[path = "brake/kia_soul_petrol/brake_module.rs"] 64 | mod brake_module; 65 | 66 | use board::FullBoard; 67 | use brake_can_protocol::BrakeReportPublisher; 68 | use brake_module::{BrakeModule, UnpreparedBrakeModule}; 69 | use can_gateway_module::CanGatewayModule; 70 | use core::fmt::Write; 71 | use fault_can_protocol::FaultReportPublisher; 72 | use nucleo_f767zi::debug_console::DebugConsole; 73 | use nucleo_f767zi::hal::can::CanError; 74 | use nucleo_f767zi::hal::can::RxFifo; 75 | use nucleo_f767zi::led::{Color, Leds}; 76 | use oxcc_error::OxccError; 77 | use rt::{entry, exception, ExceptionFrame}; 78 | use steering_can_protocol::SteeringReportPublisher; 79 | use steering_module::{SteeringModule, UnpreparedSteeringModule}; 80 | use throttle_can_protocol::ThrottleReportPublisher; 81 | use throttle_module::{ThrottleModule, UnpreparedThrottleModule}; 82 | 83 | const DEBUG_WRITE_FAILURE: &str = "Failed to write to debug_console"; 84 | 85 | /// A local container for passing around the control modules 86 | struct ControlModules { 87 | pub brake: BrakeModule, 88 | pub throttle: ThrottleModule, 89 | pub steering: SteeringModule, 90 | } 91 | 92 | #[entry] 93 | fn main() -> ! { 94 | // once the organization is cleaned up, the entire board doesn't need to be 95 | // mutable let Board {mut leds, mut delay, ..} = Board::new(); 96 | let ( 97 | mut board, 98 | brake_dac, 99 | brake_pins, 100 | brake_pedal_position_sensor, 101 | brake_grounded_fault_timer, 102 | brake_override_fault_timer, 103 | accelerator_position_sensor, 104 | throttle_dac, 105 | throttle_pins, 106 | throttle_grounded_fault_timer, 107 | throttle_override_fault_timer, 108 | torque_sensor, 109 | steering_dac, 110 | steering_pins, 111 | steering_grounded_fault_timer, 112 | mut debug_console, 113 | can_publish_timer, 114 | control_can, 115 | obd_can, 116 | ) = FullBoard::new().split_components(); 117 | 118 | // turn on the blue LED 119 | board.leds[Color::Blue].on(); 120 | 121 | // show startup message and reset warnings if debugging 122 | #[cfg(debug_assertions)] 123 | { 124 | writeln!(debug_console, "OxCC is running").unwrap(); 125 | 126 | // TODO - some of these are worthy of disabling controls? 127 | if board.reset_conditions.low_power { 128 | writeln!(debug_console, "WARNING: low-power reset detected") 129 | .expect(DEBUG_WRITE_FAILURE); 130 | } 131 | if board.reset_conditions.window_watchdog || board.reset_conditions.independent_watchdog { 132 | writeln!(debug_console, "WARNING: watchdog reset detected").expect(DEBUG_WRITE_FAILURE); 133 | } 134 | if board.reset_conditions.software { 135 | writeln!(debug_console, "WARNING: software reset detected").expect(DEBUG_WRITE_FAILURE); 136 | } 137 | if board.reset_conditions.por_pdr { 138 | writeln!(debug_console, "WARNING: POR/PDR reset detected").expect(DEBUG_WRITE_FAILURE); 139 | } 140 | if board.reset_conditions.pin { 141 | writeln!(debug_console, "WARNING: PIN reset detected").expect(DEBUG_WRITE_FAILURE); 142 | } 143 | if board.reset_conditions.bor { 144 | writeln!(debug_console, "WARNING: BOR reset detected").expect(DEBUG_WRITE_FAILURE); 145 | } 146 | } 147 | 148 | let unprepared_brake_module = UnpreparedBrakeModule::new( 149 | brake_dac, 150 | brake_pins, 151 | brake_pedal_position_sensor, 152 | brake_grounded_fault_timer, 153 | brake_override_fault_timer, 154 | ); 155 | let unprepared_throttle_module = UnpreparedThrottleModule::new( 156 | accelerator_position_sensor, 157 | throttle_dac, 158 | throttle_pins, 159 | throttle_grounded_fault_timer, 160 | throttle_override_fault_timer, 161 | ); 162 | let unprepared_steering_module = UnpreparedSteeringModule::new( 163 | torque_sensor, 164 | steering_dac, 165 | steering_pins, 166 | steering_grounded_fault_timer, 167 | ); 168 | let mut can_gateway = CanGatewayModule::new(can_publish_timer, control_can, obd_can); 169 | 170 | let mut modules = ControlModules { 171 | brake: unprepared_brake_module.prepare_module(), 172 | throttle: unprepared_throttle_module.prepare_module(), 173 | steering: unprepared_steering_module.prepare_module(), 174 | }; 175 | 176 | // send reports immediately 177 | if let Err(e) = publish_reports(&mut modules, &mut can_gateway) { 178 | handle_error( 179 | e, 180 | &mut modules, 181 | &mut can_gateway, 182 | &mut debug_console, 183 | &mut board.leds, 184 | ); 185 | } 186 | 187 | loop { 188 | // refresh the independent watchdog 189 | board.wdg.refresh(); 190 | 191 | // check the control CAN FIFOs for any frames to be processed 192 | if let Err(e) = 193 | process_control_can_frames(&mut modules, &mut can_gateway, &mut debug_console) 194 | { 195 | handle_error( 196 | e, 197 | &mut modules, 198 | &mut can_gateway, 199 | &mut debug_console, 200 | &mut board.leds, 201 | ); 202 | } 203 | 204 | // check modules for fault conditions, sending reports as needed 205 | // NOTE 206 | // ignoring transmit timeouts until a proper error handling strategy is 207 | // implemented 208 | if let Err(e) = check_for_faults(&mut modules, &mut can_gateway, &mut debug_console) { 209 | if e != OxccError::Can(CanError::Timeout) { 210 | handle_error( 211 | e, 212 | &mut modules, 213 | &mut can_gateway, 214 | &mut debug_console, 215 | &mut board.leds, 216 | ); 217 | } 218 | } 219 | 220 | // republish OBD frames to control CAN bus 221 | if let Err(e) = can_gateway.republish_obd_frames_to_control_can_bus() { 222 | handle_error( 223 | e, 224 | &mut modules, 225 | &mut can_gateway, 226 | &mut debug_console, 227 | &mut board.leds, 228 | ); 229 | } 230 | 231 | // periodically publish all report frames 232 | if can_gateway.wait_for_publish() { 233 | board.leds[Color::Green].toggle(); 234 | 235 | if let Err(e) = publish_reports(&mut modules, &mut can_gateway) { 236 | handle_error( 237 | e, 238 | &mut modules, 239 | &mut can_gateway, 240 | &mut debug_console, 241 | &mut board.leds, 242 | ); 243 | } 244 | } 245 | 246 | // TODO - do anything with the user button? 247 | if board.user_button() { 248 | // can only do this when we're debugging/semihosting 249 | #[cfg(feature = "panic-over-semihosting")] 250 | cortex_m::asm::bkpt(); 251 | } 252 | } 253 | } 254 | 255 | fn process_control_can_frames( 256 | modules: &mut ControlModules, 257 | can_gateway: &mut CanGatewayModule, 258 | debug_console: &mut DebugConsole, 259 | ) -> Result<(), OxccError> { 260 | // poll both control CAN FIFOs 261 | for fifo in &[RxFifo::Fifo0, RxFifo::Fifo1] { 262 | match can_gateway.control_can().receive(fifo) { 263 | Ok(rx_frame) => { 264 | modules.brake.process_rx_frame(&rx_frame, debug_console)?; 265 | modules 266 | .throttle 267 | .process_rx_frame(&rx_frame, debug_console)?; 268 | modules 269 | .steering 270 | .process_rx_frame(&rx_frame, debug_console)?; 271 | } 272 | Err(e) => { 273 | // report all but BufferExhausted (no data) 274 | if e != CanError::BufferExhausted { 275 | return Err(OxccError::from(e)); 276 | } 277 | } 278 | } 279 | } 280 | 281 | Ok(()) 282 | } 283 | 284 | fn check_for_faults( 285 | modules: &mut ControlModules, 286 | can_gateway: &mut CanGatewayModule, 287 | debug_console: &mut DebugConsole, 288 | ) -> Result<(), OxccError> { 289 | let maybe_fault = modules.brake.check_for_faults(debug_console)?; 290 | if let Some(brake_fault) = maybe_fault { 291 | can_gateway.publish_fault_report(brake_fault)?; 292 | } 293 | 294 | let maybe_fault = modules.throttle.check_for_faults(debug_console)?; 295 | if let Some(throttle_fault) = maybe_fault { 296 | can_gateway.publish_fault_report(throttle_fault)?; 297 | } 298 | 299 | let maybe_fault = modules.steering.check_for_faults(debug_console)?; 300 | if let Some(steering_fault) = maybe_fault { 301 | can_gateway.publish_fault_report(steering_fault)?; 302 | } 303 | 304 | Ok(()) 305 | } 306 | 307 | // NOTE 308 | // ignoring transmit timeouts until a proper error handling strategy is 309 | // implemented 310 | fn publish_reports( 311 | modules: &mut ControlModules, 312 | can_gateway: &mut CanGatewayModule, 313 | ) -> Result<(), OxccError> { 314 | // attempt to publish them all, only report the last to fail 315 | let mut result = Ok(()); 316 | 317 | // it is typically to get timeout errors if the CAN bus is not active or 318 | // there are no other nodes connected to it 319 | if let Err(e) = can_gateway.publish_brake_report(modules.brake.supply_brake_report()) { 320 | if e != CanError::Timeout { 321 | result = Err(OxccError::from(e)); 322 | } 323 | } 324 | 325 | if let Err(e) = can_gateway.publish_throttle_report(modules.throttle.supply_throttle_report()) { 326 | if e != CanError::Timeout { 327 | result = Err(OxccError::from(e)); 328 | } 329 | } 330 | 331 | if let Err(e) = can_gateway.publish_steering_report(modules.steering.supply_steering_report()) { 332 | if e != CanError::Timeout { 333 | result = Err(OxccError::from(e)); 334 | } 335 | } 336 | 337 | result 338 | } 339 | 340 | // TODO - this is just an example for now 341 | fn handle_error( 342 | error: OxccError, 343 | modules: &mut ControlModules, 344 | can_gateway: &mut CanGatewayModule, 345 | debug_console: &mut DebugConsole, 346 | leds: &mut Leds, 347 | ) { 348 | leds[Color::Red].on(); 349 | 350 | writeln!(debug_console, "ERROR: {:#?}", error).expect(DEBUG_WRITE_FAILURE); 351 | 352 | // disable all controls 353 | let _ = modules.throttle.disable_control(debug_console); 354 | let _ = modules.steering.disable_control(debug_console); 355 | let _ = modules.brake.disable_control(debug_console); 356 | 357 | // publish reports 358 | let _ = publish_reports(modules, can_gateway); 359 | } 360 | 361 | #[exception] 362 | fn HardFault(ef: &ExceptionFrame) -> ! { 363 | panic!("HardFault at {:#?}", ef); 364 | } 365 | 366 | #[exception] 367 | fn DefaultHandler(irqn: i16) { 368 | panic!("Unhandled exception (IRQn = {})", irqn); 369 | } 370 | -------------------------------------------------------------------------------- /Cargo.lock: -------------------------------------------------------------------------------- 1 | [[package]] 2 | name = "aligned" 3 | version = "0.2.0" 4 | source = "registry+https://github.com/rust-lang/crates.io-index" 5 | 6 | [[package]] 7 | name = "bare-metal" 8 | version = "0.2.3" 9 | source = "registry+https://github.com/rust-lang/crates.io-index" 10 | 11 | [[package]] 12 | name = "cast" 13 | version = "0.2.2" 14 | source = "registry+https://github.com/rust-lang/crates.io-index" 15 | 16 | [[package]] 17 | name = "cortex-m" 18 | version = "0.5.7" 19 | source = "registry+https://github.com/rust-lang/crates.io-index" 20 | dependencies = [ 21 | "aligned 0.2.0 (registry+https://github.com/rust-lang/crates.io-index)", 22 | "bare-metal 0.2.3 (registry+https://github.com/rust-lang/crates.io-index)", 23 | "volatile-register 0.2.0 (registry+https://github.com/rust-lang/crates.io-index)", 24 | ] 25 | 26 | [[package]] 27 | name = "cortex-m-rt" 28 | version = "0.6.4" 29 | source = "registry+https://github.com/rust-lang/crates.io-index" 30 | dependencies = [ 31 | "cortex-m-rt-macros 0.1.2 (registry+https://github.com/rust-lang/crates.io-index)", 32 | "r0 0.2.2 (registry+https://github.com/rust-lang/crates.io-index)", 33 | ] 34 | 35 | [[package]] 36 | name = "cortex-m-rt-macros" 37 | version = "0.1.2" 38 | source = "registry+https://github.com/rust-lang/crates.io-index" 39 | dependencies = [ 40 | "proc-macro2 0.4.19 (registry+https://github.com/rust-lang/crates.io-index)", 41 | "quote 0.6.8 (registry+https://github.com/rust-lang/crates.io-index)", 42 | "rand 0.5.5 (registry+https://github.com/rust-lang/crates.io-index)", 43 | "syn 0.15.8 (registry+https://github.com/rust-lang/crates.io-index)", 44 | ] 45 | 46 | [[package]] 47 | name = "cortex-m-semihosting" 48 | version = "0.3.1" 49 | source = "registry+https://github.com/rust-lang/crates.io-index" 50 | 51 | [[package]] 52 | name = "embedded-hal" 53 | version = "0.2.1" 54 | source = "registry+https://github.com/rust-lang/crates.io-index" 55 | dependencies = [ 56 | "nb 0.1.1 (registry+https://github.com/rust-lang/crates.io-index)", 57 | "void 1.0.2 (registry+https://github.com/rust-lang/crates.io-index)", 58 | ] 59 | 60 | [[package]] 61 | name = "embedded_types" 62 | version = "0.3.2" 63 | source = "registry+https://github.com/rust-lang/crates.io-index" 64 | 65 | [[package]] 66 | name = "nb" 67 | version = "0.1.1" 68 | source = "registry+https://github.com/rust-lang/crates.io-index" 69 | 70 | [[package]] 71 | name = "num" 72 | version = "0.2.0" 73 | source = "registry+https://github.com/rust-lang/crates.io-index" 74 | dependencies = [ 75 | "num-complex 0.2.0 (registry+https://github.com/rust-lang/crates.io-index)", 76 | "num-integer 0.1.39 (registry+https://github.com/rust-lang/crates.io-index)", 77 | "num-iter 0.1.37 (registry+https://github.com/rust-lang/crates.io-index)", 78 | "num-rational 0.2.1 (registry+https://github.com/rust-lang/crates.io-index)", 79 | "num-traits 0.2.6 (registry+https://github.com/rust-lang/crates.io-index)", 80 | ] 81 | 82 | [[package]] 83 | name = "num-complex" 84 | version = "0.2.0" 85 | source = "registry+https://github.com/rust-lang/crates.io-index" 86 | dependencies = [ 87 | "num-traits 0.2.6 (registry+https://github.com/rust-lang/crates.io-index)", 88 | ] 89 | 90 | [[package]] 91 | name = "num-integer" 92 | version = "0.1.39" 93 | source = "registry+https://github.com/rust-lang/crates.io-index" 94 | dependencies = [ 95 | "num-traits 0.2.6 (registry+https://github.com/rust-lang/crates.io-index)", 96 | ] 97 | 98 | [[package]] 99 | name = "num-iter" 100 | version = "0.1.37" 101 | source = "registry+https://github.com/rust-lang/crates.io-index" 102 | dependencies = [ 103 | "num-integer 0.1.39 (registry+https://github.com/rust-lang/crates.io-index)", 104 | "num-traits 0.2.6 (registry+https://github.com/rust-lang/crates.io-index)", 105 | ] 106 | 107 | [[package]] 108 | name = "num-rational" 109 | version = "0.2.1" 110 | source = "registry+https://github.com/rust-lang/crates.io-index" 111 | dependencies = [ 112 | "num-integer 0.1.39 (registry+https://github.com/rust-lang/crates.io-index)", 113 | "num-traits 0.2.6 (registry+https://github.com/rust-lang/crates.io-index)", 114 | ] 115 | 116 | [[package]] 117 | name = "num-traits" 118 | version = "0.2.6" 119 | source = "registry+https://github.com/rust-lang/crates.io-index" 120 | 121 | [[package]] 122 | name = "oxcc" 123 | version = "0.1.0" 124 | dependencies = [ 125 | "cortex-m 0.5.7 (registry+https://github.com/rust-lang/crates.io-index)", 126 | "cortex-m-rt 0.6.4 (registry+https://github.com/rust-lang/crates.io-index)", 127 | "cortex-m-semihosting 0.3.1 (registry+https://github.com/rust-lang/crates.io-index)", 128 | "embedded-hal 0.2.1 (registry+https://github.com/rust-lang/crates.io-index)", 129 | "num 0.2.0 (registry+https://github.com/rust-lang/crates.io-index)", 130 | "oxcc-nucleo-f767zi 0.1.1 (registry+https://github.com/rust-lang/crates.io-index)", 131 | "panic-semihosting 0.5.0 (registry+https://github.com/rust-lang/crates.io-index)", 132 | "typenum 1.10.0 (registry+https://github.com/rust-lang/crates.io-index)", 133 | ] 134 | 135 | [[package]] 136 | name = "oxcc-nucleo-f767zi" 137 | version = "0.1.1" 138 | source = "registry+https://github.com/rust-lang/crates.io-index" 139 | dependencies = [ 140 | "cast 0.2.2 (registry+https://github.com/rust-lang/crates.io-index)", 141 | "embedded-hal 0.2.1 (registry+https://github.com/rust-lang/crates.io-index)", 142 | "nb 0.1.1 (registry+https://github.com/rust-lang/crates.io-index)", 143 | "oxcc-stm32f767-hal 0.2.0 (registry+https://github.com/rust-lang/crates.io-index)", 144 | ] 145 | 146 | [[package]] 147 | name = "oxcc-stm32f767" 148 | version = "0.1.0" 149 | source = "registry+https://github.com/rust-lang/crates.io-index" 150 | dependencies = [ 151 | "bare-metal 0.2.3 (registry+https://github.com/rust-lang/crates.io-index)", 152 | "cortex-m 0.5.7 (registry+https://github.com/rust-lang/crates.io-index)", 153 | "cortex-m-rt 0.6.4 (registry+https://github.com/rust-lang/crates.io-index)", 154 | "vcell 0.1.0 (registry+https://github.com/rust-lang/crates.io-index)", 155 | ] 156 | 157 | [[package]] 158 | name = "oxcc-stm32f767-hal" 159 | version = "0.2.0" 160 | source = "registry+https://github.com/rust-lang/crates.io-index" 161 | dependencies = [ 162 | "cast 0.2.2 (registry+https://github.com/rust-lang/crates.io-index)", 163 | "cortex-m 0.5.7 (registry+https://github.com/rust-lang/crates.io-index)", 164 | "embedded-hal 0.2.1 (registry+https://github.com/rust-lang/crates.io-index)", 165 | "embedded_types 0.3.2 (registry+https://github.com/rust-lang/crates.io-index)", 166 | "nb 0.1.1 (registry+https://github.com/rust-lang/crates.io-index)", 167 | "oxcc-stm32f767 0.1.0 (registry+https://github.com/rust-lang/crates.io-index)", 168 | "void 1.0.2 (registry+https://github.com/rust-lang/crates.io-index)", 169 | ] 170 | 171 | [[package]] 172 | name = "panic-semihosting" 173 | version = "0.5.0" 174 | source = "registry+https://github.com/rust-lang/crates.io-index" 175 | dependencies = [ 176 | "cortex-m 0.5.7 (registry+https://github.com/rust-lang/crates.io-index)", 177 | "cortex-m-semihosting 0.3.1 (registry+https://github.com/rust-lang/crates.io-index)", 178 | ] 179 | 180 | [[package]] 181 | name = "proc-macro2" 182 | version = "0.4.19" 183 | source = "registry+https://github.com/rust-lang/crates.io-index" 184 | dependencies = [ 185 | "unicode-xid 0.1.0 (registry+https://github.com/rust-lang/crates.io-index)", 186 | ] 187 | 188 | [[package]] 189 | name = "quote" 190 | version = "0.6.8" 191 | source = "registry+https://github.com/rust-lang/crates.io-index" 192 | dependencies = [ 193 | "proc-macro2 0.4.19 (registry+https://github.com/rust-lang/crates.io-index)", 194 | ] 195 | 196 | [[package]] 197 | name = "r0" 198 | version = "0.2.2" 199 | source = "registry+https://github.com/rust-lang/crates.io-index" 200 | 201 | [[package]] 202 | name = "rand" 203 | version = "0.5.5" 204 | source = "registry+https://github.com/rust-lang/crates.io-index" 205 | dependencies = [ 206 | "rand_core 0.2.1 (registry+https://github.com/rust-lang/crates.io-index)", 207 | ] 208 | 209 | [[package]] 210 | name = "rand_core" 211 | version = "0.2.1" 212 | source = "registry+https://github.com/rust-lang/crates.io-index" 213 | 214 | [[package]] 215 | name = "syn" 216 | version = "0.15.8" 217 | source = "registry+https://github.com/rust-lang/crates.io-index" 218 | dependencies = [ 219 | "proc-macro2 0.4.19 (registry+https://github.com/rust-lang/crates.io-index)", 220 | "quote 0.6.8 (registry+https://github.com/rust-lang/crates.io-index)", 221 | "unicode-xid 0.1.0 (registry+https://github.com/rust-lang/crates.io-index)", 222 | ] 223 | 224 | [[package]] 225 | name = "typenum" 226 | version = "1.10.0" 227 | source = "registry+https://github.com/rust-lang/crates.io-index" 228 | 229 | [[package]] 230 | name = "unicode-xid" 231 | version = "0.1.0" 232 | source = "registry+https://github.com/rust-lang/crates.io-index" 233 | 234 | [[package]] 235 | name = "vcell" 236 | version = "0.1.0" 237 | source = "registry+https://github.com/rust-lang/crates.io-index" 238 | 239 | [[package]] 240 | name = "void" 241 | version = "1.0.2" 242 | source = "registry+https://github.com/rust-lang/crates.io-index" 243 | 244 | [[package]] 245 | name = "volatile-register" 246 | version = "0.2.0" 247 | source = "registry+https://github.com/rust-lang/crates.io-index" 248 | dependencies = [ 249 | "vcell 0.1.0 (registry+https://github.com/rust-lang/crates.io-index)", 250 | ] 251 | 252 | [metadata] 253 | "checksum aligned 0.2.0 (registry+https://github.com/rust-lang/crates.io-index)" = "d39da9b88ae1a81c03c9c082b8db83f1d0e93914126041962af61034ab44c4a5" 254 | "checksum bare-metal 0.2.3 (registry+https://github.com/rust-lang/crates.io-index)" = "1bdcf9294ed648c7cd29b11db06ea244005aeef50ae8f605b1a3af2940bf8f92" 255 | "checksum cast 0.2.2 (registry+https://github.com/rust-lang/crates.io-index)" = "926013f2860c46252efceabb19f4a6b308197505082c609025aa6706c011d427" 256 | "checksum cortex-m 0.5.7 (registry+https://github.com/rust-lang/crates.io-index)" = "4573199c5b1e9b0eeae418b46f7c0af5fdf11b3057f83880810dfef68dd1dcb5" 257 | "checksum cortex-m-rt 0.6.4 (registry+https://github.com/rust-lang/crates.io-index)" = "d86cfa89fa220d3cb7a41133693e2d302d18e9a632298ffb3738f175c8c12325" 258 | "checksum cortex-m-rt-macros 0.1.2 (registry+https://github.com/rust-lang/crates.io-index)" = "3139fdadccaa0db6fa96637678ced9b0b97e4f10047c9ab603d125048e107d1a" 259 | "checksum cortex-m-semihosting 0.3.1 (registry+https://github.com/rust-lang/crates.io-index)" = "54d46ec4730314a01de4504328ef4ed6b2c51b63815caac4847ac9e70f88c9e5" 260 | "checksum embedded-hal 0.2.1 (registry+https://github.com/rust-lang/crates.io-index)" = "26944677e4934eb5fb4025501dc0d6cdbcf6bfabd6200fcfee2e7e8eef8c0362" 261 | "checksum embedded_types 0.3.2 (registry+https://github.com/rust-lang/crates.io-index)" = "ea8dc5db8dae723ecf68d863ff0011500e29fbbede193fa8fb3ca032595d3f6a" 262 | "checksum nb 0.1.1 (registry+https://github.com/rust-lang/crates.io-index)" = "69f380b5fe9fab8c0d7a6a99cda23e2cc0463bedb2cbc3aada0813b98496ecdc" 263 | "checksum num 0.2.0 (registry+https://github.com/rust-lang/crates.io-index)" = "cf4825417e1e1406b3782a8ce92f4d53f26ec055e3622e1881ca8e9f5f9e08db" 264 | "checksum num-complex 0.2.0 (registry+https://github.com/rust-lang/crates.io-index)" = "68de83578789e0fbda3fa923035be83cf8bfd3b30ccfdecd5aa89bf8601f408e" 265 | "checksum num-integer 0.1.39 (registry+https://github.com/rust-lang/crates.io-index)" = "e83d528d2677f0518c570baf2b7abdcf0cd2d248860b68507bdcb3e91d4c0cea" 266 | "checksum num-iter 0.1.37 (registry+https://github.com/rust-lang/crates.io-index)" = "af3fdbbc3291a5464dc57b03860ec37ca6bf915ed6ee385e7c6c052c422b2124" 267 | "checksum num-rational 0.2.1 (registry+https://github.com/rust-lang/crates.io-index)" = "4e96f040177bb3da242b5b1ecf3f54b5d5af3efbbfb18608977a5d2767b22f10" 268 | "checksum num-traits 0.2.6 (registry+https://github.com/rust-lang/crates.io-index)" = "0b3a5d7cc97d6d30d8b9bc8fa19bf45349ffe46241e8816f50f62f6d6aaabee1" 269 | "checksum oxcc-nucleo-f767zi 0.1.1 (registry+https://github.com/rust-lang/crates.io-index)" = "df4fc98258001ad387aa7678c79b66133d0775c5b5a857c25c3faef47121baa8" 270 | "checksum oxcc-stm32f767 0.1.0 (registry+https://github.com/rust-lang/crates.io-index)" = "a425a931fa001f275999e77eb96de9a60c0b39e6051255c0e7120bd28e1f0117" 271 | "checksum oxcc-stm32f767-hal 0.2.0 (registry+https://github.com/rust-lang/crates.io-index)" = "f941d9bcc6bf04cdf1e731ecc3b49477a96298b75852a6605dcc36a2c6589d4e" 272 | "checksum panic-semihosting 0.5.0 (registry+https://github.com/rust-lang/crates.io-index)" = "1017854db1621a236488ac359b89b19a56dcb1cb45127376d04f23128cea7210" 273 | "checksum proc-macro2 0.4.19 (registry+https://github.com/rust-lang/crates.io-index)" = "ffe022fb8c8bd254524b0b3305906c1921fa37a84a644e29079a9e62200c3901" 274 | "checksum quote 0.6.8 (registry+https://github.com/rust-lang/crates.io-index)" = "dd636425967c33af890042c483632d33fa7a18f19ad1d7ea72e8998c6ef8dea5" 275 | "checksum r0 0.2.2 (registry+https://github.com/rust-lang/crates.io-index)" = "e2a38df5b15c8d5c7e8654189744d8e396bddc18ad48041a500ce52d6948941f" 276 | "checksum rand 0.5.5 (registry+https://github.com/rust-lang/crates.io-index)" = "e464cd887e869cddcae8792a4ee31d23c7edd516700695608f5b98c67ee0131c" 277 | "checksum rand_core 0.2.1 (registry+https://github.com/rust-lang/crates.io-index)" = "edecf0f94da5551fc9b492093e30b041a891657db7940ee221f9d2f66e82eef2" 278 | "checksum syn 0.15.8 (registry+https://github.com/rust-lang/crates.io-index)" = "356d1c5043597c40489e9af2d2498c7fefc33e99b7d75b43be336c8a59b3e45e" 279 | "checksum typenum 1.10.0 (registry+https://github.com/rust-lang/crates.io-index)" = "612d636f949607bdf9b123b4a6f6d966dedf3ff669f7f045890d3a4a73948169" 280 | "checksum unicode-xid 0.1.0 (registry+https://github.com/rust-lang/crates.io-index)" = "fc72304796d0818e357ead4e000d19c9c174ab23dc11093ac919054d20a6a7fc" 281 | "checksum vcell 0.1.0 (registry+https://github.com/rust-lang/crates.io-index)" = "45c297f0afb6928cd08ab1ff9d95e99392595ea25ae1b5ecf822ff8764e57a0d" 282 | "checksum void 1.0.2 (registry+https://github.com/rust-lang/crates.io-index)" = "6a02e4885ed3bc0f2de90ea6dd45ebcbb66dacffe03547fadbb0eeae2770887d" 283 | "checksum volatile-register 0.2.0 (registry+https://github.com/rust-lang/crates.io-index)" = "0d67cb4616d99b940db1d6bd28844ff97108b498a6ca850e5b6191a532063286" 284 | -------------------------------------------------------------------------------- /src/board.rs: -------------------------------------------------------------------------------- 1 | //! Peripheral bootstrapping of the OxCC hardware environment 2 | 3 | use config; 4 | use cortex_m; 5 | use dac_mcp4922::Mcp4922; 6 | use dac_mcp4922::MODE as DAC_MODE; 7 | use dual_signal::HighLowReader; 8 | use nucleo_f767zi::debug_console::DebugConsole; 9 | use nucleo_f767zi::hal::adc::Adc; 10 | use nucleo_f767zi::hal::adc::Channel as AdcChannel; 11 | use nucleo_f767zi::hal::adc::Prescaler as AdcPrescaler; 12 | use nucleo_f767zi::hal::adc::Resolution as AdcResolution; 13 | use nucleo_f767zi::hal::adc::SampleTime as AdcSampleTime; 14 | use nucleo_f767zi::hal::can::Can; 15 | use nucleo_f767zi::hal::iwdg::{Iwdg, IwdgConfig, WatchdogTimeout}; 16 | use nucleo_f767zi::hal::prelude::*; 17 | use nucleo_f767zi::hal::rcc::ResetConditions; 18 | use nucleo_f767zi::hal::serial::Serial; 19 | use nucleo_f767zi::hal::spi::Spi; 20 | use nucleo_f767zi::hal::stm32f7x7; 21 | use nucleo_f767zi::hal::stm32f7x7::{ADC1, ADC2, ADC3, IWDG}; 22 | use nucleo_f767zi::led::Leds; 23 | use nucleo_f767zi::UserButtonPin; 24 | use vehicle::FAULT_HYSTERESIS; 25 | 26 | pub use types::*; 27 | 28 | /// Control module CAN report frame publish rate 29 | /// 30 | /// Brake, throttle and steering modules will publish 31 | /// their report frames to the control CAN bus at 32 | /// this rate. 33 | pub const CAN_PUBLISH_HZ: u32 = 50; 34 | 35 | /// ADC configuration 36 | /// 37 | /// The ADC(s) are configured to match the original OSCC 38 | /// Arduino resolution of 10 bits instead of the full 12 bit 39 | /// resolution. 40 | /// This allows existing vehicle configurations to be used without 41 | /// re-calibrating. 42 | /// 43 | /// The OSCC Arduino builds were likely using the default ADC 44 | /// configuration which would give about 104 microseconds per 45 | /// `analogRead()`. 46 | /// 47 | /// OxCC configures the ADC(s) such that the conversion time 48 | /// is about 18.26 microseconds. 49 | /// 50 | /// Total conversion time = sampling time + 13 cycles for 10-bit resolution 51 | /// - APB2 clock = 216 MHz / 2 52 | /// - ADC clock = APB2 clock / prescaler = 108 / Prescaler4 = 27 MHz 53 | /// - conversion time = Cycles480 + 13 = 493 cycles == 18.26 us 54 | /// 55 | /// **NOTE** 56 | /// Prescalers must be chosen such that the ADC clock 57 | /// does not exceed 30 MHz 58 | pub const ADC_PRESCALER: AdcPrescaler = AdcPrescaler::Prescaler4; 59 | pub const ADC_SAMPLE_TIME: AdcSampleTime = AdcSampleTime::Cycles480; 60 | pub const ADC_RESOLUTION: AdcResolution = AdcResolution::Bits10; 61 | 62 | /// Number of analog conversion samples read 63 | /// by the DAC signal discontinuity mechanism. 64 | /// 65 | /// **NOTE** 66 | /// This is likely the result of poor ADC hardware on the original 67 | /// OSCC Arduino hardware. I suspect we can get rid of it now that 68 | /// we're using the full conversion time of Cycles480 which is 69 | /// pretty stable. 70 | pub const DAC_SAMPLE_AVERAGE_COUNT: u32 = 20; 71 | 72 | pub struct FullBoard { 73 | pub debug_console: DebugConsole, 74 | pub leds: Leds, 75 | pub user_button: UserButtonPin, 76 | pub can_publish_timer: CanPublishTimer, 77 | pub wdg: Iwdg, 78 | pub reset_conditions: ResetConditions, 79 | control_can: ControlCan, 80 | obd_can: ObdCan, 81 | brake_pedal_position_sensor: BrakePedalPositionSensor, 82 | accelerator_position_sensor: AcceleratorPositionSensor, 83 | torque_sensor: TorqueSensor, 84 | brake_dac: BrakeDac, 85 | throttle_dac: ThrottleDac, 86 | steering_dac: SteeringDac, 87 | brake_pins: BrakePins, 88 | throttle_pins: ThrottlePins, 89 | steering_pins: SteeringPins, 90 | brake_grounded_fault_timer: BrakeGroundedFaultTimer, 91 | brake_override_fault_timer: BrakeOverrideFaultTimer, 92 | throttle_grounded_fault_timer: ThrottleGroundedFaultTimer, 93 | throttle_override_fault_timer: ThrottleOverrideFaultTimer, 94 | steering_grounded_fault_timer: SteeringGroundedFaultTimer, 95 | } 96 | 97 | pub struct Board { 98 | pub leds: Leds, 99 | pub user_button: UserButtonPin, 100 | pub wdg: Iwdg, 101 | pub reset_conditions: ResetConditions, 102 | } 103 | 104 | impl FullBoard { 105 | pub fn new() -> Self { 106 | // read the RCC reset condition flags before anything else 107 | let reset_conditions = ResetConditions::read_and_clear(); 108 | 109 | let mut core_peripherals = 110 | cortex_m::Peripherals::take().expect("Failed to take cortex_m::Peripherals"); 111 | let peripherals = 112 | stm32f7x7::Peripherals::take().expect("Failed to take stm32f7x7::Peripherals"); 113 | 114 | core_peripherals.SCB.enable_icache(); 115 | core_peripherals 116 | .SCB 117 | .enable_dcache(&mut core_peripherals.CPUID); 118 | 119 | let mut flash = peripherals.FLASH.constrain(); 120 | let mut rcc = peripherals.RCC.constrain(); 121 | let mut c_adc = peripherals.C_ADC; 122 | 123 | let mut gpiob = peripherals.GPIOB.split(&mut rcc.ahb1); 124 | let mut gpioa = peripherals.GPIOA.split(&mut rcc.ahb1); 125 | let mut gpioc = peripherals.GPIOC.split(&mut rcc.ahb1); 126 | let mut gpiod = peripherals.GPIOD.split(&mut rcc.ahb1); 127 | let mut gpioe = peripherals.GPIOE.split(&mut rcc.ahb1); 128 | let mut gpiof = peripherals.GPIOF.split(&mut rcc.ahb1); 129 | 130 | let brake_pins = BrakePins { 131 | spoof_enable: gpiod 132 | .pd12 133 | .into_push_pull_output(&mut gpiod.moder, &mut gpiod.otyper), 134 | brake_light_enable: gpiod 135 | .pd13 136 | .into_push_pull_output(&mut gpiod.moder, &mut gpiod.otyper), 137 | pedal_pos_sensor_high: gpioa 138 | .pa3 139 | .into_analog_input(&mut gpioa.moder, &mut gpioa.pupdr), 140 | pedal_pos_sensor_low: gpioc 141 | .pc0 142 | .into_analog_input(&mut gpioc.moder, &mut gpioc.pupdr), 143 | }; 144 | 145 | // TODO - move these once DAC impl is ready 146 | let brake_sck: BrakeSpiSckPin = gpioa.pa5.into_af5(&mut gpioa.moder, &mut gpioa.afrl); 147 | let brake_miso: BrakeSpiMisoPin = gpioa.pa6.into_af5(&mut gpioa.moder, &mut gpioa.afrl); 148 | let brake_mosi: BrakeSpiMosiPin = gpioa.pa7.into_af5(&mut gpioa.moder, &mut gpioa.afrl); 149 | let brake_nss: BrakeSpiNssPin = gpioa 150 | .pa4 151 | .into_push_pull_output(&mut gpioa.moder, &mut gpioa.otyper); 152 | 153 | let throttle_pins = ThrottlePins { 154 | spoof_enable: gpioe 155 | .pe2 156 | .into_push_pull_output(&mut gpioe.moder, &mut gpioe.otyper), 157 | accel_pos_sensor_high: gpioc 158 | .pc3 159 | .into_analog_input(&mut gpioc.moder, &mut gpioc.pupdr), 160 | accel_pos_sensor_low: gpiob 161 | .pb1 162 | .into_analog_input(&mut gpiob.moder, &mut gpiob.pupdr), 163 | }; 164 | 165 | let throttle_sck: ThrottleSpiSckPin = 166 | gpiob.pb10.into_af5(&mut gpiob.moder, &mut gpiob.afrh); 167 | let throttle_miso: ThrottleSpiMisoPin = 168 | gpioc.pc2.into_af5(&mut gpioc.moder, &mut gpioc.afrl); 169 | let throttle_mosi: ThrottleSpiMosiPin = 170 | gpiob.pb15.into_af5(&mut gpiob.moder, &mut gpiob.afrh); 171 | let throttle_nss: ThrottleSpiNssPin = gpiob 172 | .pb4 173 | .into_push_pull_output(&mut gpiob.moder, &mut gpiob.otyper); 174 | 175 | let steering_pins = SteeringPins { 176 | spoof_enable: gpiod 177 | .pd11 178 | .into_push_pull_output(&mut gpiod.moder, &mut gpiod.otyper), 179 | torque_sensor_high: gpiof 180 | .pf5 181 | .into_analog_input(&mut gpiof.moder, &mut gpiof.pupdr), 182 | torque_sensor_low: gpiof 183 | .pf10 184 | .into_analog_input(&mut gpiof.moder, &mut gpiof.pupdr), 185 | }; 186 | 187 | let steering_sck: SteeringSpiSckPin = 188 | gpioc.pc10.into_af5(&mut gpioc.moder, &mut gpioc.afrh); 189 | let steering_miso: SteeringSpiMisoPin = 190 | gpioc.pc11.into_af5(&mut gpioc.moder, &mut gpioc.afrh); 191 | let steering_mosi: SteeringSpiMosiPin = 192 | gpioc.pc12.into_af5(&mut gpioc.moder, &mut gpioc.afrh); 193 | let steering_nss: SteeringSpiNssPin = gpioa 194 | .pa15 195 | .into_push_pull_output(&mut gpioa.moder, &mut gpioa.otyper); 196 | 197 | let led_r = gpiob 198 | .pb14 199 | .into_push_pull_output(&mut gpiob.moder, &mut gpiob.otyper); 200 | let led_g = gpiob 201 | .pb0 202 | .into_push_pull_output(&mut gpiob.moder, &mut gpiob.otyper); 203 | let led_b = gpiob 204 | .pb7 205 | .into_push_pull_output(&mut gpiob.moder, &mut gpiob.otyper); 206 | 207 | let usart3_tx = gpiod.pd8.into_af7(&mut gpiod.moder, &mut gpiod.afrh); 208 | let usart3_rx = gpiod.pd9.into_af7(&mut gpiod.moder, &mut gpiod.afrh); 209 | 210 | let can1_tx = gpiod.pd1.into_af9(&mut gpiod.moder, &mut gpiod.afrl); 211 | let can1_rx = gpiod.pd0.into_af9(&mut gpiod.moder, &mut gpiod.afrl); 212 | 213 | let can2_tx = gpiob.pb13.into_af9(&mut gpiob.moder, &mut gpiob.afrh); 214 | let can2_rx = gpiob.pb12.into_af9(&mut gpiob.moder, &mut gpiob.afrh); 215 | 216 | // default clock configuration runs at 16 MHz 217 | // let clocks = rcc.cfgr.freeze(&mut flash.acr); 218 | 219 | // TODO - enable OverDrive to get 216 MHz 220 | // configure maximum clock frequency at 200 MHz 221 | let clocks = rcc.cfgr.freeze_max(&mut flash.acr); 222 | 223 | // TODO - this can be moved into the HAL once it's aware of the clocks 224 | let adc_clock = match ADC_PRESCALER { 225 | AdcPrescaler::Prescaler2 => clocks.pclk2().0 / 2, 226 | AdcPrescaler::Prescaler4 => clocks.pclk2().0 / 4, 227 | AdcPrescaler::Prescaler6 => clocks.pclk2().0 / 6, 228 | AdcPrescaler::Prescaler8 => clocks.pclk2().0 / 8, 229 | }; 230 | assert!(adc_clock <= 30_000_000); 231 | 232 | let mut leds = Leds::new(led_r, led_g, led_b); 233 | for led in leds.iter_mut() { 234 | led.off(); 235 | } 236 | 237 | // USART3 is routed up to the same USB port as the stlink 238 | // shows up as /dev/ttyACM0 for me 239 | let serial = Serial::usart3( 240 | peripherals.USART3, 241 | (usart3_tx, usart3_rx), 242 | 115_200.bps(), 243 | clocks, 244 | &mut rcc.apb1, 245 | ); 246 | 247 | // NOTE: the default config can fail if there are CAN bus or config issues 248 | // &CanConfig::default(), 249 | // loopback/silent mode can be used for testing 250 | // &CanConfig { loopback_mode: true, silent_mode: true, 251 | // ..CanConfig::default() }, 252 | let control_can = Can::can1( 253 | peripherals.CAN1, 254 | (can1_tx, can1_rx), 255 | &mut rcc.apb1, 256 | &config::CONTROL_CAN_CONFIG, 257 | ).expect("Failed to configure control CAN (CAN1)"); 258 | 259 | let obd_can = Can::can2( 260 | peripherals.CAN2, 261 | (can2_tx, can2_rx), 262 | &mut rcc.apb1, 263 | &config::OBD_CAN_CONFIG, 264 | ).expect("Failed to configure OBD CAN (CAN2)"); 265 | 266 | // apply control CAN filters 267 | for filter in &config::gather_control_can_filters() { 268 | control_can 269 | .configure_filter(&filter) 270 | .expect("Failed to configure control CAN filter"); 271 | } 272 | 273 | // apply OBD CAN filters 274 | for filter in &config::gather_obd_can_filters() { 275 | obd_can 276 | .configure_filter(&filter) 277 | .expect("Failed to configure OBD CAN filter"); 278 | } 279 | 280 | let brake_spi: BrakeSpi = Spi::spi1( 281 | peripherals.SPI1, 282 | (brake_sck, brake_miso, brake_mosi), 283 | DAC_MODE, 284 | 1.mhz().into(), 285 | clocks, 286 | &mut rcc.apb2, 287 | ); 288 | 289 | let throttle_spi: ThrottleSpi = Spi::spi2( 290 | peripherals.SPI2, 291 | (throttle_sck, throttle_miso, throttle_mosi), 292 | DAC_MODE, 293 | 1.mhz().into(), 294 | clocks, 295 | &mut rcc.apb1, 296 | ); 297 | 298 | let steering_spi: SteeringSpi = Spi::spi3( 299 | peripherals.SPI3, 300 | (steering_sck, steering_miso, steering_mosi), 301 | DAC_MODE, 302 | 1.mhz().into(), 303 | clocks, 304 | &mut rcc.apb1, 305 | ); 306 | 307 | FullBoard { 308 | debug_console: DebugConsole::new(serial), 309 | leds, 310 | user_button: gpioc 311 | .pc13 312 | .into_pull_down_input(&mut gpioc.moder, &mut gpioc.pupdr), 313 | can_publish_timer: CanPublishTimer::tim2( 314 | peripherals.TIM2, 315 | CAN_PUBLISH_HZ.hz(), 316 | clocks, 317 | &mut rcc.apb1, 318 | ), 319 | wdg: Iwdg::new( 320 | peripherals.IWDG, 321 | IwdgConfig::from(WatchdogTimeout::Wdto50ms), 322 | ), 323 | reset_conditions, 324 | control_can, 325 | obd_can, 326 | brake_pedal_position_sensor: BrakePedalPositionSensor { 327 | adc1: Adc::adc1( 328 | peripherals.ADC1, 329 | &mut c_adc, 330 | &mut rcc.apb2, 331 | ADC_PRESCALER, 332 | ADC_RESOLUTION, 333 | ), 334 | }, 335 | accelerator_position_sensor: AcceleratorPositionSensor { 336 | adc2: Adc::adc2( 337 | peripherals.ADC2, 338 | &mut c_adc, 339 | &mut rcc.apb2, 340 | ADC_PRESCALER, 341 | ADC_RESOLUTION, 342 | ), 343 | }, 344 | torque_sensor: TorqueSensor { 345 | adc3: Adc::adc3( 346 | peripherals.ADC3, 347 | &mut c_adc, 348 | &mut rcc.apb2, 349 | ADC_PRESCALER, 350 | ADC_RESOLUTION, 351 | ), 352 | }, 353 | brake_dac: Mcp4922::new(brake_spi, brake_nss), 354 | throttle_dac: Mcp4922::new(throttle_spi, throttle_nss), 355 | steering_dac: Mcp4922::new(steering_spi, steering_nss), 356 | brake_pins, 357 | throttle_pins, 358 | steering_pins, 359 | brake_grounded_fault_timer: BrakeGroundedFaultTimer::tim3( 360 | peripherals.TIM3, 361 | (1000 / FAULT_HYSTERESIS).hz(), 362 | clocks, 363 | &mut rcc.apb1, 364 | ), 365 | brake_override_fault_timer: BrakeOverrideFaultTimer::tim4( 366 | peripherals.TIM4, 367 | (1000 / FAULT_HYSTERESIS).hz(), 368 | clocks, 369 | &mut rcc.apb1, 370 | ), 371 | throttle_grounded_fault_timer: ThrottleGroundedFaultTimer::tim5( 372 | peripherals.TIM5, 373 | (1000 / FAULT_HYSTERESIS).hz(), 374 | clocks, 375 | &mut rcc.apb1, 376 | ), 377 | throttle_override_fault_timer: ThrottleOverrideFaultTimer::tim6( 378 | peripherals.TIM6, 379 | (1000 / FAULT_HYSTERESIS).hz(), 380 | clocks, 381 | &mut rcc.apb1, 382 | ), 383 | steering_grounded_fault_timer: SteeringGroundedFaultTimer::tim7( 384 | peripherals.TIM7, 385 | (1000 / FAULT_HYSTERESIS).hz(), 386 | clocks, 387 | &mut rcc.apb1, 388 | ), 389 | } 390 | } 391 | 392 | pub fn split_components( 393 | self, 394 | ) -> ( 395 | Board, 396 | BrakeDac, 397 | BrakePins, 398 | BrakePedalPositionSensor, 399 | BrakeGroundedFaultTimer, 400 | BrakeOverrideFaultTimer, 401 | AcceleratorPositionSensor, 402 | ThrottleDac, 403 | ThrottlePins, 404 | ThrottleGroundedFaultTimer, 405 | ThrottleOverrideFaultTimer, 406 | TorqueSensor, 407 | SteeringDac, 408 | SteeringPins, 409 | SteeringGroundedFaultTimer, 410 | DebugConsole, 411 | CanPublishTimer, 412 | ControlCan, 413 | ObdCan, 414 | ) { 415 | let FullBoard { 416 | debug_console, 417 | leds, 418 | user_button, 419 | can_publish_timer, 420 | wdg, 421 | reset_conditions, 422 | control_can, 423 | obd_can, 424 | brake_pedal_position_sensor, 425 | accelerator_position_sensor, 426 | torque_sensor, 427 | brake_dac, 428 | throttle_dac, 429 | steering_dac, 430 | brake_pins, 431 | throttle_pins, 432 | steering_pins, 433 | brake_grounded_fault_timer, 434 | brake_override_fault_timer, 435 | throttle_grounded_fault_timer, 436 | throttle_override_fault_timer, 437 | steering_grounded_fault_timer, 438 | } = self; 439 | ( 440 | Board { 441 | leds, 442 | user_button, 443 | wdg, 444 | reset_conditions, 445 | }, 446 | brake_dac, 447 | brake_pins, 448 | brake_pedal_position_sensor, 449 | brake_grounded_fault_timer, 450 | brake_override_fault_timer, 451 | accelerator_position_sensor, 452 | throttle_dac, 453 | throttle_pins, 454 | throttle_grounded_fault_timer, 455 | throttle_override_fault_timer, 456 | torque_sensor, 457 | steering_dac, 458 | steering_pins, 459 | steering_grounded_fault_timer, 460 | debug_console, 461 | can_publish_timer, 462 | control_can, 463 | obd_can, 464 | ) 465 | } 466 | } 467 | 468 | impl Board { 469 | pub fn user_button(&mut self) -> bool { 470 | self.user_button.is_high() 471 | } 472 | } 473 | 474 | // brake module owns ADC1 475 | pub struct BrakePedalPositionSensor { 476 | adc1: Adc, 477 | } 478 | 479 | impl HighLowReader for BrakePedalPositionSensor { 480 | fn read_high(&self) -> u16 { 481 | self.adc1.read(AdcChannel::Adc123In3, ADC_SAMPLE_TIME) 482 | } 483 | fn read_low(&self) -> u16 { 484 | self.adc1.read(AdcChannel::Adc123In10, ADC_SAMPLE_TIME) 485 | } 486 | } 487 | 488 | // throttle module owns ADC2 489 | pub struct AcceleratorPositionSensor { 490 | adc2: Adc, 491 | } 492 | 493 | impl HighLowReader for AcceleratorPositionSensor { 494 | fn read_high(&self) -> u16 { 495 | self.adc2.read(AdcChannel::Adc123In13, ADC_SAMPLE_TIME) 496 | } 497 | fn read_low(&self) -> u16 { 498 | self.adc2.read(AdcChannel::Adc12In9, ADC_SAMPLE_TIME) 499 | } 500 | } 501 | 502 | // steering module owns ADC3 503 | pub struct TorqueSensor { 504 | adc3: Adc, 505 | } 506 | 507 | impl HighLowReader for TorqueSensor { 508 | fn read_high(&self) -> u16 { 509 | self.adc3.read(AdcChannel::Adc3In15, ADC_SAMPLE_TIME) 510 | } 511 | fn read_low(&self) -> u16 { 512 | self.adc3.read(AdcChannel::Adc3In8, ADC_SAMPLE_TIME) 513 | } 514 | } 515 | --------------------------------------------------------------------------------