Compare commits
No commits in common. "8a10953cc43777e95e467235ec2616103b0b1dc9" and "bc4c69e17e1e229bca3996f9ea1ee78c95d9663d" have entirely different histories.
8a10953cc4
...
bc4c69e17e
20 changed files with 20 additions and 5765 deletions
|
|
@ -1,10 +0,0 @@
|
||||||
#[target.'cfg(all(target_arch = "arm", target_os = "none"))']
|
|
||||||
#runner = "probe-rs run --chip RP2040"
|
|
||||||
[target.'cfg(all(target_arch = "arm", target_os = "none"))']
|
|
||||||
runner = "elf2uf2-rs --deploy --serial --verbose"
|
|
||||||
|
|
||||||
[build]
|
|
||||||
target = "thumbv6m-none-eabi" # Cortex-M0 and Cortex-M0+
|
|
||||||
|
|
||||||
[env]
|
|
||||||
DEFMT_LOG = "debug"
|
|
||||||
1
controller/.gitignore
vendored
1
controller/.gitignore
vendored
|
|
@ -1 +0,0 @@
|
||||||
target
|
|
||||||
2479
controller/Cargo.lock
generated
2479
controller/Cargo.lock
generated
File diff suppressed because it is too large
Load diff
|
|
@ -1,71 +0,0 @@
|
||||||
[package]
|
|
||||||
edition = "2021"
|
|
||||||
name = "controller"
|
|
||||||
version = "0.1.0"
|
|
||||||
license = "MIT OR Apache-2.0"
|
|
||||||
|
|
||||||
|
|
||||||
[dependencies]
|
|
||||||
embassy-executor = { version = "0.6.0", git="https://github.com/embassy-rs/embassy", features = ["task-arena-size-98304", "arch-cortex-m", "executor-thread", "executor-interrupt", "defmt", "integrated-timers"] }
|
|
||||||
embassy-time = { version = "0.3.2", git="https://github.com/embassy-rs/embassy", features = ["defmt", "defmt-timestamp-uptime"] }
|
|
||||||
embassy-rp = { version = "0.2.0", git="https://github.com/embassy-rs/embassy", features = ["defmt", "unstable-pac", "time-driver", "critical-section-impl", "rp2040"] }
|
|
||||||
embassy-usb = { version = "0.3.0", git="https://github.com/embassy-rs/embassy", features = ["defmt"] }
|
|
||||||
embassy-net = { version = "0.4.0", git="https://github.com/embassy-rs/embassy", features = ["defmt", "tcp", "udp", "raw", "dhcpv4", "medium-ethernet", "dns"] }
|
|
||||||
embassy-net-wiznet = { version = "0.1.0", git="https://github.com/embassy-rs/embassy", features = ["defmt"] }
|
|
||||||
embassy-futures = { version = "0.1.0", git="https://github.com/embassy-rs/embassy"}
|
|
||||||
embassy-usb-logger = { version = "0.2.0", git="https://github.com/embassy-rs/embassy" }
|
|
||||||
cyw43 = { version = "0.2.0", git="https://github.com/embassy-rs/embassy", features = ["defmt", "firmware-logs", "bluetooth"] }
|
|
||||||
cyw43-pio = { version = "0.2.0", git="https://github.com/embassy-rs/embassy", features = ["defmt"] }
|
|
||||||
|
|
||||||
defmt = "0.3"
|
|
||||||
defmt-rtt = "0.4"
|
|
||||||
fixed = "1.23.1"
|
|
||||||
fixed-macro = "1.2"
|
|
||||||
|
|
||||||
# for web request example
|
|
||||||
reqwless = { version = "0.12.0", features = ["defmt",]}
|
|
||||||
serde = { version = "1.0.203", default-features = false, features = ["derive"] }
|
|
||||||
serde-json-core = "0.5.1"
|
|
||||||
|
|
||||||
# for assign resources example
|
|
||||||
assign-resources = { git = "https://github.com/adamgreig/assign-resources", rev = "94ad10e2729afdf0fd5a77cd12e68409a982f58a" }
|
|
||||||
|
|
||||||
#cortex-m = { version = "0.7.6", features = ["critical-section-single-core"] }
|
|
||||||
cortex-m = { version = "0.7.6", features = ["inline-asm"] }
|
|
||||||
cortex-m-rt = "0.7.0"
|
|
||||||
critical-section = "1.1"
|
|
||||||
panic-probe = { version = "0.3", features = ["print-defmt"] }
|
|
||||||
display-interface-spi = "0.4.1"
|
|
||||||
embedded-graphics = "0.7.1"
|
|
||||||
st7789 = "0.6.1"
|
|
||||||
display-interface = "0.4.1"
|
|
||||||
byte-slice-cast = { version = "1.2.0", default-features = false }
|
|
||||||
smart-leds = "0.3.0"
|
|
||||||
heapless = "0.8"
|
|
||||||
usbd-hid = "0.8.1"
|
|
||||||
|
|
||||||
embedded-hal-1 = { package = "embedded-hal", version = "1.0" }
|
|
||||||
embedded-hal-async = "1.0"
|
|
||||||
embedded-hal-bus = { version = "0.1", features = ["async"] }
|
|
||||||
embedded-io-async = { version = "0.6.1", features = ["defmt-03"] }
|
|
||||||
embedded-storage = { version = "0.3" }
|
|
||||||
static_cell = "2.1"
|
|
||||||
portable-atomic = { version = "1.5", features = ["critical-section"] }
|
|
||||||
log = "0.4"
|
|
||||||
pio-proc = "0.2"
|
|
||||||
pio = "0.2.1"
|
|
||||||
rand = { version = "0.8.5", default-features = false }
|
|
||||||
embedded-sdmmc = "0.7.0"
|
|
||||||
|
|
||||||
bt-hci = { version = "0.1.0", default-features = false, features = ["defmt"] }
|
|
||||||
hex = { version = "0.4.3", default-features=false}
|
|
||||||
|
|
||||||
[profile.release]
|
|
||||||
debug = 2
|
|
||||||
lto = true
|
|
||||||
opt-level = 'z'
|
|
||||||
|
|
||||||
[profile.dev]
|
|
||||||
debug = 2
|
|
||||||
lto = true
|
|
||||||
opt-level = "z"
|
|
||||||
|
|
@ -1,36 +0,0 @@
|
||||||
//! This build script copies the `memory.x` file from the crate root into
|
|
||||||
//! a directory where the linker can always find it at build time.
|
|
||||||
//! For many projects this is optional, as the linker always searches the
|
|
||||||
//! project root directory -- wherever `Cargo.toml` is. However, if you
|
|
||||||
//! are using a workspace or have a more complicated build setup, this
|
|
||||||
//! build script becomes required. Additionally, by requesting that
|
|
||||||
//! Cargo re-run the build script whenever `memory.x` is changed,
|
|
||||||
//! updating `memory.x` ensures a rebuild of the application with the
|
|
||||||
//! new memory settings.
|
|
||||||
|
|
||||||
use std::env;
|
|
||||||
use std::fs::File;
|
|
||||||
use std::io::Write;
|
|
||||||
use std::path::PathBuf;
|
|
||||||
|
|
||||||
fn main() {
|
|
||||||
// Put `memory.x` in our output directory and ensure it's
|
|
||||||
// on the linker search path.
|
|
||||||
let out = &PathBuf::from(env::var_os("OUT_DIR").unwrap());
|
|
||||||
File::create(out.join("memory.x"))
|
|
||||||
.unwrap()
|
|
||||||
.write_all(include_bytes!("memory.x"))
|
|
||||||
.unwrap();
|
|
||||||
println!("cargo:rustc-link-search={}", out.display());
|
|
||||||
|
|
||||||
// By default, Cargo will re-run a build script whenever
|
|
||||||
// any file in the project changes. By specifying `memory.x`
|
|
||||||
// here, we ensure the build script is only re-run when
|
|
||||||
// `memory.x` is changed.
|
|
||||||
println!("cargo:rerun-if-changed=memory.x");
|
|
||||||
|
|
||||||
println!("cargo:rustc-link-arg-bins=--nmagic");
|
|
||||||
println!("cargo:rustc-link-arg-bins=-Tlink.x");
|
|
||||||
println!("cargo:rustc-link-arg-bins=-Tlink-rp.x");
|
|
||||||
println!("cargo:rustc-link-arg-bins=-Tdefmt.x");
|
|
||||||
}
|
|
||||||
|
|
@ -1,17 +0,0 @@
|
||||||
MEMORY {
|
|
||||||
BOOT2 : ORIGIN = 0x10000000, LENGTH = 0x100
|
|
||||||
FLASH : ORIGIN = 0x10000100, LENGTH = 2048K - 0x100
|
|
||||||
|
|
||||||
/* Pick one of the two options for RAM layout */
|
|
||||||
|
|
||||||
/* OPTION A: Use all RAM banks as one big block */
|
|
||||||
/* Reasonable, unless you are doing something */
|
|
||||||
/* really particular with DMA or other concurrent */
|
|
||||||
/* access that would benefit from striping */
|
|
||||||
RAM : ORIGIN = 0x20000000, LENGTH = 264K
|
|
||||||
|
|
||||||
/* OPTION B: Keep the unstriped sections separate */
|
|
||||||
/* RAM: ORIGIN = 0x20000000, LENGTH = 256K */
|
|
||||||
/* SCRATCH_A: ORIGIN = 0x20040000, LENGTH = 4K */
|
|
||||||
/* SCRATCH_B: ORIGIN = 0x20041000, LENGTH = 4K */
|
|
||||||
}
|
|
||||||
|
|
@ -1,493 +0,0 @@
|
||||||
//! This example uses the RP Pico W board Wifi chip (cyw43).
|
|
||||||
//! Creates an Access point Wifi network and creates a TCP endpoint on port 1234.
|
|
||||||
|
|
||||||
#![no_std]
|
|
||||||
#![no_main]
|
|
||||||
#![allow(async_fn_in_trait)]
|
|
||||||
|
|
||||||
mod vl53l0;
|
|
||||||
|
|
||||||
use core::array;
|
|
||||||
use core::fmt::Formatter;
|
|
||||||
use core::panic::PanicInfo;
|
|
||||||
use core::str::from_utf8;
|
|
||||||
|
|
||||||
use bt_hci::cmd::info;
|
|
||||||
use cyw43_pio::PioSpi;
|
|
||||||
use embassy_rp::i2c::{Async, I2c};
|
|
||||||
use log::*;
|
|
||||||
//use embassy_rp::i2c::InterruptHandler;
|
|
||||||
use embassy_executor::Spawner;
|
|
||||||
use embassy_net::tcp::TcpSocket;
|
|
||||||
use embassy_net::{Config, StackResources};
|
|
||||||
use embassy_rp::bind_interrupts;
|
|
||||||
use embassy_rp::clocks::RoscRng;
|
|
||||||
use embassy_rp::gpio::{Level, Output};
|
|
||||||
use embassy_rp::peripherals::{DMA_CH0, PIO0, USB};
|
|
||||||
use embassy_rp::pio::{InterruptHandler, Pio};
|
|
||||||
use embassy_rp::usb::Driver;
|
|
||||||
use embassy_time::{Duration, Timer};
|
|
||||||
use embedded_io_async::Write;
|
|
||||||
use rand::RngCore;
|
|
||||||
use reqwless::response;
|
|
||||||
use static_cell::StaticCell;
|
|
||||||
use defmt_rtt as _;
|
|
||||||
use vl53l0::RegAddr::*;
|
|
||||||
|
|
||||||
bind_interrupts!(struct Irqs {
|
|
||||||
PIO0_IRQ_0 => InterruptHandler<PIO0>;
|
|
||||||
I2C1_IRQ => embassy_rp::i2c::InterruptHandler<embassy_rp::peripherals::I2C1>;
|
|
||||||
USBCTRL_IRQ => embassy_rp::usb::InterruptHandler<USB>;
|
|
||||||
});
|
|
||||||
|
|
||||||
#[embassy_executor::task]
|
|
||||||
async fn logger_task(driver: Driver<'static, USB>) {
|
|
||||||
embassy_usb_logger::run!(1024, log::LevelFilter::Debug, driver);
|
|
||||||
}
|
|
||||||
|
|
||||||
#[embassy_executor::task]
|
|
||||||
async fn cyw43_task(runner: cyw43::Runner<'static, Output<'static>, PioSpi<'static, PIO0, 0, DMA_CH0>>) -> ! {
|
|
||||||
runner.run().await
|
|
||||||
}
|
|
||||||
|
|
||||||
#[embassy_executor::task]
|
|
||||||
async fn net_task(mut runner: embassy_net::Runner<'static, cyw43::NetDriver<'static>>) -> ! {
|
|
||||||
runner.run().await
|
|
||||||
}
|
|
||||||
|
|
||||||
#[panic_handler]
|
|
||||||
fn panic( info: &PanicInfo) -> ! {
|
|
||||||
error!("{}", info);
|
|
||||||
loop { }
|
|
||||||
}
|
|
||||||
|
|
||||||
#[embassy_executor::main]
|
|
||||||
async fn main(spawner: Spawner) {
|
|
||||||
info!("Hello World!");
|
|
||||||
|
|
||||||
let p = embassy_rp::init(Default::default());
|
|
||||||
let mut rng = RoscRng;
|
|
||||||
|
|
||||||
let driver = Driver::new(p.USB, Irqs);
|
|
||||||
spawner.spawn(logger_task(driver)).unwrap();
|
|
||||||
|
|
||||||
let sda = p.PIN_26;
|
|
||||||
let scl = p.PIN_27;
|
|
||||||
let config = embassy_rp::i2c::Config::default();
|
|
||||||
let mut bus = embassy_rp::i2c::I2c::new_async(p.I2C1, scl, sda, Irqs, config);
|
|
||||||
|
|
||||||
let fw = include_bytes!("../../cyw43-firmware/43439A0.bin");
|
|
||||||
let clm = include_bytes!("../../cyw43-firmware/43439A0_clm.bin");
|
|
||||||
|
|
||||||
// To make flashing faster for development, you may want to flash the firmwares independently
|
|
||||||
// at hardcoded addresses, instead of baking them into the program with `include_bytes!`:
|
|
||||||
// probe-rs download 43439A0.bin --binary-format bin --chip RP2040 --base-address 0x10100000
|
|
||||||
// probe-rs download 43439A0_clm.bin --binary-format bin --chip RP2040 --base-address 0x10140000
|
|
||||||
//let fw = unsafe { core::slice::from_raw_parts(0x10100000 as *const u8, 230321) };
|
|
||||||
//let clm = unsafe { core::slice::from_raw_parts(0x10140000 as *const u8, 4752) };
|
|
||||||
|
|
||||||
let pwr = Output::new(p.PIN_23, Level::Low);
|
|
||||||
let cs = Output::new(p.PIN_25, Level::High);
|
|
||||||
let mut pio = Pio::new(p.PIO0, Irqs);
|
|
||||||
let spi = PioSpi::new(&mut pio.common, pio.sm0, pio.irq0, cs, p.PIN_24, p.PIN_29, p.DMA_CH0);
|
|
||||||
|
|
||||||
static STATE: StaticCell<cyw43::State> = StaticCell::new();
|
|
||||||
let state = STATE.init(cyw43::State::new());
|
|
||||||
let (net_device, mut control, runner) = cyw43::new(state, pwr, spi, fw).await;
|
|
||||||
defmt::unwrap!(spawner.spawn(cyw43_task(runner)));
|
|
||||||
|
|
||||||
control.init(clm).await;
|
|
||||||
control
|
|
||||||
.set_power_management(cyw43::PowerManagementMode::PowerSave)
|
|
||||||
.await;
|
|
||||||
|
|
||||||
// Use a link-local address for communication without DHCP server
|
|
||||||
let config = Config::ipv4_static(embassy_net::StaticConfigV4 {
|
|
||||||
address: embassy_net::Ipv4Cidr::new(embassy_net::Ipv4Address::new(169, 254, 1, 1), 16),
|
|
||||||
dns_servers: heapless::Vec::new(),
|
|
||||||
gateway: None,
|
|
||||||
});
|
|
||||||
|
|
||||||
// Generate random seed
|
|
||||||
let seed = rng.next_u64();
|
|
||||||
|
|
||||||
// Init network stack
|
|
||||||
static RESOURCES: StaticCell<StackResources<3>> = StaticCell::new();
|
|
||||||
let (stack, runner) = embassy_net::new(net_device, config, RESOURCES.init(StackResources::new()), seed);
|
|
||||||
|
|
||||||
defmt::unwrap!(spawner.spawn(net_task(runner)));
|
|
||||||
|
|
||||||
//control.start_ap_open("cyw43", 5).await;
|
|
||||||
control.start_ap_wpa2("cyw43", "password", 5).await;
|
|
||||||
|
|
||||||
// And now we can use it!
|
|
||||||
|
|
||||||
let mut rx_buffer = [0; 4096];
|
|
||||||
let mut tx_buffer = [0; 4096];
|
|
||||||
let mut buf = [0; 4096];
|
|
||||||
|
|
||||||
//embassy_time::Timer::after_millis(7000).await;
|
|
||||||
|
|
||||||
async fn write_to_device<'a, T,const N: usize>(bus: &mut embassy_rp::i2c::I2c<'a, T,Async>, addr: u16, data: [[u8;2];N])
|
|
||||||
where T: embassy_rp::i2c::Instance {
|
|
||||||
for transaction in data {
|
|
||||||
let _ = bus.write_async(addr, transaction).await;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
async fn write_flag<'a, T>(bus: &mut embassy_rp::i2c::I2c<'a, T,Async>, addr: u16, reg: u8, bit: u8, value: bool)
|
|
||||||
where T: embassy_rp::i2c::Instance {
|
|
||||||
let mut initial: [u8;1] = [0];
|
|
||||||
let _ = bus.write_read_async(addr, [reg], &mut initial).await;
|
|
||||||
let mask = 1 << bit;
|
|
||||||
if value {
|
|
||||||
initial[0] |= mask;
|
|
||||||
} else {
|
|
||||||
initial[0] &= !mask;
|
|
||||||
}
|
|
||||||
let _ = bus.write_async(addr, [reg, initial[0]]).await;
|
|
||||||
}
|
|
||||||
|
|
||||||
let id = 0x29;
|
|
||||||
Timer::after_millis(50).await; // sensor boot
|
|
||||||
write_to_device(&mut bus, id, [
|
|
||||||
[0x88, 0x00],
|
|
||||||
[0x80, 0x01],
|
|
||||||
[0xFF, 0x01],
|
|
||||||
[0x00, 0x00],
|
|
||||||
]).await;
|
|
||||||
let mut stop: [u8;1] = [0];
|
|
||||||
let _ = bus.write_read_async(id, [0x91], &mut stop);
|
|
||||||
write_to_device(&mut bus, id, [
|
|
||||||
[0x00, 0x01],
|
|
||||||
[0xFF, 0x00],
|
|
||||||
[0x80, 0x00],
|
|
||||||
]).await;
|
|
||||||
|
|
||||||
// disable SIGNAL_RATE_MSRC (bit 1) and SIGNAL_RATE_PRE_RANGE (bit 4) limit checks
|
|
||||||
write_flag(&mut bus, id, 0x60, 1, true).await;
|
|
||||||
write_flag(&mut bus, id, 0x60, 4, true).await;
|
|
||||||
|
|
||||||
let mega_counts_per_second = 0.25;
|
|
||||||
let mega_counts_per_second: u16 = (mega_counts_per_second * (1<<7) as f64) as u16;
|
|
||||||
let _ = bus.write_async(id, [FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT as u8,
|
|
||||||
(mega_counts_per_second >> 8) as u8,
|
|
||||||
(mega_counts_per_second & 255) as u8
|
|
||||||
]).await;
|
|
||||||
|
|
||||||
// get spad info
|
|
||||||
let _ = bus.write_async(id, [SYSTEM_SEQUENCE_CONFIG as u8, 0xFF]).await;
|
|
||||||
|
|
||||||
write_to_device(&mut bus, id , [
|
|
||||||
[0x80, 0x01],
|
|
||||||
[0xff, 0x01],
|
|
||||||
[0x00, 0x00],
|
|
||||||
[0xff, 0x06],
|
|
||||||
]).await;
|
|
||||||
write_flag(&mut bus, id, 0x83, 3, true).await;
|
|
||||||
write_to_device(&mut bus, id , [
|
|
||||||
[0xff, 0x07],
|
|
||||||
[0x81, 0x01],
|
|
||||||
[0x80, 0x01],
|
|
||||||
[0x94, 0x6b],
|
|
||||||
[0x83, 0x00],
|
|
||||||
]).await;
|
|
||||||
debug!("starting spad wait");
|
|
||||||
loop {
|
|
||||||
let mut wait: [u8;1] = [0];
|
|
||||||
let _ = bus.write_read_async(id, [0x83], &mut wait).await;
|
|
||||||
if wait[0] != 0 {
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
Timer::after_micros(5).await;
|
|
||||||
}
|
|
||||||
debug!("ended spad wait");
|
|
||||||
|
|
||||||
let _ = bus.write_async(id, [0x83, 0x01]).await;
|
|
||||||
|
|
||||||
let mut value: [u8;1] = [0];
|
|
||||||
let _ = bus.write_read_async(id, [0x92], &mut value).await;
|
|
||||||
write_to_device(&mut bus, id, [
|
|
||||||
[0x81, 0x00],
|
|
||||||
[0xff, 0x06],
|
|
||||||
]).await;
|
|
||||||
write_flag(&mut bus, id, 0x83, 3, false).await;
|
|
||||||
write_to_device(&mut bus, id, [
|
|
||||||
[0xff, 0x01],
|
|
||||||
[0x00, 0x01],
|
|
||||||
[0xff, 0x00],
|
|
||||||
[0x80, 0x00],
|
|
||||||
]).await;
|
|
||||||
let count = value[0] & 0x7f;
|
|
||||||
let is_aperture = value[0] & 0b10000000;
|
|
||||||
let is_aperture = is_aperture != 0;
|
|
||||||
// TODO: vl53l0x.py post line 200
|
|
||||||
|
|
||||||
let mut spad_map: [u8;6]=[0;6];
|
|
||||||
let _ = bus.write_read_async(id, [GLOBAL_CONFIG_SPAD_ENABLES_REF_0 as u8], &mut spad_map);
|
|
||||||
|
|
||||||
write_to_device(&mut bus, id, [
|
|
||||||
[0xff, 0x01],
|
|
||||||
[DYNAMIC_SPAD_REF_EN_START_OFFSET as u8, 0x00],
|
|
||||||
[DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD as u8, 0x2c],
|
|
||||||
[0xff, 0x00],
|
|
||||||
[DYNAMIC_SPAD_REF_EN_START_OFFSET as u8, 0xb4],
|
|
||||||
]).await;
|
|
||||||
|
|
||||||
let mut spads_enabled = 0;
|
|
||||||
for i in 0..48 {
|
|
||||||
if i < 12 && is_aperture || spads_enabled >= count {
|
|
||||||
spad_map[i/8] &= !(1<< (i>>2));
|
|
||||||
} else if (spad_map[i/8] & (1<< (i>>2))) != 0 {
|
|
||||||
spads_enabled += 1;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
let mut spad_write: [u8;7] = [0;7];
|
|
||||||
spad_write[0] = GLOBAL_CONFIG_SPAD_ENABLES_REF_0 as u8;
|
|
||||||
spad_write[1..].clone_from_slice(&spad_map);
|
|
||||||
let _ = bus.write_async(id, spad_write).await;
|
|
||||||
|
|
||||||
|
|
||||||
write_to_device(&mut bus, id, [
|
|
||||||
[0xFF, 0x01],
|
|
||||||
[0x00, 0x00],
|
|
||||||
[0xFF, 0x00],
|
|
||||||
[0x09, 0x00],
|
|
||||||
[0x10, 0x00],
|
|
||||||
[0x11, 0x00],
|
|
||||||
[0x24, 0x01],
|
|
||||||
[0x25, 0xFF],
|
|
||||||
[0x75, 0x00],
|
|
||||||
[0xFF, 0x01],
|
|
||||||
[0x4E, 0x2C],
|
|
||||||
[0x48, 0x00],
|
|
||||||
[0x30, 0x20],
|
|
||||||
[0xFF, 0x00],
|
|
||||||
[0x30, 0x09],
|
|
||||||
[0x54, 0x00],
|
|
||||||
[0x31, 0x04],
|
|
||||||
[0x32, 0x03],
|
|
||||||
[0x40, 0x83],
|
|
||||||
[0x46, 0x25],
|
|
||||||
[0x60, 0x00],
|
|
||||||
[0x27, 0x00],
|
|
||||||
[0x50, 0x06],
|
|
||||||
[0x51, 0x00],
|
|
||||||
[0x52, 0x96],
|
|
||||||
[0x56, 0x08],
|
|
||||||
[0x57, 0x30],
|
|
||||||
[0x61, 0x00],
|
|
||||||
[0x62, 0x00],
|
|
||||||
[0x64, 0x00],
|
|
||||||
[0x65, 0x00],
|
|
||||||
[0x66, 0xA0],
|
|
||||||
[0xFF, 0x01],
|
|
||||||
[0x22, 0x32],
|
|
||||||
[0x47, 0x14],
|
|
||||||
[0x49, 0xFF],
|
|
||||||
[0x4A, 0x00],
|
|
||||||
[0xFF, 0x00],
|
|
||||||
[0x7A, 0x0A],
|
|
||||||
[0x7B, 0x00],
|
|
||||||
[0x78, 0x21],
|
|
||||||
[0xFF, 0x01],
|
|
||||||
[0x23, 0x34],
|
|
||||||
[0x42, 0x00],
|
|
||||||
[0x44, 0xFF],
|
|
||||||
[0x45, 0x26],
|
|
||||||
[0x46, 0x05],
|
|
||||||
[0x40, 0x40],
|
|
||||||
[0x0E, 0x06],
|
|
||||||
[0x20, 0x1A],
|
|
||||||
[0x43, 0x40],
|
|
||||||
[0xFF, 0x00],
|
|
||||||
[0x34, 0x03],
|
|
||||||
[0x35, 0x44],
|
|
||||||
[0xFF, 0x01],
|
|
||||||
[0x31, 0x04],
|
|
||||||
[0x4B, 0x09],
|
|
||||||
[0x4C, 0x05],
|
|
||||||
[0x4D, 0x04],
|
|
||||||
[0xFF, 0x00],
|
|
||||||
[0x44, 0x00],
|
|
||||||
[0x45, 0x20],
|
|
||||||
[0x47, 0x08],
|
|
||||||
[0x48, 0x28],
|
|
||||||
[0x67, 0x00],
|
|
||||||
[0x70, 0x04],
|
|
||||||
[0x71, 0x01],
|
|
||||||
[0x72, 0xFE],
|
|
||||||
[0x76, 0x00],
|
|
||||||
[0x77, 0x00],
|
|
||||||
[0xFF, 0x01],
|
|
||||||
[0x0D, 0x01],
|
|
||||||
[0xFF, 0x00],
|
|
||||||
[0x80, 0x01],
|
|
||||||
[0x01, 0xF8],
|
|
||||||
[0xFF, 0x01],
|
|
||||||
[0x8E, 0x01],
|
|
||||||
[0x00, 0x01],
|
|
||||||
[0xFF, 0x00],
|
|
||||||
[0x80, 0x00],
|
|
||||||
]).await;
|
|
||||||
//write_flag(&mut bus, id, GPIO_HV_MUX_ACTIVE_HIGH as u8, 4, false).await;
|
|
||||||
//let _ = bus.write_async(id, [SYSTEM_SEQUENCE_CONFIG as u8, 0x01]).await;
|
|
||||||
//calibrate(&mut bus, id, 0x40).await;
|
|
||||||
//let _ = bus.write_async(id, [SYSTEM_SEQUENCE_CONFIG as u8, 0x02]).await;
|
|
||||||
//calibrate(&mut bus, id, 0x00).await;
|
|
||||||
//let _ = bus.write_async(id, [SYSTEM_SEQUENCE_CONFIG as u8, 0xe8]).await;
|
|
||||||
|
|
||||||
async fn calibrate<'a, T>(bus: &mut embassy_rp::i2c::I2c<'a, T,Async>, addr: u16, data: u8)
|
|
||||||
where T: embassy_rp::i2c::Instance {
|
|
||||||
let _ = bus.write_async(addr, [SYSRANGE_START as u8 ,data | 0x01]).await;
|
|
||||||
debug!("started calib wait");
|
|
||||||
loop {
|
|
||||||
let mut wait: [u8;1] = [0];
|
|
||||||
let _ = bus.write_read_async(addr, [0x13], &mut wait).await;
|
|
||||||
if wait[0] & 0x07 != 0 {
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
Timer::after_micros(5).await;
|
|
||||||
}
|
|
||||||
debug!("ended calib wait");
|
|
||||||
write_to_device(bus, addr, [
|
|
||||||
[SYSTEM_INTERRUPT_CLEAR as u8, 0x01],
|
|
||||||
[SYSRANGE_START as u8, 0x00],
|
|
||||||
]).await;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
//TODO VL53L0X.cpp L 236-280
|
|
||||||
|
|
||||||
|
|
||||||
// start continuous mode
|
|
||||||
write_to_device(&mut bus, id, [
|
|
||||||
[0x80, 0x01],
|
|
||||||
[0xFF, 0x01],
|
|
||||||
[0x00, 0x00],
|
|
||||||
[0x91, stop[0]],
|
|
||||||
[0x00, 0x01],
|
|
||||||
[0xFF, 0x00],
|
|
||||||
[0x80, 0x00],
|
|
||||||
//[0x04, 0x00], // measurement delay
|
|
||||||
[0x00, 0x02], // back to back shots
|
|
||||||
]).await;
|
|
||||||
|
|
||||||
//let _ = bus.write_async(0x88u16, [0x00]).await;
|
|
||||||
//let _ = bus.write_async(0x80u16, [0x01]).await;
|
|
||||||
//let _ = bus.write_async(0xFFu16, [0x01]).await;
|
|
||||||
//let _ = bus.write_async(0x00u16, [0x00]).await;
|
|
||||||
//let _ = bus.write_async(0x00u16, [0x01]).await;
|
|
||||||
//let _ = bus.write_async(0xFFu16, [0x00]).await;
|
|
||||||
//let _ = bus.write_async(0x80u16, [0x00]).await;
|
|
||||||
|
|
||||||
//loop {
|
|
||||||
// debug!("starting wait part 1");
|
|
||||||
// loop {
|
|
||||||
// let mut wait: [u8;1] = [0];
|
|
||||||
// let _ = bus.write_read_async(id, [SYSRANGE_START as u8], &mut wait).await;
|
|
||||||
// debug!("{wait:?}");
|
|
||||||
// if wait[0] & 0x01 != 0 {
|
|
||||||
// break;
|
|
||||||
// }
|
|
||||||
// Timer::after_micros(5).await;
|
|
||||||
// }
|
|
||||||
// debug!("starting wait part 2");
|
|
||||||
// loop {
|
|
||||||
// let mut wait: [u8;1] = [0];
|
|
||||||
// let _ = bus.write_read_async(id, [RESULT_INTERRUPT_STATUS as u8], &mut wait).await;
|
|
||||||
// if wait[0] & 0x07 != 0 {
|
|
||||||
// break;
|
|
||||||
// }
|
|
||||||
// Timer::after_micros(5).await;
|
|
||||||
// }
|
|
||||||
// debug!("ending wait part 2");
|
|
||||||
// let mut output: [u8; 2] = [0;2];
|
|
||||||
// let _ = bus.write_read_async(id, [0x14], &mut output).await;
|
|
||||||
// let _ = bus.write_async(id, [0x0B, 0x01]).await;
|
|
||||||
// info!("{:?}",output);
|
|
||||||
// Timer::after_millis(20).await;
|
|
||||||
//}
|
|
||||||
|
|
||||||
loop {
|
|
||||||
let mut socket = TcpSocket::new(stack, &mut rx_buffer, &mut tx_buffer);
|
|
||||||
socket.set_timeout(Some(Duration::from_secs(10)));
|
|
||||||
|
|
||||||
control.gpio_set(0, false).await;
|
|
||||||
info!("Listening on TCP:1234...");
|
|
||||||
if let Err(e) = socket.accept(1234).await {
|
|
||||||
warn!("accept error: {:?}", e);
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
|
|
||||||
info!("Received connection from {:?}", socket.remote_endpoint());
|
|
||||||
control.gpio_set(0, true).await;
|
|
||||||
|
|
||||||
loop {
|
|
||||||
let mut n = match socket.read(&mut buf).await {
|
|
||||||
Ok(0) => {
|
|
||||||
warn!("read EOF");
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
Ok(n) => n,
|
|
||||||
Err(e) => {
|
|
||||||
warn!("read error: {:?}", e);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
info!("rxd {}", from_utf8(&buf[..n]).unwrap());
|
|
||||||
Timer::after_millis(30).await;
|
|
||||||
|
|
||||||
let mut segs = buf[..n].trim_ascii().split(|c| *c == ' ' as u8);
|
|
||||||
|
|
||||||
match char::from_u32(segs.next().unwrap()[0] as u32).unwrap() {
|
|
||||||
'W' => {
|
|
||||||
let addr: [u8;1] = hex::FromHex::from_hex(segs.next().unwrap()).unwrap();
|
|
||||||
let reg: [u8;1] = hex::FromHex::from_hex(segs.next().unwrap()).unwrap();
|
|
||||||
let data: [u8;1] = hex::FromHex::from_hex(segs.next().unwrap()).unwrap();
|
|
||||||
info!("writing {:?}", reg);
|
|
||||||
Timer::after_millis(30).await;
|
|
||||||
|
|
||||||
bus.write_async(addr[0] as u16, [reg[0], data[0]]).await.unwrap();
|
|
||||||
|
|
||||||
buf[0] = b"O"[0];
|
|
||||||
buf[1] = b"K"[0];
|
|
||||||
n=2;
|
|
||||||
},
|
|
||||||
'R' => {
|
|
||||||
let addr: [u8;1] = hex::FromHex::from_hex(segs.next().unwrap()).unwrap();
|
|
||||||
let reg: [u8;1] = hex::FromHex::from_hex(segs.next().unwrap()).unwrap();
|
|
||||||
info!("writing {:?}, addr {addr:?}", reg);
|
|
||||||
|
|
||||||
let mut response: [u8;2] = [0;2];
|
|
||||||
|
|
||||||
let _ = bus.write_read_async(addr[0] as u16, reg, &mut response).await;
|
|
||||||
info!("recd {:?}", response);
|
|
||||||
info!("recd {:#02x}", response[0]);
|
|
||||||
let _ = hex::encode_to_slice(response, &mut buf);
|
|
||||||
n = 4;
|
|
||||||
|
|
||||||
|
|
||||||
},
|
|
||||||
_ => {}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
//let mut response: [u8;2] = [0;2];
|
|
||||||
|
|
||||||
//let _ = bus.read_async(0xC0u16, &mut response).await;
|
|
||||||
//let _ = hex::encode_to_slice(response, &mut buf);
|
|
||||||
|
|
||||||
match socket.write_all(&buf[..n]).await {
|
|
||||||
Ok(()) => {}
|
|
||||||
Err(e) => {
|
|
||||||
warn!("write error: {:?}", e);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
};
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -1,49 +0,0 @@
|
||||||
Permissive Binary License
|
|
||||||
|
|
||||||
Version 1.0, July 2019
|
|
||||||
|
|
||||||
Redistribution. Redistribution and use in binary form, without
|
|
||||||
modification, are permitted provided that the following conditions are
|
|
||||||
met:
|
|
||||||
|
|
||||||
1) Redistributions must reproduce the above copyright notice and the
|
|
||||||
following disclaimer in the documentation and/or other materials
|
|
||||||
provided with the distribution.
|
|
||||||
|
|
||||||
2) Unless to the extent explicitly permitted by law, no reverse
|
|
||||||
engineering, decompilation, or disassembly of this software is
|
|
||||||
permitted.
|
|
||||||
|
|
||||||
3) Redistribution as part of a software development kit must include the
|
|
||||||
accompanying file named <20>DEPENDENCIES<45> and any dependencies listed in
|
|
||||||
that file.
|
|
||||||
|
|
||||||
4) Neither the name of the copyright holder nor the names of its
|
|
||||||
contributors may be used to endorse or promote products derived from
|
|
||||||
this software without specific prior written permission.
|
|
||||||
|
|
||||||
Limited patent license. The copyright holders (and contributors) grant a
|
|
||||||
worldwide, non-exclusive, no-charge, royalty-free patent license to
|
|
||||||
make, have made, use, offer to sell, sell, import, and otherwise
|
|
||||||
transfer this software, where such license applies only to those patent
|
|
||||||
claims licensable by the copyright holders (and contributors) that are
|
|
||||||
necessarily infringed by this software. This patent license shall not
|
|
||||||
apply to any combinations that include this software. No hardware is
|
|
||||||
licensed hereunder.
|
|
||||||
|
|
||||||
If you institute patent litigation against any entity (including a
|
|
||||||
cross-claim or counterclaim in a lawsuit) alleging that the software
|
|
||||||
itself infringes your patent(s), then your rights granted under this
|
|
||||||
license shall terminate as of the date such litigation is filed.
|
|
||||||
|
|
||||||
DISCLAIMER. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
|
|
||||||
CONTRIBUTORS "AS IS." ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT
|
|
||||||
NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
||||||
FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
|
|
||||||
HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
|
|
||||||
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
|
|
||||||
TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
|
|
||||||
PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
|
|
||||||
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
|
|
||||||
NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
|
||||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|
||||||
|
|
@ -1,14 +0,0 @@
|
||||||
# WiFi + Bluetooth firmware blobs
|
|
||||||
|
|
||||||
Firmware obtained from https://github.com/georgerobotics/cyw43-driver/tree/main/firmware
|
|
||||||
|
|
||||||
Licensed under the [Infineon Permissive Binary License](./LICENSE-permissive-binary-license-1.0.txt)
|
|
||||||
|
|
||||||
## Changelog
|
|
||||||
|
|
||||||
* 2023-08-21: synced with `a1dc885` - Update 43439 fw + clm to come from `wb43439A0_7_95_49_00_combined.h` + add Bluetooth firmware
|
|
||||||
* 2023-07-28: synced with `ad3bad0` - Update 43439 fw from 7.95.55 to 7.95.62
|
|
||||||
|
|
||||||
## Notes
|
|
||||||
|
|
||||||
If you update these files, please update the lengths in the `tests/rp/src/bin/cyw43_perf.rs` test (which relies on these files running from RAM).
|
|
||||||
|
|
@ -1,158 +0,0 @@
|
||||||
"""
|
|
||||||
A lightweight MicroPython implementation for interfacing with an MPU-6050 via I2C.
|
|
||||||
Author: Tim Hanewich - https://github.com/TimHanewich
|
|
||||||
Version: 1.0
|
|
||||||
Get updates to this code file here: https://github.com/TimHanewich/MicroPython-Collection/blob/master/MPU6050/MPU6050.py
|
|
||||||
|
|
||||||
License: MIT License
|
|
||||||
Copyright 2023 Tim Hanewich
|
|
||||||
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the “Software”), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
|
|
||||||
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
|
|
||||||
THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
|
||||||
"""
|
|
||||||
|
|
||||||
import machine
|
|
||||||
|
|
||||||
class MPU6050:
|
|
||||||
"""Class for reading gyro rates and acceleration data from an MPU-6050 module via I2C."""
|
|
||||||
|
|
||||||
def __init__(self, i2c:machine.I2C, address:int = 0x68):
|
|
||||||
"""
|
|
||||||
Creates a new MPU6050 class for reading gyro rates and acceleration data.
|
|
||||||
:param i2c: A setup I2C module of the machine module.
|
|
||||||
:param address: The I2C address of the MPU-6050 you are using (0x68 is the default).
|
|
||||||
"""
|
|
||||||
self.address = address
|
|
||||||
self.i2c = i2c
|
|
||||||
|
|
||||||
def wake(self) -> None:
|
|
||||||
"""Wake up the MPU-6050."""
|
|
||||||
self.i2c.writeto_mem(self.address, 0x6B, bytes([0x01]))
|
|
||||||
|
|
||||||
def sleep(self) -> None:
|
|
||||||
"""Places MPU-6050 in sleep mode (low power consumption). Stops the internal reading of new data. Any calls to get gyro or accel data while in sleep mode will remain unchanged - the data is not being updated internally within the MPU-6050!"""
|
|
||||||
self.i2c.writeto_mem(self.address, 0x6B, bytes([0x40]))
|
|
||||||
|
|
||||||
def who_am_i(self) -> int:
|
|
||||||
"""Returns the address of the MPU-6050 (ensure it is working)."""
|
|
||||||
return self.i2c.readfrom_mem(self.address, 0x75, 1)[0]
|
|
||||||
|
|
||||||
def read_temperature(self) -> float:
|
|
||||||
"""Reads the temperature, in celsius, of the onboard temperature sensor of the MPU-6050."""
|
|
||||||
data = self.i2c.readfrom_mem(self.address, 0x41, 2)
|
|
||||||
raw_temp:float = self._translate_pair(data[0], data[1])
|
|
||||||
temp:float = (raw_temp / 340.0) + 36.53
|
|
||||||
return temp
|
|
||||||
|
|
||||||
def read_gyro_range(self) -> int:
|
|
||||||
"""Reads the gyroscope range setting."""
|
|
||||||
return self._hex_to_index(self.i2c.readfrom_mem(self.address, 0x1B, 1)[0])
|
|
||||||
|
|
||||||
def write_gyro_range(self, range:int) -> None:
|
|
||||||
"""Sets the gyroscope range setting."""
|
|
||||||
self.i2c.writeto_mem(self.address, 0x1B, bytes([self._index_to_hex(range)]))
|
|
||||||
|
|
||||||
def read_gyro_data(self) -> tuple[float, float, float]:
|
|
||||||
"""Read the gyroscope data, in a (x, y, z) tuple."""
|
|
||||||
|
|
||||||
# set the modified based on the gyro range (need to divide to calculate)
|
|
||||||
gr:int = self.read_gyro_range()
|
|
||||||
modifier:float = None
|
|
||||||
if gr == 0:
|
|
||||||
modifier = 131.0
|
|
||||||
elif gr == 1:
|
|
||||||
modifier = 65.5
|
|
||||||
elif gr == 2:
|
|
||||||
modifier = 32.8
|
|
||||||
elif gr == 3:
|
|
||||||
modifier = 16.4
|
|
||||||
|
|
||||||
# read data
|
|
||||||
data = self.i2c.readfrom_mem(self.address, 0x43, 6) # read 6 bytes (gyro data)
|
|
||||||
x:float = (self._translate_pair(data[0], data[1])) / modifier
|
|
||||||
y:float = (self._translate_pair(data[2], data[3])) / modifier
|
|
||||||
z:float = (self._translate_pair(data[4], data[5])) / modifier
|
|
||||||
|
|
||||||
return (x, y, z)
|
|
||||||
|
|
||||||
def read_accel_range(self) -> int:
|
|
||||||
"""Reads the accelerometer range setting."""
|
|
||||||
return self._hex_to_index(self.i2c.readfrom_mem(self.address, 0x1C, 1)[0])
|
|
||||||
|
|
||||||
def write_accel_range(self, range:int) -> None:
|
|
||||||
"""Sets the gyro accelerometer setting."""
|
|
||||||
self.i2c.writeto_mem(self.address, 0x1C, bytes([self._index_to_hex(range)]))
|
|
||||||
|
|
||||||
def read_accel_data(self) -> tuple[float, float, float]:
|
|
||||||
"""Read the accelerometer data, in a (x, y, z) tuple."""
|
|
||||||
|
|
||||||
# set the modified based on the gyro range (need to divide to calculate)
|
|
||||||
ar:int = self.read_accel_range()
|
|
||||||
modifier:float = None
|
|
||||||
if ar == 0:
|
|
||||||
modifier = 16384.0
|
|
||||||
elif ar == 1:
|
|
||||||
modifier = 8192.0
|
|
||||||
elif ar == 2:
|
|
||||||
modifier = 4096.0
|
|
||||||
elif ar == 3:
|
|
||||||
modifier = 2048.0
|
|
||||||
|
|
||||||
# read data
|
|
||||||
data = self.i2c.readfrom_mem(self.address, 0x3B, 6) # read 6 bytes (accel data)
|
|
||||||
x:float = (self._translate_pair(data[0], data[1])) / modifier
|
|
||||||
y:float = (self._translate_pair(data[2], data[3])) / modifier
|
|
||||||
z:float = (self._translate_pair(data[4], data[5])) / modifier
|
|
||||||
|
|
||||||
return (x, y, z)
|
|
||||||
|
|
||||||
def read_lpf_range(self) -> int:
|
|
||||||
return self.i2c.readfrom_mem(self.address, 0x1A, 1)[0]
|
|
||||||
|
|
||||||
def write_lpf_range(self, range:int) -> None:
|
|
||||||
"""
|
|
||||||
Sets low pass filter range.
|
|
||||||
:param range: Low pass range setting, 0-6. 0 = minimum filter, 6 = maximum filter.
|
|
||||||
"""
|
|
||||||
|
|
||||||
# check range
|
|
||||||
if range < 0 or range > 6:
|
|
||||||
raise Exception("Range '" + str(range) + "' is not a valid low pass filter setting.")
|
|
||||||
|
|
||||||
self.i2c.writeto_mem(self.address, 0x1A, bytes([range]))
|
|
||||||
|
|
||||||
|
|
||||||
#### UTILITY FUNCTIONS BELOW ####
|
|
||||||
|
|
||||||
def _translate_pair(self, high:int, low:int) -> int:
|
|
||||||
"""Converts a byte pair to a usable value. Borrowed from https://github.com/m-rtijn/mpu6050/blob/0626053a5e1182f4951b78b8326691a9223a5f7d/mpu6050/mpu6050.py#L76C39-L76C39."""
|
|
||||||
value = (high << 8) + low
|
|
||||||
if value >= 0x8000:
|
|
||||||
value = -((65535 - value) + 1)
|
|
||||||
return value
|
|
||||||
|
|
||||||
def _hex_to_index(self, range:int) -> int:
|
|
||||||
"""Converts a hexadecimal range setting to an integer (index), 0-3. This is used for both the gyroscope and accelerometer ranges."""
|
|
||||||
if range== 0x00:
|
|
||||||
return 0
|
|
||||||
elif range == 0x08:
|
|
||||||
return 1
|
|
||||||
elif range == 0x10:
|
|
||||||
return 2
|
|
||||||
elif range == 0x18:
|
|
||||||
return 3
|
|
||||||
else:
|
|
||||||
raise Exception("Found unknown gyro range setting '" + str(range) + "'")
|
|
||||||
|
|
||||||
def _index_to_hex(self, index:int) -> int:
|
|
||||||
"""Converts an index integer (0-3) to a hexadecimal range setting. This is used for both the gyroscope and accelerometer ranges."""
|
|
||||||
if index == 0:
|
|
||||||
return 0x00
|
|
||||||
elif index == 1:
|
|
||||||
return 0x08
|
|
||||||
elif index == 2:
|
|
||||||
return 0x10
|
|
||||||
elif index == 3:
|
|
||||||
return 0x18
|
|
||||||
else:
|
|
||||||
raise Exception("Range index '" + index + "' invalid. Must be 0-3.")
|
|
||||||
|
|
@ -1,648 +0,0 @@
|
||||||
from micropython import const
|
|
||||||
import ustruct
|
|
||||||
import utime
|
|
||||||
from machine import Timer
|
|
||||||
import time
|
|
||||||
|
|
||||||
_IO_TIMEOUT = 1000
|
|
||||||
_SYSRANGE_START = const(0x00)
|
|
||||||
_EXTSUP_HV = const(0x89)
|
|
||||||
_MSRC_CONFIG = const(0x60)
|
|
||||||
_FINAL_RATE_RTN_LIMIT = const(0x44)
|
|
||||||
_SYSTEM_SEQUENCE = const(0x01)
|
|
||||||
_SPAD_REF_START = const(0x4f)
|
|
||||||
_SPAD_ENABLES = const(0xb0)
|
|
||||||
_REF_EN_START_SELECT = const(0xb6)
|
|
||||||
_SPAD_NUM_REQUESTED = const(0x4e)
|
|
||||||
_INTERRUPT_GPIO = const(0x0a)
|
|
||||||
_INTERRUPT_CLEAR = const(0x0b)
|
|
||||||
_GPIO_MUX_ACTIVE_HIGH = const(0x84)
|
|
||||||
_RESULT_INTERRUPT_STATUS = const(0x13)
|
|
||||||
_RESULT_RANGE_STATUS = const(0x14)
|
|
||||||
_OSC_CALIBRATE = const(0xf8)
|
|
||||||
_MEASURE_PERIOD = const(0x04)
|
|
||||||
|
|
||||||
SYSRANGE_START = 0x00
|
|
||||||
|
|
||||||
SYSTEM_THRESH_HIGH = 0x0C
|
|
||||||
SYSTEM_THRESH_LOW = 0x0E
|
|
||||||
|
|
||||||
SYSTEM_SEQUENCE_CONFIG = 0x01
|
|
||||||
SYSTEM_RANGE_CONFIG = 0x09
|
|
||||||
SYSTEM_INTERMEASUREMENT_PERIOD = 0x04
|
|
||||||
|
|
||||||
SYSTEM_INTERRUPT_CONFIG_GPIO = 0x0A
|
|
||||||
|
|
||||||
GPIO_HV_MUX_ACTIVE_HIGH = 0x84
|
|
||||||
|
|
||||||
SYSTEM_INTERRUPT_CLEAR = 0x0B
|
|
||||||
|
|
||||||
RESULT_INTERRUPT_STATUS = 0x13
|
|
||||||
RESULT_RANGE_STATUS = 0x14
|
|
||||||
|
|
||||||
RESULT_CORE_AMBIENT_WINDOW_EVENTS_RTN = 0xBC
|
|
||||||
RESULT_CORE_RANGING_TOTAL_EVENTS_RTN = 0xC0
|
|
||||||
RESULT_CORE_AMBIENT_WINDOW_EVENTS_REF = 0xD0
|
|
||||||
RESULT_CORE_RANGING_TOTAL_EVENTS_REF = 0xD4
|
|
||||||
RESULT_PEAK_SIGNAL_RATE_REF = 0xB6
|
|
||||||
|
|
||||||
ALGO_PART_TO_PART_RANGE_OFFSET_MM = 0x28
|
|
||||||
|
|
||||||
I2C_SLAVE_DEVICE_ADDRESS = 0x8A
|
|
||||||
|
|
||||||
MSRC_CONFIG_CONTROL = 0x60
|
|
||||||
|
|
||||||
PRE_RANGE_CONFIG_MIN_SNR = 0x27
|
|
||||||
PRE_RANGE_CONFIG_VALID_PHASE_LOW = 0x56
|
|
||||||
PRE_RANGE_CONFIG_VALID_PHASE_HIGH = 0x57
|
|
||||||
PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT = 0x64
|
|
||||||
|
|
||||||
FINAL_RANGE_CONFIG_MIN_SNR = 0x67
|
|
||||||
FINAL_RANGE_CONFIG_VALID_PHASE_LOW = 0x47
|
|
||||||
FINAL_RANGE_CONFIG_VALID_PHASE_HIGH = 0x48
|
|
||||||
FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT = 0x44
|
|
||||||
|
|
||||||
PRE_RANGE_CONFIG_SIGMA_THRESH_HI = 0x61
|
|
||||||
PRE_RANGE_CONFIG_SIGMA_THRESH_LO = 0x62
|
|
||||||
|
|
||||||
PRE_RANGE_CONFIG_VCSEL_PERIOD = 0x50
|
|
||||||
PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI = 0x51
|
|
||||||
PRE_RANGE_CONFIG_TIMEOUT_MACROP_LO = 0x52
|
|
||||||
|
|
||||||
SYSTEM_HISTOGRAM_BIN = 0x81
|
|
||||||
HISTOGRAM_CONFIG_INITIAL_PHASE_SELECT = 0x33
|
|
||||||
HISTOGRAM_CONFIG_READOUT_CTRL = 0x55
|
|
||||||
|
|
||||||
FINAL_RANGE_CONFIG_VCSEL_PERIOD = 0x70
|
|
||||||
FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI = 0x71
|
|
||||||
FINAL_RANGE_CONFIG_TIMEOUT_MACROP_LO = 0x72
|
|
||||||
CROSSTALK_COMPENSATION_PEAK_RATE_MCPS = 0x20
|
|
||||||
|
|
||||||
MSRC_CONFIG_TIMEOUT_MACROP = 0x46
|
|
||||||
|
|
||||||
SOFT_RESET_GO2_SOFT_RESET_N = 0xBF
|
|
||||||
IDENTIFICATION_MODEL_ID = 0xC0
|
|
||||||
IDENTIFICATION_REVISION_ID = 0xC2
|
|
||||||
|
|
||||||
OSC_CALIBRATE_VAL = 0xF8
|
|
||||||
|
|
||||||
GLOBAL_CONFIG_VCSEL_WIDTH = 0x32
|
|
||||||
GLOBAL_CONFIG_SPAD_ENABLES_REF_0 = 0xB0
|
|
||||||
GLOBAL_CONFIG_SPAD_ENABLES_REF_1 = 0xB1
|
|
||||||
GLOBAL_CONFIG_SPAD_ENABLES_REF_2 = 0xB2
|
|
||||||
GLOBAL_CONFIG_SPAD_ENABLES_REF_3 = 0xB3
|
|
||||||
GLOBAL_CONFIG_SPAD_ENABLES_REF_4 = 0xB4
|
|
||||||
GLOBAL_CONFIG_SPAD_ENABLES_REF_5 = 0xB5
|
|
||||||
|
|
||||||
GLOBAL_CONFIG_REF_EN_START_SELECT = 0xB6
|
|
||||||
DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD = 0x4E
|
|
||||||
DYNAMIC_SPAD_REF_EN_START_OFFSET = 0x4F
|
|
||||||
POWER_MANAGEMENT_GO1_POWER_FORCE = 0x80
|
|
||||||
|
|
||||||
VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV = 0x89
|
|
||||||
|
|
||||||
ALGO_PHASECAL_LIM = 0x30
|
|
||||||
ALGO_PHASECAL_CONFIG_TIMEOUT = 0x30
|
|
||||||
|
|
||||||
|
|
||||||
class TimeoutError(RuntimeError):
|
|
||||||
pass
|
|
||||||
|
|
||||||
|
|
||||||
class VL53L0X:
|
|
||||||
def __init__(self, i2c, address=0x29):
|
|
||||||
self.i2c = i2c
|
|
||||||
self.address = address
|
|
||||||
self.init()
|
|
||||||
self._started = False
|
|
||||||
self.measurement_timing_budget_us = 0
|
|
||||||
self.set_measurement_timing_budget(self.measurement_timing_budget_us)
|
|
||||||
self.enables = {"tcc": 0,
|
|
||||||
"dss": 0,
|
|
||||||
"msrc": 0,
|
|
||||||
"pre_range": 0,
|
|
||||||
"final_range": 0}
|
|
||||||
self.timeouts = {"pre_range_vcsel_period_pclks": 0,
|
|
||||||
"msrc_dss_tcc_mclks": 0,
|
|
||||||
"msrc_dss_tcc_us": 0,
|
|
||||||
"pre_range_mclks": 0,
|
|
||||||
"pre_range_us": 0,
|
|
||||||
"final_range_vcsel_period_pclks": 0,
|
|
||||||
"final_range_mclks": 0,
|
|
||||||
"final_range_us": 0
|
|
||||||
}
|
|
||||||
self.vcsel_period_type = ["VcselPeriodPreRange", "VcselPeriodFinalRange"]
|
|
||||||
|
|
||||||
def _registers(self, register, values=None, struct='B'):
|
|
||||||
if values is None:
|
|
||||||
size = ustruct.calcsize(struct)
|
|
||||||
data = self.i2c.readfrom_mem(self.address, register, size)
|
|
||||||
values = ustruct.unpack(struct, data)
|
|
||||||
return values
|
|
||||||
data = ustruct.pack(struct, *values)
|
|
||||||
self.i2c.writeto_mem(self.address, register, data)
|
|
||||||
|
|
||||||
def _register(self, register, value=None, struct='B'):
|
|
||||||
if value is None:
|
|
||||||
return self._registers(register, struct=struct)[0]
|
|
||||||
self._registers(register, (value,), struct=struct)
|
|
||||||
|
|
||||||
def _flag(self, register=0x00, bit=0, value=None):
|
|
||||||
data = self._register(register)
|
|
||||||
mask = 1 << bit
|
|
||||||
if value is None:
|
|
||||||
return bool(data & mask)
|
|
||||||
elif value:
|
|
||||||
data |= mask
|
|
||||||
else:
|
|
||||||
data &= ~mask
|
|
||||||
self._register(register, data)
|
|
||||||
|
|
||||||
def _config(self, *config):
|
|
||||||
for register, value in config:
|
|
||||||
self._register(register, value)
|
|
||||||
|
|
||||||
def init(self, power2v8=True):
|
|
||||||
self._flag(_EXTSUP_HV, 0, power2v8)
|
|
||||||
|
|
||||||
# I2C standard mode
|
|
||||||
self._config(
|
|
||||||
(0x88, 0x00),
|
|
||||||
|
|
||||||
(0x80, 0x01),
|
|
||||||
(0xff, 0x01),
|
|
||||||
(0x00, 0x00),
|
|
||||||
)
|
|
||||||
self._stop_variable = self._register(0x91)
|
|
||||||
self._config(
|
|
||||||
(0x00, 0x01),
|
|
||||||
(0xff, 0x00),
|
|
||||||
(0x80, 0x00),
|
|
||||||
)
|
|
||||||
|
|
||||||
# disable signal_rate_msrc and signal_rate_pre_range limit checks
|
|
||||||
self._flag(_MSRC_CONFIG, 1, True)
|
|
||||||
self._flag(_MSRC_CONFIG, 4, True)
|
|
||||||
|
|
||||||
# rate_limit = 0.25
|
|
||||||
self._register(_FINAL_RATE_RTN_LIMIT, int(0.1 * (1 << 7)),
|
|
||||||
struct='>H')
|
|
||||||
|
|
||||||
self._register(_SYSTEM_SEQUENCE, 0xff)
|
|
||||||
|
|
||||||
spad_count, is_aperture = self._spad_info()
|
|
||||||
spad_map = bytearray(self._registers(_SPAD_ENABLES, struct='6B'))
|
|
||||||
|
|
||||||
# set reference spads
|
|
||||||
self._config(
|
|
||||||
(0xff, 0x01),
|
|
||||||
(_SPAD_REF_START, 0x00),
|
|
||||||
(_SPAD_NUM_REQUESTED, 0x2c),
|
|
||||||
(0xff, 0x00),
|
|
||||||
(_REF_EN_START_SELECT, 0xb4),
|
|
||||||
)
|
|
||||||
|
|
||||||
spads_enabled = 0
|
|
||||||
for i in range(48):
|
|
||||||
if i < 12 and is_aperture or spads_enabled >= spad_count:
|
|
||||||
spad_map[i // 8] &= ~(1 << (i >> 2))
|
|
||||||
elif spad_map[i // 8] & (1 << (i >> 2)):
|
|
||||||
spads_enabled += 1
|
|
||||||
|
|
||||||
self._registers(_SPAD_ENABLES, spad_map, struct='6B')
|
|
||||||
|
|
||||||
self._config(
|
|
||||||
(0xff, 0x01),
|
|
||||||
(0x00, 0x00),
|
|
||||||
|
|
||||||
(0xff, 0x00),
|
|
||||||
(0x09, 0x00),
|
|
||||||
(0x10, 0x00),
|
|
||||||
(0x11, 0x00),
|
|
||||||
|
|
||||||
(0x24, 0x01),
|
|
||||||
(0x25, 0xFF),
|
|
||||||
(0x75, 0x00),
|
|
||||||
|
|
||||||
(0xFF, 0x01),
|
|
||||||
(0x4E, 0x2C),
|
|
||||||
(0x48, 0x00),
|
|
||||||
(0x30, 0x20),
|
|
||||||
|
|
||||||
(0xFF, 0x00),
|
|
||||||
(0x30, 0x09),
|
|
||||||
(0x54, 0x00),
|
|
||||||
(0x31, 0x04),
|
|
||||||
(0x32, 0x03),
|
|
||||||
(0x40, 0x83),
|
|
||||||
(0x46, 0x25),
|
|
||||||
(0x60, 0x00),
|
|
||||||
(0x27, 0x00),
|
|
||||||
(0x50, 0x06),
|
|
||||||
(0x51, 0x00),
|
|
||||||
(0x52, 0x96),
|
|
||||||
(0x56, 0x08),
|
|
||||||
(0x57, 0x30),
|
|
||||||
(0x61, 0x00),
|
|
||||||
(0x62, 0x00),
|
|
||||||
(0x64, 0x00),
|
|
||||||
(0x65, 0x00),
|
|
||||||
(0x66, 0xA0),
|
|
||||||
|
|
||||||
(0xFF, 0x01),
|
|
||||||
(0x22, 0x32),
|
|
||||||
(0x47, 0x14),
|
|
||||||
(0x49, 0xFF),
|
|
||||||
(0x4A, 0x00),
|
|
||||||
|
|
||||||
(0xFF, 0x00),
|
|
||||||
(0x7A, 0x0A),
|
|
||||||
(0x7B, 0x00),
|
|
||||||
(0x78, 0x21),
|
|
||||||
|
|
||||||
(0xFF, 0x01),
|
|
||||||
(0x23, 0x34),
|
|
||||||
(0x42, 0x00),
|
|
||||||
(0x44, 0xFF),
|
|
||||||
(0x45, 0x26),
|
|
||||||
(0x46, 0x05),
|
|
||||||
(0x40, 0x40),
|
|
||||||
(0x0E, 0x06),
|
|
||||||
(0x20, 0x1A),
|
|
||||||
(0x43, 0x40),
|
|
||||||
|
|
||||||
(0xFF, 0x00),
|
|
||||||
(0x34, 0x03),
|
|
||||||
(0x35, 0x44),
|
|
||||||
|
|
||||||
(0xFF, 0x01),
|
|
||||||
(0x31, 0x04),
|
|
||||||
(0x4B, 0x09),
|
|
||||||
(0x4C, 0x05),
|
|
||||||
(0x4D, 0x04),
|
|
||||||
|
|
||||||
(0xFF, 0x00),
|
|
||||||
(0x44, 0x00),
|
|
||||||
(0x45, 0x20),
|
|
||||||
(0x47, 0x08),
|
|
||||||
(0x48, 0x28),
|
|
||||||
(0x67, 0x00),
|
|
||||||
(0x70, 0x04),
|
|
||||||
(0x71, 0x01),
|
|
||||||
(0x72, 0xFE),
|
|
||||||
(0x76, 0x00),
|
|
||||||
(0x77, 0x00),
|
|
||||||
|
|
||||||
(0xFF, 0x01),
|
|
||||||
(0x0D, 0x01),
|
|
||||||
|
|
||||||
(0xFF, 0x00),
|
|
||||||
(0x80, 0x01),
|
|
||||||
(0x01, 0xF8),
|
|
||||||
|
|
||||||
(0xFF, 0x01),
|
|
||||||
(0x8E, 0x01),
|
|
||||||
(0x00, 0x01),
|
|
||||||
(0xFF, 0x00),
|
|
||||||
(0x80, 0x00),
|
|
||||||
)
|
|
||||||
|
|
||||||
self._register(_INTERRUPT_GPIO, 0x04)
|
|
||||||
self._flag(_GPIO_MUX_ACTIVE_HIGH, 4, False)
|
|
||||||
self._register(_INTERRUPT_CLEAR, 0x01)
|
|
||||||
|
|
||||||
# XXX Need to implement this.
|
|
||||||
# budget = self._timing_budget()
|
|
||||||
# self._register(_SYSTEM_SEQUENCE, 0xe8)
|
|
||||||
# self._timing_budget(budget)
|
|
||||||
|
|
||||||
self._register(_SYSTEM_SEQUENCE, 0x01)
|
|
||||||
self._calibrate(0x40)
|
|
||||||
self._register(_SYSTEM_SEQUENCE, 0x02)
|
|
||||||
self._calibrate(0x00)
|
|
||||||
|
|
||||||
self._register(_SYSTEM_SEQUENCE, 0xe8)
|
|
||||||
|
|
||||||
def _spad_info(self):
|
|
||||||
self._config(
|
|
||||||
(0x80, 0x01),
|
|
||||||
(0xff, 0x01),
|
|
||||||
(0x00, 0x00),
|
|
||||||
|
|
||||||
(0xff, 0x06),
|
|
||||||
)
|
|
||||||
self._flag(0x83, 3, True)
|
|
||||||
self._config(
|
|
||||||
(0xff, 0x07),
|
|
||||||
(0x81, 0x01),
|
|
||||||
|
|
||||||
(0x80, 0x01),
|
|
||||||
|
|
||||||
(0x94, 0x6b),
|
|
||||||
(0x83, 0x00),
|
|
||||||
)
|
|
||||||
for timeout in range(_IO_TIMEOUT):
|
|
||||||
if self._register(0x83):
|
|
||||||
break
|
|
||||||
utime.sleep_ms(1)
|
|
||||||
else:
|
|
||||||
raise TimeoutError()
|
|
||||||
self._config(
|
|
||||||
(0x83, 0x01),
|
|
||||||
)
|
|
||||||
value = self._register(0x92)
|
|
||||||
self._config(
|
|
||||||
(0x81, 0x00),
|
|
||||||
(0xff, 0x06),
|
|
||||||
)
|
|
||||||
self._flag(0x83, 3, False)
|
|
||||||
self._config(
|
|
||||||
(0xff, 0x01),
|
|
||||||
(0x00, 0x01),
|
|
||||||
|
|
||||||
(0xff, 0x00),
|
|
||||||
(0x80, 0x00),
|
|
||||||
)
|
|
||||||
count = value & 0x7f
|
|
||||||
is_aperture = bool(value & 0b10000000)
|
|
||||||
return count, is_aperture
|
|
||||||
|
|
||||||
def _calibrate(self, vhv_init_byte):
|
|
||||||
self._register(_SYSRANGE_START, 0x01 | vhv_init_byte)
|
|
||||||
for timeout in range(_IO_TIMEOUT):
|
|
||||||
if self._register(_RESULT_INTERRUPT_STATUS) & 0x07:
|
|
||||||
break
|
|
||||||
utime.sleep_ms(1)
|
|
||||||
else:
|
|
||||||
raise TimeoutError()
|
|
||||||
self._register(_INTERRUPT_CLEAR, 0x01)
|
|
||||||
self._register(_SYSRANGE_START, 0x00)
|
|
||||||
|
|
||||||
def start(self, period=0):
|
|
||||||
self._config(
|
|
||||||
(0x80, 0x01),
|
|
||||||
(0xFF, 0x01),
|
|
||||||
(0x00, 0x00),
|
|
||||||
(0x91, self._stop_variable),
|
|
||||||
(0x00, 0x01),
|
|
||||||
(0xFF, 0x00),
|
|
||||||
(0x80, 0x00),
|
|
||||||
)
|
|
||||||
if period:
|
|
||||||
oscilator = self._register(_OSC_CALIBRATE, struct='>H')
|
|
||||||
if oscilator:
|
|
||||||
period *= oscilator
|
|
||||||
self._register(_MEASURE_PERIOD, period, struct='>H')
|
|
||||||
self._register(_SYSRANGE_START, 0x04)
|
|
||||||
else:
|
|
||||||
self._register(_SYSRANGE_START, 0x02)
|
|
||||||
self._started = True
|
|
||||||
|
|
||||||
def stop(self):
|
|
||||||
self._register(_SYSRANGE_START, 0x01)
|
|
||||||
self._config(
|
|
||||||
(0xFF, 0x01),
|
|
||||||
(0x00, 0x00),
|
|
||||||
(0x91, self._stop_variable),
|
|
||||||
(0x00, 0x01),
|
|
||||||
(0xFF, 0x00),
|
|
||||||
)
|
|
||||||
self._started = False
|
|
||||||
|
|
||||||
def read(self):
|
|
||||||
if not self._started:
|
|
||||||
self._config(
|
|
||||||
(0x80, 0x01),
|
|
||||||
(0xFF, 0x01),
|
|
||||||
(0x00, 0x00),
|
|
||||||
(0x91, self._stop_variable),
|
|
||||||
(0x00, 0x01),
|
|
||||||
(0xFF, 0x00),
|
|
||||||
(0x80, 0x00),
|
|
||||||
(_SYSRANGE_START, 0x01),
|
|
||||||
)
|
|
||||||
for timeout in range(_IO_TIMEOUT):
|
|
||||||
if not self._register(_SYSRANGE_START) & 0x01:
|
|
||||||
break
|
|
||||||
utime.sleep_ms(1)
|
|
||||||
else:
|
|
||||||
raise TimeoutError()
|
|
||||||
for timeout in range(_IO_TIMEOUT):
|
|
||||||
if self._register(_RESULT_INTERRUPT_STATUS) & 0x07:
|
|
||||||
break
|
|
||||||
utime.sleep_ms(1)
|
|
||||||
else:
|
|
||||||
raise TimeoutError()
|
|
||||||
value = self._register(_RESULT_RANGE_STATUS + 10, struct='>H')
|
|
||||||
self._register(_INTERRUPT_CLEAR, 0x01)
|
|
||||||
return value
|
|
||||||
|
|
||||||
def set_signal_rate_limit(self, limit_Mcps):
|
|
||||||
if limit_Mcps < 0 or limit_Mcps > 511.99:
|
|
||||||
return False
|
|
||||||
self._register(0x44, limit_Mcps * (1 << 7))
|
|
||||||
return True
|
|
||||||
|
|
||||||
def decode_Vcsel_period(self, reg_val):
|
|
||||||
return (((reg_val) + 1) << 1)
|
|
||||||
|
|
||||||
def encode_Vcsel_period(self, period_pclks):
|
|
||||||
return (((period_pclks) >> 1) - 1)
|
|
||||||
|
|
||||||
def set_Vcsel_pulse_period(self, type, period_pclks):
|
|
||||||
vcsel_period_reg = self.encode_Vcsel_period(period_pclks)
|
|
||||||
|
|
||||||
self.get_sequence_step_enables()
|
|
||||||
self.get_sequence_step_timeouts()
|
|
||||||
|
|
||||||
if type == self.vcsel_period_type[0]:
|
|
||||||
if period_pclks == 12:
|
|
||||||
self._register(PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x18)
|
|
||||||
elif period_pclks == 14:
|
|
||||||
self._register(PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x30)
|
|
||||||
elif period_pclks == 16:
|
|
||||||
self._register(PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x40)
|
|
||||||
elif period_pclks == 18:
|
|
||||||
self._register(PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x50)
|
|
||||||
else:
|
|
||||||
return False
|
|
||||||
|
|
||||||
self._register(PRE_RANGE_CONFIG_VALID_PHASE_LOW, 0x08)
|
|
||||||
self._register(PRE_RANGE_CONFIG_VCSEL_PERIOD, vcsel_period_reg)
|
|
||||||
|
|
||||||
new_pre_range_timeout_mclks = self.timeout_microseconds_to_Mclks(self.timeouts["pre_range_us"],
|
|
||||||
period_pclks)
|
|
||||||
self._register(PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI, self.encode_timeout(new_pre_range_timeout_mclks))
|
|
||||||
|
|
||||||
new_msrc_timeout_mclks = self.timeout_microseconds_to_Mclks(self.timeouts["msrc_dss_tcc_us"],
|
|
||||||
period_pclks)
|
|
||||||
self._register(MSRC_CONFIG_TIMEOUT_MACROP, 255 if new_msrc_timeout_mclks > 256 else (new_msrc_timeout_mclks - 1))
|
|
||||||
elif type == self.vcsel_period_type[1]:
|
|
||||||
if period_pclks == 8:
|
|
||||||
self._register(FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x10)
|
|
||||||
self._register(FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08)
|
|
||||||
self._register(GLOBAL_CONFIG_VCSEL_WIDTH, 0x02)
|
|
||||||
self._(ALGO_PHASECAL_CONFIG_TIMEOUT, 0x0C)
|
|
||||||
self._register(0xFF, 0x01)
|
|
||||||
self._register(ALGO_PHASECAL_LIM, 0x30)
|
|
||||||
self._register(0xFF, 0x00)
|
|
||||||
elif period_pclks == 10:
|
|
||||||
self._register(FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x28)
|
|
||||||
self._register(FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08)
|
|
||||||
self._register(GLOBAL_CONFIG_VCSEL_WIDTH, 0x03)
|
|
||||||
self._register(ALGO_PHASECAL_CONFIG_TIMEOUT, 0x09)
|
|
||||||
self._register(0xFF, 0x01)
|
|
||||||
self._register(ALGO_PHASECAL_LIM, 0x20)
|
|
||||||
self._register(0xFF, 0x00)
|
|
||||||
elif period_pclks == 12:
|
|
||||||
self._register(FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x38)
|
|
||||||
self._register(FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08)
|
|
||||||
self._register(GLOBAL_CONFIG_VCSEL_WIDTH, 0x03)
|
|
||||||
self._register(ALGO_PHASECAL_CONFIG_TIMEOUT, 0x08)
|
|
||||||
self._register(0xFF, 0x01)
|
|
||||||
self._register(ALGO_PHASECAL_LIM, 0x20)
|
|
||||||
self._register(0xFF, 0x00)
|
|
||||||
elif period_pclks == 14:
|
|
||||||
self._register(FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x48)
|
|
||||||
self._register(FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08)
|
|
||||||
self._register(GLOBAL_CONFIG_VCSEL_WIDTH, 0x03)
|
|
||||||
self._register(ALGO_PHASECAL_CONFIG_TIMEOUT, 0x07)
|
|
||||||
self._register(0xFF, 0x01)
|
|
||||||
self._register(ALGO_PHASECAL_LIM, 0x20)
|
|
||||||
self._register(0xFF, 0x00)
|
|
||||||
else:
|
|
||||||
return False
|
|
||||||
|
|
||||||
self._register(FINAL_RANGE_CONFIG_VCSEL_PERIOD, vcsel_period_reg)
|
|
||||||
|
|
||||||
new_final_range_timeout_mclks = self.timeout_microseconds_to_Mclks(self.timeouts["final_range_us"], period_pclks)
|
|
||||||
|
|
||||||
if self.enables["pre_range"]:
|
|
||||||
new_final_range_timeout_mclks += 1
|
|
||||||
self._register(FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI, self.encode_timeout(new_final_range_timeout_mclks))
|
|
||||||
else:
|
|
||||||
return False
|
|
||||||
self.set_measurement_timing_budget(self.measurement_timing_budget_us)
|
|
||||||
sequence_config = self._register(SYSTEM_SEQUENCE_CONFIG)
|
|
||||||
self._register(SYSTEM_SEQUENCE_CONFIG, 0x02)
|
|
||||||
self.perform_single_ref_calibration(0x0)
|
|
||||||
self._register(SYSTEM_SEQUENCE_CONFIG, sequence_config)
|
|
||||||
|
|
||||||
return True
|
|
||||||
|
|
||||||
def get_sequence_step_enables(self):
|
|
||||||
sequence_config = self._register(0x01)
|
|
||||||
|
|
||||||
self.enables["tcc"] = (sequence_config >> 4) & 0x1
|
|
||||||
self.enables["dss"] = (sequence_config >> 3) & 0x1
|
|
||||||
self.enables["msrc"] = (sequence_config >> 2) & 0x1
|
|
||||||
self.enables["pre_range"] = (sequence_config >> 6) & 0x1
|
|
||||||
self.enables["final_range"] = (sequence_config >> 7) & 0x1
|
|
||||||
|
|
||||||
def get_vcsel_pulse_period(self, type):
|
|
||||||
if type == self.vcsel_period_type[0]:
|
|
||||||
return self.decode_Vcsel_period(0x50)
|
|
||||||
elif type == self.vcsel_period_type[1]:
|
|
||||||
return self.decode_Vcsel_period(0x70)
|
|
||||||
else:
|
|
||||||
return 255
|
|
||||||
|
|
||||||
def get_sequence_step_timeouts(self):
|
|
||||||
self.timeouts["pre_range_vcsel_period_pclks"] = self.get_vcsel_pulse_period(self.vcsel_period_type[0])
|
|
||||||
self.timeouts["msrc_dss_tcc_mclks"] = int(self._register(MSRC_CONFIG_TIMEOUT_MACROP)) + 1
|
|
||||||
self.timeouts["msrc_dss_tcc_us"] = self.timeout_Mclks_to_microseconds(self.timeouts["msrc_dss_tcc_mclks"],
|
|
||||||
self.timeouts[
|
|
||||||
"pre_range_vcsel_period_pclks"])
|
|
||||||
self.timeouts["pre_range_mclks"] = self.decode_timeout(PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI)
|
|
||||||
self.timeouts["pre_range_us"] = self.timeout_Mclks_to_microseconds(self.timeouts["pre_range_mclks"],
|
|
||||||
self.timeouts[
|
|
||||||
"pre_range_vcsel_period_pclks"])
|
|
||||||
self.timeouts["final_range_vcsel_period_pclks"] = self.get_vcsel_pulse_period(self.vcsel_period_type[1])
|
|
||||||
self.timeouts["final_range_mclks"] = self.decode_timeout(self._register(FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI))
|
|
||||||
|
|
||||||
if self.enables["pre_range"]:
|
|
||||||
self.timeouts["final_range_mclks"] -= self.timeouts["pre_range_mclks"]
|
|
||||||
self.timeouts["final_range_us"] = self.timeout_Mclks_to_microseconds(self.timeouts["final_range_mclks"],
|
|
||||||
self.timeouts[
|
|
||||||
"final_range_vcsel_period_pclks"])
|
|
||||||
|
|
||||||
def timeout_Mclks_to_microseconds(self, timeout_period_mclks, vcsel_period_pclks):
|
|
||||||
macro_period_ns = self.calc_macro_period(vcsel_period_pclks)
|
|
||||||
return ((timeout_period_mclks * macro_period_ns) + (macro_period_ns / 2)) / 1000
|
|
||||||
|
|
||||||
def timeout_microseconds_to_Mclks(self, timeout_period_us, vcsel_period_pclks):
|
|
||||||
macro_period_ns = self.calc_macro_period(vcsel_period_pclks)
|
|
||||||
return (((timeout_period_us * 1000) + (macro_period_ns / 2)) / macro_period_ns)
|
|
||||||
|
|
||||||
def calc_macro_period(self, vcsel_period_pclks):
|
|
||||||
return (((2304 * (vcsel_period_pclks) * 1655) + 500) / 1000)
|
|
||||||
|
|
||||||
def decode_timeout(self, reg_val):
|
|
||||||
return ((reg_val & 0x00FF) << ((reg_val & 0xFF00) >> 8)) + 1
|
|
||||||
|
|
||||||
def encode_timeout(self, timeout_mclks):
|
|
||||||
timeout_mclks = int(timeout_mclks)
|
|
||||||
ls_byte = 0
|
|
||||||
ms_byte = 0
|
|
||||||
|
|
||||||
if timeout_mclks > 0:
|
|
||||||
ls_byte = timeout_mclks - 1
|
|
||||||
|
|
||||||
while (ls_byte & 0xFFFFFF00) > 0:
|
|
||||||
ls_byte >>= 1
|
|
||||||
ms_byte += 1
|
|
||||||
return (ms_byte << 8) or (ls_byte & 0xFF)
|
|
||||||
else:
|
|
||||||
return 0
|
|
||||||
|
|
||||||
def set_measurement_timing_budget(self, budget_us):
|
|
||||||
start_overhead = 1320
|
|
||||||
end_overhead = 960
|
|
||||||
msrc_overhead = 660
|
|
||||||
tcc_overhead = 590
|
|
||||||
dss_overhead = 690
|
|
||||||
pre_range_overhead = 660
|
|
||||||
final_range_overhead = 550
|
|
||||||
|
|
||||||
min_timing_budget = 20000
|
|
||||||
|
|
||||||
if budget_us < min_timing_budget:
|
|
||||||
return False
|
|
||||||
used_budget_us = start_overhead + end_overhead
|
|
||||||
|
|
||||||
self.get_sequence_step_enables()
|
|
||||||
self.get_sequence_step_timeouts()
|
|
||||||
|
|
||||||
if self.enables["tcc"]:
|
|
||||||
used_budget_us += self.timeouts["msrc_dss_tcc_us"] + tcc_overhead
|
|
||||||
if self.enables["dss"]:
|
|
||||||
used_budget_us += 2* self.timeouts["msrc_dss_tcc_us"] + dss_overhead
|
|
||||||
if self.enables["msrc"]:
|
|
||||||
used_budget_us += self.timeouts["msrc_dss_tcc_us"] + msrc_overhead
|
|
||||||
if self.enables["pre_range"]:
|
|
||||||
used_budget_us += self.timeouts["pre_range_us"] + pre_range_overhead
|
|
||||||
if self.enables["final_range"]:
|
|
||||||
used_budget_us += final_range_overhead
|
|
||||||
|
|
||||||
if used_budget_us > budget_us:
|
|
||||||
return False
|
|
||||||
final_range_timeout_us = budget_us - used_budget_us
|
|
||||||
final_range_timeout_mclks = self.timeout_microseconds_to_Mclks(final_range_timeout_us, self.timeouts["final_range_vcsel_period_pclks"])
|
|
||||||
|
|
||||||
if self.enables["pre_range"]:
|
|
||||||
final_range_timeout_mclks += self.timeouts["pre_range_mclks"]
|
|
||||||
self._register(FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI, self.encode_timeout(final_range_timeout_mclks))
|
|
||||||
self.measurement_timing_budget_us = budget_us
|
|
||||||
return True
|
|
||||||
|
|
||||||
def perform_single_ref_calibration(self, vhv_init_byte):
|
|
||||||
chrono = Timer.Chrono()
|
|
||||||
self._register(SYSRANGE_START, 0x01|vhv_init_byte)
|
|
||||||
chrono.start()
|
|
||||||
while self._register((RESULT_INTERRUPT_STATUS & 0x07) == 0):
|
|
||||||
time_elapsed = chrono.read_ms()
|
|
||||||
if time_elapsed > _IO_TIMEOUT:
|
|
||||||
return False
|
|
||||||
self._register(SYSTEM_INTERRUPT_CLEAR, 0x01)
|
|
||||||
self._register(SYSRANGE_START, 0x00)
|
|
||||||
return True
|
|
||||||
|
|
@ -1,64 +0,0 @@
|
||||||
import time
|
|
||||||
import network
|
|
||||||
import socket
|
|
||||||
from machine import Pin
|
|
||||||
from machine import I2C
|
|
||||||
import VL53L0X
|
|
||||||
import MPU6050
|
|
||||||
|
|
||||||
ssid = "cruise"
|
|
||||||
password = "rust"
|
|
||||||
|
|
||||||
ap = network.WLAN(network.AP_IF)
|
|
||||||
ap.config(essid=ssid)
|
|
||||||
ap.active(True)
|
|
||||||
|
|
||||||
i2c_tof = I2C(1, scl=Pin(27), sda=Pin(26))
|
|
||||||
tof = False
|
|
||||||
try:
|
|
||||||
tof = VL53L0X.VL53L0X(i2c_tof, address=0x29)
|
|
||||||
except Exception as e:
|
|
||||||
tof = False
|
|
||||||
|
|
||||||
i2c_gyro = machine.I2C(0, sda=machine.Pin(20), scl=machine.Pin(21));
|
|
||||||
mpu = False
|
|
||||||
try:
|
|
||||||
mpu = MPU6050.MPU6050(i2c_gyro, address = 0x68)
|
|
||||||
mpu.wake()
|
|
||||||
except Exception as e:
|
|
||||||
mpu = False
|
|
||||||
|
|
||||||
while (ap.active() == False):
|
|
||||||
pass
|
|
||||||
print("network active")
|
|
||||||
print("ip: "+ap.ifconfig()[0])
|
|
||||||
|
|
||||||
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
|
||||||
s.bind(('', 1337))
|
|
||||||
s.listen(5)
|
|
||||||
|
|
||||||
while True:
|
|
||||||
try:
|
|
||||||
cl, addr = s.accept()
|
|
||||||
request = cl.recv(1024)
|
|
||||||
print(request)
|
|
||||||
cl.send("recvd")
|
|
||||||
cl.close()
|
|
||||||
except OSError as e:
|
|
||||||
cl.close()
|
|
||||||
print("closed")
|
|
||||||
|
|
||||||
pass
|
|
||||||
|
|
||||||
|
|
||||||
while True:
|
|
||||||
if tof:
|
|
||||||
tof.start()
|
|
||||||
print(tof.read())
|
|
||||||
|
|
||||||
if mpu:
|
|
||||||
gyro = mpu.read_gyro_data()
|
|
||||||
accel = mpu.read_accel_data()
|
|
||||||
print("Gyro: " + str(gyro) + ", Accel: " + str(accel))
|
|
||||||
time.sleep(0.1)
|
|
||||||
|
|
||||||
1598
simcontroller/Cargo.lock
generated
1598
simcontroller/Cargo.lock
generated
File diff suppressed because it is too large
Load diff
|
|
@ -4,7 +4,6 @@ version = "0.1.0"
|
||||||
edition = "2021"
|
edition = "2021"
|
||||||
|
|
||||||
[dependencies]
|
[dependencies]
|
||||||
const-tweaker = "0.3.1"
|
|
||||||
opencv = {version = "0.92.3", features = ["clang-runtime"]}
|
opencv = {version = "0.92.3", features = ["clang-runtime"]}
|
||||||
thiserror = "1.0.63"
|
thiserror = "1.0.63"
|
||||||
webots = { git = "https://github.com/katharostech/webots-rust" }
|
webots = { git = "https://github.com/katharostech/webots-rust" }
|
||||||
|
|
|
||||||
|
|
@ -1,4 +0,0 @@
|
||||||
debug:
|
|
||||||
cargo build
|
|
||||||
cp target/debug/simcontroller ../simulation/controllers/cruise/cruise
|
|
||||||
|
|
||||||
|
|
@ -1,5 +1,5 @@
|
||||||
use camera::Camera;
|
use camera::Camera;
|
||||||
use opencv::{core::{bitwise_and, bitwise_and_def, bitwise_or_def, in_range, Kernel, KeyPoint, KeyPointTraitConst, Mat, MatTraitConst, MatTraitConstManual, Ptr, Size, Size_, VecN, Vector, CV_64F, CV_8UC4}, features2d::{draw_matches_def, BFMatcher, AKAZE, ORB}, gapi::{bgr2_luv, erode_def, GMat}, highgui::{self, MouseCallback}, imgcodecs::ImreadModes, imgproc::{self, cvt_color_def, gaussian_blur, gaussian_blur_def, get_structuring_element_def, sobel_def, MorphShapes, COLOR_BGR2GRAY, COLOR_BGR2HLS}, prelude::{DescriptorMatcherTrait, DescriptorMatcherTraitConst, Feature2DTrait}, ximgproc::erode};
|
use opencv::{core::{Ptr, VecN, CV_8UC4}, features2d::ORB, highgui, imgcodecs::ImreadModes, imgproc, prelude::Feature2DTrait};
|
||||||
use webots::prelude::*;
|
use webots::prelude::*;
|
||||||
use std::{f64::consts::TAU, time::Duration};
|
use std::{f64::consts::TAU, time::Duration};
|
||||||
mod camera;
|
mod camera;
|
||||||
|
|
@ -7,32 +7,12 @@ mod camera;
|
||||||
const TIME_STEP: Duration = Duration::from_millis(20);
|
const TIME_STEP: Duration = Duration::from_millis(20);
|
||||||
const MAX_SPEED: f64 = TAU;
|
const MAX_SPEED: f64 = TAU;
|
||||||
|
|
||||||
// const tweaker only supports floating point, these are mapped
|
|
||||||
#[const_tweaker::tweak]
|
|
||||||
const LOWERB_H: f32 = 18. / 180.;
|
|
||||||
#[const_tweaker::tweak]
|
|
||||||
const LOWERB_L: f32 = 23. / 255.;
|
|
||||||
#[const_tweaker::tweak]
|
|
||||||
const LOWERB_S: f32 = 36. / 255.;
|
|
||||||
#[const_tweaker::tweak]
|
|
||||||
const UPPERB_H: f32 = 92. / 180.;
|
|
||||||
#[const_tweaker::tweak]
|
|
||||||
const UPPERB_L: f32 = 151. / 255.;
|
|
||||||
#[const_tweaker::tweak]
|
|
||||||
const UPPERB_S: f32 = 169. / 255.;
|
|
||||||
|
|
||||||
struct MyRobot {
|
struct MyRobot {
|
||||||
camera: Camera,
|
camera: Camera,
|
||||||
left_motor: Motor,
|
left_motor: Motor,
|
||||||
right_motor: Motor,
|
right_motor: Motor,
|
||||||
window: &'static str,
|
window: &'static str,
|
||||||
orb_detector: Ptr<ORB>,
|
orb_detector: Ptr<ORB>,
|
||||||
matcher: BFMatcher,
|
|
||||||
last_frame_descriptors: Option<Mat>,
|
|
||||||
last_frame_keypoints: Option<Vector<KeyPoint>>,
|
|
||||||
last_image: Option<Mat>,
|
|
||||||
direction: f32,
|
|
||||||
rotational_velocity: f32,
|
|
||||||
}
|
}
|
||||||
|
|
||||||
impl Robot for MyRobot {
|
impl Robot for MyRobot {
|
||||||
|
|
@ -54,11 +34,7 @@ impl Robot for MyRobot {
|
||||||
let window = "processed";
|
let window = "processed";
|
||||||
//highgui::named_window(window, 1).unwrap();
|
//highgui::named_window(window, 1).unwrap();
|
||||||
|
|
||||||
//highgui::set_mouse_callback(window, |x,y,_,_| {dbg!(x+y);}).unwrap();
|
|
||||||
|
|
||||||
let orb_detector = ORB::create_def().unwrap();
|
let orb_detector = ORB::create_def().unwrap();
|
||||||
let matcher = opencv::features2d::BFMatcher::new_def().unwrap();
|
|
||||||
//let last_frame_descriptors = opencv::core::Mat::default();
|
|
||||||
|
|
||||||
Self {
|
Self {
|
||||||
camera,
|
camera,
|
||||||
|
|
@ -66,12 +42,6 @@ impl Robot for MyRobot {
|
||||||
right_motor,
|
right_motor,
|
||||||
window,
|
window,
|
||||||
orb_detector,
|
orb_detector,
|
||||||
matcher,
|
|
||||||
last_frame_descriptors: None,
|
|
||||||
last_frame_keypoints: None,
|
|
||||||
last_image: None,
|
|
||||||
direction: 0.,
|
|
||||||
rotational_velocity: 0.,
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -85,105 +55,22 @@ impl Robot for MyRobot {
|
||||||
//opencv::imgcodecs::imwrite_def("cam.png", &img).unwrap();
|
//opencv::imgcodecs::imwrite_def("cam.png", &img).unwrap();
|
||||||
//highgui::imshow(self.window, &img).unwrap();
|
//highgui::imshow(self.window, &img).unwrap();
|
||||||
|
|
||||||
|
|
||||||
let mut blurred = opencv::core::Mat::default();
|
|
||||||
let mut edge = opencv::core::Mat::default();
|
|
||||||
gaussian_blur_def(&img, &mut blurred, Size::new(3, 3), 0.).unwrap();
|
|
||||||
//sobel_def(&blurred, &mut edge, CV_64F, 0, 2).unwrap();
|
|
||||||
cvt_color_def(&blurred, &mut edge, COLOR_BGR2HLS).unwrap();
|
|
||||||
highgui::imshow(self.window, &blurred).unwrap();
|
|
||||||
let mut mask_a = opencv::core::Mat::default();
|
|
||||||
let mut mask_b = opencv::core::Mat::default();
|
|
||||||
let mut mask = opencv::core::Mat::default();
|
|
||||||
opencv::imgcodecs::imwrite_def("blurred.png", &blurred).unwrap();
|
|
||||||
println!("Lowrerh {}", LOWERB_H);
|
|
||||||
let lowerb = dbg!([(*LOWERB_H*255.) as u8,(*LOWERB_L*255.) as u8,(*LOWERB_S*255.) as u8,255]);
|
|
||||||
let upperb = [(*UPPERB_H*255.) as u8,(*UPPERB_L*255.) as u8,(*UPPERB_S*255.) as u8,255];
|
|
||||||
in_range(&blurred, &lowerb, &upperb, &mut mask_a).unwrap(); // h low
|
|
||||||
let gmask = GMat::new(mask_a).unwrap();
|
|
||||||
let kernel= get_structuring_element_def(MorphShapes::MORPH_CROSS as i32, Size_::new(2, 2)).unwrap();
|
|
||||||
let eroded = erode_def(&gmask, &kernel).unwrap();
|
|
||||||
// 103/180
|
|
||||||
in_range(&blurred, &[0,30,4,255], &[52,209,63,255], &mut mask_b).unwrap(); // hh28
|
|
||||||
//highgui::imshow("a", &mask_a).unwrap();
|
|
||||||
//highgui::imshow("b", &mask_b).unwrap();
|
|
||||||
dbg!(blurred.row(240/2).unwrap().col(320/2).unwrap().data_bytes().unwrap());
|
|
||||||
let mut range = opencv::core::Mat::default();
|
|
||||||
//bitwise_or_def(&mask_a, &mask_a, &mut mask).unwrap();
|
|
||||||
bitwise_and(&img, &img, &mut range, &eroded.into()).unwrap();
|
|
||||||
|
|
||||||
highgui::imshow("edge", &range).unwrap();
|
|
||||||
|
|
||||||
let mask = opencv::core::Mat::default();
|
let mask = opencv::core::Mat::default();
|
||||||
|
let mut debugout = opencv::core::Mat::default();
|
||||||
let mut keypoints = opencv::core::Vector::default();
|
let mut keypoints = opencv::core::Vector::default();
|
||||||
let mut descriptors = opencv::core::Mat::default();
|
let mut descriptors = opencv::core::Mat::default();
|
||||||
self.orb_detector.detect_and_compute_def(&blurred, &mask, &mut keypoints, &mut descriptors).unwrap();
|
self.orb_detector.detect_and_compute_def(&img, &mask, &mut keypoints, &mut descriptors).unwrap();
|
||||||
let mut debugout = opencv::core::Mat::default();
|
|
||||||
opencv::features2d::draw_keypoints(&img, &keypoints, &mut debugout, opencv::core::VecN([0.,255.,255.,255.]), opencv::features2d::DrawMatchesFlags::DEFAULT).unwrap();
|
opencv::features2d::draw_keypoints(&img, &keypoints, &mut debugout, opencv::core::VecN([0.,255.,255.,255.]), opencv::features2d::DrawMatchesFlags::DEFAULT).unwrap();
|
||||||
//highgui::imshow("keypoints", &debugout).unwrap();
|
highgui::imshow("keypoints", &debugout).unwrap();
|
||||||
|
highgui::poll_key().unwrap();
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
let mut matches: Vector<opencv::core::DMatch> = Vector::new();
|
|
||||||
|
|
||||||
|
|
||||||
if let Some(mut last_frame_descriptors) = self.last_frame_descriptors.as_ref() {
|
|
||||||
if let Err(e) = self.matcher.train_match_def(&mut last_frame_descriptors, &mut descriptors, &mut matches) {
|
|
||||||
eprintln!("no features to match");
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
let matches = matches.iter().filter(|m| m.distance < 105.).collect();
|
|
||||||
draw_matches_def(self.last_image.as_ref().unwrap(), self.last_frame_keypoints.as_ref().unwrap(), &img , &keypoints, &matches, &mut debugout).unwrap();
|
|
||||||
highgui::imshow("matches", &debugout).unwrap();
|
|
||||||
|
|
||||||
let mut horiz = Vec::new();
|
|
||||||
for hit in matches {
|
|
||||||
if hit.distance > 45. {continue;}
|
|
||||||
let last = self.last_frame_keypoints.as_ref().unwrap().get(hit.query_idx as usize).unwrap();
|
|
||||||
let current = keypoints.get(hit.train_idx as usize).unwrap();
|
|
||||||
|
|
||||||
let dx = last.pt().x - current.pt().x;
|
|
||||||
|
|
||||||
horiz.push(dx);
|
|
||||||
}
|
|
||||||
if horiz.len() > 0 {
|
|
||||||
let avg = horiz.iter().sum::<f32>()/horiz.len() as f32;
|
|
||||||
|
|
||||||
let lerp = 0.1 + horiz.len() as f32 * 0.05;
|
|
||||||
let lerp = lerp.max(0.65);
|
|
||||||
self.rotational_velocity = self.rotational_velocity * (1.-lerp) + avg * lerp;
|
|
||||||
//self.direction += avg;
|
|
||||||
}
|
|
||||||
self.direction += self.rotational_velocity;
|
|
||||||
}
|
|
||||||
self.last_frame_descriptors = Some(descriptors);
|
|
||||||
self.last_frame_keypoints = Some(keypoints);
|
|
||||||
self.last_image = Some(img.clone_pointee());
|
|
||||||
}
|
}
|
||||||
highgui::poll_key().unwrap();
|
|
||||||
|
|
||||||
const DEG_TO_PX: f64 = 320. / 62.2;
|
// initialize motor speeds at 50% of MAX_SPEED.
|
||||||
let goal = if _time.elapsed.as_secs() % 8 < 4 {
|
let mut left_speed = 0.5 * MAX_SPEED;
|
||||||
90. * DEG_TO_PX
|
let mut right_speed = 0.5 * MAX_SPEED;
|
||||||
} else {
|
|
||||||
0. * DEG_TO_PX
|
|
||||||
};
|
|
||||||
let error = goal - self.direction as f64;
|
|
||||||
let mut error = error * 0.02;
|
|
||||||
if error.abs() < 0.5 {
|
|
||||||
error = 0.;
|
|
||||||
}
|
|
||||||
dbg!(self.direction);
|
|
||||||
dbg!(self.rotational_velocity);
|
|
||||||
let mut left_speed = error * MAX_SPEED;
|
|
||||||
let mut right_speed = -error * MAX_SPEED;
|
|
||||||
|
|
||||||
self.left_motor.set_velocity(left_speed.clamp(-10., 10.));
|
self.left_motor.set_velocity(left_speed);
|
||||||
self.right_motor.set_velocity(right_speed.clamp(-10., 10.));
|
self.right_motor.set_velocity(right_speed);
|
||||||
//self.left_motor.set_velocity(0.);
|
|
||||||
//self.right_motor.set_velocity(0.);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -1,11 +1,10 @@
|
||||||
Webots Project File version R2023b
|
Webots Project File version R2023b
|
||||||
perspectives: 000000ff00000000fd0000000300000000000001ca0000039ffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f000000000000000000000001000001f30000040cfc0200000001fb0000001400540065007800740045006400690074006f007200000000160000040c0000003f00ffffff000000030000045400000039fc0100000003fb0000000e0043006f006e0073006f006c0065010000000000000a010000000000000000fc00000000000004540000006900fffffffa000000000100000001fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000ffffffff0000006900fffffffb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c01000000000000076c0000000000000000000004540000025e00000004000000040000000100000008fc00000000
|
perspectives: 000000ff00000000fd0000000300000000000001ca0000039ffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f000000000000000000000001000001f3000003e1fc0200000001fb0000001400540065007800740045006400690074006f00720000000016000003e10000003f00ffffff000000030000076c00000110fc0100000002fb0000000e0043006f006e0073006f006c0065010000000000000a010000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c01000000000000076c0000000000000000000007800000040c00000004000000040000000100000008fc00000000
|
||||||
simulationViewPerspectives: 000000ff000000010000000200000100000003520100000002010000000101
|
simulationViewPerspectives: 000000ff000000010000000200000100000002af0100000002010000000101
|
||||||
sceneTreePerspectives: 000000ff00000001000000030000001f0000013e000000fa0100000002010000000201
|
sceneTreePerspectives: 000000ff00000001000000030000001f000000c0000000fa0100000002010000000201
|
||||||
maximizedDockId: -1
|
maximizedDockId: -1
|
||||||
centralWidgetVisible: 1
|
centralWidgetVisible: 1
|
||||||
renderingMode: PLAIN
|
renderingMode: PLAIN
|
||||||
orthographicViewHeight: 1
|
orthographicViewHeight: 1
|
||||||
textFiles: -1
|
textFiles: -1
|
||||||
consoles: Console:All:All
|
renderingDevicePerspectives: robot:camera7;0;0.133333;0;0.119402;1920;1080;1128;752;0
|
||||||
renderingDevicePerspectives: robot:camera7;0;0.133333;0;0.119402
|
|
||||||
|
|
|
||||||
Loading…
Reference in a new issue