-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
1 parent
d3382a3
commit 3939470
Showing
5 changed files
with
85 additions
and
64 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,80 +1,86 @@ | ||
| use embassy_executor; | ||
| use embassy_stm32::adc::Adc; | ||
| use embassy_stm32::gpio::{Output, Level, Speed}; | ||
| use embassy_stm32::spi::{Config, Phase, Polarity, Spi}; | ||
| use embassy_stm32::time::Hertz; | ||
| use embassy_stm32::peripherals::*; | ||
| use embassy_time::{with_timeout, Delay, Duration, Ticker, Timer}; | ||
| use smart_leds::RGB8; | ||
| use w25q::prelude; | ||
| use w25q::series25::Flash; | ||
| use ws2812_spi::Ws2812; | ||
| use smart_leds::SmartLedsWrite; | ||
|
|
||
| fn to_millivolts(vref_sample: u16, sample: u16) -> u16 { | ||
| const VREFINT_MV: u32 = 1200; // mV | ||
| (u32::from(sample) * VREFINT_MV / u32::from(vref_sample)) as u16 | ||
| use embassy_time::{with_timeout, Duration, Ticker}; | ||
| use shared_types::{CanBusMessage, CanBusMessageId, FinBoardDataMessage, FlightMode}; | ||
|
|
||
| const PRIMARY_STRAIN_GAUGE_SAMPLE_RATE_HZ: u64 = 1000; | ||
| const SECONDARY_STRAIN_GAUGE_SAMPLE_RATE_HZ: u64 = 100; | ||
|
|
||
| const DIVISOR: i16 = 4; // 1 => no loss in quality, clipping after sample > +/- 128 | ||
| // 16 => no clipping inside ADC range | ||
|
|
||
| pub async fn sample_strain_gauges( | ||
| adc: &mut Adc<'static, ADC1>, | ||
| sg_vout0_pin: &mut PC1, | ||
| _sg_ref_pin: &mut PC2, | ||
| sg_vout1_pin: &mut PC3, | ||
| ) -> (u8, u8) { | ||
| const TIMEOUT: Duration = Duration::from_micros(100); | ||
|
|
||
| let sg0_sample = with_timeout(TIMEOUT, adc.read(sg_vout0_pin)).await.unwrap_or_default(); | ||
| let sg1_sample = with_timeout(TIMEOUT, adc.read(sg_vout1_pin)).await.unwrap_or_default(); | ||
|
|
||
| let sg0 = ((sg0_sample as i16 - 2048) / DIVISOR + 128) as u8; | ||
| let sg1 = ((sg1_sample as i16 - 2048) / DIVISOR + 128) as u8; | ||
|
|
||
| (sg0, sg1) | ||
| } | ||
|
|
||
| #[embassy_executor::task] | ||
| pub async fn run( | ||
| address: u8, | ||
| mut adc: Adc<'static, ADC1>, | ||
| sg_en_pin: PC0, | ||
| mut sg_vout0_pin: PC1, | ||
| _sg_ref_pin: PC2, | ||
| mut sg_ref_pin: PC2, | ||
| mut sg_vout1_pin: PC3, | ||
| flight_mode_subscriber: crate::can::FlightModeSubscriber, | ||
| spi: SPI2, | ||
| cs: PC6, | ||
| sck: PB13, | ||
| miso: PB14, | ||
| mosi: PB15, | ||
| dma_out: DMA1_CH5, | ||
| dma_in: DMA1_CH4 | ||
| mut flight_mode_subscriber: crate::can::FlightModeSubscriber, | ||
| can_out: crate::can::CanOutPublisher, | ||
| ) -> ! { | ||
| const TIMEOUT: Duration = Duration::from_micros(100); | ||
|
|
||
| let mut enable = Output::new(sg_en_pin, Level::Low, Speed::Low); | ||
| let mut fm = FlightMode::default(); | ||
|
|
||
| // 2000 Hz | ||
| let mut ticker = Ticker::every(Duration::from_micros(50000)); | ||
|
|
||
| let mut config = Config::default(); | ||
| config.frequency = Hertz::mhz(2); | ||
| let mut spi_bus = embassy_stm32::spi::Spi::new(spi, sck, mosi, miso, dma_out, dma_in, config); | ||
| let mut cs = Output::new(cs, Level::High, Speed::Low); | ||
| //let mut flash = Flash::init(spi_bus, out).unwrap(); | ||
| let sg0_id = CanBusMessageId::FinBoardInput(address, 0); // TODO: fin id | ||
| let sg1_id = CanBusMessageId::FinBoardInput(address, 1); // TODO: fin id | ||
|
|
||
| let mut data0 = [0; 6]; | ||
| let mut data1 = [0; 6]; | ||
| let mut i = 0; | ||
|
|
||
| let mut ticker = if address == 0 { | ||
| Ticker::every(Duration::from_micros(1_000_000 / PRIMARY_STRAIN_GAUGE_SAMPLE_RATE_HZ)) | ||
| } else { | ||
| Ticker::every(Duration::from_micros(1_000_000 / SECONDARY_STRAIN_GAUGE_SAMPLE_RATE_HZ)) | ||
| }; | ||
|
|
||
| loop { | ||
| enable.set_high(); | ||
| //let sg_ref_sample = with_timeout(TIMEOUT, adc.read(&mut sg_ref_pin)).await.unwrap_or_default(); | ||
| let sg_vout0_sample = with_timeout(TIMEOUT, adc.read(&mut sg_vout0_pin)).await.unwrap_or_default(); | ||
| let sg_vout1_sample = with_timeout(TIMEOUT, adc.read(&mut sg_vout1_pin)).await.unwrap_or_default(); | ||
| enable.set_low(); | ||
|
|
||
| i += 1; | ||
| if i > 100 { | ||
| defmt::println!("[SG] {:?} {:?}", sg_vout0_sample, sg_vout1_sample); | ||
| i = 0; | ||
| while let Some(new_fm) = flight_mode_subscriber.try_next_message_pure() { | ||
| fm = new_fm; | ||
| } | ||
|
|
||
| //let sg_ref_voltage = to_millivolts(vref_sample, sg_ref_sample); | ||
| //let sg_vout0_voltage = to_millivolts(vref_sample, sg_vout0_sample); | ||
| //let sg_vout1_voltage = to_millivolts(vref_sample, sg_vout1_sample); | ||
| if fm >= FlightMode::ArmedLaunchImminent && fm < FlightMode::Landed { // TODO | ||
| enable.set_high(); | ||
|
|
||
| let (sg0, sg1) = sample_strain_gauges(&mut adc, &mut sg_vout0_pin, &mut sg_ref_pin, &mut sg_vout1_pin).await; | ||
| data0[i % 6] = sg0; | ||
| data1[i % 6] = sg1; | ||
| i += 1; | ||
|
|
||
| //defmt::println!("{:?} {:?} {:?}", sg_ref_voltage, sg_vout0_voltage, sg_vout1_voltage); | ||
| if i >= 6 { | ||
| let sg0_message = FinBoardDataMessage { data: data0.try_into().unwrap_or_default() }; | ||
| can_out.publish((sg0_id.into(), sg0_message.serialize_with_crc())).await; | ||
| let sg1_message = FinBoardDataMessage { data: data1.try_into().unwrap_or_default() }; | ||
| can_out.publish((sg1_id.into(), sg1_message.serialize_with_crc())).await; | ||
| i = 0; | ||
| } | ||
| } else { | ||
| i = 0; | ||
| enable.set_low(); | ||
| } | ||
|
|
||
| cs.set_low(); | ||
| Timer::after(Duration::from_micros(100)).await; | ||
| let mut read: [u8; 4] = [0x00, 0x00, 0x00, 0x00]; | ||
| let mut write: [u8; 4] = [0x9f, 0x00, 0x00, 0x00]; | ||
| let res = spi_bus.transfer(&mut read, &write).await.unwrap(); | ||
| Timer::after(Duration::from_micros(100)).await; | ||
| cs.set_high(); | ||
| defmt::println!("{:?} {:?}", read, write); | ||
|
|
||
| // TODO read strain gauge value and write to flash | ||
| ticker.next().await; | ||
| } | ||
| } |