From f6e5d6d899bd7972101db998a5f1a9d62e0e95f4 Mon Sep 17 00:00:00 2001 From: FDSoftware Date: Fri, 6 Oct 2023 20:15:38 -0300 Subject: [PATCH 1/2] update ignition task, added watchdog --- Cargo.toml | 4 +- src/app/engine/cpwm.rs | 3 +- src/app/ignition/mod.rs | 41 ++++++++++++++++++++ src/app/injection/mod.rs | 4 ++ src/app/memory/efi_cfg.rs | 16 ++++++-- src/app/tasks/engine.rs | 20 ---------- src/app/tasks/ignition.rs | 12 ------ src/app/webserial/handle_engine.rs | 13 ++++++- src/app/webserial/handle_tables.rs | 2 +- src/app/webserial/mod.rs | 2 +- src/main.rs | 61 +++++++++++++++++++++++++++--- 11 files changed, 130 insertions(+), 48 deletions(-) create mode 100644 src/app/ignition/mod.rs delete mode 100644 src/app/tasks/ignition.rs diff --git a/Cargo.toml b/Cargo.toml index 004fe8e4..0e089d41 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -59,5 +59,5 @@ version = "2.0.1" features = ["thumbv7-backend"] [dependencies.rtic-monotonics] -version = "1.0.0" -features = ["cortex-m-systick"] +version = "1.1.0" +features = ["cortex-m-systick", "systick-10khz"] \ No newline at end of file diff --git a/src/app/engine/cpwm.rs b/src/app/engine/cpwm.rs index 9a9d4402..57cca24d 100644 --- a/src/app/engine/cpwm.rs +++ b/src/app/engine/cpwm.rs @@ -1,5 +1,6 @@ use crate::app::engine::efi_cfg::VRSensor; +#[derive(Copy, Clone)] pub struct VRStatus { pub current_time: u32, pub current_gap: u32, @@ -96,7 +97,7 @@ pub fn time_to_angle(trigger: &VRStatus, time: &u32) -> i32 { return (*time as f32 * trigger.degreesPeruSx32768) as i32 / 32768; } -pub fn get_crank_angle(trigger: &mut VRStatus, trigger_config: &VRSensor, cpu_tick: u32) -> i32 { +pub fn get_crank_angle(trigger: &VRStatus, trigger_config: &VRSensor, cpu_tick: u32) -> i32 { const CRANK_ANGLE_MAX: i32 = 360; //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. diff --git a/src/app/ignition/mod.rs b/src/app/ignition/mod.rs new file mode 100644 index 00000000..d71cb29a --- /dev/null +++ b/src/app/ignition/mod.rs @@ -0,0 +1,41 @@ +use rtic::Mutex; +use crate::app; +use rtic_monotonics::systick::*; +use rtic_monotonics::systick::fugit::{Duration, TimerDurationU32}; +use crate::app::engine::cpwm::{angle_to_time, get_crank_angle}; + +pub async fn ignition_checks(mut ctx: app::ignition_checks::Context<'_>){ + loop { + let mut cycle_time = ctx.shared.timer4.lock(|t4| t4.now().ticks()); + let ckp = ctx.shared.ckp.lock(|ckp| ckp.clone()); + let ckp_config = ctx.shared.efi_cfg.lock(|cfg| cfg.engine.ckp); + + // ignition timing: + let mut crank_angle = get_crank_angle(&ckp, &ckp_config, cycle_time); + const CRANK_ANGLE_MAX_IGN: i32 = 720; //ponele? no entendi bien como se setea dependiendo el tipo de encendido/cilindros + 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 + + let dwell_angle = 20; //TODO: get from table + let dwell_time = angle_to_time(&ckp,&dwell_angle); + + // esto luego se repetiria para el resto de canales + // TODO: compensar drift del clock por lo que tarda en ejectuarse la task + if !*ctx.local.ign_channel_1 && dwell_time > 0 { + *ctx.local.ign_channel_1 = true; + app::ignition_trigger::spawn(dwell_time).unwrap(); + } + Systick::delay(100.micros()).await; + } +} + +//SDUBTUD: si esto anda es de puro milagro +pub async fn ignition_trigger(mut ctx: app::ignition_trigger::Context<'_>,dwell_time:i32){ + let dwell_ticks = (120_000_000 + 10_000 / 2) / 10_000 - 1u32; + + let dwell_duration = TimerDurationU32::<10_000>::from_ticks(dwell_ticks); + Systick::delay(dwell_duration).await; + + // FIXME: por ahora solo prendo el led, luego implementar las bobinas/resto canales + ctx.shared.led.lock(|l| l.led_check.set_low()); + ctx.shared.timer3.lock(|t3| t3.start((50).millis()).unwrap()); // este tiempo estaria bien para probar +} \ No newline at end of file diff --git a/src/app/injection/mod.rs b/src/app/injection/mod.rs index 6d64773b..50e38b96 100644 --- a/src/app/injection/mod.rs +++ b/src/app/injection/mod.rs @@ -78,3 +78,7 @@ pub fn calculate_time_isr(es: &mut EngineStatus, ecfg: &EngineConfig,tables: &mu "asddsa" ); } + +pub async fn injection_checks(){ + +} diff --git a/src/app/memory/efi_cfg.rs b/src/app/memory/efi_cfg.rs index 1fa36904..403605c2 100644 --- a/src/app/memory/efi_cfg.rs +++ b/src/app/memory/efi_cfg.rs @@ -3,7 +3,7 @@ use serde_json_core::heapless::Vec; use stm32f4xx_hal::crc32::Crc32; use w25q::series25::FlashInfo; -use crate::app::engine::efi_cfg::EngineConfig; +use crate::app::engine::efi_cfg::{EngineConfig, get_default_efi_cfg}; use crate::app::logging::host; use crate::app::memory::tables::FlashT; @@ -12,6 +12,7 @@ const ENGINE_CONFIG_MEMORY_ADDRESS: u32 = 0; impl EngineConfig { pub fn save(&mut self, flash: &mut FlashT, flash_info: &FlashInfo, crc: &mut Crc32) { //TODO: ajustar tamaƱo del vector + host::debug!("Guardando cfg"); let mut output: Vec = to_vec(&self).unwrap(); crc.init(); @@ -45,7 +46,14 @@ impl EngineConfig { let memory_crc = u32::from_le_bytes(crc_buff); if memory_crc != calculated_crc { - self.ready = true; + // self.ready = true; + // host::debug!("Checksum config no coincide {:?} {:?}", memory_crc,calculated_crc); + // + // let default= get_default_efi_cfg(); + // self.injection = default.injection.clone(); + // self.engine = default.engine.clone(); + // + // self.save(flash,flash_info,crc); host::debug!("Checksum config no coincide {:?} {:?}", memory_crc,calculated_crc); return; } @@ -53,8 +61,8 @@ impl EngineConfig { { let mut memory_config: EngineConfig = from_bytes(&read_buff).unwrap(); - self.injection = memory_config.injection; - self.engine = memory_config.engine; + self.injection = memory_config.injection.clone(); + self.engine = memory_config.engine.clone(); self.ready = true; } } diff --git a/src/app/tasks/engine.rs b/src/app/tasks/engine.rs index 2a4a1703..92f18eea 100644 --- a/src/app/tasks/engine.rs +++ b/src/app/tasks/engine.rs @@ -147,26 +147,6 @@ pub(crate) async fn ckp_checks(mut ctx: app::ckp_checks::Context<'_>) { // ckp.degreesPeruSx2048 = 2048 / timePerDegree; ckp.degreesPeruSx32768 = (524288 / time_per_degreex16) as f32; - // ignition timing: - let mut crank_angle = get_crank_angle(ckp, &cfg.engine.ckp, cycle_time); - const CRANK_ANGLE_MAX_IGN: i32 = 720; //ponele? no entendi bien como se setea dependiendo el tipo de encendido/cilindros - 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 - - let dwell_angle = 20; //TODO: get from table - let dwell_time = angle_to_time(ckp,&dwell_angle); - - // tiene que ser task externa - // if !*ignition_running /* && get_cranking_rpm(ckp, &cfg.engine.ckp) != 0*/{ - // *ignition_running = true; - // //seteamos nuevo triggerrr - // // let dwell_ticks = (120_000_000 / 100_000) * dwell_time as u64; - // // let dwell_duration = Duration::::from_ticks(dwell_ticks); - // // SDUBTUD: si esto anda es de puro milagro - // // app::ignition_trigger::spawn_after(dwell_duration).unwrap(); - // } - - // esto se usa para iny - get_crank_angle(ckp, &cfg.engine.ckp, cycle_time); } else { ckp.reset(); } diff --git a/src/app/tasks/ignition.rs b/src/app/tasks/ignition.rs deleted file mode 100644 index ee1d89d8..00000000 --- a/src/app/tasks/ignition.rs +++ /dev/null @@ -1,12 +0,0 @@ -use rtic::Mutex; -use stm32f4xx_hal::gpio::ExtiPin; -use crate::{ - app, -}; -pub(crate) async fn ignition_schedule(mut ctx: app::ignition_schedule::Context<'_>) { - -} - - - - diff --git a/src/app/webserial/handle_engine.rs b/src/app/webserial/handle_engine.rs index b5d11be7..1c58f3d8 100644 --- a/src/app/webserial/handle_engine.rs +++ b/src/app/webserial/handle_engine.rs @@ -29,10 +29,21 @@ pub async fn engine_cdc_callback(mut ctx: app::engine_cdc_callback::Context<'_>, let mut crc = ctx.shared.crc; let efi_cfg_snapshot = efi_cfg.lock(|c| c.clone()); + if !efi_cfg_snapshot.ready { + host::trace!("engine cfg webserial error"); + app::send_message::spawn( + SerialStatus::Error, + SerialCode::ParseError as u8, + response_buf, + ).unwrap(); + return; + } + + match serial_cmd.command & 0b00001111 { // read engine cfg: 0x01 => { - // host::trace!("read engine cfg"); + host::trace!("read engine cfg"); result = serde_json_core::to_slice(&efi_cfg_snapshot, &mut json_payload); } 0x02 => { diff --git a/src/app/webserial/handle_tables.rs b/src/app/webserial/handle_tables.rs index d249f3cd..c4b97df6 100644 --- a/src/app/webserial/handle_tables.rs +++ b/src/app/webserial/handle_tables.rs @@ -61,7 +61,7 @@ pub(crate) async fn table_cdc_callback(mut ctx: app::table_cdc_callback::Context // get X table 0x02 => { let mut table = [[0i32; 17]; 17]; - logging::host::debug!("Table get"); + // logging::host::debug!("Table get"); match selected_table { 0x01 => { // TODO: read table if not read prev. diff --git a/src/app/webserial/mod.rs b/src/app/webserial/mod.rs index c228d500..e322a132 100644 --- a/src/app/webserial/mod.rs +++ b/src/app/webserial/mod.rs @@ -109,7 +109,7 @@ pub fn process_command(buf: [u8; 128] , mut webserial_sender: &mut Sender { /* TODO: ignition */ } 0x40 => { /* TODO: DTC */ } 0x50 => app::realtime_data_cdc_callback::spawn(serial_cmd).unwrap(), - 0x60 => app::engine_cdc_callback::spawn(serial_cmd).unwrap(), + // 0x60 => app::engine_cdc_callback::spawn(serial_cmd).unwrap(), 0x70 => { /* TODO: Debug console */ } 0x80 => app::pmic_cdc_callback::spawn(serial_cmd).unwrap(), //0x90 => app::debug_demo::spawn(serial_cmd.command & 0b00001111).unwrap(), diff --git a/src/main.rs b/src/main.rs index 8b77b308..995d8830 100644 --- a/src/main.rs +++ b/src/main.rs @@ -34,6 +34,7 @@ use stm32f4xx_hal::{ prelude::*, spi::*, timer::{self, Event}, + watchdog::IndependentWatchdog, }; use panic_halt as _; @@ -61,6 +62,7 @@ mod app { StepperMapping, }, injection::{calculate_time_isr, injection_setup}, + ignition::{ignition_checks, ignition_trigger}, logging::host, memory::tables::{SpiT, Tables}, webserial::{ @@ -73,7 +75,7 @@ mod app { SerialMessage, SerialStatus, }, - tasks::{engine::ckp_checks/* , engine::motor_checks, ignition::ignition_schedule */} + tasks::{engine::ckp_checks/* , engine::motor_checks, ignition::ignition_schedule */}, }; use crate::app::tasks::engine::ckp_trigger; @@ -84,6 +86,7 @@ mod app { pub mod engine; pub mod gpio; pub mod injection; + pub mod ignition; pub mod logging; pub mod memory; pub mod util; @@ -109,7 +112,7 @@ mod app { // USB: // TODO: se puede reducir implementando los channels de RTIC, de paso fixeo el bug de las tablas - usb_cdc: SerialPort<'static, UsbBusType, [u8; 128], [u8; 256]>, + usb_cdc: SerialPort<'static, UsbBusType, [u8; 128], [u8; 512]>, usb_web: WebUsb, cdc_sender: Sender<'static, SerialMessage, CDC_BUFF_CAPACITY>, @@ -133,8 +136,10 @@ mod app { #[local] struct Local { + // core usb_dev: UsbDevice<'static, UsbBusType>, cdc_input_buffer: ArrayVec, + watchdog: IndependentWatchdog, // EFI Related: ckp: stm32f4xx_hal::gpio::PC6, @@ -152,7 +157,11 @@ mod app { real_time_sender: Sender<'static, SerialMessage, CDC_BUFF_CAPACITY>, pmic_sender: Sender<'static, SerialMessage, CDC_BUFF_CAPACITY>, engine_sender: Sender<'static, SerialMessage, CDC_BUFF_CAPACITY>, + + // ignition, + ign_channel_1: bool, } + const CDC_BUFF_CAPACITY: usize = 30; #[init(local = [USB_BUS: Option < UsbBusAllocator < UsbBusType >> = None])] @@ -197,8 +206,10 @@ mod app { // timer Tiempo inyeccion let mut timer: timer::CounterUs = device.TIM2.counter_us(&_clocks); + // timer tiempo de ignicion let mut timer3: timer::CounterUs = device.TIM3.counter_us(&_clocks); + // timer CPWM let mut timer4: timer::CounterUs = device.TIM5.counter_us(&_clocks); @@ -207,7 +218,7 @@ mod app { // TIM5, TIM7, TIM4 debug!("init timers"); - timer.start((150).millis()).unwrap(); + // timer.start((150).millis()).unwrap(); // Set up to generate interrupt when timer expires timer.listen(Event::Update); @@ -278,7 +289,7 @@ mod app { let mut spi_lock = false; let mut pmic = PMIC::init(spi_pmic, gpio_config.pmic.pmic_cs).unwrap(); - ckp.enable_interrupt(&mut device.EXTI); + // ckp.enable_interrupt(&mut device.EXTI); let mut ckp_status = VRStatus::new(); @@ -293,7 +304,7 @@ mod app { }; static mut EP_MEMORY: [u32; 1024] = [0; 1024]; - static mut __USB_TX: [u8; 256] = [0; 256]; + static mut __USB_TX: [u8; 512] = [0; 512]; static mut __USB_RX: [u8; 128] = [0; 128]; host::debug!("init usb"); let usb_bus = cx.local.USB_BUS; @@ -324,6 +335,11 @@ mod app { cdc_receiver::spawn(cdc_receiver).unwrap(); + let mut watchdog = IndependentWatchdog::new(device.IWDG); + // se puede desactivar en debug + // watchdog.start(100.millis()); + // watch_dog_update::spawn().unwrap(); + (Shared { // Timers: // delay, @@ -365,6 +381,7 @@ mod app { // USB usb_dev, cdc_input_buffer: cdc_buff, + watchdog, // Serial table_sender: cdc_sender.clone(), @@ -378,9 +395,19 @@ mod app { state: false, state2: false, + // ignition, + ign_channel_1: false, }) } + #[task(local = [watchdog])] + async fn watch_dog_update(mut ctx: watch_dog_update::Context) { + loop { + ctx.local.watchdog.feed(); + Systick::delay(50.millis()).await; + } + } + #[task(local = [state], shared = [led])] async fn blink(mut cx: blink::Context) { loop { @@ -395,6 +422,23 @@ mod app { } } + //TODO: reciclar para encendido + #[task(binds = TIM2, local = [], shared = [timer, led])] + fn timer2_exp(mut ctx: timer2_exp::Context) { + ctx.shared.timer.lock(|tim| tim.clear_interrupt(Event::Update)); + + ctx.shared.led.lock(|l| l.led_0.toggle()); + } + + #[task(binds = TIM3, local = [], shared = [timer3, led])] + fn timer3_exp(mut ctx: timer3_exp::Context) { + ctx.shared.timer3.lock(|tim| { + tim.clear_interrupt(Event::Update); + tim.cancel().unwrap(); + }); + + ctx.shared.led.lock(|l| l.led_check.set_high()); + } #[task(binds = OTG_FS, local = [usb_dev, cdc_input_buffer], shared = [usb_cdc, usb_web, cdc_sender])] fn usb_handler(mut ctx: usb_handler::Context) { @@ -453,9 +497,14 @@ mod app { // from: https://github.com/noisymime/speeduino/blob/master/speeduino/decoders.ino#L453 #[task(binds = EXTI9_5, local = [ckp], shared = [led, efi_status, flash_info, efi_cfg, timer, timer3, timer4, ckp, ign_pins], priority = 5)] fn ckp_trigger(ctx: ckp_trigger::Context); - #[task(shared = [efi_cfg, ckp, timer4, efi_status, ignition_running], priority = 4)] + #[task(shared = [efi_cfg, ckp, timer4, efi_status, ignition_running])] async fn ckp_checks(ctx: ckp_checks::Context); + #[task(shared = [efi_cfg, efi_status, ckp, timer4], local = [ign_channel_1])] + async fn ignition_checks(ctx: ignition_checks::Context); + + #[task(shared = [efi_cfg, efi_status, ckp, timer3, led])] + async fn ignition_trigger(ctx: ignition_trigger::Context, time: i32); // // #[task( From 6e2aae6bcc4c176872dc5dd5053a959cc20b6065 Mon Sep 17 00:00:00 2001 From: FDSoftware Date: Thu, 23 Nov 2023 23:05:15 -0300 Subject: [PATCH 2/2] update GPIO for pcb V4.x, added DMA for most used sensors --- src/app/engine/pmic.rs | 2 +- src/app/engine/sensors.rs | 13 ++- src/app/gpio.rs | 165 +++++++++++++++++++++++++------------- src/main.rs | 101 +++++++++++++++++++++-- 4 files changed, 218 insertions(+), 63 deletions(-) diff --git a/src/app/engine/pmic.rs b/src/app/engine/pmic.rs index 20075742..c275d43a 100644 --- a/src/app/engine/pmic.rs +++ b/src/app/engine/pmic.rs @@ -7,7 +7,7 @@ use stm32f4xx_hal::gpio::{Output, PushPull}; use crate::app::engine::error::Error; use crate::app::gpio::ISPI; use serde::{ Serialize}; -pub type PmicT = PMIC, gpio::PA15>>; +pub type PmicT = PMIC, gpio::PB11>>; #[derive(Serialize)] #[repr(u8)] diff --git a/src/app/engine/sensors.rs b/src/app/engine/sensors.rs index 162078f8..f3a60ff3 100644 --- a/src/app/engine/sensors.rs +++ b/src/app/engine/sensors.rs @@ -3,7 +3,7 @@ use micromath::F32Ext; use stm32f4xx_hal::{ adc::{config::SampleTime, Adc}, - pac::ADC1, + pac::ADC2, }; const EMA_LP_ALPHA: f32 = 0.45f32; @@ -14,6 +14,7 @@ pub enum SensorTypes { CooltanTemp, AirTemp, BatteryVoltage, + ExternalLambda, } #[derive(Debug,Clone,Copy)] @@ -23,6 +24,7 @@ pub struct SensorValues { pub cooltan_temp: f32, pub air_temp: f32, pub batt: f32, + pub ext_o2: f32, // private: raw_map: f32, @@ -30,6 +32,7 @@ pub struct SensorValues { raw_temp: f32, raw_air_temp: f32, raw_batt: f32, + raw_ext_o2: f32, } impl SensorValues { @@ -40,12 +43,14 @@ impl SensorValues { cooltan_temp: 45.69f32, air_temp: 0.0f32, batt: 13.42f32, + ext_o2: 0.0f32, // valores en raw son en bits del ADC; luego se pasan a mV raw_map: 0.0f32, raw_tps: 0.0f32, raw_temp: 0.0f32, raw_air_temp: 0.0f32, raw_batt: 0.0f32, + raw_ext_o2: 0.0f32, } } @@ -86,6 +91,10 @@ impl SensorValues { self.batt = 14.2; } + + SensorTypes::ExternalLambda => { + self.ext_o2 = raw_value as f32; + } } } } @@ -93,7 +102,7 @@ impl SensorValues { pub fn get_sensor_raw( sensor_type: SensorTypes, adc_pins: &mut ADCMapping, - adc: &mut Adc, + adc: &mut Adc, ) -> u16 { let a = sensor_type as u8; let b = a; diff --git a/src/app/gpio.rs b/src/app/gpio.rs index 90303259..15fc1d50 100644 --- a/src/app/gpio.rs +++ b/src/app/gpio.rs @@ -11,43 +11,50 @@ use stm32f4xx_hal::{ pub struct InjectionGpioMapping { pub iny_1: gpio::PD8>, pub iny_2: gpio::PD9>, - pub iny_3: gpio::PD10>, - pub iny_4: gpio::PD11>, } pub struct IgnitionGpioMapping { - pub ecn_1: gpio::PD12>, - pub ecn_2: gpio::PD13>, - pub ecn_3: gpio::PD14>, - pub ecn_4: gpio::PD15>, + pub ecn_1: gpio::PD10>, + pub ecn_2: gpio::PD11>, } pub struct PMICGpioMapping { - pub pmic_enable: gpio::PB12>, - pub pmic_spark: gpio::PB13, - pub pmic_cs: gpio::PA15>, - pub pmic_nomi: gpio::PC10, - pub pmic_maxi: gpio::PC11, + pub pmic1_enable: gpio::PB12>, + pub pmic1_cs: gpio::PB11>, + pub pmic2_enable: gpio::PB13>, + pub pmic2_cs: gpio::PB9>, } pub struct LedGpioMapping { - pub led_0: gpio::PC13>, - pub led_1: gpio::PC14>, - pub led_2: gpio::PC15>, + pub led_0: gpio::PC1>, + pub led_1: gpio::PC2>, + pub led_2: gpio::PC3>, pub led_can_tx: gpio::PD6>, pub led_can_rx: gpio::PD7>, - pub led_check: gpio::PC9>, - pub led_mil: gpio::PC8>, + pub led_check: gpio::PB1>, + pub led_mil: gpio::PA8>, } pub struct AuxIoMapping { - pub in_1: gpio::PC4, - pub in_2: gpio::PC5, - pub out_1: gpio::PB5>, - pub out_2: gpio::PB6>, - pub out_3: gpio::PB7>, - pub out_4: gpio::PB8>, + // on external connector + pub in_1: gpio::PD12, + pub in_2: gpio::PD13, + pub in_3: gpio::PD14, + pub in_4: gpio::PD15, + + // on expansion header + + pub in_5: gpio::PB5, + pub in_6: gpio::PB7, + pub in_7: gpio::PB8, + pub in_8: gpio::PE1, + + + pub out_1: gpio::PC4>, + pub out_2: gpio::PC5>, + pub out_3: gpio::PB0>, + pub out_4: gpio::PA10>, pub cs_1: gpio::PE14>, pub cs_2: gpio::PE15>, @@ -61,12 +68,24 @@ pub struct RelayMapping { } pub struct ADCMapping { - pub mux_a: gpio::PD2>, - pub mux_b: gpio::PD3>, - pub mux_c: gpio::PD4>, - pub analog_in: gpio::PA0, + pub baro_cs: gpio::PA6>, + + pub mux_a: gpio::PD3>, + pub mux_b: gpio::PD4>, + pub mux_c: gpio::PD5>, + pub analog_in: gpio::PA7, } +pub struct ADC_DMA_Mapping { + pub tps: gpio::PA0, + pub iat: gpio::PA1, + pub map: gpio::PA2, + pub clt: gpio::PA3, + pub o2: gpio::PA4, + pub vbatt: gpio::PA5, +} + + pub struct StepperMapping{ pub step: gpio::PE7>, pub dir: gpio::PE8>, @@ -74,6 +93,17 @@ pub struct StepperMapping{ pub enable: gpio::PE10>, } +pub struct SDCard { + pub dat_0: Pin<'C', 8>, + pub dat_1: Pin<'C', 9>, + pub dat_2: Pin<'C', 10>, + pub dat_3: Pin<'C',11>, + pub dat_clk: gpio::PC12, + pub cmd: Pin<'D', 2>, + + pub cd: gpio::PB6, +} + pub struct GpioMapping { // LED's / User feedback pub led: LedGpioMapping, @@ -110,7 +140,10 @@ pub struct GpioMapping { pub spi_miso: gpio::PB14>, pub spi_mosi: gpio::PB15>, + pub sd_card: SDCard, + pub adc: ADCMapping, + pub adc_dma: ADC_DMA_Mapping, } pub fn init_gpio( @@ -123,38 +156,33 @@ pub fn init_gpio( let mut gpio = GpioMapping { // LED's / User feedback led: LedGpioMapping { - led_0: gpio_c.pc13.into_push_pull_output(), - led_1: gpio_c.pc14.into_push_pull_output(), - led_2: gpio_c.pc15.into_push_pull_output(), + led_0: gpio_c.pc1.into_push_pull_output(), + led_1: gpio_c.pc2.into_push_pull_output(), + led_2: gpio_c.pc3.into_push_pull_output(), led_can_tx: gpio_d.pd6.into_push_pull_output(), led_can_rx: gpio_d.pd7.into_push_pull_output(), - led_check: gpio_c.pc9.into_push_pull_output(), - led_mil: gpio_c.pc8.into_push_pull_output(), + led_check: gpio_b.pb1.into_push_pull_output(), + led_mil: gpio_a.pa8.into_push_pull_output(), }, // Injection injection: InjectionGpioMapping { iny_1: gpio_d.pd8.into_push_pull_output(), iny_2: gpio_d.pd9.into_push_pull_output(), - iny_3: gpio_d.pd10.into_push_pull_output(), - iny_4: gpio_d.pd11.into_push_pull_output(), }, // Ignition ignition: IgnitionGpioMapping { - ecn_1: gpio_d.pd12.into_push_pull_output(), - ecn_2: gpio_d.pd13.into_push_pull_output(), - ecn_3: gpio_d.pd14.into_push_pull_output(), - ecn_4: gpio_d.pd15.into_push_pull_output(), + ecn_1: gpio_d.pd10.into_push_pull_output(), + ecn_2: gpio_d.pd11.into_push_pull_output(), }, // PMIC pmic: PMICGpioMapping { - pmic_enable: gpio_b.pb12.into_push_pull_output(), - pmic_spark: gpio_b.pb13.into_input(), - pmic_cs: gpio_a.pa15.into_push_pull_output(), - pmic_nomi: gpio_c.pc10.into_input(), - pmic_maxi: gpio_c.pc11.into_input(), + pmic1_enable: gpio_b.pb12.into_push_pull_output(), + pmic1_cs: gpio_b.pb11.into_push_pull_output(), + pmic2_enable: gpio_b.pb13.into_push_pull_output(), + pmic2_cs: gpio_b.pb9.into_push_pull_output(), }, // CKP/CMP @@ -182,13 +210,19 @@ pub fn init_gpio( // AUX I/O aux: AuxIoMapping { - in_1: gpio_c.pc4.into_input(), - in_2: gpio_c.pc5.into_input(), - - out_1: gpio_b.pb5.into_push_pull_output(), - out_2: gpio_b.pb6.into_push_pull_output(), - out_3: gpio_b.pb7.into_push_pull_output(), - out_4: gpio_b.pb8.into_push_pull_output(), + in_1: gpio_d.pd12.into_input(), + in_2: gpio_d.pd13.into_input(), + in_3: gpio_d.pd14.into_input(), + in_4: gpio_d.pd15.into_input(), + in_5: gpio_b.pb5.into_input(), + in_6: gpio_b.pb7.into_input(), + in_7: gpio_b.pb8.into_input(), + in_8: gpio_e.pe1.into_input(), + + out_1: gpio_c.pc4.into_push_pull_output(), + out_2: gpio_c.pc5.into_push_pull_output(), + out_3: gpio_b.pb0.into_push_pull_output(), + out_4: gpio_a.pa10.into_push_pull_output(), cs_1: gpio_e.pe14.into_push_pull_output(), cs_2: gpio_e.pe15.into_push_pull_output(), @@ -203,11 +237,33 @@ pub fn init_gpio( spi_miso: gpio_b.pb14.into_alternate(), spi_mosi: gpio_b.pb15.into_alternate().internal_pull_up(true), + adc_dma: ADC_DMA_Mapping { + tps: gpio_a.pa0.into_analog(), + iat: gpio_a.pa1.into_analog(), + map:gpio_a.pa2.into_analog(), + clt: gpio_a.pa3.into_analog(), + o2: gpio_a.pa4.into_analog(), + vbatt: gpio_a.pa5.into_analog(), + }, + adc: ADCMapping { - mux_a: gpio_d.pd2.into_push_pull_output(), - mux_b: gpio_d.pd3.into_push_pull_output(), - mux_c: gpio_d.pd4.into_push_pull_output(), - analog_in: gpio_a.pa0.into_analog(), + + baro_cs: gpio_a.pa6.into_push_pull_output(), + + mux_a: gpio_d.pd3.into_push_pull_output(), + mux_b: gpio_d.pd4.into_push_pull_output(), + mux_c: gpio_d.pd5.into_push_pull_output(), + + analog_in: gpio_a.pa7.into_analog(), + }, + + sd_card: SDCard {dat_0 : gpio_c.pc8.internal_pull_up(true), + dat_1 : gpio_c.pc9.internal_pull_up(true), + dat_2 : gpio_c.pc10.internal_pull_up(true), + dat_3 : gpio_c.pc11.internal_pull_up(true), + dat_clk : gpio_c.pc12, + cmd : gpio_d.pd2.internal_pull_up(true), + cd: gpio_b.pb6.into_input(), }, }; @@ -226,7 +282,8 @@ pub fn init_gpio( // 4231 reversa // 1234 derecho - gpio.pmic.pmic_cs.set_high(); + gpio.pmic.pmic1_cs.set_high(); + gpio.pmic.pmic2_cs.set_high(); gpio.memory_cs.set_high(); return gpio; diff --git a/src/main.rs b/src/main.rs index 995d8830..06a1e914 100644 --- a/src/main.rs +++ b/src/main.rs @@ -24,13 +24,17 @@ use usbd_serial::SerialPort; use usbd_webusb::{url_scheme, WebUsb}; use stm32f4xx_hal::{ - adc::{Adc, config::AdcConfig}, + adc::{ + config::{AdcConfig, Dma, SampleTime, Scan, Sequence}, + Adc, Temperature, + }, + dma::{config::DmaConfig, PeripheralToMemory, Stream0, StreamsTuple, Transfer}, crc32, crc32::Crc32, gpio::{Edge, Input}, otg_fs, otg_fs::{USB, UsbBusType}, - pac::{ADC1, TIM13, TIM2, TIM3, TIM5}, + pac::{ADC1, TIM13, TIM2, TIM3, TIM5,DMA2,ADC2}, prelude::*, spi::*, timer::{self, Event}, @@ -77,6 +81,7 @@ mod app { }, tasks::{engine::ckp_checks/* , engine::motor_checks, ignition::ignition_schedule */}, }; + use crate::app::engine::sensors; use crate::app::tasks::engine::ckp_trigger; use super::*; @@ -94,6 +99,10 @@ mod app { pub mod tasks; + + type DMATransfer = + Transfer, 0, Adc, PeripheralToMemory, &'static mut [u16; 6]>; + #[shared] struct Shared { // Timers: @@ -109,6 +118,7 @@ mod app { aux_pins: AuxIoMapping, relay_pins: RelayMapping, stepper_pins: StepperMapping, + adc_transfer: DMATransfer, // USB: // TODO: se puede reducir implementando los channels de RTIC, de paso fixeo el bug de las tablas @@ -140,10 +150,11 @@ mod app { usb_dev: UsbDevice<'static, UsbBusType>, cdc_input_buffer: ArrayVec, watchdog: IndependentWatchdog, + adc_buffer: Option<&'static mut [u16; 6]>, // EFI Related: ckp: stm32f4xx_hal::gpio::PC6, - adc: Adc, + adc: Adc, analog_pins: ADCMapping, // cdc_sender: Sender<'static, u32, 8>, @@ -190,10 +201,35 @@ mod app { let mut gpio_config = init_gpio(gpio_a, gpio_b, gpio_c, gpio_d, gpio_e); // ADC - let mut adc = Adc::adc1(device.ADC1, true, AdcConfig::default()); + let dma = StreamsTuple::new(device.DMA2); + + let config = DmaConfig::default() + .transfer_complete_interrupt(true) + .memory_increment(true) + .double_buffer(false); + + let adc_config = AdcConfig::default() + .dma(Dma::Continuous) + .scan(Scan::Enabled); + + let mut adc1 = Adc::adc1(device.ADC1, true, adc_config); + + // aca van todos los canales a revisar con DMA + adc1.configure_channel(&gpio_config.adc_dma.tps, Sequence::One, SampleTime::Cycles_480); + adc1.configure_channel(&gpio_config.adc_dma.clt, Sequence::Two, SampleTime::Cycles_480); + adc1.configure_channel(&gpio_config.adc_dma.iat, Sequence::Three, SampleTime::Cycles_480); + adc1.configure_channel(&gpio_config.adc_dma.map, Sequence::Four, SampleTime::Cycles_480); + adc1.configure_channel(&gpio_config.adc_dma.o2, Sequence::Five, SampleTime::Cycles_480); + adc1.configure_channel(&gpio_config.adc_dma.vbatt, Sequence::Six, SampleTime::Cycles_480); + let adc_first_buffer = cortex_m::singleton!(: [u16; 6] = [0; 6]).unwrap(); + let adc_second_buffer = Some(cortex_m::singleton!(: [u16; 6] = [0; 6]).unwrap()); + // Give the first buffer to the DMA. The second buffer is held in an Option in `local.buffer` until the transfer is complete + let adc_transfer = Transfer::init_peripheral_to_memory(dma.0, adc1, adc_first_buffer, None, config); + + let mut adc = Adc::adc2(device.ADC2, true, AdcConfig::default()); adc.enable(); - adc.calibrate(); + //adc.calibrate(); // configure CKP/CMP Pin for Interrupts let mut ckp = gpio_config.ckp; @@ -287,7 +323,7 @@ mod app { let mut sensors = SensorValues::new(); let mut spi_lock = false; - let mut pmic = PMIC::init(spi_pmic, gpio_config.pmic.pmic_cs).unwrap(); + let mut pmic = PMIC::init(spi_pmic, gpio_config.pmic.pmic1_cs).unwrap(); // ckp.enable_interrupt(&mut device.EXTI); @@ -334,6 +370,7 @@ mod app { let (cdc_sender, cdc_receiver) = make_channel!(SerialMessage, CDC_BUFF_CAPACITY); cdc_receiver::spawn(cdc_receiver).unwrap(); + polling_adc::spawn().unwrap(); let mut watchdog = IndependentWatchdog::new(device.IWDG); // se puede desactivar en debug @@ -367,6 +404,7 @@ mod app { flash, flash_info, spi_lock, + adc_transfer, // EFI Related efi_cfg, @@ -383,6 +421,8 @@ mod app { cdc_input_buffer: cdc_buff, watchdog, + adc_buffer: adc_second_buffer, + // Serial table_sender: cdc_sender.clone(), real_time_sender: cdc_sender.clone(), @@ -400,6 +440,55 @@ mod app { }) } + + #[task(shared = [adc_transfer])] + async fn polling_adc(mut cx: polling_adc::Context) { + loop { + cx.shared.adc_transfer.lock(|transfer| { + transfer.start(|adc| { + adc.start_conversion(); + }); + }); + Systick::delay(250.millis()).await; + } + } + + #[task(binds = DMA2_STREAM0, shared = [adc_transfer,sensors], local = [adc_buffer])] + fn sensors_adc_dma(mut cx: sensors_adc_dma::Context) { + + let (buffer, sample_to_millivolts) = cx.shared.adc_transfer.lock(|transfer| { + // When the DMA completes it will return the buffer we gave it last time - we now store that as `buffer` + // We still have our other buffer waiting in `local.buffer`, so `take` that and give it to the `transfer` + let (buffer, _) = transfer + .next_transfer(cx.local.adc_buffer.take().unwrap()) + .unwrap(); + + let sample_to_millivolts = transfer.peripheral().make_sample_to_millivolts(); + (buffer, sample_to_millivolts) + }); + + // Pull the ADC data out of the buffer that the DMA transfer gave us + + let raw_tps = buffer[0]; + let raw_clt = buffer[1]; + let raw_iat = buffer[2]; + let raw_map = buffer[3]; + let raw_o2 = buffer[4]; + let raw_vbatt = buffer[5]; + + // Now that we're finished with this buffer, put it back in `local.buffer` so it's ready for the next transfer + // If we don't do this before the next transfer, we'll get a panic + *cx.local.adc_buffer = Some(buffer); + + cx.shared.sensors.lock(|s| { s.update(sample_to_millivolts(raw_tps),SensorTypes::TPS) }); + cx.shared.sensors.lock(|s| { s.update(sample_to_millivolts(raw_clt),SensorTypes::CooltanTemp) }); + cx.shared.sensors.lock(|s| { s.update(sample_to_millivolts(raw_iat),SensorTypes::AirTemp) }); + cx.shared.sensors.lock(|s| { s.update(sample_to_millivolts(raw_map),SensorTypes::MAP) }); + cx.shared.sensors.lock(|s| { s.update(sample_to_millivolts(raw_o2),SensorTypes::ExternalLambda) }); + cx.shared.sensors.lock(|s| { s.update(sample_to_millivolts(raw_vbatt),SensorTypes::BatteryVoltage) }); + + } + #[task(local = [watchdog])] async fn watch_dog_update(mut ctx: watch_dog_update::Context) { loop {