├── src ├── app │ ├── core.rs │ ├── memory │ │ ├── mod.rs │ │ ├── efi_cfg.rs │ │ └── tables.rs │ ├── tasks │ │ ├── mod.rs │ │ └── engine.rs │ ├── engine │ │ ├── mod.rs │ │ ├── error.rs │ │ ├── engine_status.rs │ │ ├── sensors.rs │ │ ├── efi_cfg.rs │ │ ├── cpwm.rs │ │ └── pmic.rs │ ├── my_module.rs │ ├── util.rs │ ├── webserial │ │ ├── handle_core.rs │ │ ├── handle_pmic.rs │ │ ├── handle_engine.rs │ │ ├── handle_realtime_data.rs │ │ ├── mod.rs │ │ └── handle_tables.rs │ ├── injection │ │ ├── injectors.rs │ │ ├── alpha_n.rs │ │ └── mod.rs │ ├── logging.rs │ ├── ignition │ │ └── mod.rs │ └── gpio.rs └── main.rs ├── rustfmt.toml ├── check_size.sh ├── attach.sh ├── .github ├── FUNDING.yml ├── workflows │ ├── docs.yml │ └── build.yml └── ISSUE_TEMPLATE │ ├── feature_request.md │ └── bug_report.md ├── memory.x ├── .devcontainer ├── setup.sh ├── devcontainer.json └── Dockerfile ├── .gitignore ├── test ├── README ├── unity_config.h ├── efi_test_main.cpp ├── cpwm_test.cpp ├── unity_config.c ├── rpm_calc.cpp ├── alphan_test.cpp └── ignition_test.cpp ├── upload.sh ├── debug.gdb ├── .cargo └── config ├── .gdbinit ├── .run └── Debug attach.run.xml ├── Cargo.toml ├── gen_doc.sh ├── README.md ├── LICENSE ├── OpenEFI_CubeMX.ioc └── Cargo.lock /src/app/core.rs: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /rustfmt.toml: -------------------------------------------------------------------------------- 1 | imports_granularity="Crate" -------------------------------------------------------------------------------- /src/app/memory/mod.rs: -------------------------------------------------------------------------------- 1 | pub mod tables; 2 | pub mod efi_cfg; -------------------------------------------------------------------------------- /src/app/tasks/mod.rs: -------------------------------------------------------------------------------- 1 | pub mod engine; 2 | // pub mod ignition; -------------------------------------------------------------------------------- /check_size.sh: -------------------------------------------------------------------------------- 1 | arm-none-eabi-size target/thumbv7em-none-eabihf/debug/open_efi 2 | -------------------------------------------------------------------------------- /attach.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | cargo b 3 | export PYTHONPATH=/usr/lib64/python3.8 && export PYTHONHOME=/usr/bin/python.3.8 4 | arm-none-eabi-gdb -x ./debug.gdb ./target/thumbv7em-none-eabihf/release/open_efi 5 | -------------------------------------------------------------------------------- /.github/FUNDING.yml: -------------------------------------------------------------------------------- 1 | # These are supported funding model platforms 2 | 3 | github: [FDSoftware] # Replace with up to 4 GitHub Sponsors-enabled usernames e.g., [user1, user2] 4 | custom: https://cafecito.app/openefi 5 | -------------------------------------------------------------------------------- /memory.x: -------------------------------------------------------------------------------- 1 | MEMORY 2 | { 3 | /* NOTE K = KiBi = 1024 bytes */ 4 | FLASH : ORIGIN = 0x08000000, LENGTH = 1024K 5 | RAM : ORIGIN = 0x20000000, LENGTH = 112K 6 | } 7 | 8 | /* This is where the call stack will be allocated. */ 9 | /* The stack is of the full descending type. */ 10 | /* NOTE Do NOT modify `_stack_start` unless you know what you are doing */ 11 | _stack_start = ORIGIN(RAM) + LENGTH(RAM); -------------------------------------------------------------------------------- /.devcontainer/setup.sh: -------------------------------------------------------------------------------- 1 | ## setup and install oh-my-zsh 2 | sh -c "$(curl -fsSL https://raw.githubusercontent.com/robbyrussell/oh-my-zsh/master/tools/install.sh)" 3 | cp -R /root/.oh-my-zsh /home/$USERNAME 4 | cp /root/.zshrc /home/$USERNAME 5 | sed -i -e "s/\/root\/.oh-my-zsh/\/home\/$USERNAME\/.oh-my-zsh/g" /home/$USERNAME/.zshrc 6 | #chown -R $USER_UID:$USER_GID /home/$USERNAME/.oh-my-zsh /home/$USERNAME/.zshrc 7 | exit 0 8 | -------------------------------------------------------------------------------- /src/app/engine/mod.rs: -------------------------------------------------------------------------------- 1 | pub mod cpwm; 2 | pub mod efi_cfg; 3 | pub mod engine_status; 4 | pub mod sensors; 5 | pub mod pmic; 6 | mod error; 7 | 8 | /** 9 | * @brief tiempo que tarda una vuelta del cigueñal 10 | */ 11 | pub fn get_degree_time(rpm: i32) -> f32 { 12 | 1000000.0f32 * 60.0 / 360.0 / rpm as f32 13 | } 14 | 15 | /** 16 | * @brief, degree time / 2, ni que fueramos a usar motores 2T 17 | */ 18 | pub fn get_engine_cycle_duration(rpm: i32) -> f32 { 19 | get_degree_time(rpm) / 2.0 20 | } 21 | -------------------------------------------------------------------------------- /src/app/my_module.rs: -------------------------------------------------------------------------------- 1 | use rtic::Mutex; 2 | use rtic_monotonics::systick::*; 3 | use crate::app; 4 | 5 | pub(crate) async fn blink2(mut cx: app::blink2::Context<'_>) { 6 | loop { 7 | if *cx.local.state2 { 8 | cx.shared.led.lock(|l|{l.led_2.set_low()}); 9 | *cx.local.state2 = false; 10 | } else { 11 | cx.shared.led.lock(|l|{l.led_2.set_high()}); 12 | *cx.local.state2 = true; 13 | } 14 | Systick::delay(50.millis()).await; 15 | } 16 | } -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | bin 2 | *.d 3 | *.obj 4 | *.hex 5 | *.o 6 | *.map 7 | *.dump 8 | libopencm3 9 | qfplib 10 | test2.c 11 | utils/node_modules 12 | lib/unity 13 | logs 14 | docs 15 | .pio 16 | .pioenvs 17 | .piolibdeps 18 | .vscode/.browse.c_cpp.db* 19 | .vscode/c_cpp_properties.json 20 | DAC_OUT_PUT* 21 | .mxproject 22 | TESTING_CI 23 | RELEASE_CI 24 | dist 25 | 26 | xpack* 27 | qemu_output.log 28 | .venv 29 | 30 | .idea/ 31 | CMakeListsPrivate.txt 32 | CMakeLists.txt 33 | cmake-* 34 | 35 | 36 | # Added by cargo 37 | 38 | /target 39 | -------------------------------------------------------------------------------- /test/README: -------------------------------------------------------------------------------- 1 | 2 | This directory is intended for PIO Unit Testing and project tests. 3 | 4 | Unit Testing is a software testing method by which individual units of 5 | source code, sets of one or more MCU program modules together with associated 6 | control data, usage procedures, and operating procedures, are tested to 7 | determine whether they are fit for use. Unit testing finds problems early 8 | in the development cycle. 9 | 10 | More information about PIO Unit Testing: 11 | - https://docs.platformio.org/page/plus/unit-testing.html 12 | -------------------------------------------------------------------------------- /.devcontainer/devcontainer.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "Codespaces Rust Starter", 3 | "customizations": { 4 | "vscode": { 5 | "extensions": [ 6 | "matklad.rust-analyzer", 7 | "serayuzgur.crates", 8 | "rust-lang.rust-analyzer", 9 | "dracula-theme.theme-dracula" 10 | ], 11 | "settings": { 12 | "editor.formatOnSave": true, 13 | "terminal.integrated.profiles.linux": { 14 | "zsh": { 15 | "path": "/bin/zsh" 16 | } 17 | }, 18 | "terminal.integrated.defaultProfile.linux": "zsh" 19 | } 20 | } 21 | }, 22 | "dockerFile": "Dockerfile" 23 | } -------------------------------------------------------------------------------- /.github/workflows/docs.yml: -------------------------------------------------------------------------------- 1 | name: DOCS 2 | 3 | on: 4 | push: 5 | branches: [master] 6 | 7 | jobs: 8 | docs: 9 | name: docs 10 | runs-on: "ubuntu-latest" 11 | container: 12 | image: ghcr.io/churrosoft/openefi_docker:main 13 | credentials: 14 | username: ${{ github.actor }} 15 | password: ${{ secrets.GITHUB_TOKEN }} 16 | steps: 17 | - uses: actions/checkout@v3 18 | 19 | - name: Build docs 20 | env: 21 | GH_REPO_TOKEN: ${{ secrets.GH_REPO_TOKEN }} 22 | shell: bash 23 | run: | 24 | cd "$GITHUB_WORKSPACE" 25 | chmod +x ./gen_doc.sh 26 | ./gen_doc.sh "$GH_REPO_TOKEN" 27 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/feature_request.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Feature request 3 | about: Suggest an idea for this project 4 | title: '' 5 | labels: '' 6 | assignees: '' 7 | 8 | --- 9 | 10 | **Is your feature request related to a problem? Please describe.** 11 | A clear and concise description of what the problem is. Ex. I'm always frustrated when [...] 12 | 13 | **Describe the solution you'd like** 14 | A clear and concise description of what you want to happen. 15 | 16 | **Describe alternatives you've considered** 17 | A clear and concise description of any alternative solutions or features you've considered. 18 | 19 | **Additional context** 20 | Add any other context or screenshots about the feature request here. 21 | -------------------------------------------------------------------------------- /upload.sh: -------------------------------------------------------------------------------- 1 | vidpid_to_devs(){ 2 | find $(grep -l "PRODUCT=$(printf "%x/%x" "0x${1%:*}" "0x${1#*:}")" \ 3 | /sys/bus/usb/devices/[0-9]*:*/uevent | sed 's,uevent$,,') \ 4 | /dev/null -name dev -o -name dev_id | 5 | sed 's,[^/]*$,uevent,' | 6 | xargs sed -n -e s,DEVNAME=,/dev/,p -e s,INTERFACE=,,p 7 | } 8 | ttyPorts=$(vidpid_to_devs 1d50:6018) 9 | ttyPortArr=($ttyPorts) 10 | 11 | arm-none-eabi-gdb -nx --batch \ 12 | -ex "target extended-remote ${ttyPortArr}" \ 13 | -ex 'mon tpwr enable' \ 14 | -ex 'monitor swdp_scan' \ 15 | -ex 'attach 1' \ 16 | -ex 'load' \ 17 | -ex 'compare-sections' \ 18 | -ex 'kill' \ 19 | ./target/thumbv7em-none-eabihf/release/open_efi 20 | 21 | git rev-list HEAD --count 22 | -------------------------------------------------------------------------------- /.devcontainer/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM alpine:3.18 2 | RUN apk add --no-cache git curl build-base gcc alpine-sdk eudev-dev libusb-dev libusb linux-headers bash libftdi1 libftdi1-dev zsh 3 | 4 | ENV RUSTUP_HOME=/rust 5 | ENV CARGO_HOME=/cargo 6 | ENV PATH=/cargo/bin:/rust/bin:$PATH 7 | 8 | RUN curl https://sh.rustup.rs -sSf | sh -s -- -y --default-toolchain nightly --no-modify-path 9 | 10 | RUN rustup default nightly 11 | RUN rustup target add thumbv7em-none-eabihf 12 | RUN rustup component add llvm-tools-preview 13 | 14 | RUN cargo install cargo-binutils 15 | RUN cargo install cargo-embed 16 | RUN cargo install cargo-flash 17 | RUN cargo install cargo-expand 18 | 19 | WORKDIR /home/ 20 | 21 | COPY . . 22 | 23 | RUN bash ./setup.sh 24 | 25 | ENV PATH="/root/.cargo/bin:$PATH" -------------------------------------------------------------------------------- /debug.gdb: -------------------------------------------------------------------------------- 1 | shell bash -c 'for f in /dev/serial/by-id/usb-Black_Magic_Debug_Black_Magic_Probe_*-if00; do [ -e "$f" ] && echo "target extended-remote $f" || echo -e "echo \033[1;31mDEBUGGER IS NOT CONNECTED\033[0m\\n\nquit"; break; done > /tmp/gdb-conn-source' 2 | source /tmp/gdb-conn-source 3 | 4 | mon connect_rst enable 5 | # mon tpwr enable 6 | shell bash -c 'sleep 0.05s' 7 | mon swdp_scan 8 | attach 1 9 | 10 | # print demangled symbols 11 | set print asm-demangle on 12 | 13 | # set backtrace limit to not have infinite backtrace loops 14 | set backtrace limit 32 15 | 16 | # detect unhandled exceptions, hard faults and panics 17 | break DefaultHandler 18 | break HardFault 19 | break rust_begin_unwind 20 | 21 | # *try* to stop at the user entry point (it might be gone due to inlining) 22 | break main 23 | 24 | load -------------------------------------------------------------------------------- /test/unity_config.h: -------------------------------------------------------------------------------- 1 | #ifndef UNITY_CONFIG_H 2 | #define UNITY_CONFIG_H 3 | 4 | #ifndef NULL 5 | #ifndef __cplusplus 6 | #define NULL (void*)0 7 | #else 8 | #define NULL 0 9 | #endif 10 | #endif 11 | 12 | #ifdef __cplusplus 13 | extern "C" 14 | { 15 | #endif 16 | 17 | void unityOutputStart(); 18 | void unityOutputChar(char); 19 | void unityOutputFlush(); 20 | void unityOutputComplete(); 21 | 22 | #define UNITY_OUTPUT_START() unityOutputStart() 23 | /* #define UNITY_OUTPUT_CHAR(c) unityOutputChar(c) */ 24 | #define UNITY_OUTPUT_FLUSH() unityOutputFlush() 25 | #define UNITY_OUTPUT_COMPLETE() unityOutputComplete() 26 | 27 | int run_tests(void); 28 | int run_rpm_tests(void); 29 | int runIgnitionTests(void); 30 | int run_aplha_n_tests(void); 31 | 32 | 33 | #ifdef __cplusplus 34 | } 35 | #endif /* extern "C" */ 36 | 37 | #endif /* UNITY_CONFIG_H */ -------------------------------------------------------------------------------- /.cargo/config: -------------------------------------------------------------------------------- 1 | [target.thumbv7em-none-eabihf] 2 | runner = "arm-none-eabi-gdb -q -x debug.gdb" 3 | 4 | rustflags = [ 5 | # `flip-link` moves stack at the end of flash 6 | #"-C", "linker=flip-link", 7 | "-C", "link-arg=-Tlink.x", 8 | # "-C", "link-arg=-Tdefmt.x", 9 | # This is needed if your flash or ram addresses are not aligned to 0x10000 in memory.x 10 | # See https://github.com/rust-embedded/cortex-m-quickstart/pull/95 11 | #"-C", "link-arg=--nmagic", 12 | ] 13 | 14 | [build] 15 | target = "thumbv7em-none-eabihf" 16 | 17 | [profile.dev] 18 | debug = false 19 | codegen-units = 1 20 | opt-level = "s" 21 | 22 | [profile.release] 23 | debug = true 24 | opt-level = "s" 25 | lto = true 26 | 27 | [alias] 28 | b = ["build", "--release", "--target", "thumbv7em-none-eabihf"] 29 | bd = ["build", "--target", "thumbv7em-none-eabihf"] 30 | 31 | [env] 32 | DEFMT_LOG = "info" -------------------------------------------------------------------------------- /test/efi_test_main.cpp: -------------------------------------------------------------------------------- 1 | #define UNITY_INCLUDE_PRINT_FORMATTED 2 | 3 | #include 4 | 5 | #include "../Inc/main.h" 6 | #include "efi_config.hpp" 7 | #include "unity_config.h" 8 | #include "../Src/memory/include/config.hpp" 9 | extern "C" { 10 | #include 11 | #include 12 | #include 13 | } 14 | 15 | int main() { 16 | HAL_Init(); 17 | efi_cfg::set_default(); 18 | UNITY_BEGIN(); 19 | trace_initialize(); 20 | debug_printf("INIT_TESTING \n\r"); 21 | run_tests(); 22 | run_rpm_tests(); 23 | runIgnitionTests(); 24 | run_aplha_n_tests(); 25 | debug_printf("END_TESTING \n\r"); 26 | 27 | /* // Que Dios y la Patria me lo demanden 28 | SCB->AIRCR = (0x5FA << SCB_AIRCR_VECTKEY_Pos) | SCB_AIRCR_SYSRESETREQ_Msk; 29 | abort(); */ 30 | UNITY_END(); // stop unit testing 31 | 32 | while (1) { 33 | } 34 | } 35 | 36 | /* void SysTick_Handler(void) { HAL_IncTick(); } */ -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/bug_report.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Bug report 3 | about: Create a report to help us improve 4 | title: '' 5 | labels: '' 6 | assignees: '' 7 | 8 | --- 9 | 10 | **Describe the bug** 11 | A clear and concise description of what the bug is. 12 | 13 | **To Reproduce** 14 | Steps to reproduce the behavior: 15 | 1. Go to '...' 16 | 2. Click on '....' 17 | 3. Scroll down to '....' 18 | 4. See error 19 | 20 | **Expected behavior** 21 | A clear and concise description of what you expected to happen. 22 | 23 | **Screenshots** 24 | If applicable, add screenshots to help explain your problem. 25 | 26 | **Desktop (please complete the following information):** 27 | - OS: [e.g. iOS] 28 | - Browser [e.g. chrome, safari] 29 | - Version [e.g. 22] 30 | 31 | **Smartphone (please complete the following information):** 32 | - Device: [e.g. iPhone6] 33 | - OS: [e.g. iOS8.1] 34 | - Browser [e.g. stock browser, safari] 35 | - Version [e.g. 22] 36 | 37 | **Additional context** 38 | Add any other context about the problem here. 39 | -------------------------------------------------------------------------------- /.gdbinit: -------------------------------------------------------------------------------- 1 | define target remote 2 | # Ignoring whatever the user passes and using the BMP 3 | shell bash -c 'for f in /dev/serial/by-id/usb-Black_Magic_Debug_Black_Magic_Probe_*-if00; do [ -e "$f" ] && echo "target extended-remote $f" || echo -e "echo \033[1;31mDEBUGGER IS NOT CONNECTED\033[0m\\n\nquit"; break; done > /tmp/gdb-conn-source' 4 | source /tmp/gdb-conn-source 5 | 6 | # Reset target before connecting (fixes random stalls) 7 | mon connect_rst enable 8 | 9 | # Enable target power and sleep for a bit 10 | # mon tpwr enable 11 | shell bash -c 'sleep 0.5s' 12 | 13 | # Attach to target 14 | mon swdp_scan 15 | attach 1 16 | end 17 | 18 | define target hookpost-remote 19 | 20 | # print demangled symbols 21 | set print asm-demangle on 22 | 23 | # set backtrace limit to not have infinite backtrace loops 24 | set backtrace limit 32 25 | 26 | # detect unhandled exceptions, hard faults and panics 27 | break DefaultHandler 28 | break HardFault 29 | break rust_begin_unwind 30 | 31 | # *try* to stop at the user entry point (it might be gone due to inlining) 32 | break main 33 | 34 | load 35 | 36 | end -------------------------------------------------------------------------------- /.run/Debug attach.run.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 22 | -------------------------------------------------------------------------------- /.github/workflows/build.yml: -------------------------------------------------------------------------------- 1 | name: Build 2 | 3 | on: 4 | push: 5 | branches: [master] 6 | pull_request: 7 | branches: [master] 8 | 9 | jobs: 10 | build: 11 | runs-on: ubuntu-latest 12 | env: 13 | GH_REPO_TOKEN: ${{ secrets.GH_REPO_TOKEN }} 14 | container: 15 | image: ghcr.io/churrosoft/openefi_docker:main 16 | credentials: 17 | username: ${{ github.actor }} 18 | password: ${{ secrets.GH_REPO_TOKEN }} 19 | 20 | steps: 21 | - uses: actions/checkout@v3 22 | 23 | - name: Build firmware 24 | run: cargo b 25 | 26 | - name: create build tag 27 | run: | 28 | echo "git_hash=$(echo $GITHUB_SHA | cut -c1-7)" >> $GITHUB_ENV 29 | 30 | - name: Check Hash 31 | run: echo "${{ env.git_hash }}" 32 | 33 | - name: rename build file 34 | run: mv target/thumbv7em-none-eabihf/release/open_efi "OpenEFI_${git_hash}.bin" 35 | 36 | - name: Check if firmware files exits 37 | run: | 38 | echo "BINARY=$(find . -type f -name "*.bin")" >> $GITHUB_ENV 39 | 40 | - uses: actions/upload-artifact@v3 41 | name: Upload firmware files 42 | with: 43 | name: ${{ env.git_hash }} 44 | path: | 45 | ${{ env.BINARY }} 46 | -------------------------------------------------------------------------------- /src/app/engine/error.rs: -------------------------------------------------------------------------------- 1 | use core::fmt::{self, Debug}; 2 | use embedded_hal::blocking::spi::Transfer; 3 | use embedded_hal::digital::v2::OutputPin; 4 | 5 | mod private { 6 | #[derive(Debug)] 7 | pub enum Private {} 8 | } 9 | 10 | // TODO: esto serviria tambien para todo el resto de modulos dentro de engine 11 | 12 | /// This can encapsulate an SPI or GPIO error, and adds its own protocol errors 13 | /// on top of that. 14 | pub enum Error, GPIO: OutputPin> { 15 | /// An SPI transfer failed. 16 | Spi(SPI::Error), 17 | 18 | /// A GPIO could not be set. 19 | Gpio(GPIO::Error), 20 | 21 | /// Status register contained unexpected flags. 22 | /// 23 | /// This can happen when the chip is faulty, incorrectly connected 24 | UnexpectedStatus, 25 | 26 | #[doc(hidden)] 27 | __NonExhaustive(private::Private), 28 | } 29 | 30 | impl, GPIO: OutputPin> Debug for Error 31 | where 32 | SPI::Error: Debug, 33 | GPIO::Error: Debug, 34 | { 35 | fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { 36 | match self { 37 | Error::Spi(spi) => write!(f, "Error::Spi({:?})", spi), 38 | Error::Gpio(gpio) => write!(f, "Error::Gpio({:?})", gpio), 39 | Error::UnexpectedStatus => f.write_str("Error::UnexpectedStatus"), 40 | Error::__NonExhaustive(_) => unreachable!(), 41 | } 42 | } 43 | } -------------------------------------------------------------------------------- /src/app/util.rs: -------------------------------------------------------------------------------- 1 | #![allow(unsafe_code)] 2 | use core::str; 3 | 4 | use arrayvec::ArrayString; 5 | use numtoa::NumToA; 6 | use stm32f4xx_hal::signature::Uid; 7 | 8 | pub fn get_serial_str() -> &'static str { 9 | static mut SERIAL: [u8; 16] = [b' '; 16]; 10 | let serial = unsafe { SERIAL.as_mut() }; 11 | 12 | // let sn = read_serial(); 13 | let uid = Uid::get(); 14 | 15 | let mut text = ArrayString::<32>::new(); 16 | let mut num_buffer = [0u8; 20]; 17 | 18 | text.push_str("C"); 19 | text.push_str(uid.lot_num()); 20 | text.push_str("/"); 21 | text.push_str(uid.waf_num().numtoa_str(16, &mut num_buffer)); 22 | text.push_str("."); 23 | text.push_str(uid.x().numtoa_str(16, &mut num_buffer)); 24 | text.push_str("-"); 25 | text.push_str(uid.y().numtoa_str(16, &mut num_buffer)); 26 | 27 | let sn = text.as_bytes(); 28 | 29 | for (i, d) in serial.iter_mut().enumerate() { 30 | *d = sn[i] 31 | } 32 | 33 | unsafe { str::from_utf8_unchecked(serial) } 34 | } 35 | 36 | pub fn crc16(data: &[u8], length: u8) -> u16 { 37 | let mut crc: u16 = 0xFFFF; 38 | let mut iter: usize = 0; 39 | let mut in_len = length; 40 | 41 | while in_len != 0 { 42 | let mut x = (crc >> 8) as u8 ^ data[iter]; 43 | x ^= x >> 4; 44 | crc = (crc << 8) ^ ((x as u16) << 12) ^ ((x as u16) << 5) ^ (x as u16); 45 | 46 | iter += 1; 47 | in_len -= 1; 48 | } 49 | 50 | crc 51 | } -------------------------------------------------------------------------------- /Cargo.toml: -------------------------------------------------------------------------------- 1 | [package] 2 | name = "open_efi" 3 | version = "3.4.535" 4 | edition = "2021" 5 | authors = ["Ramiro Bou ", "Diego Frenoux "] 6 | license = "MIT" 7 | 8 | # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html 9 | 10 | [profile.release] 11 | opt-level = 's' 12 | strip = true 13 | lto = true 14 | codegen-units = 1 15 | panic = "abort" 16 | incremental = false 17 | debug = true 18 | 19 | [profile.dev] 20 | opt-level = 's' 21 | strip = true 22 | debug = false 23 | codegen-units = 1 24 | lto = false 25 | 26 | 27 | [dependencies] 28 | cortex-m = { version = "0.7.7", features = ["critical-section-single-core"]} 29 | cortex-m-rt = "0.7.2" 30 | cortex-m-semihosting = "0.5.0" 31 | embedded-hal = "0.2.7" 32 | panic-halt = "0.2.0" 33 | rtic-sync = "1.0.2" 34 | 35 | arrayvec = { version = "0.7.2", default-features = false } 36 | numtoa = "0.2" 37 | 38 | usb-device = "0.2.9" 39 | usbd-serial = "0.1.1" 40 | usbd-webusb = "1.0.2" 41 | 42 | w25q = "0.2.9" 43 | 44 | serde = { version = "1.0.152", default-features = false, features = ["derive"] } 45 | serde-json-core = "0.5.0" 46 | 47 | postcard = "1.0.4" 48 | micromath = "2.0.0" 49 | shared-bus-rtic = "0.2.2" 50 | #panic-semihosting = "0.6.0" 51 | 52 | 53 | [dependencies.stm32f4xx-hal] 54 | version = "0.14.0" 55 | features = ["stm32f407", "usb_fs", "rtic"] 56 | 57 | [dependencies.rtic] 58 | version = "2.0.1" 59 | features = ["thumbv7-backend"] 60 | 61 | [dependencies.rtic-monotonics] 62 | version = "1.1.0" 63 | features = ["cortex-m-systick", "systick-10khz"] -------------------------------------------------------------------------------- /src/app/webserial/handle_core.rs: -------------------------------------------------------------------------------- 1 | use rtic_sync::channel::Sender; 2 | use crate::{ 3 | app::webserial::{SerialCode, SerialMessage, SerialStatus}, 4 | }; 5 | 6 | pub fn handler(command: SerialMessage, mut webserial_sender: &mut Sender) { 7 | let mut response_buf = SerialMessage { 8 | protocol: 1, 9 | command: command.command, 10 | status: 0, 11 | code: 0, 12 | payload: [0u8; 122], 13 | crc: 0, 14 | }; 15 | 16 | match command.command & 0b00001111 { 17 | 0x01 => { 18 | response_buf.payload[0] = 1; // BOARD TYPE 19 | 20 | let efi_ver_major: u16 = env!("CARGO_PKG_VERSION_MAJOR").parse().unwrap(); 21 | let efi_ver_minor: u16 = env!("CARGO_PKG_VERSION_MINOR").parse().unwrap(); 22 | let efi_ver_patch: u16 = env!("CARGO_PKG_VERSION_PATCH").parse().unwrap(); 23 | 24 | response_buf.payload[1] = ((efi_ver_major >> 8) & 0xFF) as u8; 25 | response_buf.payload[2] = (efi_ver_major & 0xFF) as u8; 26 | 27 | response_buf.payload[3] = ((efi_ver_minor >> 8) & 0xFF) as u8; 28 | response_buf.payload[4] = (efi_ver_minor & 0xFF) as u8; 29 | 30 | response_buf.payload[5] = ((efi_ver_patch >> 8) & 0xFF) as u8; 31 | response_buf.payload[6] = (efi_ver_patch & 0xFF) as u8; 32 | 33 | response_buf.status = SerialStatus::Ok as u8; 34 | response_buf.code = 0; 35 | webserial_sender.try_send(response_buf).ok(); 36 | } 37 | _ => { 38 | response_buf.status = SerialStatus::Error as u8; 39 | response_buf.code = SerialCode::UnknownCmd as u8; 40 | webserial_sender.try_send(response_buf).ok(); 41 | } 42 | }; 43 | } 44 | -------------------------------------------------------------------------------- /src/app/engine/engine_status.rs: -------------------------------------------------------------------------------- 1 | use crate::app::engine::sensors::SensorValues; 2 | 3 | #[allow(non_camel_case_types)] 4 | #[repr(u8)] 5 | #[derive(Debug)] 6 | pub enum __rpm_status { 7 | STOPPED = 0, // < 50 8 | SPIN_UP, // > 50 9 | CRANK, // > 400 rpm 10 | RUNNING, // > 750 11 | } 12 | 13 | // #[allow(non_camel_case_types)] 14 | #[repr(u8)] 15 | #[derive(Debug)] 16 | pub enum InjectionStatus { 17 | FuelCutoff = 0, 18 | FuelIdle, 19 | FullLoad, 20 | FuelAcc, 21 | } 22 | 23 | #[allow(non_snake_case)] 24 | #[derive(Debug)] 25 | pub struct InjectionInfo { 26 | pub targetAFR: f32, 27 | pub injection_bank_1_time: f32, 28 | pub injection_bank_2_time: f32, 29 | pub air_flow: f32, 30 | pub base_air: f32, 31 | pub base_fuel: f32, 32 | pub fuel_flow_rate: f32, 33 | pub fuel_load: f32, 34 | pub injection_status: InjectionStatus, 35 | } 36 | 37 | #[derive(Debug)] 38 | pub struct EngineStatus { 39 | pub injection: InjectionInfo, 40 | pub cycle_tick: u32, 41 | pub cycle_duration: f32, 42 | pub cycle_status: __rpm_status, 43 | pub rpm: i32, 44 | pub sensors: SensorValues, 45 | } 46 | 47 | pub fn get_default_engine_status() -> EngineStatus { 48 | let status = EngineStatus { 49 | sensors: SensorValues::new(), 50 | injection: InjectionInfo { 51 | targetAFR: 0.0, 52 | injection_bank_1_time: 0.0, 53 | injection_bank_2_time: 0.0, 54 | air_flow: 0.0, 55 | base_air: 0.0, 56 | base_fuel: 0.0, 57 | fuel_flow_rate: 0.0, 58 | fuel_load: 0.0, 59 | injection_status: InjectionStatus::FuelCutoff, 60 | }, 61 | cycle_tick: 0, 62 | cycle_duration: 0.0, 63 | cycle_status: __rpm_status::STOPPED, 64 | rpm: 0, 65 | }; 66 | return status; 67 | } 68 | -------------------------------------------------------------------------------- /test/cpwm_test.cpp: -------------------------------------------------------------------------------- 1 | #define UNITY_INCLUDE_PRINT_FORMATTED 2 | 3 | extern "C" { 4 | #include "stdio.h" 5 | #include "unity_config.h" 6 | } 7 | #include 8 | 9 | #include "../Src/cpwm/cpwm.hpp" 10 | 11 | void setUp(void) {} /* Is run before every test, put unit init calls here. */ 12 | void tearDown(void) {} /* Is run after every test, put unit clean-up calls here. */ 13 | 14 | // vamo' a testear el cilindro '1' 15 | // FIXME: en realidad son 48/55 revisar como redondea los numeros en la FPU 16 | // para eso, vamos a mover el tick de CPWM a 49, o 144.05° 17 | // tendria que hacer ignicion en el mismo, luego se cambia a 56/171.05° para probar la ignicion 18 | void test_first_cilinder_egn(void) { 19 | CPWM::ckp_tick = 56; 20 | CPWM::interrupt(); 21 | } 22 | 23 | void test_first_cilinder_iny(void) { 24 | CPWM::ckp_tick = 49; 25 | CPWM::interrupt(); 26 | } 27 | 28 | void test_second_cilinder_egn(void) { 29 | CPWM::ckp_tick = 99; 30 | CPWM::interrupt(); 31 | } 32 | 33 | void test_second_cilinder_iny(void) { 34 | CPWM::ckp_tick = 115; 35 | CPWM::interrupt(); 36 | } 37 | void test_third_cilinder_egn(void) { 38 | CPWM::ckp_tick = 149; 39 | CPWM::interrupt(); 40 | } 41 | 42 | void test_third_cilinder_iny(void) { 43 | CPWM::ckp_tick = 173; 44 | CPWM::interrupt(); 45 | } 46 | void test_fourth_cilinder_egn(void) { 47 | CPWM::ckp_tick = 199; 48 | CPWM::interrupt(); 49 | } 50 | 51 | void test_fourth_cilinder_iny(void) { 52 | CPWM::ckp_tick = 232; 53 | CPWM::interrupt(); 54 | } 55 | 56 | int run_tests() { 57 | UnityBegin("Src/cpwm/src/cpwm.cpp:57"); 58 | 59 | RUN_TEST(test_first_cilinder_egn); 60 | RUN_TEST(test_first_cilinder_iny); 61 | 62 | RUN_TEST(test_second_cilinder_egn); 63 | RUN_TEST(test_second_cilinder_iny); 64 | 65 | RUN_TEST(test_third_cilinder_egn); 66 | RUN_TEST(test_third_cilinder_iny); 67 | 68 | RUN_TEST(test_fourth_cilinder_egn); 69 | RUN_TEST(test_fourth_cilinder_iny); 70 | 71 | return UnityEnd(); 72 | } -------------------------------------------------------------------------------- /src/app/injection/injectors.rs: -------------------------------------------------------------------------------- 1 | #![allow(dead_code)] 2 | 3 | use crate::app::engine::{efi_cfg::InjectorConfig, engine_status::InjectionInfo,sensors::SensorValues}; 4 | 5 | pub fn get_base_time(cfg: &InjectorConfig) -> f32 { 6 | return cfg.on_time - cfg.on_time; 7 | } 8 | 9 | pub fn get_battery_correction(base_time: &f32, cfg: &InjectorConfig, status: &SensorValues) -> f32 { 10 | let correction = cfg.battery_correction.map_or(1.0f32, |table| 11 | { 12 | let vbat_index = table.get(1).unwrap_or(&[0, 0]).into_iter().position(|vbat| *vbat <= (status.batt * 100.0) as i32); 13 | 14 | if vbat_index.is_some() { 15 | let table_correction_value = &table[0][vbat_index.unwrap_or(10000)]; 16 | return (table_correction_value / 10000) as f32; 17 | } 18 | return 1.0f32; 19 | }); 20 | 21 | base_time * correction 22 | } 23 | 24 | pub fn get_pressure_correction() -> f32 { 25 | // por ahora solo disponible en OpenEFI no en uEFI por falta de canales 26 | // (o uEFI aprovechando las entradas de sensores de dashdash) 27 | // FIXME: en teoría con llamar a set_injectorFlow ya estaría 28 | return 0.0; 29 | } 30 | 31 | pub fn get_wall_wetting_correction() -> f32 { 32 | 1.0 33 | } 34 | 35 | pub fn fuel_mass_to_time(status: &InjectionInfo, fuel: f32) -> f32 { 36 | // TODO: revisar modo de inyección y cilindros 37 | // monopunto al hacer 4 inyecciones por ciclo seria lo mismo que full secuencial 38 | // perooo en semi-secuencial al haber dos inyectores, y la mitad de injecciones por ciclo 39 | // tendria que ser 0.5 40 | return (fuel / status.fuel_flow_rate * 1000.0f32) / 2.0f32; 41 | } 42 | 43 | pub fn set_injector_flow(mut status: InjectionInfo, config: InjectorConfig) { 44 | // NOTE: como no se revisa la presión del combustible solo se llama una sola vez esto 45 | let flow = config.fuel_pressure * (config.flow_cc_min * (config.fuel_density / 60.0f32)); 46 | 47 | status.fuel_flow_rate = flow; 48 | } 49 | -------------------------------------------------------------------------------- /src/app/logging.rs: -------------------------------------------------------------------------------- 1 | // Logging to the host (if a debugger is plugged) 2 | #[macro_use] 3 | pub mod host { 4 | // TODO: Remove these macros on RELEASE builds! 5 | #[macro_export] 6 | macro_rules! debug { 7 | ($fmt:expr $(, $($arg:tt)*)?) => { 8 | 9 | if stm32f4xx_hal::pac::DCB::is_debugger_attached() { 10 | use cortex_m_semihosting::hprintln; 11 | 12 | hprintln!(concat!("[{:0>12.3}] DEBUG: ", $fmt), stm32f4xx_hal::pac::DWT::cycle_count() as f32 / 120000000f32, $($($arg)*)?); 13 | } 14 | }; 15 | } 16 | 17 | #[macro_export] 18 | macro_rules! trace { 19 | ($fmt:expr $(, $($arg:tt)*)?) => { 20 | 21 | if stm32f4xx_hal::pac::DCB::is_debugger_attached() { 22 | use cortex_m_semihosting::hprintln; 23 | 24 | hprintln!(concat!("[{:0>12.3}] TRACE: ", $fmt), stm32f4xx_hal::pac::DWT::cycle_count() as f32 / 120000000f32, $($($arg)*)?); 25 | } 26 | }; 27 | } 28 | 29 | #[macro_export] 30 | macro_rules! info { 31 | ($fmt:expr $(, $($arg:tt)*)?) => { 32 | 33 | // if (stm32f4xx_hal::pac::DCB::is_debugger_attached()) { 34 | // use cortex_m_semihosting::hprintln; 35 | // 36 | // hprintln!(concat!("[{:0>12.3}] INFO: ", $fmt), stm32f4xx_hal::pac::DWT::cycle_count() as f32 / 120000000f32, $($($arg)*)?); 37 | // } 38 | }; 39 | } 40 | 41 | #[macro_export] 42 | macro_rules! error { 43 | ($fmt:expr $(, $($arg:tt)*)?) => { 44 | 45 | // if (stm32f4xx_hal::pac::DCB::is_debugger_attached()) { 46 | // use cortex_m_semihosting::hprintln; 47 | // 48 | // hprintln!(concat!("[{:0>12.3}] ERROR: ", $fmt), stm32f4xx_hal::pac::DWT::cycle_count() as f32 / 120000000f32, $($($arg)*)?); 49 | // } 50 | }; 51 | } 52 | 53 | pub use debug; 54 | pub use trace; 55 | pub use error; 56 | pub use info; 57 | } 58 | -------------------------------------------------------------------------------- /src/app/ignition/mod.rs: -------------------------------------------------------------------------------- 1 | use rtic::Mutex; 2 | use crate::app; 3 | use rtic_monotonics::systick::*; 4 | use rtic_monotonics::systick::fugit::{Duration, TimerDurationU32}; 5 | use crate::app::engine::cpwm::{angle_to_time, get_crank_angle}; 6 | 7 | pub async fn ignition_checks(mut ctx: app::ignition_checks::Context<'_>){ 8 | loop { 9 | let mut cycle_time = ctx.shared.timer4.lock(|t4| t4.now().ticks()); 10 | let ckp = ctx.shared.ckp.lock(|ckp| ckp.clone()); 11 | let ckp_config = ctx.shared.efi_cfg.lock(|cfg| cfg.engine.ckp); 12 | 13 | // ignition timing: 14 | let mut crank_angle = get_crank_angle(&ckp, &ckp_config, cycle_time); 15 | const CRANK_ANGLE_MAX_IGN: i32 = 720; //ponele? no entendi bien como se setea dependiendo el tipo de encendido/cilindros 16 | while crank_angle > CRANK_ANGLE_MAX_IGN { crank_angle -= CRANK_ANGLE_MAX_IGN; } // SDUBTUD: no entendi para que es esto pero quien soy para cuestionar a speeduino 17 | 18 | let dwell_angle = 20; //TODO: get from table 19 | let dwell_time = angle_to_time(&ckp,&dwell_angle); 20 | 21 | // esto luego se repetiria para el resto de canales 22 | // TODO: compensar drift del clock por lo que tarda en ejectuarse la task 23 | if !*ctx.local.ign_channel_1 && dwell_time > 0 { 24 | *ctx.local.ign_channel_1 = true; 25 | app::ignition_trigger::spawn(dwell_time).unwrap(); 26 | } 27 | Systick::delay(100.micros()).await; 28 | } 29 | } 30 | 31 | //SDUBTUD: si esto anda es de puro milagro 32 | pub async fn ignition_trigger(mut ctx: app::ignition_trigger::Context<'_>,dwell_time:i32){ 33 | let dwell_ticks = (120_000_000 + 10_000 / 2) / 10_000 - 1u32; 34 | 35 | let dwell_duration = TimerDurationU32::<10_000>::from_ticks(dwell_ticks); 36 | Systick::delay(dwell_duration).await; 37 | 38 | // FIXME: por ahora solo prendo el led, luego implementar las bobinas/resto canales 39 | ctx.shared.led.lock(|l| l.led_check.set_low()); 40 | ctx.shared.timer3.lock(|t3| t3.start((50).millis()).unwrap()); // este tiempo estaria bien para probar 41 | } -------------------------------------------------------------------------------- /test/unity_config.c: -------------------------------------------------------------------------------- 1 | #include "unity_config.h" 2 | 3 | #include "stdio.h" 4 | #include "stm32f4xx_hal.h" 5 | 6 | /* #define TESTING */ 7 | 8 | #define USARTx USART2 9 | #define USARTx_CLK_ENABLE() __HAL_RCC_USART2_CLK_ENABLE() 10 | #define USARTx_CLK_DISABLE() __HAL_RCC_USART2_CLK_DISABLE() 11 | #define USARTx_RX_GPIO_CLK_ENABLE() __HAL_RCC_GPIOA_CLK_ENABLE() 12 | #define USARTx_TX_GPIO_CLK_ENABLE() __HAL_RCC_GPIOA_CLK_ENABLE() 13 | #define USARTx_RX_GPIO_CLK_DISABLE() __HAL_RCC_GPIOA_CLK_DISABLE() 14 | #define USARTx_TX_GPIO_CLK_DISABLE() __HAL_RCC_GPIOA_CLK_DISABLE() 15 | 16 | #define USARTx_FORCE_RESET() __HAL_RCC_USART2_FORCE_RESET() 17 | #define USARTx_RELEASE_RESET() __HAL_RCC_USART2_RELEASE_RESET() 18 | 19 | #define USARTx_TX_PIN GPIO_PIN_2 20 | #define USARTx_TX_GPIO_PORT GPIOA 21 | #define USARTx_TX_AF GPIO_AF7_USART2 22 | #define USARTx_RX_PIN GPIO_PIN_3 23 | #define USARTx_RX_GPIO_PORT GPIOA 24 | #define USARTx_RX_AF GPIO_AF7_USART2 25 | 26 | static UART_HandleTypeDef UartHandle; 27 | 28 | void unityOutputStart() { 29 | GPIO_InitTypeDef GPIO_InitStruct; 30 | 31 | USARTx_TX_GPIO_CLK_ENABLE(); 32 | USARTx_RX_GPIO_CLK_ENABLE(); 33 | 34 | USARTx_CLK_ENABLE(); 35 | 36 | GPIO_InitStruct.Pin = USARTx_TX_PIN; 37 | GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; 38 | GPIO_InitStruct.Pull = GPIO_PULLUP; 39 | GPIO_InitStruct.Speed = GPIO_SPEED_FAST; 40 | GPIO_InitStruct.Alternate = USARTx_TX_AF; 41 | 42 | HAL_GPIO_Init(USARTx_TX_GPIO_PORT, &GPIO_InitStruct); 43 | 44 | GPIO_InitStruct.Pin = USARTx_RX_PIN; 45 | GPIO_InitStruct.Alternate = USARTx_RX_AF; 46 | 47 | HAL_GPIO_Init(USARTx_RX_GPIO_PORT, &GPIO_InitStruct); 48 | UartHandle.Instance = USARTx; 49 | 50 | UartHandle.Init.BaudRate = 115200; 51 | UartHandle.Init.WordLength = UART_WORDLENGTH_8B; 52 | UartHandle.Init.StopBits = UART_STOPBITS_1; 53 | UartHandle.Init.Parity = UART_PARITY_NONE; 54 | UartHandle.Init.HwFlowCtl = UART_HWCONTROL_NONE; 55 | UartHandle.Init.Mode = UART_MODE_TX_RX; 56 | UartHandle.Init.OverSampling = UART_OVERSAMPLING_16; 57 | 58 | if (HAL_UART_Init(&UartHandle) != HAL_OK) { 59 | while (1) { 60 | } 61 | } 62 | } 63 | 64 | void unityOutputChar(char c) { putchar(c); } 65 | 66 | void unityOutputFlush() { fflush(stdout); } 67 | 68 | void unityOutputComplete() { 69 | USARTx_CLK_DISABLE(); 70 | USARTx_RX_GPIO_CLK_DISABLE(); 71 | USARTx_TX_GPIO_CLK_DISABLE(); 72 | } -------------------------------------------------------------------------------- /src/app/injection/alpha_n.rs: -------------------------------------------------------------------------------- 1 | #![allow(dead_code)] 2 | 3 | use crate::app::engine::{ 4 | efi_cfg::EngineConfig, engine_status::EngineStatus, get_engine_cycle_duration, 5 | }; 6 | use crate::app::logging::host; 7 | use crate::app::memory::tables::{DataT, Tables}; 8 | 9 | use super::injectors::fuel_mass_to_time; 10 | 11 | const STD_AIR_PRES: f32 = 101.325f32; 12 | const STD_AIR_TEMP: f32 = 293.15f32; 13 | const AIR_R: f32 = 8.31445f32 / 28.965f32; 14 | 15 | pub fn calculate_injection_fuel(es: &mut EngineStatus, ecfg: &EngineConfig, tables: &mut Tables) -> f32 { 16 | if es.rpm == 0 || !ecfg.ready { 17 | return 0.0; 18 | } 19 | 20 | let ve = get_ve(es, tables.tps_rpm_ve); 21 | let air_mass = get_air_mass(ve, ecfg.engine.cylinder_count, ecfg.engine.displacement); 22 | 23 | let lambda = ecfg.injection.target_lambda; 24 | let stoich = ecfg.injection.target_stoich; 25 | 26 | let afr = stoich * lambda; 27 | 28 | let base_fuel = air_mass / afr; 29 | 30 | { 31 | es.injection.air_flow = (air_mass * ecfg.engine.cylinder_count as f32 32 | / get_engine_cycle_duration(es.rpm)) 33 | * 3600000.0 34 | / 1000.0; 35 | 36 | es.injection.base_air = air_mass; 37 | es.injection.base_fuel = base_fuel; 38 | es.injection.injection_bank_1_time = fuel_mass_to_time(&es.injection, base_fuel) / 1000.0; 39 | es.cycle_duration = get_engine_cycle_duration(es.rpm); 40 | es.injection.targetAFR = afr; 41 | } 42 | return base_fuel; 43 | } 44 | 45 | pub fn calculate_correction_time() -> f32 { 46 | 0.0 47 | } 48 | 49 | // esta funcion es solo para alphaN, cuando implemente speed-density o maf se puede migrar modificando las constantes 50 | pub fn get_air_mass(ve: f32, engine_cilinders: u8, engine_displacement: u32) -> f32 { 51 | let full_cycle: f32 = 52 | (ve / 10000.0) * (engine_displacement as f32 * STD_AIR_PRES / (AIR_R * STD_AIR_TEMP)); 53 | 54 | full_cycle / engine_cilinders as f32 55 | } 56 | 57 | // en este caso se revisa RPM y % del TPS 58 | pub fn get_ve(es: &EngineStatus, ve_table: Option) -> f32 { 59 | if es.rpm == 0 || ve_table.is_none() { 60 | return 0.0; 61 | } 62 | 63 | let table = ve_table.unwrap(); 64 | let rpm_index = table[0].into_iter().position(|x| x >= (es.rpm * 100)).unwrap_or(1).clamp(1, 17); 65 | let load_index = table.into_iter().rposition(|y| y[0] >= es.sensors.tps as i32).unwrap_or(1).clamp(1, 17); 66 | 67 | return table[load_index][rpm_index] as f32 / 100.0; 68 | } 69 | -------------------------------------------------------------------------------- /src/app/webserial/handle_pmic.rs: -------------------------------------------------------------------------------- 1 | use rtic::Mutex; 2 | use crate::app::{ 3 | self, // logging::host, 4 | webserial::{SerialCode, SerialMessage, SerialStatus}, 5 | }; 6 | pub async fn pmic_cdc_callback(mut ctx: app::pmic_cdc_callback::Context<'_>, serial_cmd: SerialMessage) { 7 | let mut response_buf = SerialMessage { 8 | protocol: 1, 9 | command: serial_cmd.command, 10 | status: 0, 11 | code: 0, 12 | payload: [0u8; 122], 13 | crc: 0, 14 | }; 15 | 16 | let mut json_payload = [0u8; 350]; 17 | let result; 18 | 19 | match serial_cmd.command & 0b00001111 { 20 | // get all status 21 | 0x01 => { 22 | // host::trace!("PMIC: get fast status"); 23 | let data = ctx.shared.pmic.lock(|pmic|pmic.get_fast_status()); 24 | result = serde_json_core::to_slice(&data, &mut json_payload); 25 | } 26 | // get injection status 27 | 0x02 => { 28 | // host::trace!("PMIC: get injection status"); 29 | let data = ctx.shared.pmic.lock(|pmic|pmic.get_injector_status()); 30 | result = serde_json_core::to_slice(&data, &mut json_payload); 31 | } 32 | // get ignition status 33 | 0x03 => { 34 | // host::trace!("PMIC: get ignition status"); 35 | let data =ctx.shared.pmic.lock(|pmic|pmic.get_ignition_status()); 36 | result = serde_json_core::to_slice(&data, &mut json_payload); 37 | } 38 | _ => { 39 | response_buf.status = SerialStatus::Error as u8; 40 | response_buf.code = SerialCode::UnknownCmd as u8; 41 | 42 | let _ = ctx.local.pmic_sender.send(response_buf).await; 43 | return; 44 | } 45 | } 46 | 47 | if result.is_ok_and(|s| s > 0) { 48 | let command_count = result.unwrap().div_ceil(122); 49 | 50 | for i in 0..command_count { 51 | let from = i * 122; 52 | let to = from + 122; 53 | response_buf.payload.copy_from_slice(&json_payload[from..to]); 54 | 55 | response_buf.status = SerialStatus::DataChunk as u8; 56 | response_buf.code = 0; 57 | let _ = ctx.local.pmic_sender.send(response_buf).await; 58 | 59 | response_buf.payload.fill(0x0); 60 | } 61 | response_buf.status = SerialStatus::DataChunkEnd as u8; 62 | response_buf.code = 0; 63 | let _ = ctx.local.pmic_sender.send(response_buf).await; 64 | return; 65 | } 66 | 67 | response_buf.status = SerialStatus::Error as u8; 68 | response_buf.code = SerialCode::ParseError as u8; 69 | 70 | let _ = ctx.local.pmic_sender.send(response_buf).await; 71 | } 72 | -------------------------------------------------------------------------------- /src/app/injection/mod.rs: -------------------------------------------------------------------------------- 1 | use stm32f4xx_hal::crc32::Crc32; 2 | use w25q::series25::FlashInfo; 3 | 4 | use crate::app::{ 5 | engine::{efi_cfg::EngineConfig, engine_status::EngineStatus, get_engine_cycle_duration}, 6 | memory::tables::{FlashT, TableData, Tables}, 7 | }; 8 | 9 | pub mod alpha_n; 10 | pub mod injectors; 11 | 12 | pub fn injection_setup(table: &mut Tables, flash: &mut FlashT, fi: &FlashInfo, crc: &mut Crc32) { 13 | let mut tps_rpm_ve = TableData { 14 | data: None, 15 | crc: 0, 16 | address: 0x3, 17 | max_x: 17, 18 | max_y: 17, 19 | }; 20 | 21 | let mut tps_rpm_afr = TableData { 22 | data: None, 23 | crc: 0, 24 | address: 0x4, 25 | max_x: 17, 26 | max_y: 17, 27 | }; 28 | 29 | let mut injector_delay = TableData { 30 | data: None, 31 | crc: 0, 32 | address: 0x5, 33 | max_x: 8, 34 | max_y: 8, 35 | }; 36 | 37 | //TODO: move to ignition module 38 | let mut load_tps_deg = TableData { 39 | data: None, 40 | crc: 0, 41 | address: 0x10, 42 | max_x: 17, 43 | max_y: 17, 44 | }; 45 | 46 | { 47 | table.tps_rpm_ve = tps_rpm_ve.read_from_memory(flash, &fi, crc); 48 | table.tps_rpm_afr = tps_rpm_afr.read_from_memory(flash, &fi, crc); 49 | table.injector_delay = injector_delay.read_from_memory(flash, &fi, crc); 50 | table.load_tps_deg = load_tps_deg.read_from_memory(flash, &fi, crc); 51 | } 52 | } 53 | 54 | pub fn calculate_time_isr(es: &mut EngineStatus, ecfg: &EngineConfig,tables: &mut Tables) { 55 | let mut bank_a_time: f32 = 0.0; 56 | { 57 | // desactivar inyeccion mientras se calcula el tiempo nuevo 58 | es.injection.injection_bank_1_time = 0.0; 59 | } 60 | 61 | // TODO: revisar RPM::status para que en SPIN_UP/CRANK tengan tiempos fijos desde la memoria flash 62 | 63 | bank_a_time += alpha_n::calculate_injection_fuel(es, &ecfg, tables); 64 | bank_a_time += alpha_n::calculate_correction_time(); 65 | bank_a_time += injectors::get_base_time(&ecfg.injection.injector); 66 | bank_a_time += injectors::get_battery_correction(&bank_a_time, &ecfg.injection.injector, &es.sensors); 67 | bank_a_time += injectors::get_wall_wetting_correction(); 68 | bank_a_time += injectors::get_pressure_correction(); 69 | 70 | { 71 | es.injection.injection_bank_1_time = bank_a_time; 72 | } 73 | 74 | // en criollo: 75 | // "si hay mas tiempo que la duracion de un ciclo, es porque tenes el multiple lleno de nasta" 76 | debug_assert!( 77 | bank_a_time * 2.0 < get_engine_cycle_duration(es.rpm) && es.rpm >= 0 || es.rpm >= 0, 78 | "asddsa" 79 | ); 80 | } 81 | 82 | pub async fn injection_checks(){ 83 | 84 | } 85 | -------------------------------------------------------------------------------- /test/rpm_calc.cpp: -------------------------------------------------------------------------------- 1 | /* #ifndef TESTING 2 | #define TESTING 3 | #endif */ 4 | 5 | #include "../Src/cpwm/rpm_calc.h" 6 | 7 | #include 8 | 9 | #include "../Inc/defines.h" 10 | #include "../Inc/variables.h" 11 | #include "../Src/cpwm/cpwm.hpp" 12 | 13 | // void setUp(void) { 14 | // /* Is run before every test, put unit init calls here. */ 15 | // for (int8_t i = 0; i < LOGIC_DNT + 1; i++) RPM::interrupt(); 16 | // } 17 | 18 | // void tearDown(void) { 19 | // /* Is run after every test, put unit clean-up calls here. */ 20 | // } 21 | 22 | void test_stoped_rpm() { 23 | tickStep = 12000000; 24 | 25 | for (int8_t i = 0; i < LOGIC_DNT + 1; i++) RPM::interrupt(); 26 | 27 | for (int8_t i = 0; i < LOGIC_DNT + 1; i++) RPM::interrupt(); 28 | 29 | TEST_ASSERT_EQUAL_FLOAT_MESSAGE(5.0, RPM::_RPM, "Check RPM::interrupt"); 30 | TEST_ASSERT_EQUAL_UINT8_MESSAGE((uint8_t)RPM_STATUS::STOPPED, RPM::status, "Check RPM::interrupt, status calculation (STOPPED)"); 31 | } 32 | 33 | void test_spinup_rpm() { 34 | tickStep = 800000; 35 | for (int8_t i = 0; i < LOGIC_DNT + 1; i++) RPM::interrupt(); 36 | 37 | for (int8_t i = 0; i < LOGIC_DNT + 1; i++) RPM::interrupt(); 38 | 39 | TEST_ASSERT_EQUAL_FLOAT_MESSAGE(75.0, RPM::_RPM, "Check RPM::interrupt"); 40 | TEST_ASSERT_EQUAL_UINT8_MESSAGE((uint8_t)RPM_STATUS::SPIN_UP, RPM::status, "Check RPM::interrupt, status calculation (SPIN_UP)"); 41 | } 42 | 43 | void test_crank_rpm() { 44 | tickStep = 100000; 45 | for (int8_t i = 0; i < LOGIC_DNT + 1; i++) RPM::interrupt(); 46 | 47 | for (int8_t i = 0; i < LOGIC_DNT + 1; i++) RPM::interrupt(); 48 | 49 | TEST_ASSERT_EQUAL_FLOAT_MESSAGE(600.0, RPM::_RPM, "Check RPM::interrupt"); 50 | TEST_ASSERT_EQUAL_UINT8_MESSAGE((uint8_t)RPM_STATUS::CRANK, RPM::status, "Check RPM::interrupt, status calculation (CRANK)"); 51 | } 52 | 53 | void test_running_rpm() { 54 | tickStep = 15000; 55 | 56 | for (int8_t i = 0; i < LOGIC_DNT + 1; i++) RPM::interrupt(); 57 | 58 | for (int8_t i = 0; i < LOGIC_DNT + 1; i++) RPM::interrupt(); 59 | 60 | TEST_ASSERT_EQUAL_FLOAT_MESSAGE(4000.0, RPM::_RPM, "Check RPM::interrupt"); 61 | TEST_ASSERT_EQUAL_UINT8_MESSAGE((uint8_t)RPM_STATUS::RUNNING, RPM::status, "Check RPM::interrupt, status calculation (RUNNING)"); 62 | } 63 | 64 | void test_idle_rpm() { 65 | tickStep = 50000; 66 | 67 | for (int8_t i = 0; i < LOGIC_DNT + 1; i++) RPM::interrupt(); 68 | 69 | for (int8_t i = 0; i < LOGIC_DNT + 1; i++) RPM::interrupt(); 70 | 71 | TEST_ASSERT_EQUAL_FLOAT_MESSAGE(1200.0, RPM::_RPM, "Check RPM::interrupt"); 72 | TEST_ASSERT_EQUAL_UINT8_MESSAGE((uint8_t)RPM_STATUS::RUNNING, RPM::status, "Check RPM::interrupt, status calculation (RUNNING)"); 73 | } 74 | 75 | int run_rpm_tests() { 76 | UnityBegin("Src/cpwm/src/rpm_calc.cpp:57"); 77 | 78 | RUN_TEST(test_stoped_rpm); 79 | RUN_TEST(test_spinup_rpm); 80 | 81 | RUN_TEST(test_crank_rpm); 82 | 83 | RUN_TEST(test_running_rpm); 84 | 85 | RUN_TEST(test_idle_rpm); 86 | 87 | return UnityEnd(); 88 | } -------------------------------------------------------------------------------- /src/app/memory/efi_cfg.rs: -------------------------------------------------------------------------------- 1 | use postcard::{from_bytes, to_vec}; 2 | use serde_json_core::heapless::Vec; 3 | use stm32f4xx_hal::crc32::Crc32; 4 | use w25q::series25::FlashInfo; 5 | 6 | use crate::app::engine::efi_cfg::{EngineConfig, get_default_efi_cfg}; 7 | use crate::app::logging::host; 8 | use crate::app::memory::tables::FlashT; 9 | 10 | const ENGINE_CONFIG_MEMORY_ADDRESS: u32 = 0; 11 | 12 | impl EngineConfig { 13 | pub fn save(&mut self, flash: &mut FlashT, flash_info: &FlashInfo, crc: &mut Crc32) { 14 | //TODO: ajustar tamaño del vector 15 | host::debug!("Guardando cfg"); 16 | let mut output: Vec = to_vec(&self).unwrap(); 17 | 18 | crc.init(); 19 | let calculated_crc = crc.update_bytes(&output); 20 | let mut crc_arr: [u8; 4] = u32::to_le_bytes(calculated_crc); 21 | 22 | let write_address = flash_info.sector_to_page(&(ENGINE_CONFIG_MEMORY_ADDRESS + 4)) * (flash_info.page_size as u32); 23 | let crc_addr = flash_info.sector_to_page(&ENGINE_CONFIG_MEMORY_ADDRESS) * (flash_info.page_size as u32); 24 | 25 | { 26 | flash.erase_sectors(ENGINE_CONFIG_MEMORY_ADDRESS, 1).unwrap(); 27 | flash.write_bytes(crc_addr, &mut crc_arr).unwrap(); 28 | flash.write_bytes(write_address, &mut output).unwrap(); 29 | } 30 | } 31 | pub fn read(&mut self, flash: &mut FlashT, flash_info: &FlashInfo, crc: &mut Crc32) { 32 | // TODO: tirar error en caso de que la palme el crc 33 | 34 | let mut read_buff: [u8; 800] = [0; 800]; 35 | let mut crc_buff: [u8; 4] = [0; 4]; 36 | let read_address = flash_info.sector_to_page(&(ENGINE_CONFIG_MEMORY_ADDRESS + 4)) * (flash_info.page_size as u32); 37 | let crc_addr = flash_info.sector_to_page(&ENGINE_CONFIG_MEMORY_ADDRESS) * (flash_info.page_size as u32); 38 | 39 | { 40 | flash.read(read_address, &mut read_buff).unwrap(); 41 | flash.read(crc_addr, &mut crc_buff).unwrap(); 42 | } 43 | 44 | crc.init(); 45 | let calculated_crc = crc.update_bytes(&read_buff); 46 | let memory_crc = u32::from_le_bytes(crc_buff); 47 | 48 | if memory_crc != calculated_crc { 49 | // self.ready = true; 50 | // host::debug!("Checksum config no coincide {:?} {:?}", memory_crc,calculated_crc); 51 | // 52 | // let default= get_default_efi_cfg(); 53 | // self.injection = default.injection.clone(); 54 | // self.engine = default.engine.clone(); 55 | // 56 | // self.save(flash,flash_info,crc); 57 | host::debug!("Checksum config no coincide {:?} {:?}", memory_crc,calculated_crc); 58 | return; 59 | } 60 | 61 | { 62 | let mut memory_config: EngineConfig = from_bytes(&read_buff).unwrap(); 63 | 64 | self.injection = memory_config.injection.clone(); 65 | self.engine = memory_config.engine.clone(); 66 | self.ready = true; 67 | } 68 | } 69 | } -------------------------------------------------------------------------------- /gen_doc.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | ################################################################################ 3 | GH_REPO_TOKEN=$1 4 | GH_REPO_NAME=OpenEFI 5 | GH_REPO_REF=github.com/churrosoft/OpenEFI.git 6 | ################################################################################ 7 | ##### Setup this script and get the current gh-pages branch. ##### 8 | echo 'Setting up the script...' 9 | # Exit with nonzero exit code if anything fails 10 | set -e 11 | 12 | # Create a clean working directory for this script. 13 | echo $PWD 14 | 15 | # Get the current gh-pages branch 16 | git clone -b gh-pages https://git@$GH_REPO_REF 17 | cd ./OpenEFI 18 | 19 | ##### Configure git. 20 | # Set the push default to simple i.e. push only the current branch. 21 | git config --global push.default simple 22 | # Pretend to be an user called GitHub CI. 23 | git config user.name "GitHub CI" 24 | git config user.email "github@github-ci.org" 25 | 26 | # Remove everything currently in the gh-pages branch. 27 | # GitHub is smart enough to know which files have changed and which files have 28 | # stayed the same and will only update the changed files. So the gh-pages branch 29 | # can be safely cleaned, and it is sure that everything pushed later is the new 30 | # documentation. 31 | rm -rf * 32 | 33 | # Need to create a .nojekyll file to allow filenames starting with an underscore 34 | # to be seen on the gh-pages site. Therefore creating an empty .nojekyll file. 35 | # Presumably this is only needed when the SHORT_NAMES option in Rust is set 36 | # to NO, which it is by default. So creating the file just in case. 37 | echo "" > .nojekyll 38 | echo "devdocs.openefi.tech" > CNAME 39 | 40 | ################################################################################ 41 | ##### Generate the Rust code documentation and log the output. ##### 42 | echo 'Generating Rust code documentation...' 43 | cd .. 44 | cargo doc --no-deps 45 | mv target/thumbv7em-none-eabihf/doc/** ./OpenEFI 46 | cd ./OpenEFI 47 | 48 | ################################################################################ 49 | ##### Upload the documentation to the gh-pages branch of the repository. ##### 50 | # Only upload if Rust successfully created the documentation. 51 | # Check this by verifying that the html directory and the file html/index.html 52 | # both exist. This is a good indication that Rust did it's work. 53 | if [ -f "open_efi/index.html" ]; then 54 | 55 | echo 'Uploading documentation to the gh-pages branch...' 56 | echo "" > index.html 57 | 58 | # Add everything in this directory (the Rust code documentation) to the 59 | # gh-pages branch. 60 | # GitHub is smart enough to know which files have changed and which files have 61 | # stayed the same and will only update the changed files. 62 | git add --all 63 | 64 | # Commit the added files with a title and description containing the Travis CI 65 | # build number and the GitHub commit reference that issued this build. 66 | git commit -m "Deploy code docs to GitHub Pages build: ${GITHUB_RUN_ID}" -m "Commit: ${GITHUB_SHA}" || true 67 | 68 | # Force push to the remote gh-pages branch. 69 | # The ouput is redirected to /dev/null to hide any sensitive credential data 70 | # that might otherwise be exposed. 71 | git push --force "https://${GH_REPO_TOKEN}@${GH_REPO_REF}" > /dev/null 2>&1 || true 72 | else 73 | echo '' >&2 74 | echo 'Warning: No documentation (html) files have been found!' >&2 75 | echo 'Warning: Not going to push the documentation to GitHub!' >&2 76 | exit 1 77 | fi -------------------------------------------------------------------------------- /src/app/webserial/handle_engine.rs: -------------------------------------------------------------------------------- 1 | use postcard::{from_bytes, to_vec}; 2 | use rtic::Mutex; 3 | use rtic::mutex_prelude::*; 4 | use serde_json_core::heapless::Vec; 5 | 6 | use crate::app::{ 7 | self, 8 | webserial::{SerialCode, SerialMessage, SerialStatus}, 9 | }; 10 | use crate::app::engine::efi_cfg::EngineConfig; 11 | use crate::app::logging::host; 12 | 13 | pub async fn engine_cdc_callback(mut ctx: app::engine_cdc_callback::Context<'_>, serial_cmd: SerialMessage) { 14 | let mut response_buf = SerialMessage { 15 | protocol: 1, 16 | command: serial_cmd.command, 17 | status: 0, 18 | code: 0, 19 | payload: [0u8; 122], 20 | crc: 0, 21 | }; 22 | 23 | let mut json_payload = [0u8; 3500]; 24 | let mut result; 25 | 26 | let mut flash = ctx.shared.flash; 27 | let mut flash_info = ctx.shared.flash_info; 28 | let mut efi_cfg = ctx.shared.efi_cfg; 29 | let mut crc = ctx.shared.crc; 30 | let efi_cfg_snapshot = efi_cfg.lock(|c| c.clone()); 31 | 32 | if !efi_cfg_snapshot.ready { 33 | host::trace!("engine cfg webserial error"); 34 | app::send_message::spawn( 35 | SerialStatus::Error, 36 | SerialCode::ParseError as u8, 37 | response_buf, 38 | ).unwrap(); 39 | return; 40 | } 41 | 42 | 43 | match serial_cmd.command & 0b00001111 { 44 | // read engine cfg: 45 | 0x01 => { 46 | host::trace!("read engine cfg"); 47 | result = serde_json_core::to_slice(&efi_cfg_snapshot, &mut json_payload); 48 | } 49 | 0x02 => { 50 | //TODO: se puede controlar el caso de que falle la grabacion 51 | (efi_cfg, flash, flash_info, crc).lock(|efi_cfg, flash, flash_info, crc| efi_cfg.save(flash, flash_info, crc)); 52 | 53 | app::send_message::spawn( 54 | SerialStatus::Ok, 55 | SerialCode::None as u8, 56 | response_buf, 57 | ).unwrap(); 58 | return; 59 | } 60 | _ => { 61 | host::trace!("engine cfg webserial error"); 62 | app::send_message::spawn( 63 | SerialStatus::Error, 64 | SerialCode::UnknownCmd as u8, 65 | response_buf, 66 | ).unwrap(); 67 | return; 68 | } 69 | }; 70 | 71 | if result.is_ok_and(|s| s > 0) { 72 | let command_count = result.unwrap().div_ceil(122); 73 | 74 | for i in 0..command_count { 75 | let from = i * 122; 76 | let to = from + 122; 77 | response_buf.payload.copy_from_slice(&json_payload[from..to]); 78 | 79 | response_buf.status = SerialStatus::DataChunk as u8; 80 | response_buf.code = 0; 81 | 82 | app::send_message::spawn(SerialStatus::DataChunk, 0, response_buf).unwrap(); 83 | 84 | response_buf.payload.fill(0x0); 85 | } 86 | 87 | 88 | app::send_message::spawn(SerialStatus::DataChunkEnd, 0, response_buf).unwrap(); 89 | 90 | let output: Vec = to_vec(&efi_cfg_snapshot).unwrap(); 91 | let mut cfg_new: EngineConfig = from_bytes(&output).unwrap(); 92 | result = serde_json_core::to_slice(&cfg_new, &mut json_payload); 93 | host::trace!("struct => bytes => struct => json result {:?}",result); 94 | 95 | return; 96 | } 97 | 98 | 99 | app::send_message::spawn(SerialStatus::Error, SerialCode::ParseError as u8, response_buf).unwrap(); 100 | } 101 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | [![Gitpod ready-to-code](https://img.shields.io/badge/Gitpod-ready--to--code-blue?logo=gitpod)](https://gitpod.io/#https://github.com/openefi/OpenEFI) 2 | 3 | [![PRs Welcome](https://img.shields.io/badge/PRs-welcome-brightgreen.svg?style=flat-square)](http://makeapullrequest.com) [![Build Status](https://travis-ci.org/openefi/OpenEFI.svg?branch=master)](https://travis-ci.org/openefi/OpenEFI) 4 | # OpenEFI: 5 | 6 | OpenEFI es un sistema de inyección OpenSource configurable y adaptable a casi cualquier vehiculo con motores a Nafta/Gasohol/GNC/GLP 7 | 8 | Está planeado el soporte a corto y largo plazo para: 9 | - Motores a nafta con inyección y encendido electrónico 10 | - calculo de tiempo de inyección a partir de tablas pre-definidas | Alpha Plus | Speed Density 11 | - Soporte de inyectores de alta impedancia (los de baja más adelante por requerir PWM además del tiempo de inyeccion) 12 | - Soporte para GNC/GLP con tablas independientes 13 | - Motores diesel con inyección directa 14 | - Motores de 1 a 8 cilindros 15 | 16 | ## Sobre el Software 17 | la plataforma se ejecuta desde un STM32F103, conocido como "bluepill", para facilitar la programación del mismo, se utiliza el bootloader: [dapboot](https://github.com/devanlai/dapboot) 18 | 19 | 20 | ### Como generar los binarios: 21 | se necesitan las siguientes herramientas: 22 | 23 | GNU Arm Embedded Toolchain 24 | sudo dnf install gcc systemd-devel ncurses-devel ncurses-compat-libs 25 | rustup override set nightly 26 | rustup target add thumbv7em-none-eabihf 27 | rustup component add llvm-tools-preview arm-none-eabi-newlib 28 | cargo install cargo-binutils cargo-embed cargo-flash cargo-expand 29 | 30 | pasos para grabar el firmware: 31 | 32 | 1. Compile y grabe el bootloader => [dapboot](https://github.com/devanlai/dapboot) 33 | 2. clone el repositorio con git o descarguelo 34 | 3. dentro del repo ejecutar: `make flash-usb` 35 | 36 | ## Sobre el Hardware 37 | 38 | ### Preguntas frecuentes: 39 | ### ¿hay algún video sobre el funcionamiento de la ECU / EFI ? 40 | No, por el momento solo tenemos avances en el diseño de los pcb's, pero todavía el hardware ni el software se encuentra preparado para una prueba real en un motor, estimamos nuestro primer test a finales del 2020 si es que no nos morimos antes 41 | ### ¿cómo hacen para adaptar el hardware a una configuración tan amplia? 42 | Porque el tratamiento de las señales analógicas para la mayoría de los sensores son iguales, consta de un filtro paso bajo y un diodo para evitar el sobre voltaje, se intentan mantener simples para que sea fácil de mantener, luego se aplican otros filtros en software para mejorar aún más esto 43 | 44 | ### ¿se pueden revisar los esquemas en algún lado, y obtener más información en general de la placa? 45 | Claro!, todo lo referido al hardware se encuentra en [OpenEFI-PCB](https://github.com/openefi/OpenEFI-PCB) 46 | 47 | ## Sobre la configuracion 48 | 49 | ### Preguntas frecuentes: 50 | ### ¿como se va a configurar todo esto? 51 | para la configuracion inicial, y para la visualizacion de parametros en tiempo real, se utiliza [OpenEFI-Tuner](https://github.com/openefi/OpenEFI-Tuner) , la idea es que tengas unicamente que conectar la EFI a tu computadora, entrar a [tuner.openefi.xyz](http://tuner.openefi.xyz/) y empezar a jugar, sin tener que pasar por la tortura de instalar drivers, programas,etc , aunque tambien puedes montar tu propio OpenEFI-Tuner en tu servidor local si quieres !, aunque la aplicación web funcionara sin conexión a internet 52 | 53 | ### ¿tiene soporte para un scanner común?¿cómo se leen los DTC's? 54 | No, por el momento no tenemos planeado agregar soporte a CAN hasta llegar al MVP, así que no se pueden usar scanners automotrices normales acá 55 | para leer los DTC se utiliza OpenEFI-Tuner 56 | 57 | [![Invitame un café en cafecito.app](https://cdn.cafecito.app/imgs/buttons/button_5.svg)](https://cafecito.app/openefi) 58 | -------------------------------------------------------------------------------- /src/app/engine/sensors.rs: -------------------------------------------------------------------------------- 1 | use crate::app::gpio::ADCMapping; 2 | use micromath::F32Ext; 3 | 4 | use stm32f4xx_hal::{ 5 | adc::{config::SampleTime, Adc}, 6 | pac::ADC2, 7 | }; 8 | 9 | const EMA_LP_ALPHA: f32 = 0.45f32; 10 | 11 | pub enum SensorTypes { 12 | MAP, 13 | TPS, 14 | CooltanTemp, 15 | AirTemp, 16 | BatteryVoltage, 17 | ExternalLambda, 18 | } 19 | 20 | #[derive(Debug,Clone,Copy)] 21 | pub struct SensorValues { 22 | pub map: f32, 23 | pub tps: f32, 24 | pub cooltan_temp: f32, 25 | pub air_temp: f32, 26 | pub batt: f32, 27 | pub ext_o2: f32, 28 | 29 | // private: 30 | raw_map: f32, 31 | raw_tps: f32, 32 | raw_temp: f32, 33 | raw_air_temp: f32, 34 | raw_batt: f32, 35 | raw_ext_o2: f32, 36 | } 37 | 38 | impl SensorValues { 39 | pub fn new() -> SensorValues { 40 | SensorValues { 41 | map: 0.0f32, 42 | tps: 20.0f32, 43 | cooltan_temp: 45.69f32, 44 | air_temp: 0.0f32, 45 | batt: 13.42f32, 46 | ext_o2: 0.0f32, 47 | // valores en raw son en bits del ADC; luego se pasan a mV 48 | raw_map: 0.0f32, 49 | raw_tps: 0.0f32, 50 | raw_temp: 0.0f32, 51 | raw_air_temp: 0.0f32, 52 | raw_batt: 0.0f32, 53 | raw_ext_o2: 0.0f32, 54 | } 55 | } 56 | 57 | // TODO: falta calcular cada valor del sensor ademas del low pass 58 | pub fn update(&mut self, raw_value: u16, sensor_type: SensorTypes) { 59 | match sensor_type { 60 | SensorTypes::AirTemp => { 61 | self.raw_air_temp = EMA_LP_ALPHA * (raw_value as f32) 62 | + (1.0 - EMA_LP_ALPHA) * (self.raw_air_temp as f32); 63 | 64 | // Ford ECC-IV type sensor 65 | // TODO: make configurable 66 | // FIXME: esta para el recontra culo la cuenta 67 | let result = 143.73f32 * 0.99936f32.powf(self.raw_air_temp); 68 | 69 | self.air_temp = result; 70 | 71 | } 72 | SensorTypes::CooltanTemp => { 73 | self.raw_temp = EMA_LP_ALPHA * (raw_value as f32) 74 | + (1.0 - EMA_LP_ALPHA) * (self.raw_temp as f32); 75 | } 76 | SensorTypes::MAP => { 77 | self.raw_map = EMA_LP_ALPHA * (raw_value as f32) 78 | + (1.0 - EMA_LP_ALPHA) * (self.raw_map as f32); 79 | } 80 | SensorTypes::TPS => { 81 | self.raw_tps = EMA_LP_ALPHA * (raw_value as f32) 82 | + (1.0 - EMA_LP_ALPHA) * (self.raw_tps as f32); 83 | 84 | // SDUBTUD: tan fácil iba a ser? 85 | let result = (24.6914 * self.raw_tps - 11.11f32).clamp(0f32,100f32); 86 | self.tps = result; 87 | } 88 | SensorTypes::BatteryVoltage => { 89 | self.raw_batt = EMA_LP_ALPHA * (raw_value as f32) 90 | + (1.0 - EMA_LP_ALPHA) * (self.raw_batt as f32); 91 | 92 | self.batt = 14.2; 93 | } 94 | 95 | SensorTypes::ExternalLambda => { 96 | self.ext_o2 = raw_value as f32; 97 | } 98 | } 99 | } 100 | } 101 | 102 | pub fn get_sensor_raw( 103 | sensor_type: SensorTypes, 104 | adc_pins: &mut ADCMapping, 105 | adc: &mut Adc, 106 | ) -> u16 { 107 | let a = sensor_type as u8; 108 | let b = a; 109 | let c = a; 110 | adc_pins.mux_a.set_state(((a & (1 << 0)) != 0).into()); 111 | adc_pins.mux_b.set_state(((b & (1 << 1)) != 0).into()); 112 | adc_pins.mux_c.set_state(((c & (1 << 2)) != 0).into()); 113 | let sample = adc.convert(&adc_pins.analog_in, SampleTime::Cycles_480); 114 | 115 | return adc.sample_to_millivolts(sample); 116 | } 117 | -------------------------------------------------------------------------------- /test/alphan_test.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | 4 | #include "../../Src/injection/injection.hpp" 5 | extern "C" { 6 | #include "stdio.h" 7 | #include "unity_config.h" 8 | } 9 | 10 | void test_AlphaN_air_mass_idle() { 11 | int32_t VE = 40; 12 | float air_mass = injection::AlphaN::get_airmass(VE); 13 | 14 | TEST_ASSERT_EQUAL_FLOAT_MESSAGE(192.1739, air_mass, "Check AlphaN / get_airmass VE = 40 [IDLE]"); 15 | TEST_ASSERT_EQUAL_FLOAT_MESSAGE(72.06522, injection::AlphaN::get_airmass(15), "Check AlphaN / get_airmass VE = 15 [IDLE]"); 16 | TEST_ASSERT_EQUAL_FLOAT_MESSAGE(187.3696, injection::AlphaN::get_airmass(39), "Check AlphaN / get_airmass VE = 39 [IDLE]"); 17 | TEST_ASSERT_EQUAL_FLOAT_MESSAGE(120.1087, injection::AlphaN::get_airmass(25), "Check AlphaN / get_airmass VE = 25 [IDLE]"); 18 | } 19 | 20 | void test_AlphaN_air_mass_full_load() { 21 | int32_t VE = 93; 22 | float air_mass = injection::AlphaN::get_airmass(VE); 23 | 24 | TEST_ASSERT_EQUAL_FLOAT_MESSAGE(446.8044, air_mass, "Check AlphaN / get_airmass VE = 93 [FULL_LOAD]"); 25 | TEST_ASSERT_EQUAL_FLOAT_MESSAGE(432.3913, injection::AlphaN::get_airmass(90), "Check AlphaN / get_airmass VE = 90 [FULL_LOAD]"); 26 | TEST_ASSERT_EQUAL_FLOAT_MESSAGE(408.3696, injection::AlphaN::get_airmass(85), "Check AlphaN / get_airmass VE = 85 [FULL_LOAD]"); 27 | TEST_ASSERT_EQUAL_FLOAT_MESSAGE(360.3261, injection::AlphaN::get_airmass(75), "Check AlphaN / get_airmass VE = 75 [FULL_LOAD]"); 28 | } 29 | 30 | void test_AlphaN_air_mass_full_cruising() { 31 | int32_t VE = 65; 32 | float air_mass = injection::AlphaN::get_airmass(VE); 33 | 34 | TEST_ASSERT_EQUAL_FLOAT_MESSAGE(312.2826, air_mass, "Check AlphaN / get_airmass VE = 60 [CRUISING]"); 35 | TEST_ASSERT_EQUAL_FLOAT_MESSAGE(288.2609, injection::AlphaN::get_airmass(60), "Check AlphaN / get_airmass VE = 60 [FULL_LOAD]"); 36 | TEST_ASSERT_EQUAL_FLOAT_MESSAGE(264.2391, injection::AlphaN::get_airmass(55), "Check AlphaN / get_airmass VE = 55 [FULL_LOAD]"); 37 | TEST_ASSERT_EQUAL_FLOAT_MESSAGE(297.8696, injection::AlphaN::get_airmass(62), "Check AlphaN / get_airmass VE = 62 [FULL_LOAD]"); 38 | } 39 | 40 | void test_AlphaN_fuel_mass_idle() { 41 | float fuel_mass = injection::AlphaN::calculate_injection_fuel(); 42 | 43 | TEST_ASSERT_EQUAL_FLOAT_MESSAGE(3.334963, fuel_mass, "Check AlphaN / calculate_injection_fuel VE = 80 [IDLE]"); 44 | } 45 | 46 | int run_aplha_n_tests() { 47 | UnityBegin("Src/injection/injection.hpp:19"); 48 | table_data mock{ 49 | {0, 600, 750, 1100, 1400, 1700, 2000, 2300, 2600, 2900, 3200, 3400, 3700, 4200, 4400, 4700, 5000}, 50 | {100, 85, 88, 90, 92, 94, 96, 98, 100, 99, 99, 99, 99, 98, 97, 96, 94}, 51 | {95, 82, 85, 87, 89, 91, 93, 95, 96, 96, 96, 96, 96, 95, 94, 93, 91}, 52 | {90, 79, 82, 84, 86, 88, 89, 91, 92, 92, 92, 92, 92, 91, 90, 89, 88}, 53 | {85, 75, 78, 80, 82, 84, 86, 88, 89, 89, 89, 89, 89, 87, 87, 86, 84}, 54 | {75, 68, 72, 74, 76, 77, 79, 81, 82, 82, 82, 82, 81, 80, 80, 79, 78}, 55 | {70, 64, 68, 70, 72, 74, 76, 77, 78, 78, 78, 78, 78, 77, 76, 75, 74}, 56 | {65, 59, 64, 67, 69, 71, 72, 74, 75, 75, 75, 75, 74, 74, 73, 72, 71}, 57 | {60, 55, 61, 63, 66, 67, 69, 70, 71, 71, 71, 71, 71, 70, 70, 69, 67}, 58 | {55, 50, 57, 60, 62, 64, 65, 67, 68, 68, 68, 68, 67, 67, 66, 65, 64}, 59 | {50, 46, 52, 56, 59, 60, 62, 63, 64, 64, 64, 64, 64, 63, 63, 62, 61}, 60 | {45, 42, 48, 52, 55, 57, 58, 60, 60, 60, 60, 60, 60, 60, 59, 58, 57}, 61 | {40, 39, 44, 47, 51, 53, 55, 56, 57, 57, 57, 57, 57, 56, 56, 55, 54}, 62 | {35, 37, 40, 43, 47, 49, 51, 52, 53, 53, 53, 53, 53, 53, 52, 51, 51}, 63 | {30, 36, 37, 40, 43, 45, 47, 49, 50, 50, 50, 50, 50, 49, 49, 48, 47}, 64 | {25, 35, 36, 37, 39, 41, 43, 45, 46, 46, 46, 46, 46, 45, 45, 45, 44}, 65 | {15, 35, 35, 35, 35, 35, 36, 37, 38, 38, 38, 38, 38, 38, 38, 38, 37}, 66 | }; 67 | injection::AlphaN::tps_rpm_ve = mock; 68 | RUN_TEST(test_AlphaN_air_mass_idle); 69 | RUN_TEST(test_AlphaN_air_mass_full_load); 70 | RUN_TEST(test_AlphaN_air_mass_full_cruising); 71 | RUN_TEST(test_AlphaN_fuel_mass_idle); 72 | 73 | return UnityEnd(); 74 | } -------------------------------------------------------------------------------- /test/ignition_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "aliases/testing.hpp" 4 | /* 5 | #include "../Src/ignition/include/ignition.hpp" 6 | #include "../Src/sensors/sensors.hpp" 7 | */ 8 | extern "C" { 9 | #include "trace.h" 10 | #include 11 | #include 12 | } 13 | 14 | 15 | void setup_ignition_mocks() { 16 | table_data mock{ 17 | {0, 42000, 94000, 120000, 140000, 170000, 200000, 230000, 260000, 290000, 320000, 350000, 380000, 410000, 440000, 470000, 750000}, 18 | {2500, 1300, 1000, 1610, 1930, 2260, 2590, 2910, 3240, 3570, 3679, 3679, 3679, 3679, 3679, 3679, 3679}, 19 | {3000, 1300, 1000, 1590, 1910, 2230, 2550, 2880, 3200, 3520, 3629, 3629, 3629, 3629, 3629, 3629, 3629}, 20 | {3500, 1300, 1000, 1570, 1889, 2200, 2520, 2840, 3160, 3479, 3590, 3590, 3590, 3590, 3590, 3590, 3590}, 21 | {4000, 1300, 1000, 1550, 1860, 2180, 2490, 2810, 3120, 3440, 3540, 3540, 3540, 3540, 3540, 3540, 3540}, 22 | {4500, 1300, 1000, 1530, 1839, 2150, 2460, 2770, 3080, 3390, 3500, 3500, 3500, 3500, 3500, 3500, 3500}, 23 | {5000, 1300, 1000, 1510, 1810, 2120, 2430, 2730, 3040, 3350, 3450, 3450, 3450, 3450, 3450, 3450, 3450}, 24 | {5500, 1300, 1000, 1490, 1789, 2090, 2400, 2700, 3000, 3300, 3410, 3410, 3410, 3410, 3410, 3410, 3410}, 25 | {6000, 1300, 1000, 1470, 1770, 2070, 2360, 2660, 2960, 3260, 3360, 3360, 3360, 3360, 3360, 3360, 3360}, 26 | {6600, 1300, 1440, 1440, 1739, 2030, 2330, 2620, 2910, 3210, 3310, 3310, 3310, 3310, 3310, 3310, 3310}, 27 | {7100, 1300, 1000, 1430, 1720, 2000, 2290, 2580, 2870, 3160, 3260, 3260, 3260, 3260, 3260, 3260, 3260}, 28 | {7600, 1300, 1000, 1410, 1689, 1980, 2260, 2550, 2830, 3120, 3220, 3220, 3220, 3220, 3220, 3220, 3220}, 29 | {8100, 1300, 1000, 1390, 1670, 1950, 2230, 2510, 2800, 3080, 3170, 3170, 3170, 3170, 3170, 3170, 3170}, 30 | {8600, 1300, 1000, 1370, 1639, 1920, 2200, 2480, 2760, 3030, 3130, 3130, 3130, 3130, 3130, 3130, 3130}, 31 | {9100, 500, 1000, 1350, 1620, 1889, 2170, 2440, 2720, 2990, 3080, 3080, 3080, 3080, 3080, 3080, 3080}, 32 | {9600, 500, 1000, 1330, 1600, 1870, 2140, 2410, 2680, 2950, 3040, 3040, 3040, 3040, 3040, 3040, 3040}, 33 | {10100, 500, 1000, 1310, 1570, 1839, 2100, 2370, 2640, 2900, 2990, 2990, 2990, 2990, 2990, 2990, 2990}}; 34 | 35 | ignition::avc_tps_rpm.assign(mock.begin(), mock.end()); 36 | } 37 | 38 | void test_idle_advance() { 39 | setup_ignition_mocks(); 40 | 41 | _RPM = 94000; 42 | sensors::values._MAP = 3500; 43 | 44 | ignition::loaded = true; 45 | 46 | ignition::interrupt(); 47 | 48 | TEST_ASSERT_EQUAL_INT32_MESSAGE(1000, _AE, "Check Ignition interrupt, wrong advance value [IDLE Advance]"); 49 | } 50 | 51 | void test_idle_to_acc_advance() { 52 | setup_ignition_mocks(); 53 | 54 | _RPM = 170000; 55 | sensors::values._MAP = 6600; 56 | 57 | ignition::loaded = true; 58 | 59 | ignition::interrupt(); 60 | 61 | TEST_ASSERT_EQUAL_INT32_MESSAGE(2030, _AE, "Check Ignition interrupt, wrong advance value [IDLE to ACC Advance]"); 62 | } 63 | 64 | void test_acc_advance() { 65 | setup_ignition_mocks(); 66 | 67 | _RPM = 94000; 68 | sensors::values._MAP = 6000; 69 | 70 | ignition::loaded = true; 71 | 72 | ignition::interrupt(); 73 | 74 | TEST_ASSERT_EQUAL_INT32_MESSAGE(1000, _AE, "Check Ignition interrupt, wrong advance value [ACC Advance]"); 75 | } 76 | 77 | void test_acc_to_idle_advance() { 78 | setup_ignition_mocks(); 79 | 80 | _RPM = 260000; 81 | sensors::values._MAP = 100; 82 | 83 | ignition::loaded = true; 84 | 85 | ignition::interrupt(); 86 | 87 | TEST_ASSERT_EQUAL_INT32_MESSAGE(3240, _AE, "Check Ignition interrupt, wrong advance value [ACC to IDLE Advance]"); 88 | } 89 | 90 | int runIgnitionTests() { 91 | UnityBegin("Src/ignition/src/ignition.cpp:25"); 92 | RUN_TEST(test_idle_advance); 93 | debug_printf("----------------------- \n\r"); 94 | RUN_TEST(test_idle_to_acc_advance); 95 | debug_printf("----------------------- \n\r"); 96 | RUN_TEST(test_acc_advance); 97 | debug_printf("----------------------- \n\r"); 98 | RUN_TEST(test_acc_to_idle_advance); 99 | 100 | return UnityEnd(); 101 | } -------------------------------------------------------------------------------- /src/app/webserial/handle_realtime_data.rs: -------------------------------------------------------------------------------- 1 | use rtic::Mutex; 2 | 3 | use crate::app::{ 4 | self, 5 | webserial::{SerialCode, SerialMessage, SerialStatus}, 6 | }; 7 | 8 | use crate::app::logging::host; 9 | 10 | fn fmt_data(number: f32) -> i32 { 11 | (number * 100.0) as i32 12 | } 13 | 14 | pub async fn realtime_data_cdc_callback(mut ctx: app::realtime_data_cdc_callback::Context<'_>, serial_cmd: SerialMessage) { 15 | let mut response_buf = SerialMessage { 16 | protocol: 1, 17 | command: serial_cmd.command, 18 | status: 0, 19 | code: 0, 20 | payload: [0u8; 122], 21 | crc: 0, 22 | }; 23 | 24 | let mut number_arr: [u8; 4]; 25 | match serial_cmd.command & 0b00001111 { 26 | // read dashboard Data: 27 | 0x01 => { 28 | host::trace!("read dashboard cfg"); 29 | } 30 | 0x02 => { 31 | // RPM: 32 | number_arr = i32::to_le_bytes(ctx.shared.efi_status.lock(|es| es.rpm)); 33 | response_buf.payload[..2].copy_from_slice(&number_arr[2..]); 34 | 35 | // TPS: 36 | number_arr = i32::to_le_bytes(fmt_data(ctx.shared.sensors.lock(|sns| sns.tps))); 37 | response_buf.payload[2..6].copy_from_slice(&number_arr); 38 | 39 | // MAP: 40 | number_arr = i32::to_le_bytes(fmt_data(ctx.shared.sensors.lock(|sns| sns.map))); 41 | response_buf.payload[6..10].copy_from_slice(&number_arr); 42 | 43 | // TEMP: 44 | number_arr = i32::to_le_bytes(fmt_data(ctx.shared.sensors.lock(|sns| sns.cooltan_temp))); 45 | response_buf.payload[10..14].copy_from_slice(&number_arr); 46 | 47 | // IAT: 48 | number_arr = i32::to_le_bytes(fmt_data(ctx.shared.sensors.lock(|sns| sns.air_temp))); 49 | response_buf.payload[14..18].copy_from_slice(&number_arr); 50 | 51 | // VBAT: 52 | number_arr = i32::to_le_bytes(fmt_data(ctx.shared.sensors.lock(|sns| sns.batt))); 53 | response_buf.payload[18..22].copy_from_slice(&number_arr); 54 | 55 | // // LMB 56 | // number_arr = i32::to_le_bytes(fmt_data(0.998)); //FIXME: get correct lambda value 57 | // response_buf.payload[22..26].copy_from_slice(&number_arr); 58 | // 59 | // 60 | // // 20b to ignition 61 | // // advance 62 | // number_arr = i32::to_le_bytes(fmt_data(23.6)); 63 | // response_buf.payload[60..84].copy_from_slice(&number_arr); 64 | 65 | // 20b to injection: 66 | number_arr = i32::to_le_bytes(fmt_data(ctx.shared.efi_status.lock(|es| es.injection.injection_bank_1_time))); 67 | response_buf.payload[80..84].copy_from_slice(&number_arr); 68 | 69 | // base air 70 | number_arr = i32::to_le_bytes(fmt_data(ctx.shared.efi_status.lock(|es| es.injection.base_air))); 71 | response_buf.payload[84..88].copy_from_slice(&number_arr); 72 | 73 | // base fuel 74 | number_arr = i32::to_le_bytes(fmt_data(ctx.shared.efi_status.lock(|es| es.injection.base_fuel))); 75 | response_buf.payload[88..92].copy_from_slice(&number_arr); 76 | 77 | ctx.shared.crc.lock(|crc| 78 | { 79 | crc.init(); 80 | let crc = crc.update_bytes(&response_buf.payload[..118]); 81 | response_buf.payload[118..122].copy_from_slice(&crc.to_le_bytes()); 82 | }); 83 | 84 | response_buf.status = SerialStatus::Ok as u8; 85 | response_buf.code = 0; 86 | 87 | let _ = ctx.local.real_time_sender.send(response_buf).await; 88 | return; 89 | } 90 | _ => { 91 | response_buf.status = SerialStatus::Error as u8; 92 | response_buf.code = SerialCode::UnknownCmd as u8; 93 | 94 | let _ = ctx.local.real_time_sender.send(response_buf).await; 95 | return; 96 | } 97 | }; 98 | 99 | response_buf.status = SerialStatus::Error as u8; 100 | response_buf.code = SerialCode::ParseError as u8; 101 | 102 | let _ = ctx.local.real_time_sender.send(response_buf).await; 103 | } 104 | -------------------------------------------------------------------------------- /src/app/engine/efi_cfg.rs: -------------------------------------------------------------------------------- 1 | use crate::app::memory::tables::PlotData; 2 | 3 | #[derive(serde::Serialize, serde::Deserialize, Debug, Copy, Clone)] 4 | pub struct VRSensor { 5 | pub trigger_tooth_angle: f32, 6 | pub tooth_count: u32, 7 | pub trigger_filter_time: u32, 8 | pub missing_tooth: u32, 9 | pub trigger_actual_teeth: u32, 10 | pub sync_tooth_count: u32, 11 | pub max_stall_time: u32, 12 | } 13 | 14 | #[derive(serde::Serialize, serde::Deserialize, Debug, Copy, Clone)] 15 | pub struct Engine { 16 | pub cylinder_count: u8, 17 | pub displacement: u32, 18 | pub max_rpm: u32, 19 | pub ckp: VRSensor, 20 | } 21 | 22 | #[derive(serde::Serialize, serde::Deserialize, Debug, Copy, Clone)] 23 | pub struct InjectorConfig { 24 | pub flow_cc_min: f32, 25 | pub injector_count: u8, 26 | pub fuel_pressure: f32, 27 | pub fuel_density: f32, 28 | pub on_time: f32, 29 | pub off_time: f32, 30 | 31 | pub battery_correction: Option, 32 | } 33 | 34 | #[allow(non_snake_case)] 35 | #[derive(serde::Serialize, serde::Deserialize, Debug, Copy, Clone)] 36 | pub struct InjectionConfig { 37 | pub target_lambda: f32, 38 | pub target_stoich: f32, 39 | pub enable_alphaN: bool, 40 | pub enable_speedDensity: bool, 41 | pub injector: InjectorConfig, 42 | } 43 | 44 | #[derive(serde::Serialize, serde::Deserialize, Debug, Copy, Clone)] 45 | pub struct EngineConfig { 46 | pub ready: bool, 47 | pub injection: InjectionConfig, 48 | pub engine: Engine, 49 | } 50 | 51 | impl EngineConfig { 52 | pub fn new() -> EngineConfig { 53 | get_default_efi_cfg() 54 | } 55 | } 56 | 57 | 58 | impl VRSensor { 59 | pub fn new() -> VRSensor { 60 | VRSensor { 61 | trigger_tooth_angle: 0.0, 62 | tooth_count: 0, 63 | trigger_filter_time: 0, 64 | missing_tooth: 0, 65 | trigger_actual_teeth: 0, 66 | sync_tooth_count: 0, 67 | max_stall_time: 0, 68 | } 69 | } 70 | } 71 | 72 | pub fn get_default_efi_cfg() -> EngineConfig { 73 | let mut cfg = EngineConfig { 74 | ready: false, 75 | engine: Engine { 76 | cylinder_count: 4, 77 | displacement: 1596, 78 | max_rpm: 7000, 79 | ckp: VRSensor { 80 | trigger_tooth_angle: 0.0, 81 | tooth_count: 60, 82 | trigger_filter_time: 0, 83 | missing_tooth: 1, 84 | trigger_actual_teeth: 0, 85 | sync_tooth_count: 0, 86 | max_stall_time: 0, 87 | }, 88 | }, 89 | injection: InjectionConfig { 90 | target_lambda: 1.1, 91 | target_stoich: 14.7, 92 | enable_alphaN: true, 93 | enable_speedDensity: false, 94 | injector: InjectorConfig { 95 | flow_cc_min: 110.0, 96 | injector_count: 4, 97 | fuel_pressure: 1.0, 98 | fuel_density: 0.726, 99 | // promedio a "ojimetro" de: 100 | // https://documents.holley.com/techlibrary_terminatorxv2injectordata.pdf 101 | on_time: 750.0, 102 | off_time: 250.0, 103 | // tabla correccion por bateria: 104 | battery_correction: None, 105 | }, 106 | }, 107 | }; 108 | 109 | //The number of physical teeth on the wheel. Doing this here saves us a calculation each time in the interrupt 110 | cfg.engine.ckp.trigger_actual_teeth = cfg.engine.ckp.tooth_count - cfg.engine.ckp.missing_tooth; 111 | 112 | //The number of degrees that passes from tooth to tooth 113 | cfg.engine.ckp.trigger_tooth_angle = 360f32 / cfg.engine.ckp.tooth_count as f32; 114 | 115 | //Trigger filter time is the shortest possible time (in uS) that there can be between crank teeth (ie at max RPM). Any pulses that occur faster than this time will be discarded as noise 116 | cfg.engine.ckp.trigger_filter_time = 1000000 / (cfg.engine.max_rpm / 60 * cfg.engine.ckp.tooth_count); 117 | 118 | //Minimum 50rpm. (3333uS is the time per degree at 50rpm) 119 | cfg.engine.ckp.max_stall_time = (3333f32 * cfg.engine.ckp.trigger_tooth_angle * (cfg.engine.ckp.missing_tooth + 1) as f32) as u32; 120 | 121 | //50% of the total teeth. 122 | cfg.engine.ckp.sync_tooth_count = cfg.engine.ckp.tooth_count / 2; 123 | 124 | return cfg; 125 | } 126 | -------------------------------------------------------------------------------- /src/app/engine/cpwm.rs: -------------------------------------------------------------------------------- 1 | use crate::app::engine::efi_cfg::VRSensor; 2 | 3 | #[derive(Copy, Clone)] 4 | pub struct VRStatus { 5 | pub current_time: u32, 6 | pub current_gap: u32, 7 | pub target_gap: u32, 8 | pub is_missing_tooth: bool, 9 | pub tooth_last_minus_one_tooth_time: u32, 10 | pub tooth_last_time: u32, 11 | // esta se comparte con el loop para detectar stall 12 | pub tooth_current_count: u32, 13 | pub tooth_one_time: u32, 14 | pub tooth_one_minus_one_time: u32, 15 | pub has_sync: bool, 16 | // por ahora es el 'half-sinc' de speeduino 17 | pub sync_loss_counter: u128, 18 | pub start_revolution: u128, 19 | pub last_rpm: u32, 20 | //todo: ver de mover a structs mas peques 21 | pub revolution_time: u32, 22 | pub degreesPeruSx32768: f32, 23 | } 24 | 25 | impl VRStatus { 26 | pub fn new() -> VRStatus { 27 | return VRStatus { 28 | current_time: 0, 29 | current_gap: 0, 30 | target_gap: 0, 31 | is_missing_tooth: false, 32 | tooth_last_minus_one_tooth_time: 0, 33 | tooth_last_time: 0, 34 | tooth_current_count: 0, 35 | tooth_one_time: 0, 36 | tooth_one_minus_one_time: 0, 37 | has_sync: false, 38 | sync_loss_counter: 0, 39 | start_revolution: 0, 40 | last_rpm: 0, 41 | revolution_time: 0, 42 | degreesPeruSx32768: 0.0, 43 | }; 44 | } 45 | 46 | pub fn reset(&mut self) { 47 | self.current_time = 0; 48 | self.current_gap = 0; 49 | self.target_gap = 0; 50 | self.is_missing_tooth = false; 51 | self.tooth_last_minus_one_tooth_time = 0; 52 | self.tooth_last_time = 0; 53 | self.tooth_current_count = 0; 54 | self.tooth_one_time = 0; 55 | self.tooth_one_minus_one_time = 0; 56 | self.has_sync = false; 57 | self.sync_loss_counter = 0; 58 | self.start_revolution = 0; 59 | self.last_rpm = 0; 60 | self.revolution_time = 0; 61 | self.degreesPeruSx32768 = 0.0; 62 | } 63 | } 64 | 65 | pub fn get_cranking_rpm(trigger: &mut VRStatus, trigger_config: &VRSensor) -> u32 { 66 | let mut temp_rpm = 0; 67 | 68 | // rev desde el arranque para habilitar el encendido (para estabilizar todo el chisme) 69 | if /* trigger.start_revolution >= 50 && */ trigger.has_sync { 70 | if (trigger.tooth_last_time > 0) && (trigger.tooth_last_minus_one_tooth_time > 0) && (trigger.tooth_last_time > trigger.tooth_last_minus_one_tooth_time) { 71 | let mut revolution_time = (trigger.tooth_last_time - trigger.tooth_last_minus_one_tooth_time) * trigger_config.tooth_count; 72 | //TODO: Make configurable (720/360/etc) 73 | if 720 == 720 { 74 | revolution_time = revolution_time / 2; 75 | } 76 | // us in minute 77 | temp_rpm = 60_000_000 / revolution_time; 78 | // filtrado por ruido (18k max rpm) 79 | if temp_rpm >= 18_000 { 80 | temp_rpm = trigger.last_rpm; 81 | } 82 | trigger.revolution_time = revolution_time; 83 | } 84 | } else { return 0; } 85 | trigger.last_rpm = temp_rpm; 86 | 87 | return temp_rpm; 88 | } 89 | 90 | pub fn angle_to_time(trigger: &VRStatus, angle: &u32) -> i32 { 91 | // por ahora la estimacion de tiempo mas simple 92 | return ((*angle * trigger.revolution_time) / 360) as i32; 93 | } 94 | 95 | pub fn time_to_angle(trigger: &VRStatus, time: &u32) -> i32 { 96 | // por ahora la estimacion de tiempo mas simple 97 | return (*time as f32 * trigger.degreesPeruSx32768) as i32 / 32768; 98 | } 99 | 100 | pub fn get_crank_angle(trigger: &VRStatus, trigger_config: &VRSensor, cpu_tick: u32) -> i32 { 101 | const CRANK_ANGLE_MAX: i32 = 360; 102 | 103 | //Number of teeth that have passed since tooth 1, multiplied by the angle each tooth represents, plus the angle that tooth 1 is ATDC. This gives accuracy only to the nearest tooth. 104 | let mut crank_angle = ((trigger.tooth_current_count - 1) as f32 * trigger_config.trigger_tooth_angle) as i32 + 20; 105 | 106 | let elapsed_time = cpu_tick - trigger.tooth_last_time; 107 | crank_angle += time_to_angle(&trigger, &elapsed_time); 108 | 109 | if crank_angle >= 720 { 110 | crank_angle -= 720; 111 | } else if crank_angle > CRANK_ANGLE_MAX { 112 | crank_angle -= CRANK_ANGLE_MAX; 113 | } 114 | 115 | if crank_angle < 0 { 116 | crank_angle += CRANK_ANGLE_MAX; 117 | } 118 | 119 | return crank_angle; 120 | } -------------------------------------------------------------------------------- /src/app/webserial/mod.rs: -------------------------------------------------------------------------------- 1 | use arrayvec::ArrayVec; 2 | use rtic::Mutex; 3 | use rtic_sync::channel::Sender; 4 | use usb_device::{ 5 | bus::{UsbBus, UsbBusAllocator}, 6 | device::{UsbDevice, UsbDeviceBuilder, UsbVidPid}, 7 | }; 8 | 9 | use crate::{ 10 | app, 11 | app::util, 12 | }; 13 | 14 | pub mod handle_core; 15 | pub mod handle_tables; 16 | pub mod handle_pmic; 17 | pub mod handle_engine; 18 | pub mod handle_realtime_data; 19 | 20 | #[repr(C, packed)] 21 | #[derive(Debug, Copy, Clone)] 22 | pub struct SerialMessage { 23 | pub protocol: u8, 24 | pub command: u8, 25 | pub status: u8, 26 | pub code: u8, 27 | pub payload: [u8; 122], 28 | pub crc: u16, 29 | } 30 | 31 | #[derive(Debug)] 32 | #[repr(u8)] 33 | pub enum SerialStatus { 34 | Error = 0b0000_0000, 35 | Ok = 0b0100_0000, 36 | UploadOk = 0x7c, 37 | DataChunk = 0x7d, 38 | DataChunkEnd = 0x7e, 39 | } 40 | 41 | #[repr(u8)] 42 | pub enum SerialCode { 43 | None = 0x00, 44 | 45 | FWRequestBootloader = 0x01, 46 | FWRebootUnsafe = 0x03, 47 | 48 | ParseError = 0x5f, 49 | UnknownCmd = 0x7f, 50 | 51 | // Tables 52 | UnknownTable = 51, 53 | TableNotLoaded = 50, 54 | TableCrcError = 52, 55 | 56 | // PMIC: 57 | RequestFastStatus = 60, 58 | RequestIgnitionStatus = 61, 59 | RequestInjectionStatus = 62, 60 | } 61 | 62 | pub fn new_device(bus: &UsbBusAllocator) -> UsbDevice<'_, B> 63 | where 64 | B: UsbBus, 65 | { 66 | UsbDeviceBuilder::new(bus, UsbVidPid(0x1209, 0xeef1)) 67 | .manufacturer("Churrosoft") 68 | .product("OpenEFI | uEFI v3.4.x") 69 | .serial_number(util::get_serial_str()) 70 | .device_release(0x0200) 71 | .self_powered(false) 72 | .max_power(250) 73 | .max_packet_size_0(64) 74 | .build() 75 | } 76 | 77 | pub fn process_command(buf: [u8; 128] , mut webserial_sender: &mut Sender) { 78 | let mut payload = [0u8; 122]; 79 | payload.copy_from_slice(&buf[4..126]); 80 | 81 | let crc = ((buf[126] as u16) << 8) | (buf[127] as u16); 82 | 83 | let mut serial_cmd = SerialMessage { 84 | protocol: buf[0], 85 | status: buf[1], 86 | code: buf[2], 87 | command: buf[3], 88 | payload, 89 | crc, 90 | }; 91 | 92 | // logging::host::debug!( 93 | // "CDC Message:\n - Proto {}\n - Commd {}\n - Statu {}\n - Code {}\n - CRC: {}", 94 | // serial_cmd.protocol, 95 | // serial_cmd.command, 96 | // serial_cmd.status, 97 | // serial_cmd.code, 98 | // crc 99 | // ); 100 | 101 | if serial_cmd.protocol != 1 { 102 | return; 103 | } 104 | 105 | match serial_cmd.command & 0xf0 { 106 | 0x00 => handle_core::handler(serial_cmd, webserial_sender), 107 | 0x10 => app::table_cdc_callback::spawn(serial_cmd).unwrap(), 108 | 0x20 => { /* TODO: injection */ } 109 | 0x30 => { /* TODO: ignition */ } 110 | 0x40 => { /* TODO: DTC */ } 111 | 0x50 => app::realtime_data_cdc_callback::spawn(serial_cmd).unwrap(), 112 | // 0x60 => app::engine_cdc_callback::spawn(serial_cmd).unwrap(), 113 | 0x70 => { /* TODO: Debug console */ } 114 | 0x80 => app::pmic_cdc_callback::spawn(serial_cmd).unwrap(), 115 | //0x90 => app::debug_demo::spawn(serial_cmd.command & 0b00001111).unwrap(), 116 | _ => { 117 | webserial_sender.try_send(serial_cmd).ok(); 118 | //app::send_message::spawn(SerialStatus::Error ,SerialCode::UnknownCmd as u8,serial_cmd).unwrap(); 119 | } 120 | } 121 | } 122 | 123 | pub fn finish_message(message: SerialMessage) -> [u8; 128] { 124 | let mut message_buf = ArrayVec::::new(); 125 | message_buf.push(message.protocol); 126 | message_buf.push(message.status); 127 | message_buf.push(message.code); 128 | message_buf.push(message.command); 129 | message_buf.try_extend_from_slice(&message.payload).unwrap(); 130 | // Add empty CRC 131 | message_buf.push(0); 132 | message_buf.push(0); 133 | 134 | let payload: [u8; 126] = message_buf.take().into_inner().unwrap()[0..126] 135 | .try_into() 136 | .unwrap(); 137 | let crc = util::crc16(&payload, 126); 138 | 139 | message_buf.clear(); 140 | message_buf.try_extend_from_slice(&payload).unwrap(); 141 | message_buf 142 | .try_extend_from_slice(&crc.to_be_bytes()) 143 | .unwrap(); 144 | 145 | message_buf.take().into_inner().unwrap() 146 | } 147 | 148 | // Send a message via web serial. 149 | pub(crate) async fn send_message( 150 | mut ctx: app::send_message::Context<'_>, 151 | status: SerialStatus, 152 | code: u8, 153 | mut message: SerialMessage, 154 | ) { 155 | message.status = status as u8; 156 | message.code = code; 157 | ctx.shared.usb_cdc.lock(|cdc| { 158 | cdc.write(&finish_message(message)).unwrap(); 159 | }); 160 | } 161 | -------------------------------------------------------------------------------- /src/app/memory/tables.rs: -------------------------------------------------------------------------------- 1 | #![allow(dead_code)] 2 | 3 | use shared_bus_rtic::SharedBus; 4 | use stm32f4xx_hal::{ 5 | crc32::Crc32, 6 | gpio::{Alternate, Output, Pin}, 7 | pac::SPI2, 8 | spi::Spi, 9 | }; 10 | use w25q::series25::{Flash, FlashInfo}; 11 | 12 | use crate::app::logging::host; 13 | 14 | pub type DataT = [[i32; 17]; 17]; 15 | pub type PlotData = [[i32; 2]; 10]; 16 | 17 | pub struct Tables { 18 | // injection 19 | pub tps_rpm_ve: Option, 20 | pub tps_rpm_afr: Option, 21 | pub injector_delay: Option, 22 | pub vbat_correction: Option, 23 | pub wue: Option, 24 | pub ase_taper: Option, 25 | pub ase_intensity: Option, 26 | //ignition 27 | pub load_tps_deg: Option, 28 | } 29 | 30 | pub struct TableData { 31 | pub data: Option, 32 | pub crc: u32, 33 | pub address: u32, 34 | pub max_x: u16, 35 | pub max_y: u16, 36 | } 37 | 38 | 39 | pub type SpiT = Spi< 40 | SPI2, 41 | ( 42 | Pin<'B', 10, Alternate<5>>, 43 | Pin<'B', 14, Alternate<5>>, 44 | Pin<'B', 15, Alternate<5>>, 45 | ), 46 | false, 47 | >; 48 | 49 | pub type FlashT = Flash, Pin<'E', 13, Output>>; 50 | 51 | impl TableData { 52 | pub fn read_from_memory( 53 | &mut self, 54 | flash: &mut FlashT, 55 | fi: &FlashInfo, 56 | crc: &mut Crc32, 57 | ) -> Option { 58 | let mut vv_32 = [[0i32; 17]; 17]; 59 | let read_address = fi.sector_to_page(&self.address) * (fi.page_size as u32); 60 | let mut datarow = 4; 61 | let mut buf: [u8; 4 * 17 * 17 + 4] = [0; 4 * 17 * 17 + 4]; 62 | 63 | { 64 | flash.read(read_address, &mut buf).unwrap(); 65 | } 66 | for matrix_y in 0..self.max_y { 67 | for matrix_x in 0..self.max_x { 68 | let u8buff = [ 69 | buf[datarow], 70 | buf[datarow + 1], 71 | buf[datarow + 2], 72 | buf[datarow + 3], 73 | ]; 74 | 75 | let value = i32::from_le_bytes(u8buff); 76 | vv_32[matrix_y as usize][matrix_x as usize] = value; 77 | 78 | datarow += 4; 79 | } 80 | } 81 | // CRC Check 82 | let u8buff = [buf[0], buf[1], buf[2], buf[3]]; 83 | let memory_crc = u32::from_le_bytes(u8buff); 84 | 85 | crc.init(); 86 | let calculated_crc = crc.update_bytes(&buf[4..]); 87 | 88 | if memory_crc != calculated_crc { 89 | host::debug!("Checksum tablas no coinciden {:?} {:?}", memory_crc,calculated_crc) 90 | } 91 | 92 | 93 | { 94 | self.crc = calculated_crc; 95 | self.data = Some(vv_32); 96 | } 97 | 98 | return Some(vv_32); 99 | } 100 | 101 | pub fn write_to_memory(&self, flash: &mut FlashT, fi: &FlashInfo, crc: &mut Crc32) { 102 | let mut buf: [u8; 4 * 17 * 17 + 4] = [0; 4 * 17 * 17 + 4]; 103 | let mut datarow = 4; 104 | 105 | for matrix_y in 0..self.max_y { 106 | for matrix_x in 0..self.max_x { 107 | let value_arr: [u8; 4] = 108 | i32::to_le_bytes(self.data.unwrap()[matrix_y as usize][matrix_x as usize]); 109 | buf[datarow] = value_arr[0]; 110 | buf[datarow + 1] = value_arr[1]; 111 | buf[datarow + 2] = value_arr[2]; 112 | buf[datarow + 3] = value_arr[3]; 113 | 114 | datarow += 4; 115 | } 116 | } 117 | 118 | crc.init(); 119 | let calculated_crc = crc.update_bytes(&buf[4..]); 120 | let crc_arr: [u8; 4] = u32::to_le_bytes(calculated_crc); 121 | 122 | buf[0] = crc_arr[0]; 123 | buf[1] = crc_arr[1]; 124 | buf[2] = crc_arr[2]; 125 | buf[3] = crc_arr[3]; 126 | 127 | { 128 | let write_address = fi.sector_to_page(&self.address) * (fi.page_size as u32); 129 | flash.erase_sectors(write_address, 1).unwrap(); 130 | flash.write_bytes(write_address, &mut buf).unwrap(); 131 | } 132 | } 133 | 134 | pub fn validate(&self, crc: &mut Crc32, crc_val: u32) -> bool { 135 | let mut buf: [u8; 4 * 17 * 17] = [0; 4 * 17 * 17]; 136 | let mut datarow = 0; 137 | 138 | for matrix_y in 0..self.max_y { 139 | for matrix_x in 0..self.max_x { 140 | let value_arr: [u8; 4] = 141 | i32::to_le_bytes(self.data.unwrap()[matrix_y as usize][matrix_x as usize]); 142 | buf[datarow] = value_arr[0]; 143 | buf[datarow + 1] = value_arr[1]; 144 | buf[datarow + 2] = value_arr[2]; 145 | buf[datarow + 3] = value_arr[3]; 146 | 147 | datarow += 4; 148 | } 149 | } 150 | 151 | { 152 | crc.init(); 153 | let calculated_crc = crc.update_bytes(&buf); 154 | return calculated_crc == crc_val; 155 | } 156 | } 157 | 158 | pub fn clear(&mut self, flash: &mut FlashT, fi: &FlashInfo, crc: &mut Crc32 ) { 159 | let mut buf: [u8; 4 * 17 * 17 + 4] = [0; 4 * 17 * 17 + 4]; 160 | 161 | crc.init(); 162 | let calculated_crc = crc.update_bytes(&buf[4..]); 163 | let crc_arr: [u8; 4] = u32::to_le_bytes(calculated_crc); 164 | 165 | buf[0] = crc_arr[0]; 166 | buf[1] = crc_arr[1]; 167 | buf[2] = crc_arr[2]; 168 | buf[3] = crc_arr[3]; 169 | 170 | { 171 | let write_address = fi.sector_to_page(&self.address) * (fi.page_size as u32); 172 | flash.erase_sectors(write_address, 1).unwrap(); 173 | flash.write_bytes(write_address, &mut buf).unwrap(); 174 | } 175 | } 176 | 177 | pub fn on_bounds(&self, x: i16, y: i16) -> bool { 178 | if x > (self.max_x as i16) || y > (self.max_y as i16) { 179 | false; 180 | } 181 | 182 | if x < 0 || y < 0 { 183 | false; 184 | } 185 | 186 | return true; 187 | } 188 | } 189 | -------------------------------------------------------------------------------- /src/app/engine/pmic.rs: -------------------------------------------------------------------------------- 1 | use embedded_hal::blocking::spi::Transfer; 2 | use embedded_hal::digital::v2::OutputPin; 3 | use shared_bus_rtic::SharedBus; 4 | use stm32f4xx_hal::gpio; 5 | use stm32f4xx_hal::gpio::{Output, PushPull}; 6 | 7 | use crate::app::engine::error::Error; 8 | use crate::app::gpio::ISPI; 9 | use serde::{ Serialize}; 10 | pub type PmicT = PMIC, gpio::PB11>>; 11 | 12 | #[derive(Serialize)] 13 | #[repr(u8)] 14 | pub enum Commands { 15 | ReadRegisters = 0b0000_1010, 16 | AllStatus, 17 | SPICheck, 18 | ModeSetIgn, 19 | ModeSetGp, 20 | DriverEnable, 21 | ClockCalibration, 22 | } 23 | 24 | #[derive(Serialize)] 25 | #[repr(u8)] 26 | pub enum Registers { 27 | AllStatus = 0x0, 28 | Out01Fault = 0b0001_0000, 29 | Out23Fault = 0b0010_0000, 30 | GPGDModeFault = 0b0011_0000, 31 | IGNModeFault = 0b0100_0000, 32 | ModeCommand = 0b0101_000, 33 | LSDFault = 0b0110_0000, 34 | DRVREnable = 0b0111_0000, 35 | StartSparkFilter = 0b1000_0000, 36 | EndSparkFilter = 0b1001_0000, 37 | DACRegister = 0b1010_0000, 38 | } 39 | 40 | #[derive(Serialize,Debug)] 41 | #[repr(u8)] 42 | pub enum InjectorStatus { 43 | Ok = 0x00, 44 | GenericError, 45 | TLIMFault, 46 | BatteryShortFault, 47 | OffOpenFault, 48 | OnOpenFault, 49 | CORError, 50 | } 51 | 52 | #[derive(Serialize,Debug)] 53 | #[repr(u8)] 54 | pub enum CoilStatus { 55 | Ok, 56 | GenericError, 57 | MAXIFault, 58 | MaxDwellFault, 59 | OpenSecondaryFault, 60 | } 61 | 62 | #[derive(Serialize, Debug)] 63 | pub struct FullStatus { 64 | pub cor: bool, 65 | pub over_voltage: bool, 66 | pub under_voltage: bool, 67 | pub cil_1: T, 68 | pub cil_2: T, 69 | pub cil_3: T, 70 | pub cil_4: T, 71 | } 72 | 73 | #[derive(Serialize, Debug)] 74 | pub struct FastCil { 75 | pub cil_2: T, 76 | pub cil_3: T, 77 | pub cil_4: T, 78 | pub cil_1: T, 79 | } 80 | 81 | #[derive(Serialize, Debug)] 82 | pub struct FastStatus { 83 | pub cor: bool, 84 | pub over_voltage: bool, 85 | pub under_voltage: bool, 86 | pub ign: FastCil, 87 | pub inj: FastCil, 88 | } 89 | 90 | #[derive(Debug)] 91 | pub struct PMIC { 92 | spi: SPI, 93 | cs: CS, 94 | } 95 | 96 | fn match_injector_status(data: u8, shift: u8) -> InjectorStatus { 97 | if (data >> 3 + shift) & 1 != 0 { return InjectorStatus::TLIMFault; } 98 | if (data >> 2 + shift) & 1 != 0 { return InjectorStatus::BatteryShortFault; } 99 | if (data >> 1 + shift) & 1 != 0 { return InjectorStatus::OffOpenFault; } 100 | if (data >> 0 + shift) & 1 != 0 { return InjectorStatus::OnOpenFault; } 101 | 102 | return InjectorStatus::Ok; 103 | } 104 | 105 | fn match_ignition_status(data: u16, shift: u8) -> CoilStatus { 106 | if (data >> 2 + shift) & 1 != 0 { return CoilStatus::MAXIFault; } 107 | if (data >> 1 + shift) & 1 != 0 { return CoilStatus::MaxDwellFault; } 108 | if (data >> 0 + shift) & 1 != 0 { return CoilStatus::OpenSecondaryFault; } 109 | 110 | return CoilStatus::Ok; 111 | } 112 | 113 | impl, CS: OutputPin> PMIC { 114 | pub fn init(spi: SPI, cs: CS) -> Result> { 115 | let this = Self { cs, spi }; 116 | 117 | //TODO: usar el comando de "check" para revisar que este vivo el PMIC antes de iniciar la lib 118 | 119 | Ok(this) 120 | } 121 | 122 | //TODO: evaluar si hace falta el delay entre comando y payload 123 | fn send_command(&mut self, command: Commands, payload: u8) -> [u8; 2] { 124 | let mut mock_word = [0xf, 0x0]; 125 | let mut result_word = [0xf, 0x0]; 126 | let mut spi_payload = [command as u8, payload]; 127 | 128 | let _ = self.cs.set_low(); 129 | let _ = self.spi.transfer(&mut spi_payload); 130 | let result_data = self.spi.transfer(&mut mock_word); 131 | let _ = self.cs.set_high(); 132 | 133 | //FIXME: esto le genera infartos a mas de un catolico de rust 134 | // https://doc.rust-lang.org/std/result/#extracting-contained-values 135 | unsafe { 136 | result_word[0] = result_data.as_ref().unwrap_unchecked()[0]; 137 | result_word[1] = result_data.as_ref().unwrap_unchecked()[1]; 138 | } 139 | 140 | return result_word; 141 | } 142 | 143 | 144 | pub fn get_fast_status(&mut self) -> FastStatus { 145 | let result = self.send_command(Commands::ReadRegisters, Registers::AllStatus as u8); 146 | 147 | return FastStatus { 148 | cor: (result[1] >> 7) & 1 != 0, 149 | over_voltage: (result[1] >> 6) & 1 != 0, 150 | under_voltage: (result[1] >> 5) & 1 != 0, 151 | ign: FastCil { 152 | cil_1: if (result[1] >> 0) & 1 != 0 { CoilStatus::GenericError } else { CoilStatus::Ok }, 153 | cil_2: if (result[1] >> 1) & 1 != 0 { CoilStatus::GenericError } else { CoilStatus::Ok }, 154 | cil_3: if (result[1] >> 2) & 1 != 0 { CoilStatus::GenericError } else { CoilStatus::Ok }, 155 | cil_4: if (result[1] >> 3) & 1 != 0 { CoilStatus::GenericError } else { CoilStatus::Ok }, 156 | }, 157 | inj: FastCil { 158 | cil_1: if (result[0] >> 0) & 1 != 0 { InjectorStatus::GenericError } else { InjectorStatus::Ok }, 159 | cil_2: if (result[0] >> 1) & 1 != 0 { InjectorStatus::GenericError } else { InjectorStatus::Ok }, 160 | cil_3: if (result[0] >> 2) & 1 != 0 { InjectorStatus::GenericError } else { InjectorStatus::Ok }, 161 | cil_4: if (result[0] >> 3) & 1 != 0 { InjectorStatus::GenericError } else { InjectorStatus::Ok }, 162 | }, 163 | }; 164 | } 165 | 166 | pub fn get_injector_status(&mut self) -> FullStatus { 167 | let bank_a_status = self.send_command(Commands::ReadRegisters, Registers::Out01Fault as u8); 168 | let bank_b_status = self.send_command(Commands::ReadRegisters, Registers::Out23Fault as u8); 169 | 170 | let cil_1 = match_injector_status(bank_a_status[0], 0); 171 | let cil_2 = match_injector_status(bank_a_status[0], 4); 172 | let cil_3 = match_injector_status(bank_b_status[0], 0); 173 | let cil_4 = match_injector_status(bank_b_status[0], 4); 174 | 175 | // TODO: check COR/Over Voltage/Under voltage 176 | 177 | return FullStatus { 178 | cor: false, 179 | over_voltage: false, 180 | under_voltage: false, 181 | cil_1, 182 | cil_2, 183 | cil_3, 184 | cil_4, 185 | }; 186 | } 187 | 188 | pub fn get_ignition_status(&mut self) -> FullStatus { 189 | let status = self.send_command(Commands::ReadRegisters, Registers::IGNModeFault as u8); 190 | 191 | let data = u16::from_le_bytes(status); 192 | 193 | let cil_1 = match_ignition_status(data, 0); 194 | let cil_2 = match_ignition_status(data, 3); 195 | let cil_3 = match_ignition_status(data, 6); 196 | let cil_4 = match_ignition_status(data, 9); 197 | 198 | return FullStatus { 199 | cor: false, 200 | over_voltage: false, 201 | under_voltage: false, 202 | cil_1, 203 | cil_2, 204 | cil_3, 205 | cil_4, 206 | }; 207 | } 208 | } -------------------------------------------------------------------------------- /src/app/tasks/engine.rs: -------------------------------------------------------------------------------- 1 | // use fugit::Duration; 2 | use rtic::Mutex; 3 | use rtic::mutex_prelude::{TupleExt03, TupleExt04}; 4 | use stm32f4xx_hal::gpio::ExtiPin; 5 | use rtic_monotonics::systick::*; 6 | use crate::{ 7 | app, 8 | }; 9 | use crate::app::engine::cpwm::{angle_to_time, get_crank_angle}; 10 | 11 | use crate::app::engine::efi_cfg::VRSensor; 12 | 13 | // from: https://github.com/noisymime/speeduino/blob/master/speeduino/decoders.ino#L453 14 | pub(crate) fn ckp_trigger(mut ctx: app::ckp_trigger::Context) { 15 | let mut efi_cfg = ctx.shared.efi_cfg; 16 | let mut efi_status = ctx.shared.efi_status; 17 | 18 | let mut ckp_status = ctx.shared.ckp; 19 | 20 | let mut ckp = VRSensor::new(); 21 | let mut rpm = 0; 22 | 23 | // lock previo y libero los recursos que no se vuelven a usar para otra cosa aca adentro 24 | efi_cfg.lock(|cfg| { ckp = cfg.engine.ckp }); 25 | efi_status.lock(|status| { rpm = status.rpm }); 26 | 27 | ckp_status.lock(|ckp_status| { 28 | ctx.shared.timer4.lock(|t4| { ckp_status.current_time = t4.now().ticks(); }); 29 | ckp_status.current_gap = ckp_status.current_time - ckp_status.tooth_last_time; 30 | 31 | if ckp_status.current_gap >= ckp.trigger_filter_time { 32 | ckp_status.tooth_current_count += 1; 33 | 34 | 35 | if ckp_status.tooth_last_time > 0 && ckp_status.tooth_last_minus_one_tooth_time > 0 { 36 | ckp_status.is_missing_tooth = false; 37 | 38 | /* 39 | Performance Optimisation: 40 | Only need to try and detect the missing tooth if: 41 | 1. WE don't have sync yet 42 | 2. We have sync and are in the final 1/4 of the wheel (Missing tooth will/should never occur in the first 3/4) 43 | 3. RPM is under 2000. This is to ensure that we don't interfere with strange timing when cranking or idling. Optimisation not really required at these speeds anyway 44 | */ 45 | if ckp_status.has_sync == false || rpm < 2000 || ckp_status.tooth_current_count >= (3 * ckp.trigger_actual_teeth >> 2) { 46 | //Begin the missing tooth detection 47 | //If the time between the current tooth and the last is greater than 1.5x the time between the last tooth and the tooth before that, we make the assertion that we must be at the first tooth after the gap 48 | if ckp.missing_tooth == 1 { 49 | //Multiply by 1.5 (Checks for a gap 1.5x greater than the last one) (Uses bitshift to multiply by 3 then divide by 2. Much faster than multiplying by 1.5) 50 | ckp_status.target_gap = (3 * (ckp_status.tooth_last_time - ckp_status.tooth_last_minus_one_tooth_time)) >> 1; 51 | } else { 52 | //Multiply by 2 (Checks for a gap 2x greater than the last one) 53 | ckp_status.target_gap = (ckp_status.tooth_last_time - ckp_status.tooth_last_minus_one_tooth_time) * ckp.missing_tooth; 54 | } 55 | 56 | // initial startup, missing one time 57 | if ckp_status.tooth_last_time == 0 || ckp_status.tooth_last_minus_one_tooth_time == 0 { 58 | ckp_status.target_gap = 0; 59 | } 60 | 61 | if (ckp_status.current_gap > ckp_status.target_gap) || (ckp_status.tooth_current_count > ckp.trigger_actual_teeth) { 62 | //Missing tooth detected 63 | ckp_status.is_missing_tooth = true; 64 | 65 | if ckp_status.tooth_current_count < ckp.trigger_actual_teeth { 66 | // This occurs when we're at tooth #1, but haven't seen all the other teeth. This indicates a signal issue so we flag lost sync so this will attempt to resync on the next revolution. 67 | ckp_status.has_sync = false; 68 | ckp_status.sync_loss_counter += 1; 69 | } 70 | //This is to handle a special case on startup where sync can be obtained and the system immediately thinks the revs have jumped: 71 | else { 72 | if ckp_status.has_sync { 73 | ctx.shared.led.lock(|l| { l.led_check.toggle() }); 74 | ckp_status.start_revolution += 1; 75 | } else { 76 | ckp_status.start_revolution = 0; 77 | ctx.shared.led.lock(|l| { l.led_mil.toggle() }); 78 | } 79 | 80 | ckp_status.tooth_current_count = 1; 81 | 82 | // tiempo entre vuelta completa 83 | ckp_status.tooth_one_minus_one_time = ckp_status.tooth_one_time; 84 | ckp_status.tooth_one_time = ckp_status.current_time; 85 | 86 | // TODO: hay mas checks aca cuando es con inyección secuencial 87 | ckp_status.has_sync = true; 88 | //This is used to prevent a condition where serious intermittent signals (Eg someone furiously plugging the sensor wire in and out) can leave the filter in an unrecoverable state 89 | efi_cfg.lock(|ec| { ec.engine.ckp.trigger_filter_time = 0; }); 90 | ckp_status.tooth_last_minus_one_tooth_time = ckp_status.tooth_last_time; 91 | ckp_status.tooth_last_time = ckp_status.current_time; 92 | // ctx.shared.ckp_tooth_last_time.lock(|ckp_t| { *ckp_t = ckp_status.current_time }); 93 | } 94 | } 95 | } 96 | 97 | if !ckp_status.is_missing_tooth { 98 | efi_cfg.lock(|ec| { ec.engine.ckp.trigger_filter_time = ckp_status.current_gap >> 2; }); 99 | ckp_status.tooth_last_minus_one_tooth_time = 0; 100 | } 101 | } else { 102 | // initial startup 103 | ckp_status.tooth_last_minus_one_tooth_time = ckp_status.tooth_last_time; 104 | ckp_status.tooth_last_time = ckp_status.current_time; 105 | // ctx.shared.ckp_tooth_last_time.lock(|ckp_t| { *ckp_t = ckp_status.current_time }); 106 | } 107 | } 108 | }); 109 | // Obtain access to the peripheral and Clear Interrupt Pending Flag 110 | ctx.local.ckp.clear_interrupt_pending_bit(); 111 | } 112 | 113 | // TODO: add similar stall control of speeduino 114 | // https://github.com/noisymime/speeduino/blob/master/speeduino/speeduino.ino#L146 115 | pub(crate) async fn ckp_checks(mut ctx: app::ckp_checks::Context<'_>) { 116 | 117 | // FUTURE: esto a futuro no tendria que usar locks de acuerdo a esto, 118 | // ya que solo escribo en "ckp", mientras que los otros son solo de lectura por ahora 119 | // https://rtic.rs/2/book/en/by-example/resources.html#lock-free-access-of-shared-resources 120 | let efi_cfg = ctx.shared.efi_cfg; 121 | let mut ckp = ctx.shared.ckp; 122 | let mut efi_status = ctx.shared.efi_status; 123 | let mut cycle_time = 0; 124 | 125 | let mut ignition_running = ctx.shared.ignition_running; 126 | 127 | 128 | ctx.shared.timer4.lock(|t4| { cycle_time = t4.now().ticks(); }); 129 | 130 | (efi_cfg, ckp, efi_status,ignition_running).lock(|cfg, ckp, efi_status,ignition_running| { 131 | //ste hijodeputa fue por lo que se tosto la bobina 132 | if ckp.tooth_last_time > cycle_time || cycle_time - ckp.tooth_last_time < 366_667 /* 50RPM */ { 133 | // RPM & no stall 134 | // en speeduino revisan "BIT_DECODER_TOOTH_ANG_CORRECT" aca, por ahora no lo agregue al trigger 135 | // tambien utilizan rpmDOT para ver la variacion cada 100mS, falta implementar 136 | // TODO: mover a fun aparte 137 | 138 | let mut time_per_degreex16; 139 | 140 | // esto calcula el tiempo por grado desde el tiempo entre los ultimos 2 dientes 141 | if true /* BIT_DECODER_TOOTH_ANG_CORRECT*/ && (ckp.tooth_last_time > ckp.tooth_last_minus_one_tooth_time) && 20/*(abs(currentStatus.rpmDOT)*/ > 30 { 142 | time_per_degreex16 = ((ckp.tooth_last_time - ckp.tooth_last_minus_one_tooth_time) * 16) / cfg.engine.ckp.trigger_tooth_angle as u32; 143 | // timePerDegree = time_per_degreex16 / 16; 144 | } else { 145 | //Take into account any likely acceleration that has occurred since the last full revolution completed: 146 | //long rpm_adjust = (timeThisRevolution * (long)currentStatus.rpmDOT) / 1000000; 147 | let rpm_adjust = 0; 148 | time_per_degreex16 = (2_666_656 / efi_status.rpm + rpm_adjust) as u32; //The use of a x16 value gives accuracy down to 0.1 of a degree and can provide noticeably better timing results on low resolution triggers 149 | // timePerDegree = time_per_degreex16 / 16; 150 | } 151 | 152 | // ckp.degreesPeruSx2048 = 2048 / timePerDegree; 153 | ckp.degreesPeruSx32768 = (524288 / time_per_degreex16) as f32; 154 | } else { 155 | ckp.reset(); 156 | } 157 | cfg.engine.ckp.max_stall_time; 158 | }); 159 | 160 | Systick::delay(100.micros()).await; 161 | app::ckp_checks::spawn().unwrap(); 162 | } -------------------------------------------------------------------------------- /src/app/gpio.rs: -------------------------------------------------------------------------------- 1 | // FOR uEFI v3.x boards 2 | 3 | use stm32f4xx_hal::{ 4 | gpio::{ 5 | self, gpioa, gpiob, gpioc, gpiod, gpioe, Alternate, Analog, Input, Output, Pin, PushPull, 6 | }, 7 | pac::SPI2, 8 | spi::Spi, 9 | }; 10 | 11 | pub struct InjectionGpioMapping { 12 | pub iny_1: gpio::PD8>, 13 | pub iny_2: gpio::PD9>, 14 | } 15 | 16 | pub struct IgnitionGpioMapping { 17 | pub ecn_1: gpio::PD10>, 18 | pub ecn_2: gpio::PD11>, 19 | } 20 | 21 | pub struct PMICGpioMapping { 22 | pub pmic1_enable: gpio::PB12>, 23 | pub pmic1_cs: gpio::PB11>, 24 | pub pmic2_enable: gpio::PB13>, 25 | pub pmic2_cs: gpio::PB9>, 26 | } 27 | 28 | pub struct LedGpioMapping { 29 | pub led_0: gpio::PC1>, 30 | pub led_1: gpio::PC2>, 31 | pub led_2: gpio::PC3>, 32 | pub led_can_tx: gpio::PD6>, 33 | pub led_can_rx: gpio::PD7>, 34 | pub led_check: gpio::PB1>, 35 | pub led_mil: gpio::PA8>, 36 | } 37 | 38 | pub struct AuxIoMapping { 39 | 40 | // on external connector 41 | pub in_1: gpio::PD12, 42 | pub in_2: gpio::PD13, 43 | pub in_3: gpio::PD14, 44 | pub in_4: gpio::PD15, 45 | 46 | // on expansion header 47 | 48 | pub in_5: gpio::PB5, 49 | pub in_6: gpio::PB7, 50 | pub in_7: gpio::PB8, 51 | pub in_8: gpio::PE1, 52 | 53 | 54 | pub out_1: gpio::PC4>, 55 | pub out_2: gpio::PC5>, 56 | pub out_3: gpio::PB0>, 57 | pub out_4: gpio::PA10>, 58 | 59 | pub cs_1: gpio::PE14>, 60 | pub cs_2: gpio::PE15>, 61 | } 62 | 63 | pub struct RelayMapping { 64 | pub iny: gpio::PE2>, 65 | pub gnc: gpio::PE3>, 66 | pub ac: gpio::PE4>, 67 | pub lmb: gpio::PE0>, 68 | } 69 | 70 | pub struct ADCMapping { 71 | pub baro_cs: gpio::PA6>, 72 | 73 | pub mux_a: gpio::PD3>, 74 | pub mux_b: gpio::PD4>, 75 | pub mux_c: gpio::PD5>, 76 | pub analog_in: gpio::PA7, 77 | } 78 | 79 | pub struct ADC_DMA_Mapping { 80 | pub tps: gpio::PA0, 81 | pub iat: gpio::PA1, 82 | pub map: gpio::PA2, 83 | pub clt: gpio::PA3, 84 | pub o2: gpio::PA4, 85 | pub vbatt: gpio::PA5, 86 | } 87 | 88 | 89 | pub struct StepperMapping{ 90 | pub step: gpio::PE7>, 91 | pub dir: gpio::PE8>, 92 | pub fault: gpio::PE9>, 93 | pub enable: gpio::PE10>, 94 | } 95 | 96 | pub struct SDCard { 97 | pub dat_0: Pin<'C', 8>, 98 | pub dat_1: Pin<'C', 9>, 99 | pub dat_2: Pin<'C', 10>, 100 | pub dat_3: Pin<'C',11>, 101 | pub dat_clk: gpio::PC12, 102 | pub cmd: Pin<'D', 2>, 103 | 104 | pub cd: gpio::PB6, 105 | } 106 | 107 | pub struct GpioMapping { 108 | // LED's / User feedback 109 | pub led: LedGpioMapping, 110 | 111 | // Injection 112 | pub injection: InjectionGpioMapping, 113 | 114 | // Ignition 115 | pub ignition: IgnitionGpioMapping, 116 | 117 | // PMIC 118 | pub pmic: PMICGpioMapping, 119 | 120 | // CKP/CMP 121 | pub ckp: gpio::PC6, 122 | pub cmp: gpio::PC7, 123 | 124 | // RELAY's 125 | pub relay: RelayMapping, 126 | 127 | // Step-Step motor TODO 128 | pub stepper: StepperMapping, 129 | 130 | // SPI Flash 131 | pub memory_cs: gpio::PE13>, 132 | 133 | // AUX I/O 134 | pub aux: AuxIoMapping, 135 | 136 | pub usb_dp: gpio::PA11>, 137 | pub usb_dm: gpio::PA12>, 138 | 139 | pub spi_sck: gpio::PB10>, 140 | pub spi_miso: gpio::PB14>, 141 | pub spi_mosi: gpio::PB15>, 142 | 143 | pub sd_card: SDCard, 144 | 145 | pub adc: ADCMapping, 146 | pub adc_dma: ADC_DMA_Mapping, 147 | } 148 | 149 | pub fn init_gpio( 150 | gpio_a: gpioa::Parts, 151 | gpio_b: gpiob::Parts, 152 | gpio_c: gpioc::Parts, 153 | gpio_d: gpiod::Parts, 154 | gpio_e: gpioe::Parts, 155 | ) -> GpioMapping { 156 | let mut gpio = GpioMapping { 157 | // LED's / User feedback 158 | led: LedGpioMapping { 159 | led_0: gpio_c.pc1.into_push_pull_output(), 160 | led_1: gpio_c.pc2.into_push_pull_output(), 161 | led_2: gpio_c.pc3.into_push_pull_output(), 162 | led_can_tx: gpio_d.pd6.into_push_pull_output(), 163 | led_can_rx: gpio_d.pd7.into_push_pull_output(), 164 | led_check: gpio_b.pb1.into_push_pull_output(), 165 | led_mil: gpio_a.pa8.into_push_pull_output(), 166 | }, 167 | 168 | // Injection 169 | injection: InjectionGpioMapping { 170 | iny_1: gpio_d.pd8.into_push_pull_output(), 171 | iny_2: gpio_d.pd9.into_push_pull_output(), 172 | }, 173 | 174 | // Ignition 175 | ignition: IgnitionGpioMapping { 176 | ecn_1: gpio_d.pd10.into_push_pull_output(), 177 | ecn_2: gpio_d.pd11.into_push_pull_output(), 178 | }, 179 | 180 | // PMIC 181 | pmic: PMICGpioMapping { 182 | pmic1_enable: gpio_b.pb12.into_push_pull_output(), 183 | pmic1_cs: gpio_b.pb11.into_push_pull_output(), 184 | pmic2_enable: gpio_b.pb13.into_push_pull_output(), 185 | pmic2_cs: gpio_b.pb9.into_push_pull_output(), 186 | }, 187 | 188 | // CKP/CMP 189 | ckp: gpio_c.pc6.into_input(), 190 | cmp: gpio_c.pc7.into_input(), 191 | 192 | // RELAY's 193 | relay: RelayMapping { 194 | iny: gpio_e.pe2.into_push_pull_output(), 195 | gnc: gpio_e.pe3.into_push_pull_output(), 196 | ac: gpio_e.pe4.into_push_pull_output(), 197 | lmb: gpio_e.pe0.into_push_pull_output(), 198 | }, 199 | 200 | // Step-Step motor TODO 201 | stepper: StepperMapping { 202 | step: gpio_e.pe7.into_push_pull_output(), 203 | dir: gpio_e.pe8.into_push_pull_output(), 204 | fault: gpio_e.pe9.into_push_pull_output(), 205 | enable: gpio_e.pe10.into_push_pull_output(), 206 | }, 207 | 208 | // SPI Flash 209 | memory_cs: gpio_e.pe13.into_push_pull_output(), 210 | 211 | // AUX I/O 212 | aux: AuxIoMapping { 213 | in_1: gpio_d.pd12.into_input(), 214 | in_2: gpio_d.pd13.into_input(), 215 | in_3: gpio_d.pd14.into_input(), 216 | in_4: gpio_d.pd15.into_input(), 217 | in_5: gpio_b.pb5.into_input(), 218 | in_6: gpio_b.pb7.into_input(), 219 | in_7: gpio_b.pb8.into_input(), 220 | in_8: gpio_e.pe1.into_input(), 221 | 222 | out_1: gpio_c.pc4.into_push_pull_output(), 223 | out_2: gpio_c.pc5.into_push_pull_output(), 224 | out_3: gpio_b.pb0.into_push_pull_output(), 225 | out_4: gpio_a.pa10.into_push_pull_output(), 226 | 227 | cs_1: gpio_e.pe14.into_push_pull_output(), 228 | cs_2: gpio_e.pe15.into_push_pull_output(), 229 | }, 230 | 231 | // USB 232 | usb_dp: gpio_a.pa11.into_alternate(), 233 | usb_dm: gpio_a.pa12.into_alternate(), 234 | 235 | // SPI 236 | spi_sck: gpio_b.pb10.into_alternate(), 237 | spi_miso: gpio_b.pb14.into_alternate(), 238 | spi_mosi: gpio_b.pb15.into_alternate().internal_pull_up(true), 239 | 240 | adc_dma: ADC_DMA_Mapping { 241 | tps: gpio_a.pa0.into_analog(), 242 | iat: gpio_a.pa1.into_analog(), 243 | map:gpio_a.pa2.into_analog(), 244 | clt: gpio_a.pa3.into_analog(), 245 | o2: gpio_a.pa4.into_analog(), 246 | vbatt: gpio_a.pa5.into_analog(), 247 | }, 248 | 249 | adc: ADCMapping { 250 | 251 | baro_cs: gpio_a.pa6.into_push_pull_output(), 252 | 253 | mux_a: gpio_d.pd3.into_push_pull_output(), 254 | mux_b: gpio_d.pd4.into_push_pull_output(), 255 | mux_c: gpio_d.pd5.into_push_pull_output(), 256 | 257 | analog_in: gpio_a.pa7.into_analog(), 258 | }, 259 | 260 | sd_card: SDCard {dat_0 : gpio_c.pc8.internal_pull_up(true), 261 | dat_1 : gpio_c.pc9.internal_pull_up(true), 262 | dat_2 : gpio_c.pc10.internal_pull_up(true), 263 | dat_3 : gpio_c.pc11.internal_pull_up(true), 264 | dat_clk : gpio_c.pc12, 265 | cmd : gpio_d.pd2.internal_pull_up(true), 266 | cd: gpio_b.pb6.into_input(), 267 | }, 268 | }; 269 | 270 | // set default state on I/O 271 | gpio.led.led_0.set_high(); 272 | gpio.led.led_1.set_high(); 273 | gpio.led.led_2.set_high(); 274 | gpio.led.led_can_rx.set_high(); 275 | gpio.led.led_can_tx.set_high(); 276 | gpio.led.led_check.set_low(); 277 | gpio.led.led_mil.set_low(); 278 | 279 | gpio.aux.cs_1.set_high(); 280 | gpio.aux.cs_2.set_high(); 281 | gpio.stepper.enable.set_high(); 282 | // 4231 reversa 283 | // 1234 derecho 284 | 285 | gpio.pmic.pmic1_cs.set_high(); 286 | gpio.pmic.pmic2_cs.set_high(); 287 | gpio.memory_cs.set_high(); 288 | 289 | return gpio; 290 | } 291 | 292 | pub type ISPI = Spi< 293 | SPI2, 294 | ( 295 | Pin<'B', 10, Alternate<5>>, 296 | Pin<'B', 14, Alternate<5>>, 297 | Pin<'B', 15, Alternate<5>>, 298 | ), 299 | false, 300 | >; 301 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 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 [2022] [Churrosoft] 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/app/webserial/handle_tables.rs: -------------------------------------------------------------------------------- 1 | use rtic::Mutex; 2 | use rtic::mutex_prelude::{TupleExt03, TupleExt06}; 3 | use rtic_monotonics::systick::*; 4 | use stm32f4xx_hal::crc32::Crc32; 5 | use w25q::series25::FlashInfo; 6 | 7 | use crate::app::{ 8 | self, logging, 9 | memory::tables::{FlashT, TableData, Tables}, 10 | webserial::{SerialCode, SerialMessage, SerialStatus}, 11 | }; 12 | 13 | pub(crate) async fn table_cdc_callback(mut ctx: app::table_cdc_callback::Context<'_>, command: SerialMessage) { 14 | let flash = ctx.shared.flash; 15 | let flash_info = ctx.shared.flash_info; 16 | let mut tables = ctx.shared.tables; 17 | let crc = ctx.shared.crc; 18 | let mut cdc_sender = ctx.local.table_sender; 19 | 20 | 21 | let mut response_buf = SerialMessage { 22 | protocol: 1, 23 | command: 0x10, 24 | code: 0, 25 | status: 0, 26 | payload: [0u8; 122], 27 | crc: 0, 28 | }; 29 | 30 | let selected_table = command.code; 31 | 32 | 33 | match command.command & 0b00001111 { 34 | // table get metadata 35 | 0x01 => { 36 | //TODO: faltaria un objeto global de poneleque solo lectura para traer toda la metadata de las tablas 37 | // ya si hago mutable el struct de tablas con la data adentro rompe por lifetime 38 | 39 | match selected_table { 40 | 0x01 => { 41 | // TPS RPM VE (injection) 42 | response_buf.payload[0] = 17; 43 | response_buf.payload[1] = 17; 44 | logging::host::debug!("Table get metadata - TPS VE"); 45 | } 46 | 0x10 => { 47 | // TPS / LOAD / DEG (ignition) 48 | response_buf.payload[0] = 17; 49 | response_buf.payload[1] = 17; 50 | } 51 | 0x2..0x9 | _ => { 52 | app::send_message::spawn( 53 | SerialStatus::Error, 54 | SerialCode::UnknownTable as u8, 55 | response_buf, 56 | ).unwrap(); 57 | return; 58 | } 59 | } 60 | } 61 | // get X table 62 | 0x02 => { 63 | let mut table = [[0i32; 17]; 17]; 64 | // logging::host::debug!("Table get"); 65 | match selected_table { 66 | 0x01 => { 67 | // TODO: read table if not read prev. 68 | table = tables.lock(|table_data| table_data.tps_rpm_ve.unwrap_or(table)); 69 | } 70 | 0x10 => { 71 | // TODO: read table if not read prev. 72 | table = tables.lock(|table_data| table_data.load_tps_deg.unwrap_or(table)); 73 | } 74 | 0x2..0x9 | _ => { 75 | logging::host::debug!("Table unkown get"); 76 | app::send_message::spawn( 77 | SerialStatus::Error, 78 | SerialCode::UnknownTable as u8, 79 | response_buf, 80 | ).unwrap(); 81 | return; 82 | } 83 | } 84 | 85 | for row in table { 86 | let mut _row_index = 0; 87 | let mut i = 1; 88 | response_buf.payload[0] = _row_index; 89 | 90 | for row_number in row { 91 | let number_arr: [u8; 4] = i32::to_le_bytes(row_number); 92 | response_buf.payload[i] = number_arr[0]; 93 | response_buf.payload[i + 1] = number_arr[1]; 94 | response_buf.payload[i + 2] = number_arr[2]; 95 | response_buf.payload[i + 3] = number_arr[3]; 96 | i += 4; 97 | } 98 | _row_index += 1; 99 | response_buf.status = SerialStatus::DataChunk as u8; 100 | response_buf.code = 0; 101 | cdc_sender.send(response_buf.clone()).await.unwrap(); 102 | Systick::delay(10.millis()).await; 103 | } 104 | 105 | response_buf.status = SerialStatus::DataChunkEnd as u8; 106 | response_buf.code = 0; 107 | cdc_sender.send(response_buf.clone()).await.unwrap(); 108 | } 109 | 110 | // put X table 111 | // TODO: read table if not read prev. 112 | 0x03 => { 113 | let mut row_data = 1; 114 | let row_index = command.payload[0] as usize; 115 | let mut table_row = [0i32; 17]; 116 | let mut table = [[0i32; 17]; 17]; 117 | 118 | // FIXME: get max index from correct table 119 | for matrix_x in 0..17 { 120 | let u8buff = [ 121 | command.payload[row_data], 122 | command.payload[row_data + 1], 123 | command.payload[row_data + 2], 124 | command.payload[row_data + 3], 125 | ]; 126 | 127 | let value = i32::from_le_bytes(u8buff); 128 | table_row[matrix_x] = value; 129 | row_data += 4; 130 | } 131 | 132 | match selected_table { 133 | 0x01 => { 134 | // TPS RPM VE 135 | table = tables.lock(|table_data| table_data.tps_rpm_ve.unwrap_or(table)); 136 | table[row_index] = table_row; 137 | tables.lock(|table_data| table_data.tps_rpm_ve = Some(table)); 138 | app::send_message::spawn(SerialStatus::Ok, 0, response_buf).unwrap(); 139 | return; 140 | } 141 | 0x02 => { 142 | // TPS RPM AFR 143 | table = tables.lock(|table_data| table_data.tps_rpm_afr.unwrap_or(table)); 144 | table[row_index] = table_row; 145 | tables.lock(|table_data| table_data.tps_rpm_afr = Some(table)); 146 | app::send_message::spawn(SerialStatus::Ok, 0, response_buf).unwrap(); 147 | return; 148 | } 149 | 0x10 => { 150 | // TPS RPM VE 151 | table = tables.lock(|table_data| table_data.tps_rpm_ve.unwrap_or(table)); 152 | table[row_index] = table_row; 153 | tables.lock(|table_data| table_data.tps_rpm_ve = Some(table)); 154 | app::send_message::spawn(SerialStatus::Ok, 0, response_buf).unwrap(); 155 | return; 156 | } 157 | 0x3..0x9 | _ => { 158 | logging::host::debug!("error on get"); 159 | app::send_message::spawn( 160 | SerialStatus::Error, 161 | SerialCode::UnknownTable as u8, 162 | response_buf, 163 | ).unwrap(); 164 | return; 165 | } 166 | } 167 | } 168 | // write X table 169 | // TODO: check crc32 prev write 170 | 0x04 => { 171 | response_buf.payload[0] = 253; 172 | 173 | match selected_table { 174 | 0x01 => { 175 | // TPS RPM VE 176 | let tps_rpm_ve = TableData { 177 | data: tables.lock(|tables| tables.tps_rpm_ve), 178 | crc: 0, 179 | address: 0x3, 180 | max_x: 17, 181 | max_y: 17, 182 | }; 183 | 184 | if tps_rpm_ve.data.is_some() { 185 | (flash, flash_info, crc).lock(|flash, flash_info, crc| { 186 | tps_rpm_ve.write_to_memory(flash, flash_info, crc); 187 | }); 188 | 189 | app::send_message::spawn(SerialStatus::UploadOk, 0, response_buf).unwrap(); 190 | return; 191 | } 192 | 193 | logging::host::debug!("Table Write TableNotLoaded - TPS VE"); 194 | 195 | app::send_message::spawn( 196 | SerialStatus::Error, 197 | SerialCode::TableNotLoaded as u8, 198 | response_buf, 199 | ).unwrap(); 200 | return; 201 | } 202 | 0x02 => { 203 | // TPS RPM AFR 204 | let tps_rpm_afr = TableData { 205 | data: tables.lock(|tables| tables.tps_rpm_afr), 206 | crc: 0, 207 | address: 0x3, 208 | max_x: 17, 209 | max_y: 17, 210 | }; 211 | 212 | if tps_rpm_afr.data.is_some() { 213 | (flash, flash_info, crc).lock(|flash, flash_info, crc| { 214 | tps_rpm_afr.write_to_memory(flash, flash_info, crc); 215 | }); 216 | 217 | 218 | // tps_rpm_ve.write_to_memory(flash, flash_info, crc); 219 | app::send_message::spawn(SerialStatus::UploadOk, 0, response_buf).unwrap(); 220 | return; 221 | } 222 | 223 | logging::host::debug!("Table Write TableNotLoaded - TPS AFR"); 224 | 225 | app::send_message::spawn( 226 | SerialStatus::Error, 227 | SerialCode::TableNotLoaded as u8, 228 | response_buf, 229 | ).unwrap(); 230 | return; 231 | } 232 | 233 | 0x10 => { 234 | // TPS RPM VE 235 | let load_tps_deg = TableData { 236 | data: tables.lock(|tables| tables.load_tps_deg), 237 | crc: 0, 238 | address: 0x3, 239 | max_x: 17, 240 | max_y: 17, 241 | }; 242 | 243 | if load_tps_deg.data.is_some() { 244 | (flash, flash_info, crc).lock(|flash, flash_info, crc| { 245 | load_tps_deg.write_to_memory(flash, flash_info, crc); 246 | }); 247 | app::send_message::spawn(SerialStatus::UploadOk, 0, response_buf).unwrap(); 248 | return; 249 | } 250 | 251 | logging::host::debug!("Table Write TableNotLoaded - LOAD TPS DEG"); 252 | 253 | app::send_message::spawn( 254 | SerialStatus::Error, 255 | SerialCode::TableNotLoaded as u8, 256 | response_buf, 257 | ).unwrap(); 258 | return; 259 | } 260 | 261 | 0x3..0x9 | _ => { 262 | logging::host::debug!("error on write"); 263 | app::send_message::spawn( 264 | SerialStatus::Error, 265 | SerialCode::UnknownTable as u8, 266 | response_buf, 267 | ).unwrap(); 268 | return; 269 | } 270 | } 271 | } 272 | 273 | // Clear selected table 274 | 0x09 => { 275 | match selected_table { 276 | 0x01 => { 277 | // TPS RPM VE 278 | let mut tps_rpm_ve = TableData { 279 | data: None, 280 | crc: 0, 281 | address: 0x3, 282 | max_x: 17, 283 | max_y: 17, 284 | }; 285 | 286 | (flash, flash_info, crc).lock(|flash, flash_info, crc| { 287 | tps_rpm_ve.clear(flash, flash_info, crc); 288 | }); 289 | 290 | logging::host::debug!("Table Clear TPS/RPM/VE [injection]"); 291 | 292 | app::send_message::spawn( 293 | SerialStatus::Ok, 294 | SerialCode::None as u8, 295 | response_buf, 296 | ).unwrap(); 297 | return; 298 | } 299 | 0x10 => { 300 | // TPS RPM VE 301 | let mut tps_rpm_ve = TableData { 302 | data: None, 303 | crc: 0, 304 | address: 0x3, 305 | max_x: 17, 306 | max_y: 17, 307 | }; 308 | 309 | (flash, flash_info, crc).lock(|flash, flash_info, crc| { 310 | tps_rpm_ve.clear(flash, flash_info, crc); 311 | }); 312 | logging::host::debug!("Table Clear LOAD/TPS/DEG [ignition]"); 313 | 314 | app::send_message::spawn( 315 | SerialStatus::Ok, 316 | SerialCode::None as u8, 317 | response_buf, 318 | ).unwrap(); 319 | return; 320 | } 321 | 0x2..0x9 | _ => { 322 | app::send_message::spawn( 323 | SerialStatus::Error, 324 | SerialCode::UnknownTable as u8, 325 | response_buf, 326 | ).unwrap(); 327 | return; 328 | } 329 | } 330 | } 331 | _ => { 332 | app::send_message::spawn( 333 | SerialStatus::Error, 334 | SerialCode::UnknownCmd as u8, 335 | response_buf, 336 | ).unwrap(); 337 | return; 338 | } 339 | } 340 | } -------------------------------------------------------------------------------- /OpenEFI_CubeMX.ioc: -------------------------------------------------------------------------------- 1 | #MicroXplorer Configuration settings - do not modify 2 | CAN1.BS1=CAN_BS1_1TQ 3 | CAN1.BS2=CAN_BS2_1TQ 4 | CAN1.CalculateBaudRate=625000 5 | CAN1.CalculateTimeBit=1600 6 | CAN1.CalculateTimeQuantum=533.3333333333334 7 | CAN1.IPParameters=CalculateTimeQuantum,BS1,BS2,CalculateTimeBit,CalculateBaudRate 8 | File.Version=6 9 | GPIO.groupedBy=Group By Peripherals 10 | KeepUserPlacement=false 11 | Mcu.CPN=STM32F407VGT6 12 | Mcu.Family=STM32F4 13 | Mcu.IP0=CAN1 14 | Mcu.IP1=CRC 15 | Mcu.IP10=TIM10 16 | Mcu.IP11=TIM11 17 | Mcu.IP12=TIM13 18 | Mcu.IP13=USB_DEVICE 19 | Mcu.IP14=USB_OTG_FS 20 | Mcu.IP2=NVIC 21 | Mcu.IP3=RCC 22 | Mcu.IP4=SPI2 23 | Mcu.IP5=SYS 24 | Mcu.IP6=TIM1 25 | Mcu.IP7=TIM3 26 | Mcu.IP8=TIM4 27 | Mcu.IP9=TIM9 28 | Mcu.IPNb=15 29 | Mcu.Name=STM32F407V(E-G)Tx 30 | Mcu.Package=LQFP100 31 | Mcu.Pin0=PE2 32 | Mcu.Pin1=PE3 33 | Mcu.Pin10=PC0 34 | Mcu.Pin11=PC1 35 | Mcu.Pin12=PA0-WKUP 36 | Mcu.Pin13=PA1 37 | Mcu.Pin14=PA2 38 | Mcu.Pin15=PA3 39 | Mcu.Pin16=PC4 40 | Mcu.Pin17=PC5 41 | Mcu.Pin18=PB0 42 | Mcu.Pin19=PB2 43 | Mcu.Pin2=PE4 44 | Mcu.Pin20=PE7 45 | Mcu.Pin21=PE8 46 | Mcu.Pin22=PE9 47 | Mcu.Pin23=PE10 48 | Mcu.Pin24=PE11 49 | Mcu.Pin25=PE13 50 | Mcu.Pin26=PE14 51 | Mcu.Pin27=PE15 52 | Mcu.Pin28=PB10 53 | Mcu.Pin29=PB11 54 | Mcu.Pin3=PE5 55 | Mcu.Pin30=PB12 56 | Mcu.Pin31=PB13 57 | Mcu.Pin32=PB14 58 | Mcu.Pin33=PB15 59 | Mcu.Pin34=PD8 60 | Mcu.Pin35=PD9 61 | Mcu.Pin36=PD10 62 | Mcu.Pin37=PD11 63 | Mcu.Pin38=PD12 64 | Mcu.Pin39=PD13 65 | Mcu.Pin4=PE6 66 | Mcu.Pin40=PD14 67 | Mcu.Pin41=PD15 68 | Mcu.Pin42=PC6 69 | Mcu.Pin43=PC7 70 | Mcu.Pin44=PC8 71 | Mcu.Pin45=PC9 72 | Mcu.Pin46=PA8 73 | Mcu.Pin47=PA9 74 | Mcu.Pin48=PA10 75 | Mcu.Pin49=PA11 76 | Mcu.Pin5=PC13-ANTI_TAMP 77 | Mcu.Pin50=PA12 78 | Mcu.Pin51=PA13 79 | Mcu.Pin52=PA14 80 | Mcu.Pin53=PA15 81 | Mcu.Pin54=PC10 82 | Mcu.Pin55=PC11 83 | Mcu.Pin56=PC12 84 | Mcu.Pin57=PD0 85 | Mcu.Pin58=PD1 86 | Mcu.Pin59=PD6 87 | Mcu.Pin6=PC14-OSC32_IN 88 | Mcu.Pin60=PD7 89 | Mcu.Pin61=PB3 90 | Mcu.Pin62=PE0 91 | Mcu.Pin63=PE1 92 | Mcu.Pin64=VP_CRC_VS_CRC 93 | Mcu.Pin65=VP_SYS_VS_tim14 94 | Mcu.Pin66=VP_TIM1_VS_ClockSourceINT 95 | Mcu.Pin67=VP_TIM3_VS_ClockSourceINT 96 | Mcu.Pin68=VP_TIM3_VS_ClockSourceITR 97 | Mcu.Pin69=VP_TIM4_VS_ClockSourceITR 98 | Mcu.Pin7=PC15-OSC32_OUT 99 | Mcu.Pin70=VP_TIM10_VS_ClockSourceINT 100 | Mcu.Pin71=VP_TIM11_VS_ClockSourceINT 101 | Mcu.Pin72=VP_TIM13_VS_ClockSourceINT 102 | Mcu.Pin73=VP_USB_DEVICE_VS_USB_DEVICE_CDC_FS 103 | Mcu.Pin8=PH0-OSC_IN 104 | Mcu.Pin9=PH1-OSC_OUT 105 | Mcu.PinsNb=74 106 | Mcu.ThirdPartyNb=0 107 | Mcu.UserConstants= 108 | Mcu.UserName=STM32F407VGTx 109 | MxCube.Version=6.5.0 110 | MxDb.Version=DB.6.0.50 111 | NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:true 112 | NVIC.CAN1_RX0_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true 113 | NVIC.CAN1_SCE_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true 114 | NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:true 115 | NVIC.EXTI9_5_IRQn=true\:0\:0\:false\:true\:true\:1\:true\:true\:true 116 | NVIC.ForceEnableDMAVector=true 117 | NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:true 118 | NVIC.MemoryManagement_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:true 119 | NVIC.NonMaskableInt_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:true 120 | NVIC.OTG_FS_IRQn=true\:0\:0\:false\:true\:true\:6\:false\:true\:true 121 | NVIC.PendSV_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:true 122 | NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4 123 | NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:true 124 | NVIC.SysTick_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true 125 | NVIC.TIM1_TRG_COM_TIM11_IRQn=true\:0\:0\:false\:true\:true\:3\:true\:true\:true 126 | NVIC.TIM1_UP_TIM10_IRQn=true\:0\:0\:false\:true\:true\:2\:true\:true\:true 127 | NVIC.TIM3_IRQn=true\:0\:0\:false\:true\:true\:4\:true\:true\:true 128 | NVIC.TIM4_IRQn=true\:0\:0\:false\:true\:true\:5\:true\:true\:true 129 | NVIC.TIM8_TRG_COM_TIM14_IRQn=true\:15\:0\:false\:false\:true\:false\:true\:true 130 | NVIC.TimeBase=TIM8_TRG_COM_TIM14_IRQn 131 | NVIC.TimeBaseIP=TIM14 132 | NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:true 133 | PA0-WKUP.GPIOParameters=GPIO_Label 134 | PA0-WKUP.GPIO_Label=AUX_IN_1 135 | PA0-WKUP.Locked=true 136 | PA0-WKUP.Signal=GPIO_Input 137 | PA1.GPIOParameters=GPIO_Label 138 | PA1.GPIO_Label=AUX_IN_2 139 | PA1.Locked=true 140 | PA1.Signal=GPIO_Input 141 | PA10.GPIOParameters=GPIO_Label 142 | PA10.GPIO_Label=AUX_OUT_4 143 | PA10.Locked=true 144 | PA10.Signal=GPIO_Output 145 | PA11.Mode=Device_Only 146 | PA11.Signal=USB_OTG_FS_DM 147 | PA12.Mode=Device_Only 148 | PA12.Signal=USB_OTG_FS_DP 149 | PA13.Mode=Trace_Asynchronous_SW 150 | PA13.Signal=SYS_JTMS-SWDIO 151 | PA14.Mode=Trace_Asynchronous_SW 152 | PA14.Signal=SYS_JTCK-SWCLK 153 | PA15.GPIOParameters=GPIO_Label 154 | PA15.GPIO_Label=PMIC_CS 155 | PA15.Locked=true 156 | PA15.Signal=GPIO_Output 157 | PA2.GPIOParameters=GPIO_Label 158 | PA2.GPIO_Label=AUX_IN_3 159 | PA2.Locked=true 160 | PA2.Signal=GPIO_Input 161 | PA3.GPIOParameters=GPIO_Label 162 | PA3.GPIO_Label=AUX_IN_4 163 | PA3.Locked=true 164 | PA3.Signal=GPIO_Input 165 | PA8.GPIOParameters=GPIO_Label 166 | PA8.GPIO_Label=GNC_GLP_OUT 167 | PA8.Locked=true 168 | PA8.Signal=GPIO_Output 169 | PA9.Mode=Activate_VBUS 170 | PA9.Signal=USB_OTG_FS_VBUS 171 | PB0.GPIOParameters=GPIO_Label 172 | PB0.GPIO_Label=AUX_OUT_3 173 | PB0.Locked=true 174 | PB0.Signal=GPIO_Output 175 | PB10.Mode=Full_Duplex_Master 176 | PB10.Signal=SPI2_SCK 177 | PB11.GPIOParameters=GPIO_Label 178 | PB11.GPIO_Label=GNC_GLP_IN 179 | PB11.Locked=true 180 | PB11.Signal=GPIO_Input 181 | PB12.GPIOParameters=GPIO_Label 182 | PB12.GPIO_Label=PMIC_ENABLE 183 | PB12.Locked=true 184 | PB12.Signal=GPIO_Output 185 | PB13.GPIOParameters=GPIO_Label 186 | PB13.GPIO_Label=PMIC_SPARK 187 | PB13.Locked=true 188 | PB13.Signal=GPIO_Input 189 | PB14.Mode=Full_Duplex_Master 190 | PB14.Signal=SPI2_MISO 191 | PB15.Mode=Full_Duplex_Master 192 | PB15.Signal=SPI2_MOSI 193 | PB2.GPIOParameters=GPIO_Label 194 | PB2.GPIO_Label=TP2 195 | PB2.Locked=true 196 | PB2.Signal=GPIO_Input 197 | PB3.Mode=Trace_Asynchronous_SW 198 | PB3.Signal=SYS_JTDO-SWO 199 | PC0.GPIOParameters=GPIO_Label 200 | PC0.GPIO_Label=ADC_CS 201 | PC0.Locked=true 202 | PC0.Signal=GPIO_Output 203 | PC1.GPIOParameters=GPIO_Label 204 | PC1.GPIO_Label=ENABLE_5V 205 | PC1.Locked=true 206 | PC1.Signal=GPIO_Output 207 | PC10.GPIOParameters=GPIO_Label 208 | PC10.GPIO_Label=PMIC_NOMI 209 | PC10.Locked=true 210 | PC10.Signal=GPIO_Input 211 | PC11.GPIOParameters=GPIO_Label 212 | PC11.GPIO_Label=PMIC_MAXI 213 | PC11.Locked=true 214 | PC11.Signal=GPIO_Input 215 | PC12.GPIOParameters=GPIO_Label 216 | PC12.GPIO_Label=ACC_DET 217 | PC12.Locked=true 218 | PC12.Signal=GPIO_Input 219 | PC13-ANTI_TAMP.GPIOParameters=GPIO_Label 220 | PC13-ANTI_TAMP.GPIO_Label=LED0 221 | PC13-ANTI_TAMP.Locked=true 222 | PC13-ANTI_TAMP.Signal=GPIO_Output 223 | PC14-OSC32_IN.GPIOParameters=GPIO_Label 224 | PC14-OSC32_IN.GPIO_Label=LED1 225 | PC14-OSC32_IN.Locked=true 226 | PC14-OSC32_IN.Signal=GPIO_Output 227 | PC15-OSC32_OUT.GPIOParameters=GPIO_Label 228 | PC15-OSC32_OUT.GPIO_Label=LED2 229 | PC15-OSC32_OUT.Locked=true 230 | PC15-OSC32_OUT.Signal=GPIO_Output 231 | PC4.GPIOParameters=GPIO_Label 232 | PC4.GPIO_Label=AUX_OUT_1 233 | PC4.Locked=true 234 | PC4.Signal=GPIO_Output 235 | PC5.GPIOParameters=GPIO_Label 236 | PC5.GPIO_Label=AUX_OUT_2 237 | PC5.Locked=true 238 | PC5.Signal=GPIO_Output 239 | PC6.GPIOParameters=GPIO_Label,GPIO_ModeDefaultEXTI 240 | PC6.GPIO_Label=CKP 241 | PC6.GPIO_ModeDefaultEXTI=GPIO_MODE_IT_RISING 242 | PC6.Locked=true 243 | PC6.Signal=GPXTI6 244 | PC7.GPIOParameters=GPIO_Label,GPIO_ModeDefaultEXTI 245 | PC7.GPIO_Label=CMP 246 | PC7.GPIO_ModeDefaultEXTI=GPIO_MODE_IT_RISING_FALLING 247 | PC7.Locked=true 248 | PC7.Signal=GPXTI7 249 | PC8.GPIOParameters=GPIO_Label 250 | PC8.GPIO_Label=MIL_OUT 251 | PC8.Locked=true 252 | PC8.Signal=GPIO_Output 253 | PC9.GPIOParameters=GPIO_Label 254 | PC9.GPIO_Label=CHK_OUT 255 | PC9.Locked=true 256 | PC9.Signal=GPIO_Output 257 | PD0.Mode=CAN_Activate 258 | PD0.Signal=CAN1_RX 259 | PD1.Mode=CAN_Activate 260 | PD1.Signal=CAN1_TX 261 | PD10.GPIOParameters=GPIO_Label 262 | PD10.GPIO_Label=INY3 263 | PD10.Locked=true 264 | PD10.Signal=GPIO_Output 265 | PD11.GPIOParameters=GPIO_Label 266 | PD11.GPIO_Label=INY4 267 | PD11.Locked=true 268 | PD11.Signal=GPIO_Output 269 | PD12.Locked=true 270 | PD12.Signal=GPIO_Output 271 | PD13.GPIOParameters=GPIO_Label 272 | PD13.GPIO_Label=ECN2 273 | PD13.Locked=true 274 | PD13.Signal=GPIO_Output 275 | PD14.GPIOParameters=GPIO_Label 276 | PD14.GPIO_Label=ECN3 277 | PD14.Locked=true 278 | PD14.Signal=GPIO_Output 279 | PD15.GPIOParameters=GPIO_Label 280 | PD15.GPIO_Label=ECN4 281 | PD15.Locked=true 282 | PD15.Signal=GPIO_Output 283 | PD6.GPIOParameters=GPIO_Label 284 | PD6.GPIO_Label=LED_CAN_RX 285 | PD6.Locked=true 286 | PD6.Signal=GPIO_Output 287 | PD7.GPIOParameters=GPIO_Label 288 | PD7.GPIO_Label=LED_CAN_TX 289 | PD7.Locked=true 290 | PD7.Signal=GPIO_Output 291 | PD8.GPIOParameters=GPIO_Label 292 | PD8.GPIO_Label=INY1 293 | PD8.Locked=true 294 | PD8.Signal=GPIO_Output 295 | PD9.GPIOParameters=GPIO_Label 296 | PD9.GPIO_Label=INY2 297 | PD9.Locked=true 298 | PD9.Signal=GPIO_Output 299 | PE0.GPIOParameters=GPIO_Label 300 | PE0.GPIO_Label=R_LMB 301 | PE0.Locked=true 302 | PE0.Signal=GPIO_Output 303 | PE1.Locked=true 304 | PE1.Signal=GPIO_Output 305 | PE10.GPIOParameters=GPIO_Label 306 | PE10.GPIO_Label=MTR_ENABLE 307 | PE10.Locked=true 308 | PE10.Signal=GPIO_Output 309 | PE11.GPIOParameters=GPIO_Label 310 | PE11.GPIO_Label=RPM_OUT 311 | PE11.Locked=true 312 | PE11.Signal=GPIO_Output 313 | PE13.GPIOParameters=GPIO_Label 314 | PE13.GPIO_Label=MEMORY_CS 315 | PE13.Locked=true 316 | PE13.Signal=GPIO_Output 317 | PE14.GPIOParameters=GPIO_Label 318 | PE14.GPIO_Label=AUX_CS_1 319 | PE14.Locked=true 320 | PE14.Signal=GPIO_Output 321 | PE15.GPIOParameters=GPIO_Label 322 | PE15.GPIO_Label=AUX_CS_2 323 | PE15.Locked=true 324 | PE15.Signal=GPIO_Output 325 | PE2.GPIOParameters=GPIO_Label 326 | PE2.GPIO_Label=R_ECN_INY 327 | PE2.Locked=true 328 | PE2.Signal=GPIO_Output 329 | PE3.GPIOParameters=GPIO_Label 330 | PE3.GPIO_Label=R_GNC_GLP 331 | PE3.Locked=true 332 | PE3.Signal=GPIO_Output 333 | PE4.GPIOParameters=GPIO_Label 334 | PE4.GPIO_Label=R_AAC 335 | PE4.Locked=true 336 | PE4.Signal=GPIO_Output 337 | PE5.Signal=S_TIM9_CH1 338 | PE6.Signal=S_TIM9_CH2 339 | PE7.GPIOParameters=GPIO_Label 340 | PE7.GPIO_Label=MTR_STEP 341 | PE7.Locked=true 342 | PE7.Signal=GPIO_Output 343 | PE8.GPIOParameters=GPIO_Label 344 | PE8.GPIO_Label=MTR_DIR 345 | PE8.Locked=true 346 | PE8.Signal=GPIO_Output 347 | PE9.GPIOParameters=GPIO_Label 348 | PE9.GPIO_Label=MTR_FAULT 349 | PE9.Locked=true 350 | PE9.Signal=GPIO_Output 351 | PH0-OSC_IN.Mode=HSE-External-Oscillator 352 | PH0-OSC_IN.Signal=RCC_OSC_IN 353 | PH1-OSC_OUT.Mode=HSE-External-Oscillator 354 | PH1-OSC_OUT.Signal=RCC_OSC_OUT 355 | PinOutPanel.RotationAngle=90 356 | ProjectManager.AskForMigrate=true 357 | ProjectManager.BackupPrevious=false 358 | ProjectManager.CompilerOptimize=6 359 | ProjectManager.ComputerToolchain=false 360 | ProjectManager.CoupleFile=true 361 | ProjectManager.CustomerFirmwarePackage= 362 | ProjectManager.DefaultFWLocation=true 363 | ProjectManager.DeletePrevious=true 364 | ProjectManager.DeviceId=STM32F407VGTx 365 | ProjectManager.FirmwarePackage=STM32Cube FW_F4 V1.27.0 366 | ProjectManager.FreePins=false 367 | ProjectManager.HalAssertFull=false 368 | ProjectManager.HeapSize=0x200 369 | ProjectManager.KeepUserCode=true 370 | ProjectManager.LastFirmware=false 371 | ProjectManager.LibraryCopy=1 372 | ProjectManager.MainLocation=Core/Src 373 | ProjectManager.NoMain=false 374 | ProjectManager.PreviousToolchain= 375 | ProjectManager.ProjectBuild=false 376 | ProjectManager.ProjectFileName=OpenEFI_CubeMX.ioc 377 | ProjectManager.ProjectName=OpenEFI_CubeMX 378 | ProjectManager.RegisterCallBack= 379 | ProjectManager.StackSize=0x400 380 | ProjectManager.TargetToolchain=EWARM V8.32 381 | ProjectManager.ToolChainLocation= 382 | ProjectManager.UnderRoot=false 383 | ProjectManager.functionlistsort=1-MX_GPIO_Init-GPIO-false-HAL-true,2-SystemClock_Config-RCC-false-HAL-false,3-MX_CAN1_Init-CAN1-false-HAL-true,4-MX_SPI2_Init-SPI2-false-HAL-true,5-MX_TIM3_Init-TIM3-false-HAL-true,6-MX_TIM4_Init-TIM4-false-HAL-true,7-MX_TIM9_Init-TIM9-false-HAL-true,8-MX_USB_DEVICE_Init-USB_DEVICE-false-HAL-false,9-MX_TIM1_Init-TIM1-false-HAL-true,10-MX_TIM10_Init-TIM10-false-HAL-true,11-MX_TIM11_Init-TIM11-false-HAL-true,12-MX_CRC_Init-CRC-false-HAL-true,13-MX_TIM13_Init-TIM13-false-HAL-true 384 | RCC.48MHZClocksFreq_Value=48000000 385 | RCC.AHBFreq_Value=120000000 386 | RCC.APB1CLKDivider=RCC_HCLK_DIV4 387 | RCC.APB1Freq_Value=30000000 388 | RCC.APB1TimFreq_Value=60000000 389 | RCC.APB2CLKDivider=RCC_HCLK_DIV2 390 | RCC.APB2Freq_Value=60000000 391 | RCC.APB2TimFreq_Value=120000000 392 | RCC.CortexFreq_Value=120000000 393 | RCC.EthernetFreq_Value=120000000 394 | RCC.FCLKCortexFreq_Value=120000000 395 | RCC.FamilyName=M 396 | RCC.HCLKFreq_Value=120000000 397 | RCC.HSE_VALUE=25000000 398 | RCC.HSI_VALUE=16000000 399 | RCC.I2SClocksFreq_Value=160000000 400 | RCC.IPParameters=48MHZClocksFreq_Value,AHBFreq_Value,APB1CLKDivider,APB1Freq_Value,APB1TimFreq_Value,APB2CLKDivider,APB2Freq_Value,APB2TimFreq_Value,CortexFreq_Value,EthernetFreq_Value,FCLKCortexFreq_Value,FamilyName,HCLKFreq_Value,HSE_VALUE,HSI_VALUE,I2SClocksFreq_Value,LSE_VALUE,LSI_VALUE,MCO2PinFreq_Value,PLLCLKFreq_Value,PLLM,PLLN,PLLQ,PLLQCLKFreq_Value,RCC_MCODiv2,RTCFreq_Value,RTCHSEDivFreq_Value,SYSCLKFreq_VALUE,SYSCLKSource,VCOI2SOutputFreq_Value,VCOInputFreq_Value,VCOOutputFreq_Value,VcooutputI2S 401 | RCC.LSE_VALUE=32768 402 | RCC.LSI_VALUE=32000 403 | RCC.MCO2PinFreq_Value=24000000 404 | RCC.PLLCLKFreq_Value=120000000 405 | RCC.PLLM=15 406 | RCC.PLLN=144 407 | RCC.PLLQ=5 408 | RCC.PLLQCLKFreq_Value=48000000 409 | RCC.RCC_MCODiv2=RCC_MCODIV_5 410 | RCC.RTCFreq_Value=32000 411 | RCC.RTCHSEDivFreq_Value=12500000 412 | RCC.SYSCLKFreq_VALUE=120000000 413 | RCC.SYSCLKSource=RCC_SYSCLKSOURCE_PLLCLK 414 | RCC.VCOI2SOutputFreq_Value=320000000 415 | RCC.VCOInputFreq_Value=1666666.6666666667 416 | RCC.VCOOutputFreq_Value=240000000 417 | RCC.VcooutputI2S=160000000 418 | SH.GPXTI6.0=GPIO_EXTI6 419 | SH.GPXTI6.ConfNb=1 420 | SH.GPXTI7.0=GPIO_EXTI7 421 | SH.GPXTI7.ConfNb=1 422 | SH.S_TIM9_CH1.0=TIM9_CH1,PWM Generation1 CH1 423 | SH.S_TIM9_CH1.ConfNb=1 424 | SH.S_TIM9_CH2.0=TIM9_CH2,PWM Generation2 CH2 425 | SH.S_TIM9_CH2.ConfNb=1 426 | SPI2.BaudRatePrescaler=SPI_BAUDRATEPRESCALER_2 427 | SPI2.CalculateBaudRate=15.0 MBits/s 428 | SPI2.Direction=SPI_DIRECTION_2LINES 429 | SPI2.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate,BaudRatePrescaler 430 | SPI2.Mode=SPI_MODE_MASTER 431 | SPI2.VirtualType=VM_MASTER 432 | TIM1.IPParameters=Prescaler,Period 433 | TIM1.Period=0xffff-1 434 | TIM1.Prescaler=120-1 435 | TIM10.ClockDivision=TIM_CLOCKDIVISION_DIV4 436 | TIM10.IPParameters=ClockDivision,Prescaler 437 | TIM10.Prescaler=959 438 | TIM11.ClockDivision=TIM_CLOCKDIVISION_DIV1 439 | TIM11.IPParameters=Period,Prescaler,ClockDivision 440 | TIM11.Period=62499 441 | TIM11.Prescaler=959 442 | TIM13.ClockDivision=TIM_CLOCKDIVISION_DIV1 443 | TIM13.IPParameters=ClockDivision,Prescaler,Period 444 | TIM13.Period=999 445 | TIM13.Prescaler=119 446 | TIM3.ClockDivision=TIM_CLOCKDIVISION_DIV4 447 | TIM3.IPParameters=ClockDivision 448 | TIM9.Channel-PWM\ Generation1\ CH1=TIM_CHANNEL_1 449 | TIM9.Channel-PWM\ Generation2\ CH2=TIM_CHANNEL_2 450 | TIM9.IPParameters=Channel-PWM Generation1 CH1,Channel-PWM Generation2 CH2 451 | USB_DEVICE.CLASS_NAME_FS=CDC 452 | USB_DEVICE.IPParameters=VirtualModeFS,CLASS_NAME_FS,VirtualMode-CDC_FS 453 | USB_DEVICE.VirtualMode-CDC_FS=Cdc 454 | USB_DEVICE.VirtualModeFS=Cdc_FS 455 | USB_OTG_FS.IPParameters=VirtualMode 456 | USB_OTG_FS.VirtualMode=Device_Only 457 | VP_CRC_VS_CRC.Mode=CRC_Activate 458 | VP_CRC_VS_CRC.Signal=CRC_VS_CRC 459 | VP_SYS_VS_tim14.Mode=TIM14 460 | VP_SYS_VS_tim14.Signal=SYS_VS_tim14 461 | VP_TIM10_VS_ClockSourceINT.Mode=Enable_Timer 462 | VP_TIM10_VS_ClockSourceINT.Signal=TIM10_VS_ClockSourceINT 463 | VP_TIM11_VS_ClockSourceINT.Mode=Enable_Timer 464 | VP_TIM11_VS_ClockSourceINT.Signal=TIM11_VS_ClockSourceINT 465 | VP_TIM13_VS_ClockSourceINT.Mode=Enable_Timer 466 | VP_TIM13_VS_ClockSourceINT.Signal=TIM13_VS_ClockSourceINT 467 | VP_TIM1_VS_ClockSourceINT.Mode=Internal 468 | VP_TIM1_VS_ClockSourceINT.Signal=TIM1_VS_ClockSourceINT 469 | VP_TIM3_VS_ClockSourceINT.Mode=Internal 470 | VP_TIM3_VS_ClockSourceINT.Signal=TIM3_VS_ClockSourceINT 471 | VP_TIM3_VS_ClockSourceITR.Mode=TriggerSource_ITR0 472 | VP_TIM3_VS_ClockSourceITR.Signal=TIM3_VS_ClockSourceITR 473 | VP_TIM4_VS_ClockSourceITR.Mode=TriggerSource_ITR1 474 | VP_TIM4_VS_ClockSourceITR.Signal=TIM4_VS_ClockSourceITR 475 | VP_USB_DEVICE_VS_USB_DEVICE_CDC_FS.Mode=CDC_FS 476 | VP_USB_DEVICE_VS_USB_DEVICE_CDC_FS.Signal=USB_DEVICE_VS_USB_DEVICE_CDC_FS 477 | board=custom 478 | -------------------------------------------------------------------------------- /Cargo.lock: -------------------------------------------------------------------------------- 1 | # This file is automatically @generated by Cargo. 2 | # It is not intended for manual editing. 3 | version = 3 4 | 5 | [[package]] 6 | name = "arrayvec" 7 | version = "0.7.4" 8 | source = "registry+https://github.com/rust-lang/crates.io-index" 9 | checksum = "96d30a06541fbafbc7f82ed10c06164cfbd2c401138f6addd8404629c4b16711" 10 | 11 | [[package]] 12 | name = "atomic-polyfill" 13 | version = "0.1.11" 14 | source = "registry+https://github.com/rust-lang/crates.io-index" 15 | checksum = "e3ff7eb3f316534d83a8a2c3d1674ace8a5a71198eba31e2e2b597833f699b28" 16 | dependencies = [ 17 | "critical-section", 18 | ] 19 | 20 | [[package]] 21 | name = "atomic-polyfill" 22 | version = "1.0.3" 23 | source = "registry+https://github.com/rust-lang/crates.io-index" 24 | checksum = "8cf2bce30dfe09ef0bfaef228b9d414faaf7e563035494d7fe092dba54b300f4" 25 | dependencies = [ 26 | "critical-section", 27 | ] 28 | 29 | [[package]] 30 | name = "autocfg" 31 | version = "1.1.0" 32 | source = "registry+https://github.com/rust-lang/crates.io-index" 33 | checksum = "d468802bab17cbc0cc575e9b053f41e72aa36bfa6b7f55e3529ffa43161b97fa" 34 | 35 | [[package]] 36 | name = "bare-metal" 37 | version = "0.2.5" 38 | source = "registry+https://github.com/rust-lang/crates.io-index" 39 | checksum = "5deb64efa5bd81e31fcd1938615a6d98c82eafcbcd787162b6f63b91d6bac5b3" 40 | dependencies = [ 41 | "rustc_version 0.2.3", 42 | ] 43 | 44 | [[package]] 45 | name = "bare-metal" 46 | version = "1.0.0" 47 | source = "registry+https://github.com/rust-lang/crates.io-index" 48 | checksum = "f8fe8f5a8a398345e52358e18ff07cc17a568fbca5c6f73873d3a62056309603" 49 | 50 | [[package]] 51 | name = "bitfield" 52 | version = "0.13.2" 53 | source = "registry+https://github.com/rust-lang/crates.io-index" 54 | checksum = "46afbd2983a5d5a7bd740ccb198caf5b82f45c40c09c0eed36052d91cb92e719" 55 | 56 | [[package]] 57 | name = "bitflags" 58 | version = "1.3.2" 59 | source = "registry+https://github.com/rust-lang/crates.io-index" 60 | checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a" 61 | 62 | [[package]] 63 | name = "byteorder" 64 | version = "1.4.3" 65 | source = "registry+https://github.com/rust-lang/crates.io-index" 66 | checksum = "14c189c53d098945499cdfa7ecc63567cf3886b3332b312a5b4585d8d3a6a610" 67 | 68 | [[package]] 69 | name = "cfg-if" 70 | version = "1.0.0" 71 | source = "registry+https://github.com/rust-lang/crates.io-index" 72 | checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" 73 | 74 | [[package]] 75 | name = "cobs" 76 | version = "0.2.3" 77 | source = "registry+https://github.com/rust-lang/crates.io-index" 78 | checksum = "67ba02a97a2bd10f4b59b25c7973101c79642302776489e030cd13cdab09ed15" 79 | 80 | [[package]] 81 | name = "cortex-m" 82 | version = "0.7.7" 83 | source = "registry+https://github.com/rust-lang/crates.io-index" 84 | checksum = "8ec610d8f49840a5b376c69663b6369e71f4b34484b9b2eb29fb918d92516cb9" 85 | dependencies = [ 86 | "bare-metal 0.2.5", 87 | "bitfield", 88 | "critical-section", 89 | "embedded-hal 0.2.7", 90 | "volatile-register", 91 | ] 92 | 93 | [[package]] 94 | name = "cortex-m-rt" 95 | version = "0.7.3" 96 | source = "registry+https://github.com/rust-lang/crates.io-index" 97 | checksum = "ee84e813d593101b1723e13ec38b6ab6abbdbaaa4546553f5395ed274079ddb1" 98 | dependencies = [ 99 | "cortex-m-rt-macros", 100 | ] 101 | 102 | [[package]] 103 | name = "cortex-m-rt-macros" 104 | version = "0.7.0" 105 | source = "registry+https://github.com/rust-lang/crates.io-index" 106 | checksum = "f0f6f3e36f203cfedbc78b357fb28730aa2c6dc1ab060ee5c2405e843988d3c7" 107 | dependencies = [ 108 | "proc-macro2", 109 | "quote", 110 | "syn 1.0.109", 111 | ] 112 | 113 | [[package]] 114 | name = "cortex-m-semihosting" 115 | version = "0.5.0" 116 | source = "registry+https://github.com/rust-lang/crates.io-index" 117 | checksum = "c23234600452033cc77e4b761e740e02d2c4168e11dbf36ab14a0f58973592b0" 118 | dependencies = [ 119 | "cortex-m", 120 | ] 121 | 122 | [[package]] 123 | name = "critical-section" 124 | version = "1.1.2" 125 | source = "registry+https://github.com/rust-lang/crates.io-index" 126 | checksum = "7059fff8937831a9ae6f0fe4d658ffabf58f2ca96aa9dec1c889f936f705f216" 127 | 128 | [[package]] 129 | name = "deranged" 130 | version = "0.3.8" 131 | source = "registry+https://github.com/rust-lang/crates.io-index" 132 | checksum = "f2696e8a945f658fd14dc3b87242e6b80cd0f36ff04ea560fa39082368847946" 133 | 134 | [[package]] 135 | name = "embedded-dma" 136 | version = "0.2.0" 137 | source = "registry+https://github.com/rust-lang/crates.io-index" 138 | checksum = "994f7e5b5cb23521c22304927195f236813053eb9c065dd2226a32ba64695446" 139 | dependencies = [ 140 | "stable_deref_trait", 141 | ] 142 | 143 | [[package]] 144 | name = "embedded-hal" 145 | version = "0.2.7" 146 | source = "registry+https://github.com/rust-lang/crates.io-index" 147 | checksum = "35949884794ad573cf46071e41c9b60efb0cb311e3ca01f7af807af1debc66ff" 148 | dependencies = [ 149 | "nb 0.1.3", 150 | "void", 151 | ] 152 | 153 | [[package]] 154 | name = "embedded-hal" 155 | version = "1.0.0-alpha.8" 156 | source = "registry+https://github.com/rust-lang/crates.io-index" 157 | checksum = "c3babfc7fd332142a0b11aebf592992f211f4e01b6222fb04b03aba1bd80018d" 158 | dependencies = [ 159 | "nb 1.1.0", 160 | ] 161 | 162 | [[package]] 163 | name = "embedded-storage" 164 | version = "0.2.0" 165 | source = "registry+https://github.com/rust-lang/crates.io-index" 166 | checksum = "723dce4e9f25b6e6c5f35628e144794e5b459216ed7da97b7c4b66cdb3fa82ca" 167 | 168 | [[package]] 169 | name = "equivalent" 170 | version = "1.0.1" 171 | source = "registry+https://github.com/rust-lang/crates.io-index" 172 | checksum = "5443807d6dff69373d433ab9ef5378ad8df50ca6298caf15de6e52e24aaf54d5" 173 | 174 | [[package]] 175 | name = "fugit" 176 | version = "0.3.7" 177 | source = "registry+https://github.com/rust-lang/crates.io-index" 178 | checksum = "17186ad64927d5ac8f02c1e77ccefa08ccd9eaa314d5a4772278aa204a22f7e7" 179 | dependencies = [ 180 | "gcd", 181 | ] 182 | 183 | [[package]] 184 | name = "fugit-timer" 185 | version = "0.1.3" 186 | source = "registry+https://github.com/rust-lang/crates.io-index" 187 | checksum = "d9607bfc4c388f9d629704f56ede4a007546cad417b3bcd6fc7c87dc7edce04a" 188 | dependencies = [ 189 | "fugit", 190 | "nb 1.1.0", 191 | ] 192 | 193 | [[package]] 194 | name = "futures-core" 195 | version = "0.3.28" 196 | source = "registry+https://github.com/rust-lang/crates.io-index" 197 | checksum = "4bca583b7e26f571124fe5b7561d49cb2868d79116cfa0eefce955557c6fee8c" 198 | 199 | [[package]] 200 | name = "futures-task" 201 | version = "0.3.28" 202 | source = "registry+https://github.com/rust-lang/crates.io-index" 203 | checksum = "76d3d132be6c0e6aa1534069c705a74a5997a356c0dc2f86a47765e5617c5b65" 204 | 205 | [[package]] 206 | name = "futures-util" 207 | version = "0.3.28" 208 | source = "registry+https://github.com/rust-lang/crates.io-index" 209 | checksum = "26b01e40b772d54cf6c6d721c1d1abd0647a0106a12ecaa1c186273392a69533" 210 | dependencies = [ 211 | "futures-core", 212 | "futures-task", 213 | "pin-project-lite", 214 | "pin-utils", 215 | ] 216 | 217 | [[package]] 218 | name = "gcd" 219 | version = "2.3.0" 220 | source = "registry+https://github.com/rust-lang/crates.io-index" 221 | checksum = "1d758ba1b47b00caf47f24925c0074ecb20d6dfcffe7f6d53395c0465674841a" 222 | 223 | [[package]] 224 | name = "hash32" 225 | version = "0.2.1" 226 | source = "registry+https://github.com/rust-lang/crates.io-index" 227 | checksum = "b0c35f58762feb77d74ebe43bdbc3210f09be9fe6742234d573bacc26ed92b67" 228 | dependencies = [ 229 | "byteorder", 230 | ] 231 | 232 | [[package]] 233 | name = "hashbrown" 234 | version = "0.14.0" 235 | source = "registry+https://github.com/rust-lang/crates.io-index" 236 | checksum = "2c6201b9ff9fd90a5a3bac2e56a830d0caa509576f0e503818ee82c181b3437a" 237 | 238 | [[package]] 239 | name = "heapless" 240 | version = "0.7.16" 241 | source = "registry+https://github.com/rust-lang/crates.io-index" 242 | checksum = "db04bc24a18b9ea980628ecf00e6c0264f3c1426dac36c00cb49b6fbad8b0743" 243 | dependencies = [ 244 | "atomic-polyfill 0.1.11", 245 | "hash32", 246 | "rustc_version 0.4.0", 247 | "serde", 248 | "spin", 249 | "stable_deref_trait", 250 | ] 251 | 252 | [[package]] 253 | name = "indexmap" 254 | version = "2.0.0" 255 | source = "registry+https://github.com/rust-lang/crates.io-index" 256 | checksum = "d5477fe2230a79769d8dc68e0eabf5437907c0457a5614a9e8dddb67f65eb65d" 257 | dependencies = [ 258 | "equivalent", 259 | "hashbrown", 260 | ] 261 | 262 | [[package]] 263 | name = "lock_api" 264 | version = "0.4.10" 265 | source = "registry+https://github.com/rust-lang/crates.io-index" 266 | checksum = "c1cc9717a20b1bb222f333e6a92fd32f7d8a18ddc5a3191a11af45dcbf4dcd16" 267 | dependencies = [ 268 | "autocfg", 269 | "scopeguard", 270 | ] 271 | 272 | [[package]] 273 | name = "micromath" 274 | version = "2.0.0" 275 | source = "registry+https://github.com/rust-lang/crates.io-index" 276 | checksum = "39617bc909d64b068dcffd0e3e31679195b5576d0c83fadc52690268cc2b2b55" 277 | 278 | [[package]] 279 | name = "nb" 280 | version = "0.1.3" 281 | source = "registry+https://github.com/rust-lang/crates.io-index" 282 | checksum = "801d31da0513b6ec5214e9bf433a77966320625a37860f910be265be6e18d06f" 283 | dependencies = [ 284 | "nb 1.1.0", 285 | ] 286 | 287 | [[package]] 288 | name = "nb" 289 | version = "1.1.0" 290 | source = "registry+https://github.com/rust-lang/crates.io-index" 291 | checksum = "8d5439c4ad607c3c23abf66de8c8bf57ba8adcd1f129e699851a6e43935d339d" 292 | 293 | [[package]] 294 | name = "numtoa" 295 | version = "0.2.4" 296 | source = "registry+https://github.com/rust-lang/crates.io-index" 297 | checksum = "6aa2c4e539b869820a2b82e1aef6ff40aa85e65decdd5185e83fb4b1249cd00f" 298 | 299 | [[package]] 300 | name = "open_efi" 301 | version = "3.4.535" 302 | dependencies = [ 303 | "arrayvec", 304 | "cortex-m", 305 | "cortex-m-rt", 306 | "cortex-m-semihosting", 307 | "embedded-hal 0.2.7", 308 | "micromath", 309 | "numtoa", 310 | "panic-halt", 311 | "postcard", 312 | "rtic", 313 | "rtic-monotonics", 314 | "rtic-sync", 315 | "serde", 316 | "serde-json-core", 317 | "shared-bus-rtic", 318 | "stm32f4xx-hal", 319 | "usb-device", 320 | "usbd-serial", 321 | "usbd-webusb", 322 | "w25q", 323 | ] 324 | 325 | [[package]] 326 | name = "panic-halt" 327 | version = "0.2.0" 328 | source = "registry+https://github.com/rust-lang/crates.io-index" 329 | checksum = "de96540e0ebde571dc55c73d60ef407c653844e6f9a1e2fdbd40c07b9252d812" 330 | 331 | [[package]] 332 | name = "pin-project-lite" 333 | version = "0.2.13" 334 | source = "registry+https://github.com/rust-lang/crates.io-index" 335 | checksum = "8afb450f006bf6385ca15ef45d71d2288452bc3683ce2e2cacc0d18e4be60b58" 336 | 337 | [[package]] 338 | name = "pin-utils" 339 | version = "0.1.0" 340 | source = "registry+https://github.com/rust-lang/crates.io-index" 341 | checksum = "8b870d8c151b6f2fb93e84a13146138f05d02ed11c7e7c54f8826aaaf7c9f184" 342 | 343 | [[package]] 344 | name = "postcard" 345 | version = "1.0.7" 346 | source = "registry+https://github.com/rust-lang/crates.io-index" 347 | checksum = "d534c6e61df1c7166e636ca612d9820d486fe96ddad37f7abc671517b297488e" 348 | dependencies = [ 349 | "cobs", 350 | "heapless", 351 | "serde", 352 | ] 353 | 354 | [[package]] 355 | name = "proc-macro-error" 356 | version = "1.0.4" 357 | source = "registry+https://github.com/rust-lang/crates.io-index" 358 | checksum = "da25490ff9892aab3fcf7c36f08cfb902dd3e71ca0f9f9517bea02a73a5ce38c" 359 | dependencies = [ 360 | "proc-macro-error-attr", 361 | "proc-macro2", 362 | "quote", 363 | "syn 1.0.109", 364 | "version_check", 365 | ] 366 | 367 | [[package]] 368 | name = "proc-macro-error-attr" 369 | version = "1.0.4" 370 | source = "registry+https://github.com/rust-lang/crates.io-index" 371 | checksum = "a1be40180e52ecc98ad80b184934baf3d0d29f979574e439af5a55274b35f869" 372 | dependencies = [ 373 | "proc-macro2", 374 | "quote", 375 | "version_check", 376 | ] 377 | 378 | [[package]] 379 | name = "proc-macro2" 380 | version = "1.0.67" 381 | source = "registry+https://github.com/rust-lang/crates.io-index" 382 | checksum = "3d433d9f1a3e8c1263d9456598b16fec66f4acc9a74dacffd35c7bb09b3a1328" 383 | dependencies = [ 384 | "unicode-ident", 385 | ] 386 | 387 | [[package]] 388 | name = "quote" 389 | version = "1.0.33" 390 | source = "registry+https://github.com/rust-lang/crates.io-index" 391 | checksum = "5267fca4496028628a95160fc423a33e8b2e6af8a5302579e322e4b520293cae" 392 | dependencies = [ 393 | "proc-macro2", 394 | ] 395 | 396 | [[package]] 397 | name = "rand_core" 398 | version = "0.6.4" 399 | source = "registry+https://github.com/rust-lang/crates.io-index" 400 | checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c" 401 | 402 | [[package]] 403 | name = "rtic" 404 | version = "2.0.1" 405 | source = "registry+https://github.com/rust-lang/crates.io-index" 406 | checksum = "857ce76a2517808a303bcb7e5b6d4a9c1d84e5de88b302aec2e53744633c0f4d" 407 | dependencies = [ 408 | "atomic-polyfill 1.0.3", 409 | "bare-metal 1.0.0", 410 | "cortex-m", 411 | "critical-section", 412 | "rtic-core", 413 | "rtic-macros", 414 | ] 415 | 416 | [[package]] 417 | name = "rtic-common" 418 | version = "1.0.0" 419 | source = "registry+https://github.com/rust-lang/crates.io-index" 420 | checksum = "0786b50b81ef9d2a944a000f60405bb28bf30cd45da2d182f3fe636b2321f35c" 421 | dependencies = [ 422 | "critical-section", 423 | ] 424 | 425 | [[package]] 426 | name = "rtic-core" 427 | version = "1.0.0" 428 | source = "registry+https://github.com/rust-lang/crates.io-index" 429 | checksum = "d9369355b04d06a3780ec0f51ea2d225624db777acbc60abd8ca4832da5c1a42" 430 | 431 | [[package]] 432 | name = "rtic-macros" 433 | version = "2.0.1" 434 | source = "registry+https://github.com/rust-lang/crates.io-index" 435 | checksum = "8617533990b728e31bc65fcec8fec51fa1b4000fb33189ebeb05fb9d8625444d" 436 | dependencies = [ 437 | "indexmap", 438 | "proc-macro-error", 439 | "proc-macro2", 440 | "quote", 441 | "syn 1.0.109", 442 | ] 443 | 444 | [[package]] 445 | name = "rtic-monotonic" 446 | version = "1.0.0" 447 | source = "registry+https://github.com/rust-lang/crates.io-index" 448 | checksum = "fb8b0b822d1a366470b9cea83a1d4e788392db763539dc4ba022bcc787fece82" 449 | 450 | [[package]] 451 | name = "rtic-monotonics" 452 | version = "1.1.0" 453 | source = "registry+https://github.com/rust-lang/crates.io-index" 454 | checksum = "77e9f99e2119d5bdd927c45a31ef9e8d9888f02ede3e357bcbd7ab79c426e02d" 455 | dependencies = [ 456 | "atomic-polyfill 1.0.3", 457 | "cfg-if", 458 | "cortex-m", 459 | "fugit", 460 | "rtic-time", 461 | ] 462 | 463 | [[package]] 464 | name = "rtic-sync" 465 | version = "1.0.2" 466 | source = "registry+https://github.com/rust-lang/crates.io-index" 467 | checksum = "adc586db4c7ee4a85482ebb0542f34ffaee16bd8b8c552081f07492f531aca9f" 468 | dependencies = [ 469 | "critical-section", 470 | "heapless", 471 | "rtic-common", 472 | ] 473 | 474 | [[package]] 475 | name = "rtic-time" 476 | version = "1.0.0" 477 | source = "registry+https://github.com/rust-lang/crates.io-index" 478 | checksum = "55127cfa37ad32522eca2b70a12298bdb035c75ee3a4e403af8773ffe1a64bd3" 479 | dependencies = [ 480 | "critical-section", 481 | "futures-util", 482 | "rtic-common", 483 | ] 484 | 485 | [[package]] 486 | name = "rustc_version" 487 | version = "0.2.3" 488 | source = "registry+https://github.com/rust-lang/crates.io-index" 489 | checksum = "138e3e0acb6c9fb258b19b67cb8abd63c00679d2851805ea151465464fe9030a" 490 | dependencies = [ 491 | "semver 0.9.0", 492 | ] 493 | 494 | [[package]] 495 | name = "rustc_version" 496 | version = "0.4.0" 497 | source = "registry+https://github.com/rust-lang/crates.io-index" 498 | checksum = "bfa0f585226d2e68097d4f95d113b15b83a82e819ab25717ec0590d9584ef366" 499 | dependencies = [ 500 | "semver 1.0.18", 501 | ] 502 | 503 | [[package]] 504 | name = "ryu" 505 | version = "1.0.15" 506 | source = "registry+https://github.com/rust-lang/crates.io-index" 507 | checksum = "1ad4cc8da4ef723ed60bced201181d83791ad433213d8c24efffda1eec85d741" 508 | 509 | [[package]] 510 | name = "scopeguard" 511 | version = "1.2.0" 512 | source = "registry+https://github.com/rust-lang/crates.io-index" 513 | checksum = "94143f37725109f92c262ed2cf5e59bce7498c01bcc1502d7b9afe439a4e9f49" 514 | 515 | [[package]] 516 | name = "semver" 517 | version = "0.9.0" 518 | source = "registry+https://github.com/rust-lang/crates.io-index" 519 | checksum = "1d7eb9ef2c18661902cc47e535f9bc51b78acd254da71d375c2f6720d9a40403" 520 | dependencies = [ 521 | "semver-parser", 522 | ] 523 | 524 | [[package]] 525 | name = "semver" 526 | version = "1.0.18" 527 | source = "registry+https://github.com/rust-lang/crates.io-index" 528 | checksum = "b0293b4b29daaf487284529cc2f5675b8e57c61f70167ba415a463651fd6a918" 529 | 530 | [[package]] 531 | name = "semver-parser" 532 | version = "0.7.0" 533 | source = "registry+https://github.com/rust-lang/crates.io-index" 534 | checksum = "388a1df253eca08550bef6c72392cfe7c30914bf41df5269b68cbd6ff8f570a3" 535 | 536 | [[package]] 537 | name = "serde" 538 | version = "1.0.188" 539 | source = "registry+https://github.com/rust-lang/crates.io-index" 540 | checksum = "cf9e0fcba69a370eed61bcf2b728575f726b50b55cba78064753d708ddc7549e" 541 | dependencies = [ 542 | "serde_derive", 543 | ] 544 | 545 | [[package]] 546 | name = "serde-json-core" 547 | version = "0.5.1" 548 | source = "registry+https://github.com/rust-lang/crates.io-index" 549 | checksum = "3c9e1ab533c0bc414c34920ec7e5f097101d126ed5eac1a1aac711222e0bbb33" 550 | dependencies = [ 551 | "heapless", 552 | "ryu", 553 | "serde", 554 | ] 555 | 556 | [[package]] 557 | name = "serde_derive" 558 | version = "1.0.188" 559 | source = "registry+https://github.com/rust-lang/crates.io-index" 560 | checksum = "4eca7ac642d82aa35b60049a6eccb4be6be75e599bd2e9adb5f875a737654af2" 561 | dependencies = [ 562 | "proc-macro2", 563 | "quote", 564 | "syn 2.0.37", 565 | ] 566 | 567 | [[package]] 568 | name = "shared-bus-rtic" 569 | version = "0.2.2" 570 | source = "registry+https://github.com/rust-lang/crates.io-index" 571 | checksum = "cb1899470d03c5728db375f63be8f2bbfb93d8c35ec932061f3b593434c2273b" 572 | dependencies = [ 573 | "embedded-hal 0.2.7", 574 | "nb 1.1.0", 575 | ] 576 | 577 | [[package]] 578 | name = "spin" 579 | version = "0.9.8" 580 | source = "registry+https://github.com/rust-lang/crates.io-index" 581 | checksum = "6980e8d7511241f8acf4aebddbb1ff938df5eebe98691418c4468d0b72a96a67" 582 | dependencies = [ 583 | "lock_api", 584 | ] 585 | 586 | [[package]] 587 | name = "stable_deref_trait" 588 | version = "1.2.0" 589 | source = "registry+https://github.com/rust-lang/crates.io-index" 590 | checksum = "a8f112729512f8e442d81f95a8a7ddf2b7c6b8a1a6f509a95864142b30cab2d3" 591 | 592 | [[package]] 593 | name = "stm32f4" 594 | version = "0.15.1" 595 | source = "registry+https://github.com/rust-lang/crates.io-index" 596 | checksum = "fb94729242cd1aebe6dab42a2ca0131985ae93bc3ab2751b680df724bb35528d" 597 | dependencies = [ 598 | "bare-metal 1.0.0", 599 | "cortex-m", 600 | "cortex-m-rt", 601 | "vcell", 602 | ] 603 | 604 | [[package]] 605 | name = "stm32f4xx-hal" 606 | version = "0.14.0" 607 | source = "registry+https://github.com/rust-lang/crates.io-index" 608 | checksum = "dbb045e607d0946eca27523141d192d7ee91aea0dff9736be100b8fc0c186abf" 609 | dependencies = [ 610 | "bare-metal 1.0.0", 611 | "bitflags", 612 | "cortex-m", 613 | "cortex-m-rt", 614 | "embedded-dma", 615 | "embedded-hal 0.2.7", 616 | "embedded-hal 1.0.0-alpha.8", 617 | "embedded-storage", 618 | "fugit", 619 | "fugit-timer", 620 | "nb 1.1.0", 621 | "rand_core", 622 | "rtic-monotonic", 623 | "stm32f4", 624 | "synopsys-usb-otg", 625 | "systick-monotonic", 626 | "time", 627 | "void", 628 | ] 629 | 630 | [[package]] 631 | name = "syn" 632 | version = "1.0.109" 633 | source = "registry+https://github.com/rust-lang/crates.io-index" 634 | checksum = "72b64191b275b66ffe2469e8af2c1cfe3bafa67b529ead792a6d0160888b4237" 635 | dependencies = [ 636 | "proc-macro2", 637 | "quote", 638 | "unicode-ident", 639 | ] 640 | 641 | [[package]] 642 | name = "syn" 643 | version = "2.0.37" 644 | source = "registry+https://github.com/rust-lang/crates.io-index" 645 | checksum = "7303ef2c05cd654186cb250d29049a24840ca25d2747c25c0381c8d9e2f582e8" 646 | dependencies = [ 647 | "proc-macro2", 648 | "quote", 649 | "unicode-ident", 650 | ] 651 | 652 | [[package]] 653 | name = "synopsys-usb-otg" 654 | version = "0.3.2" 655 | source = "registry+https://github.com/rust-lang/crates.io-index" 656 | checksum = "678f3707a7b1fd4863023292c42f73c6bab0e9b0096f41ae612d1af0ff221b45" 657 | dependencies = [ 658 | "cortex-m", 659 | "embedded-hal 0.2.7", 660 | "usb-device", 661 | "vcell", 662 | ] 663 | 664 | [[package]] 665 | name = "systick-monotonic" 666 | version = "1.0.1" 667 | source = "registry+https://github.com/rust-lang/crates.io-index" 668 | checksum = "67fb822d5c615a0ae3a4795ee5b1d06381c7faf488d861c0a4fa8e6a88d5ff84" 669 | dependencies = [ 670 | "cortex-m", 671 | "fugit", 672 | "rtic-monotonic", 673 | ] 674 | 675 | [[package]] 676 | name = "time" 677 | version = "0.3.28" 678 | source = "registry+https://github.com/rust-lang/crates.io-index" 679 | checksum = "17f6bb557fd245c28e6411aa56b6403c689ad95061f50e4be16c274e70a17e48" 680 | dependencies = [ 681 | "deranged", 682 | "time-core", 683 | ] 684 | 685 | [[package]] 686 | name = "time-core" 687 | version = "0.1.1" 688 | source = "registry+https://github.com/rust-lang/crates.io-index" 689 | checksum = "7300fbefb4dadc1af235a9cef3737cea692a9d97e1b9cbcd4ebdae6f8868e6fb" 690 | 691 | [[package]] 692 | name = "unicode-ident" 693 | version = "1.0.12" 694 | source = "registry+https://github.com/rust-lang/crates.io-index" 695 | checksum = "3354b9ac3fae1ff6755cb6db53683adb661634f67557942dea4facebec0fee4b" 696 | 697 | [[package]] 698 | name = "usb-device" 699 | version = "0.2.9" 700 | source = "registry+https://github.com/rust-lang/crates.io-index" 701 | checksum = "1f6cc3adc849b5292b4075fc0d5fdcf2f24866e88e336dd27a8943090a520508" 702 | 703 | [[package]] 704 | name = "usbd-serial" 705 | version = "0.1.1" 706 | source = "registry+https://github.com/rust-lang/crates.io-index" 707 | checksum = "db75519b86287f12dcf0d171c7cf4ecc839149fe9f3b720ac4cfce52959e1dfe" 708 | dependencies = [ 709 | "embedded-hal 0.2.7", 710 | "nb 0.1.3", 711 | "usb-device", 712 | ] 713 | 714 | [[package]] 715 | name = "usbd-webusb" 716 | version = "1.0.2" 717 | source = "registry+https://github.com/rust-lang/crates.io-index" 718 | checksum = "ed33ecaa7a26365f13059e753bfa23f0a4a557565499f46d255c51e737464bd8" 719 | dependencies = [ 720 | "usb-device", 721 | ] 722 | 723 | [[package]] 724 | name = "vcell" 725 | version = "0.1.3" 726 | source = "registry+https://github.com/rust-lang/crates.io-index" 727 | checksum = "77439c1b53d2303b20d9459b1ade71a83c716e3f9c34f3228c00e6f185d6c002" 728 | 729 | [[package]] 730 | name = "version_check" 731 | version = "0.9.4" 732 | source = "registry+https://github.com/rust-lang/crates.io-index" 733 | checksum = "49874b5167b65d7193b8aba1567f5c7d93d001cafc34600cee003eda787e483f" 734 | 735 | [[package]] 736 | name = "void" 737 | version = "1.0.2" 738 | source = "registry+https://github.com/rust-lang/crates.io-index" 739 | checksum = "6a02e4885ed3bc0f2de90ea6dd45ebcbb66dacffe03547fadbb0eeae2770887d" 740 | 741 | [[package]] 742 | name = "volatile-register" 743 | version = "0.2.1" 744 | source = "registry+https://github.com/rust-lang/crates.io-index" 745 | checksum = "9ee8f19f9d74293faf70901bc20ad067dc1ad390d2cbf1e3f75f721ffee908b6" 746 | dependencies = [ 747 | "vcell", 748 | ] 749 | 750 | [[package]] 751 | name = "w25q" 752 | version = "0.2.9" 753 | source = "registry+https://github.com/rust-lang/crates.io-index" 754 | checksum = "2ce302bf815ae84f5de90a61df0f9c2698069cb13e73342717a1c9e14a73dc94" 755 | dependencies = [ 756 | "bitflags", 757 | "embedded-hal 0.2.7", 758 | ] 759 | -------------------------------------------------------------------------------- /src/main.rs: -------------------------------------------------------------------------------- 1 | #![feature(type_alias_impl_trait)] 2 | #![feature(exclusive_range_pattern)] 3 | #![feature(proc_macro_hygiene)] 4 | #![feature(int_roundings)] 5 | #![feature(is_some_and)] 6 | #![feature(stdsimd)] 7 | #![feature(async_closure)] 8 | 9 | #![no_main] 10 | #![no_std] 11 | 12 | #![allow(stable_features)] 13 | #![allow(unused_mut)] 14 | 15 | use rtic; 16 | use rtic_monotonics::systick::*; 17 | use rtic_sync::{channel::*, make_channel}; 18 | 19 | use w25q::series25::FlashInfo; 20 | use arrayvec::ArrayVec; 21 | 22 | use usb_device::{bus::UsbBusAllocator, device::UsbDevice}; 23 | use usbd_serial::SerialPort; 24 | use usbd_webusb::{url_scheme, WebUsb}; 25 | 26 | use stm32f4xx_hal::{ 27 | adc::{ 28 | config::{AdcConfig, Dma, SampleTime, Scan, Sequence}, 29 | Adc, Temperature, 30 | }, 31 | dma::{config::DmaConfig, PeripheralToMemory, Stream0, StreamsTuple, Transfer}, 32 | crc32, 33 | crc32::Crc32, 34 | gpio::{Edge, Input}, 35 | otg_fs, 36 | otg_fs::{USB, UsbBusType}, 37 | pac::{ADC1, TIM13, TIM2, TIM3, TIM5,DMA2,ADC2}, 38 | prelude::*, 39 | spi::*, 40 | timer::{self, Event}, 41 | watchdog::IndependentWatchdog, 42 | }; 43 | 44 | use panic_halt as _; 45 | 46 | #[rtic::app(device = stm32f4xx_hal::pac, peripherals = true, dispatchers = [TIM4, TIM7, TIM8_CC])] 47 | mod app { 48 | use my_module::blink2; 49 | 50 | use crate::app::{ 51 | engine::{ 52 | cpwm::VRStatus, 53 | efi_cfg::{EngineConfig, get_default_efi_cfg}, 54 | engine_status::{EngineStatus, get_default_engine_status}, 55 | pmic::{PMIC, PmicT}, 56 | sensors::{get_sensor_raw, SensorTypes, SensorValues}, 57 | }, 58 | gpio::{ 59 | ADCMapping, 60 | AuxIoMapping, 61 | IgnitionGpioMapping, 62 | init_gpio, 63 | InjectionGpioMapping, 64 | LedGpioMapping, 65 | RelayMapping, 66 | StepperMapping, 67 | }, 68 | injection::{calculate_time_isr, injection_setup}, 69 | ignition::{ignition_checks, ignition_trigger}, 70 | logging::host, 71 | memory::tables::{SpiT, Tables}, 72 | webserial::{ 73 | finish_message, 74 | handle_engine::engine_cdc_callback, 75 | handle_pmic::pmic_cdc_callback, 76 | handle_realtime_data::realtime_data_cdc_callback, 77 | handle_tables::table_cdc_callback, 78 | send_message, 79 | SerialMessage, 80 | SerialStatus, 81 | }, 82 | tasks::{engine::ckp_checks/* , engine::motor_checks, ignition::ignition_schedule */}, 83 | }; 84 | use crate::app::engine::sensors; 85 | use crate::app::tasks::engine::ckp_trigger; 86 | 87 | use super::*; 88 | 89 | mod my_module; 90 | // pub mod debug; 91 | pub mod engine; 92 | pub mod gpio; 93 | pub mod injection; 94 | pub mod ignition; 95 | pub mod logging; 96 | pub mod memory; 97 | pub mod util; 98 | pub mod webserial; 99 | pub mod tasks; 100 | 101 | 102 | 103 | type DMATransfer = 104 | Transfer, 0, Adc, PeripheralToMemory, &'static mut [u16; 6]>; 105 | 106 | #[shared] 107 | struct Shared { 108 | // Timers: 109 | timer: timer::CounterUs, 110 | timer3: timer::CounterUs, 111 | timer4: timer::CounterUs, 112 | timer13: timer::DelayUs, 113 | 114 | // Core I/O 115 | led: LedGpioMapping, 116 | inj_pins: InjectionGpioMapping, 117 | ign_pins: IgnitionGpioMapping, 118 | aux_pins: AuxIoMapping, 119 | relay_pins: RelayMapping, 120 | stepper_pins: StepperMapping, 121 | adc_transfer: DMATransfer, 122 | 123 | // USB: 124 | // TODO: se puede reducir implementando los channels de RTIC, de paso fixeo el bug de las tablas 125 | usb_cdc: SerialPort<'static, UsbBusType, [u8; 128], [u8; 512]>, 126 | usb_web: WebUsb, 127 | cdc_sender: Sender<'static, SerialMessage, CDC_BUFF_CAPACITY>, 128 | 129 | // core: 130 | spi_lock: bool, 131 | crc: Crc32, 132 | 133 | // EFI Related: 134 | efi_cfg: EngineConfig, 135 | efi_status: EngineStatus, 136 | flash: memory::tables::FlashT, 137 | flash_info: FlashInfo, 138 | tables: Tables, 139 | sensors: SensorValues, 140 | pmic: PmicT, 141 | 142 | // CKP/SYNC 143 | ckp: VRStatus, 144 | ignition_running: bool, 145 | } 146 | 147 | #[local] 148 | struct Local { 149 | // core 150 | usb_dev: UsbDevice<'static, UsbBusType>, 151 | cdc_input_buffer: ArrayVec, 152 | watchdog: IndependentWatchdog, 153 | adc_buffer: Option<&'static mut [u16; 6]>, 154 | 155 | // EFI Related: 156 | ckp: stm32f4xx_hal::gpio::PC6, 157 | adc: Adc, 158 | analog_pins: ADCMapping, 159 | 160 | // cdc_sender: Sender<'static, u32, 8>, 161 | 162 | // TODO: Remove 163 | state: bool, 164 | state2: bool, 165 | 166 | // WebSerial: 167 | table_sender: Sender<'static, SerialMessage, CDC_BUFF_CAPACITY>, 168 | real_time_sender: Sender<'static, SerialMessage, CDC_BUFF_CAPACITY>, 169 | pmic_sender: Sender<'static, SerialMessage, CDC_BUFF_CAPACITY>, 170 | engine_sender: Sender<'static, SerialMessage, CDC_BUFF_CAPACITY>, 171 | 172 | // ignition, 173 | ign_channel_1: bool, 174 | } 175 | 176 | const CDC_BUFF_CAPACITY: usize = 30; 177 | 178 | #[init(local = [USB_BUS: Option < UsbBusAllocator < UsbBusType >> = None])] 179 | fn init(mut cx: init::Context) -> (Shared, Local) { 180 | // Setup clocks 181 | //let mut flash = cx.device.FLASH.constrain(); 182 | let mut device = cx.device; 183 | let mut rcc = device.RCC.constrain(); 184 | cx.core.DWT.enable_cycle_counter(); 185 | 186 | // Initialize the systick interrupt & obtain the token to prove that we did 187 | let systick_mono_token = rtic_monotonics::create_systick_token!(); 188 | Systick::start(cx.core.SYST, 120_000_000, systick_mono_token); // default STM32F407 clock-rate is 36MHz 189 | 190 | let _clocks = rcc.cfgr.use_hse(25.MHz()).sysclk(120.MHz()).require_pll48clk().freeze(); 191 | 192 | // TODO: Disable this in release builds 193 | debug!("Hello v1 :)"); 194 | 195 | let gpio_a = device.GPIOA.split(); 196 | let gpio_b = device.GPIOB.split(); 197 | let gpio_c = device.GPIOC.split(); 198 | let gpio_d = device.GPIOD.split(); 199 | let gpio_e = device.GPIOE.split(); 200 | 201 | let mut gpio_config = init_gpio(gpio_a, gpio_b, gpio_c, gpio_d, gpio_e); 202 | 203 | // ADC 204 | let dma = StreamsTuple::new(device.DMA2); 205 | 206 | let config = DmaConfig::default() 207 | .transfer_complete_interrupt(true) 208 | .memory_increment(true) 209 | .double_buffer(false); 210 | 211 | let adc_config = AdcConfig::default() 212 | .dma(Dma::Continuous) 213 | .scan(Scan::Enabled); 214 | 215 | let mut adc1 = Adc::adc1(device.ADC1, true, adc_config); 216 | 217 | // aca van todos los canales a revisar con DMA 218 | adc1.configure_channel(&gpio_config.adc_dma.tps, Sequence::One, SampleTime::Cycles_480); 219 | adc1.configure_channel(&gpio_config.adc_dma.clt, Sequence::Two, SampleTime::Cycles_480); 220 | adc1.configure_channel(&gpio_config.adc_dma.iat, Sequence::Three, SampleTime::Cycles_480); 221 | adc1.configure_channel(&gpio_config.adc_dma.map, Sequence::Four, SampleTime::Cycles_480); 222 | adc1.configure_channel(&gpio_config.adc_dma.o2, Sequence::Five, SampleTime::Cycles_480); 223 | adc1.configure_channel(&gpio_config.adc_dma.vbatt, Sequence::Six, SampleTime::Cycles_480); 224 | 225 | let adc_first_buffer = cortex_m::singleton!(: [u16; 6] = [0; 6]).unwrap(); 226 | let adc_second_buffer = Some(cortex_m::singleton!(: [u16; 6] = [0; 6]).unwrap()); 227 | // Give the first buffer to the DMA. The second buffer is held in an Option in `local.buffer` until the transfer is complete 228 | let adc_transfer = Transfer::init_peripheral_to_memory(dma.0, adc1, adc_first_buffer, None, config); 229 | 230 | let mut adc = Adc::adc2(device.ADC2, true, AdcConfig::default()); 231 | adc.enable(); 232 | //adc.calibrate(); 233 | 234 | // configure CKP/CMP Pin for Interrupts 235 | let mut ckp = gpio_config.ckp; 236 | let mut syscfg = device.SYSCFG.constrain(); 237 | debug!("init gpio"); 238 | ckp.make_interrupt_source(&mut syscfg); 239 | ckp.trigger_on_edge(&mut device.EXTI, Edge::Falling); 240 | 241 | // configure the timers 242 | 243 | // timer Tiempo inyeccion 244 | let mut timer: timer::CounterUs = device.TIM2.counter_us(&_clocks); 245 | 246 | // timer tiempo de ignicion 247 | let mut timer3: timer::CounterUs = device.TIM3.counter_us(&_clocks); 248 | 249 | // timer CPWM 250 | let mut timer4: timer::CounterUs = device.TIM5.counter_us(&_clocks); 251 | 252 | // timer uso generico 253 | let mut timer13: timer::DelayUs = device.TIM13.delay_us(&_clocks); 254 | 255 | // TIM5, TIM7, TIM4 256 | debug!("init timers"); 257 | // timer.start((150).millis()).unwrap(); 258 | 259 | // Set up to generate interrupt when timer expires 260 | timer.listen(Event::Update); 261 | timer3.listen(Event::Update); 262 | timer13.listen(Event::Update); 263 | // timer4.start((70).minutes()).unwrap(); 264 | timer4.start(1_000_000_u32.micros()).unwrap(); 265 | 266 | let mut efi_cfg = get_default_efi_cfg(); 267 | let mut _efi_status = get_default_engine_status(); 268 | 269 | // SPI: 270 | let mode = Mode { 271 | polarity: Polarity::IdleLow, 272 | phase: Phase::CaptureOnFirstTransition, 273 | }; 274 | 275 | let spi2 = Spi::new( 276 | device.SPI2, 277 | (gpio_config.spi_sck, gpio_config.spi_miso, gpio_config.spi_mosi), 278 | mode, 279 | (30).MHz(), 280 | &_clocks, 281 | ); 282 | 283 | let spi_bus = shared_bus_rtic::new!(spi2, SpiT); 284 | let spi_pmic = spi_bus.acquire(); 285 | 286 | // CRC32: 287 | let mut crc = crc32::Crc32::new(device.CRC); 288 | host::debug!("init flash"); 289 | let mut flash = w25q::series25::Flash::init(spi_bus.acquire(), gpio_config.memory_cs).unwrap(); 290 | 291 | // let id = flash.read_jedec_id().unwrap(); 292 | 293 | let flash_info = flash.get_device_info().unwrap(); 294 | 295 | // EFI Setup: 296 | let mut table = Tables { 297 | tps_rpm_ve: None, 298 | tps_rpm_afr: None, 299 | injector_delay: None, 300 | load_tps_deg: None, 301 | // get not implemented 302 | vbat_correction: None, 303 | wue: None, 304 | ase_taper: None, 305 | ase_intensity: None, 306 | }; 307 | 308 | efi_cfg.read(&mut flash, &flash_info, &mut crc); 309 | 310 | injection_setup(&mut table, &mut flash, &flash_info, &mut crc); 311 | 312 | // REMOVE: solo lo estoy hardcodeando aca para probar el AlphaN 313 | _efi_status.rpm = 1500; 314 | 315 | calculate_time_isr(&mut _efi_status, &efi_cfg, &mut table); 316 | 317 | host::debug!("AirFlow {:?} g/s", _efi_status.injection.air_flow); 318 | host::debug!("AF/r {:?}", _efi_status.injection.targetAFR); 319 | host::debug!("Base Fuel {:?} cm3", _efi_status.injection.base_fuel); 320 | host::debug!("Base Air {:?} cm3", _efi_status.injection.base_air); 321 | host::debug!("Time {:?} mS", _efi_status.injection.injection_bank_1_time); 322 | 323 | let mut sensors = SensorValues::new(); 324 | 325 | let mut spi_lock = false; 326 | let mut pmic = PMIC::init(spi_pmic, gpio_config.pmic.pmic1_cs).unwrap(); 327 | 328 | // ckp.enable_interrupt(&mut device.EXTI); 329 | 330 | let mut ckp_status = VRStatus::new(); 331 | 332 | // Init USB 333 | let usb = USB { 334 | usb_global: device.OTG_FS_GLOBAL, 335 | usb_device: device.OTG_FS_DEVICE, 336 | usb_pwrclk: device.OTG_FS_PWRCLK, 337 | pin_dm: gpio_config.usb_dp, 338 | pin_dp: gpio_config.usb_dm, 339 | hclk: _clocks.hclk(), 340 | }; 341 | 342 | static mut EP_MEMORY: [u32; 1024] = [0; 1024]; 343 | static mut __USB_TX: [u8; 512] = [0; 512]; 344 | static mut __USB_RX: [u8; 128] = [0; 128]; 345 | host::debug!("init usb"); 346 | let usb_bus = cx.local.USB_BUS; 347 | unsafe { 348 | *usb_bus = Some(otg_fs::UsbBus::new(usb, &mut EP_MEMORY)); 349 | } 350 | 351 | let usb_cdc = unsafe { 352 | SerialPort::new_with_store(usb_bus.as_ref().unwrap(), __USB_RX, __USB_TX) 353 | }; 354 | let usb_web = WebUsb::new( 355 | usb_bus.as_ref().unwrap(), 356 | url_scheme::HTTPS, 357 | "tuner.openefi.tech", 358 | ); 359 | 360 | let usb_dev = webserial::new_device(usb_bus.as_ref().unwrap()); 361 | 362 | let cdc_buff = ArrayVec::::new(); 363 | 364 | 365 | // DEMO 366 | // Schedule the blinking task 367 | blink::spawn().ok(); 368 | blink2::spawn().ok(); 369 | 370 | let (cdc_sender, cdc_receiver) = make_channel!(SerialMessage, CDC_BUFF_CAPACITY); 371 | 372 | cdc_receiver::spawn(cdc_receiver).unwrap(); 373 | polling_adc::spawn().unwrap(); 374 | 375 | let mut watchdog = IndependentWatchdog::new(device.IWDG); 376 | // se puede desactivar en debug 377 | // watchdog.start(100.millis()); 378 | // watch_dog_update::spawn().unwrap(); 379 | 380 | (Shared { 381 | // Timers: 382 | // delay, 383 | timer, 384 | timer3, 385 | timer4, 386 | timer13, 387 | 388 | // USB 389 | usb_cdc, 390 | usb_web, 391 | cdc_sender: cdc_sender.clone(), 392 | 393 | // GPIO: 394 | led: gpio_config.led, 395 | inj_pins: gpio_config.injection, 396 | ign_pins: gpio_config.ignition, 397 | aux_pins: gpio_config.aux, 398 | relay_pins: gpio_config.relay, 399 | stepper_pins: gpio_config.stepper, 400 | sensors, 401 | 402 | // CORE: 403 | crc, 404 | flash, 405 | flash_info, 406 | spi_lock, 407 | adc_transfer, 408 | 409 | // EFI Related 410 | efi_cfg, 411 | efi_status: _efi_status, 412 | tables: table, 413 | pmic, 414 | 415 | //CKP/SYNC 416 | ckp: ckp_status, 417 | ignition_running: false, 418 | }, Local { 419 | // USB 420 | usb_dev, 421 | cdc_input_buffer: cdc_buff, 422 | watchdog, 423 | 424 | adc_buffer: adc_second_buffer, 425 | 426 | // Serial 427 | table_sender: cdc_sender.clone(), 428 | real_time_sender: cdc_sender.clone(), 429 | pmic_sender: cdc_sender.clone(), 430 | engine_sender: cdc_sender.clone(), 431 | 432 | adc, 433 | ckp, 434 | analog_pins: gpio_config.adc, 435 | 436 | state: false, 437 | state2: false, 438 | // ignition, 439 | ign_channel_1: false, 440 | }) 441 | } 442 | #[idle()] 443 | fn idle(cx: idle::Context) -> ! { 444 | // Locals in idle have lifetime 'static 445 | //let _x: &'static mut u32 = cx.local.x; 446 | loop { 447 | cortex_m::asm::nop(); 448 | ckp_checks::spawn().unwrap(); 449 | //Systick::delay(250.millis()).await; 450 | } 451 | } 452 | 453 | #[task(shared = [adc_transfer] ,priority = 3)] 454 | async fn polling_adc(mut cx: polling_adc::Context) { 455 | loop { 456 | cx.shared.adc_transfer.lock(|transfer| { 457 | transfer.start(|adc| { 458 | adc.start_conversion(); 459 | }); 460 | }); 461 | Systick::delay(250.millis()).await; 462 | } 463 | } 464 | 465 | #[task(binds = DMA2_STREAM0, shared = [adc_transfer,sensors], local = [adc_buffer])] 466 | fn sensors_adc_dma(mut cx: sensors_adc_dma::Context) { 467 | 468 | let (buffer, sample_to_millivolts) = cx.shared.adc_transfer.lock(|transfer| { 469 | // When the DMA completes it will return the buffer we gave it last time - we now store that as `buffer` 470 | // We still have our other buffer waiting in `local.buffer`, so `take` that and give it to the `transfer` 471 | let (buffer, _) = transfer 472 | .next_transfer(cx.local.adc_buffer.take().unwrap()) 473 | .unwrap(); 474 | 475 | let sample_to_millivolts = transfer.peripheral().make_sample_to_millivolts(); 476 | (buffer, sample_to_millivolts) 477 | }); 478 | 479 | // Pull the ADC data out of the buffer that the DMA transfer gave us 480 | 481 | let raw_tps = buffer[0]; 482 | let raw_clt = buffer[1]; 483 | let raw_iat = buffer[2]; 484 | let raw_map = buffer[3]; 485 | let raw_o2 = buffer[4]; 486 | let raw_vbatt = buffer[5]; 487 | 488 | // Now that we're finished with this buffer, put it back in `local.buffer` so it's ready for the next transfer 489 | // If we don't do this before the next transfer, we'll get a panic 490 | *cx.local.adc_buffer = Some(buffer); 491 | 492 | cx.shared.sensors.lock(|s| { s.update(sample_to_millivolts(raw_tps),SensorTypes::TPS) }); 493 | cx.shared.sensors.lock(|s| { s.update(sample_to_millivolts(raw_clt),SensorTypes::CooltanTemp) }); 494 | cx.shared.sensors.lock(|s| { s.update(sample_to_millivolts(raw_iat),SensorTypes::AirTemp) }); 495 | cx.shared.sensors.lock(|s| { s.update(sample_to_millivolts(raw_map),SensorTypes::MAP) }); 496 | cx.shared.sensors.lock(|s| { s.update(sample_to_millivolts(raw_o2),SensorTypes::ExternalLambda) }); 497 | cx.shared.sensors.lock(|s| { s.update(sample_to_millivolts(raw_vbatt),SensorTypes::BatteryVoltage) }); 498 | 499 | } 500 | 501 | #[task(local = [watchdog],priority = 3)] 502 | async fn watch_dog_update(mut ctx: watch_dog_update::Context) { 503 | loop { 504 | ctx.local.watchdog.feed(); 505 | Systick::delay(50.millis()).await; 506 | } 507 | } 508 | 509 | #[task(local = [state], shared = [led],priority = 1)] 510 | async fn blink(mut cx: blink::Context) { 511 | loop { 512 | if *cx.local.state { 513 | cx.shared.led.lock(|l| { l.led_0.set_high() }); 514 | *cx.local.state = false; 515 | } else { 516 | cx.shared.led.lock(|l| { l.led_0.set_low() }); 517 | *cx.local.state = true; 518 | } 519 | Systick::delay(100.millis()).await; 520 | } 521 | } 522 | 523 | //TODO: reciclar para encendido 524 | #[task(binds = TIM2, local = [], shared = [timer, led])] 525 | fn timer2_exp(mut ctx: timer2_exp::Context) { 526 | ctx.shared.timer.lock(|tim| tim.clear_interrupt(Event::Update)); 527 | 528 | ctx.shared.led.lock(|l| l.led_0.toggle()); 529 | } 530 | 531 | #[task(binds = TIM3, local = [], shared = [timer3, led])] 532 | fn timer3_exp(mut ctx: timer3_exp::Context) { 533 | ctx.shared.timer3.lock(|tim| { 534 | tim.clear_interrupt(Event::Update); 535 | tim.cancel().unwrap(); 536 | }); 537 | 538 | ctx.shared.led.lock(|l| l.led_check.set_high()); 539 | } 540 | 541 | #[task(binds = OTG_FS, local = [usb_dev, cdc_input_buffer], shared = [usb_cdc, usb_web, cdc_sender])] 542 | fn usb_handler(mut ctx: usb_handler::Context) { 543 | let device = ctx.local.usb_dev; 544 | 545 | ctx.shared.usb_cdc.lock(|cdc| { 546 | // USB dev poll only in the interrupt handler 547 | (ctx.shared.usb_web, ctx.shared.cdc_sender).lock(|web, cdc_sender| { 548 | if device.poll(&mut [web, cdc]) { 549 | let mut buf = [0u8; 64]; 550 | 551 | match cdc.read(&mut buf[..]) { 552 | Ok(count) => { 553 | // Push bytes into the buffer 554 | for i in 0..count { 555 | ctx.local.cdc_input_buffer.push(buf[i]); 556 | if ctx.local.cdc_input_buffer.is_full() { 557 | webserial::process_command( 558 | ctx.local.cdc_input_buffer.take().into_inner().unwrap(), 559 | cdc_sender, 560 | ); 561 | 562 | ctx.local.cdc_input_buffer.clear(); 563 | } 564 | } 565 | } 566 | Err(_) => {} 567 | } 568 | } 569 | }); 570 | }); 571 | } 572 | 573 | // Externally defined tasks 574 | extern "Rust" { 575 | // Low-priority task to send back replies via the serial port. , capacity = 30 576 | #[task(shared = [usb_cdc], priority = 2)] 577 | async fn send_message( 578 | ctx: send_message::Context, 579 | status: SerialStatus, 580 | code: u8, 581 | message: SerialMessage, 582 | ); 583 | 584 | #[task(local = [table_sender], shared = [flash_info, efi_cfg, tables, crc, flash, spi_lock],priority = 1)] 585 | async fn table_cdc_callback(ctx: table_cdc_callback::Context, serial_cmd: SerialMessage); 586 | #[task(local = [real_time_sender], shared = [efi_status, sensors, crc],priority = 1)] 587 | async fn realtime_data_cdc_callback(ctx: realtime_data_cdc_callback::Context, serial_cmd: SerialMessage); 588 | 589 | #[task(local = [pmic_sender], shared = [efi_status, pmic, crc],priority = 1)] 590 | async fn pmic_cdc_callback(ctx: pmic_cdc_callback::Context, serial_cmd: SerialMessage); 591 | 592 | #[task(local = [engine_sender], shared = [flash, flash_info, efi_cfg, crc],priority = 1)] 593 | async fn engine_cdc_callback(ctx: engine_cdc_callback::Context, serial_cmd: SerialMessage); 594 | 595 | // from: https://github.com/noisymime/speeduino/blob/master/speeduino/decoders.ino#L453 596 | #[task(binds = EXTI9_5, local = [ckp], shared = [led, efi_status, flash_info, efi_cfg, timer, timer3, timer4, ckp, ign_pins], priority = 5)] 597 | fn ckp_trigger(ctx: ckp_trigger::Context); 598 | #[task(shared = [efi_cfg, ckp, timer4, efi_status, ignition_running],priority = 3)] 599 | async fn ckp_checks(ctx: ckp_checks::Context); 600 | 601 | #[task(shared = [efi_cfg, efi_status, ckp, timer4], local = [ign_channel_1],priority = 3)] 602 | async fn ignition_checks(ctx: ignition_checks::Context); 603 | 604 | #[task(shared = [efi_cfg, efi_status, ckp, timer3, led],priority = 3)] 605 | async fn ignition_trigger(ctx: ignition_trigger::Context, time: i32); 606 | 607 | // 608 | // #[task( 609 | // shared = [led, efi_status, efi_cfg, timer3, timer4, ckp, ign_pins], 610 | // priority = 4 611 | // )] 612 | // async fn ignition_schedule(ctx: ignition_schedule::Context); 613 | // 614 | } 615 | 616 | // Externally defined tasks 617 | extern "Rust" { 618 | #[task(local = [state2], shared = [led],priority = 1)] 619 | async fn blink2(cx: blink2::Context); 620 | } 621 | 622 | #[task(shared = [usb_cdc],priority = 2)] 623 | async fn cdc_receiver(mut ctx: cdc_receiver::Context, mut receiver: Receiver<'static, SerialMessage, CDC_BUFF_CAPACITY>) { 624 | while let Ok(message) = receiver.recv().await { 625 | ctx.shared.usb_cdc.lock(|cdc| { 626 | cdc.write(&finish_message(message)).unwrap(); 627 | }); 628 | } 629 | } 630 | } 631 | --------------------------------------------------------------------------------