protoflight 0.1.4

Protoflight flight controller.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153

#![cfg(feature = "madflight_fc2")]
#![allow(unused)]

// ESP32 S3
// see https://madflight.com/Board-ESP-FC2/
// pins: https://github.com/qqqlab/madflight/blob/main/src/brd/madflight_FC2.h
// schematic: https://madflight.com/img/madflight-ESP-FC2.pdf

use crate::boards::{
    SharedI2cBus,
    board::{BoardHardware, BoardInit, BoardInitError},
};

use dshot_codec::DshotSpeed;
use imu_sensors::{Imu426xx, ImuMock, ImuSpiBus, MockImuBus};
use motor_mixers::{MotorDriver, MotorDriverDshot, MotorDriverPwm, MotorProtocol};

use static_cell::StaticCell;

use embassy_time::Delay;
use embedded_hal_bus::spi::ExclusiveDevice;

use esp_hal::{
    gpio::{AnyPin, DriveMode, Output},
    ledc::{
        Ledc, LowSpeed,
        channel::{self, Channel, ChannelIFace},
        timer::{self, LSClockSource, TimerIFace},
    },
    peripherals,
    spi::slave::Spi,
    time::Rate,
    uart::{UartRx, UartTx},
};

pub type BoardImu = ImuMock<MockImuBus>;
pub type Board = BoardHardware<BoardImu>;

pub type SdCardSpiDevice = ();

impl Board {
    #[allow(clippy::too_many_lines, clippy::similar_names, clippy::no_effect_underscore_binding)]
    pub fn new(init: &BoardInit) -> Result<Self, BoardInitError> {
        // NOTE: resp32 numbers peripherals starting at 0, eg SPI0, SPI1, I2C0, I2C1 etc
        let imu = ImuMock::new(MockImuBus::new(), init.axis_order);

        static I2C_BUS: StaticCell<SharedI2cBus> = StaticCell::new();
        //static RADIO_UART_TX: StaticCell<UartTx<'static, UartTx>> = StaticCell::new();
        //static RADIO_UART_RX: StaticCell<UartRx<'static, UartRx>> = StaticCell::new();
        static LEDC_TIMER: static_cell::StaticCell<timer::Timer<'static, LowSpeed>> = static_cell::StaticCell::new();

        // Take ownership of the hardware peripherals block
        #[allow(clippy::default_trait_access)]
        //let peripherals = esp_hal::init(esp_hal::Config::default());
        let peripherals = esp_hal::init(Default::default());

        // SPI0
        let spi0_clk = peripherals.GPIO16;
        let spi0_mosi = peripherals.GPIO15;
        let spi0_miso = peripherals.GPIO14;
        let imu_cs_pin = peripherals.GPIO17;
        let imu_exti_pin = peripherals.GPIO13;

        // UART0
        // #define UART_0_PINS uart_pins_t{.rx=7,.tx=21}
        let uart0_tx = peripherals.GPIO21;
        let uart0_rx = peripherals.GPIO7;

        // UART1
        // #define UART_1_PINS uart_pins_t{.rx=6,.tx=9}
        let uart1_tx = peripherals.GPIO9;
        let uart1_rx = peripherals.GPIO6;

        // I2C0
        // #define I2C_0_PINS i2c_pins_t{.sda=11,.scl=10,.irq=BusI2c::IRQ_NOT_SET}
        let i2c0_scl = peripherals.GPIO10;
        let i2c0_sda = peripherals.GPIO11;

        // I2C1
        // #define I2C_1_PINS i2c_pins_t{.sda=33,.scl=39,.irq=BusI2c::IRQ_NOT_SET}
        let _i2c1_scl = peripherals.GPIO39;
        let _i2c1_sda = peripherals.GPIO33;

        // #define MOTOR_PINS  motor_pins_t{.m0=1,.m1=2,.m2=3,.m3=4} // BR, TR, BL, TL
        // Motors
        let m1 = peripherals.GPIO1;
        let m2 = peripherals.GPIO2;
        let m3 = peripherals.GPIO3;
        let m4 = peripherals.GPIO4;

        let ledc = Ledc::new(peripherals.LEDC);

        let frequency_hz = u32::from(init.motor_pwm_rate);

        let timer = LEDC_TIMER.init(ledc.timer::<LowSpeed>(timer::Number::Timer0));
        timer
            .configure(timer::config::Config {
                duty: timer::config::Duty::Duty14Bit,
                clock_source: LSClockSource::APBClk,
                frequency: Rate::from_hz(frequency_hz),
            })
            .expect("failed to configure LEDC timer");

        let mut ch0 = ledc.channel(channel::Number::Channel0, m1);
        let mut ch1 = ledc.channel(channel::Number::Channel1, m2);
        let mut ch2 = ledc.channel(channel::Number::Channel2, m3);
        let mut ch3 = ledc.channel(channel::Number::Channel3, m4);

        ch0.configure(channel::config::Config { timer, duty_pct: 0, drive_mode: DriveMode::PushPull })
            .expect("failed to configure LEDC channel 0");
        ch1.configure(channel::config::Config { timer, duty_pct: 0, drive_mode: DriveMode::PushPull })
            .expect("failed to configure LEDC channel 1");
        ch2.configure(channel::config::Config { timer, duty_pct: 0, drive_mode: DriveMode::PushPull })
            .expect("failed to configure LEDC channel 2");
        ch3.configure(channel::config::Config { timer, duty_pct: 0, drive_mode: DriveMode::PushPull })
            .expect("failed to configure LEDC channel 3");

        let frequency_hz = f32::from(init.motor_pwm_rate);
        let motor_driver_pwm = MotorDriverPwm::new(ch0, ch1, ch2, ch3, frequency_hz);
        let motor_driver = MotorDriver::Pwm(motor_driver_pwm);

        let radio_uart_tx = None;
        let radio_uart_rx = None;

        let gps_uart_tx = None;
        let gps_uart_rx = None;

        let sdcard_volume = None;

        let barometer = None;
        let magnetometer = None;
        let rangefinder = None;
        let optical_flow = None;

        // Map physical device names to logical device names and return.
        Ok(Self {
            gyro_pid_spawner: init.spawner,
            realtime_spawner: init.spawner,
            background_spawner: init.spawner,
            imu,
            motor_driver,
            radio_uart_rx,
            radio_uart_tx,
            gps_uart_rx,
            gps_uart_tx,
            sdcard_volume,
            barometer,
            magnetometer,
            rangefinder,
            optical_flow,
        })
    }
}