この記事は「マイクロマウスアドベントカレンダー2023」７日目の記事です。
昨日は笹谷禎伸さんの金沢の魅力とマイクロマウスでした。金沢行きたいです。そしてステッピングモーターでコンパクトなマイクロマウスを作るのすごい。
この記事ではESP32S3とRustで、マイクロマウスの走行中のログをI2C接続のFRAMに保存する方法について述べようと思います。
ここでいうログとは、センサの値などを時系列で保存したものではなく、メッセージの形で書かれたログのことを言います。 たとえば、探索走行中に左右前の壁の有無と判断した進行方向のログを以下のような形で残すことを考えています。
INFO - X: 2, Y: 7, H:N, Wall:|- , Go:r >
INFO - X: 3, Y: 7, H:E, Wall:| |, Go:f ^
INFO - X: 4, Y: 7, H:E, Wall: -|, Go:l <
INFO - X: 4, Y: 8, H:N, Wall:|-|, Go:b v
|と-が左右前の壁の有無を、<,^,>,vが進行方向を表しています。迷路上の座標や向いている方向、探索アルゴリズムの結果などがよくわかります。
また配列の範囲外アクセスなどによる回復不可能なエラーのログを以下のような形で残すこともできます。これを見ればソースコードのどこでどんなエラーが発生したのか一目瞭然です。
Panic occurred in file 'src/console/mod.rs' at line 577
panicked at src/console/mod.rs:577:18:
index out of bounds: the len is 32 but the index is 50
わざわざFRAMを使わなくても、ログなんてRAMに保存しておけば良いじゃん、と思うかもしれません。 ですが、RAMは電源オフと同時に失われてしまいます。 定期的に内蔵Flashに書き込んだとしても、電源が切れるタイミングによって失われるログがあったり、そもそも書き込みが遅いという欠点があります。 （実際のところは、せっかくFRAM載せたんだから使いたい、というのが本音です）
ソースコードは https://github.com/elkel53930/fram に公開されています。
また、開発中のマウスの情報は以下のリポジトリに公開されています。
- ハードウェア : https://github.com/elkel53930/extraice4
- ソフトウェア : https://github.com/elkel53930/micromouse-std-esp32/
- 探索アルゴリズム : https://github.com/elkel53930/mm_maze_solver
- logクレートを使ったログをFRAMに保存します。
- panic handlerをフックして、回復不能なエラー(panic)の情報をFRAMに保存します。
マイクロマウス全体の回路図は以下のとおりです。
FRAMに関係する箇所を拡大したのが下図です。
FRAMはMB85RC64TAを使用します。 サイズは2[mm]x3[mm]で容量は8KByte(64KBit)です。
ESP32S3はすべてのGPIOをSDA/SCLとして使用できます。 今回はGPIO18をSDA、GPIO17をSCLとしました。I2Cですので10kΩの抵抗でプルアップしています。
WPはWriteProtectピンです。内部でプルダウンされているため、未接続にしておけば書き込みが可能になります、
The Rust on ESP Book内でセットアップ手順が説明されています。
ESP32用のRustフレームワークはstd環境が利用できるesp-idf-halと、利用できないesp-halの２種類があります。 今回使用するのはstdを利用できるesp-idf-halです。
ESP32にはesp-idfというC++向けのフレームワークがあります。 これはFreeRTOSを含んでおり、マルチタスクやマルチコア、メモリの動的確保などもサポートしています。
esp-idf-halはesp-idfをRust用にラッピングしたものです。 stdのthreadやallocといった機能がesp-idfを用いて実装されています。
I2Cドライバがesp-idf-halによって提供されているのでそれを利用します。
I2Cドライバのインスタンスはstatic mutを使ってグローバル変数に格納します（グローバル変数へのアクセスにはunsafeブロックが必要です）。
グローバル変数にはデフォルト値が必要なのでOption型で包みます。デフォルト値はNoneです。こうしておけば初期化を忘れてしまってもunwrapでpanicが発生して気づくことができます。
今回は複数のスレッドから同時に呼び出されることは想定していません。
その場合はMutexとRefCellを使うと良いでしょう。
use anyhow::Ok;
use esp_idf_hal::{
delay::BLOCK,
gpio::AnyIOPin,
i2c::{I2c, I2cConfig, I2cDriver},
peripheral::Peripheral,
prelude::*,
units::Hertz,
};
const I2C_ADDRESS: u8 = 0x50;
// I2Cドライバのインスタンス
static mut I2C: Option<I2cDriver<'static>> = None;
// I2Cの初期化
fn i2c_master_init<'d>(
i2c: impl Peripheral<P = impl I2c> + 'd,
sda: AnyIOPin,
scl: AnyIOPin,
baudrate: Hertz,
) -> anyhow::Result<I2cDriver<'d>> {
let config = I2cConfig::new().baudrate(baudrate);
let driver = I2cDriver::new(i2c, sda, scl, &config)?;
Ok(driver)
}
fn write_30_byte(adrs: u16, data: &[u8]) -> anyhow::Result<()> {
let mut buffer: [u8; 32] = [0; 32];
// 最初の2byteがアドレスなので、一度に書き込めるデータは30byteまで
buffer[0] = (adrs >> 8) as u8;
buffer[1] = adrs as u8;
buffer[2..2 + data.len()].copy_from_slice(data);
unsafe {
I2C.as_mut().unwrap().write(I2C_ADDRESS, &buffer, BLOCK)?;
}
Ok(())
}
fn write_fram(adrs: u16, data: &[u8]) -> anyhow::Result<()> {
let mut i = 0;
// 30byteずつ書き込む
while i < data.len() {
let mut j = i + 30;
if j > data.len() {
j = data.len();
}
write_30_byte(adrs + i as u16, &data[i..j])?;
i += 30;
}
Ok(())
}
pub fn read_fram(adrs: u16, data: &mut [u8]) -> anyhow::Result<()> {
let buffer: [u8; 2] = [(adrs >> 8) as u8, adrs as u8];
unsafe {
// アドレスを書き込んでから読み込む
I2C.as_mut().unwrap().write(I2C_ADDRESS, &buffer, BLOCK)?;
I2C.as_mut().unwrap().read(I2C_ADDRESS, data, BLOCK)?;
}
Ok(())
}
pub fn init(peripherals: &mut Peripherals) -> anyhow::Result<()> {
unsafe {
let i2c = i2c_master_init(
peripherals.i2c0.clone_unchecked(),
peripherals.pins.gpio18.clone_unchecked().into(),
peripherals.pins.gpio17.clone_unchecked().into(),
1000.kHz().into(),
)?;
I2C = Some(i2c);
};
Ok(())
}Byte列を書き込む関数だけでは不便です。println!やprint!のように使えるマクロ、fprintln!とfprint!を用意します。
(RustのマクロについてはThe Little Book of Rust Macrosが参考になります。)
println!, print!の自作は組み込みRustでは定番のコードで、詳しい説明はEmbedded Rust Techniquesのこちらのページを見ていただくのが良いかと思います。
FRAMに文字列を書き込んだあと終端文字b"\0"を書き込みます。C言語の文字列と同じですね。
逆に読み込む(show_log)ときには、データが0になるまで読み込みを繰り返すようにします。
FRAMに書き込む位置はCURSORというグローバル変数に格納しておきます。
use core::fmt::{self, Write};
// FRAMへの書き込み位置
static mut CURSOR: u16 = 0;
pub fn fram_print(args: fmt::Arguments) {
let mut writer = FramWriter {};
writer.write_fmt(args).unwrap();
}
#[macro_export]
macro_rules! fprint {
($($arg:tt)*) => ($crate::fram_print(format_args!($($arg)*)));
}
#[macro_export]
macro_rules! fprintln {
($fmt:expr) => (fprint!(concat!($fmt, "\n")));
($fmt:expr, $($arg:tt)*) => (fprint!(concat!($fmt, "\n"), $($arg)*));
}
struct FramWriter;
fn write(s: &str) -> fmt::Result {
// 文字列をFRAMに書き込む
write_fram(unsafe { CURSOR }, s.as_bytes()).unwrap();
// 書き込んだ分だけカーソルを進める
unsafe {
CURSOR += s.len() as u16;
while CURSOR > 0x2000 {
CURSOR -= 0x2000;
}
}
// 終端文字を書き込む
write_fram(unsafe { CURSOR }, b"\0").unwrap();
core::result::Result::Ok(())
}
impl Write for FramWriter {
fn write_str(&mut self, s: &str) -> Result<(), std::fmt::Error> {
write(&s)
}
}
// FRAMに書き込まれたログを表示
pub fn show_log() {
let mut buffer: [u8; 32] = [0; 32];
let mut adrs = 0;
let mut flag = true;
println!("\n\nLog - - - - - - - - - - - - - - -");
while flag {
let mut size = 0;
read_fram(adrs, &mut buffer).unwrap();
adrs += buffer.len() as u16;
for b in buffer {
size += 1;
if b == 0 {
flag = false;
break;
}
}
let s = std::str::from_utf8(&buffer[0..size]).unwrap();
print!("{}", s);
}
println!("- - - - - - - - - - - - - - - - -");
}Rustのpanicはプログラムの実行を不可能/不安全にするような致命的なエラーが発生したときに使用される機構です。 簡潔なQ Rustのパニック機構に詳しく説明されていますので、詳細を知りたい方はこちらを参照してください。
Rustにはpanicをフックする機能があります。これを用いればpanicに至る致命的なエラーが発生した際に、任意のコードを実行させることができます。
こちらのページに詳しい説明がありますが、基本的には自作のpanicハンドラをpanic::set_hookを使って設定してやるだけです。
さて、この記事の最後でわざとpanicを発生させて動作チェックを行いますが、panicが発生するとマイコンがリセットされます。 そのままだとリセットとパニックを繰り返してしまうので、panicハンドラが無限ループに入るようにしています。
この無限ループのなかではFreeRtos::delay_msを呼んでいます。
実は、esp-idf-halを使用するとウォッチドッグタイマ(WDT)が自動的に設定されます。
なのでなにもしない無限ループloop{}では、しばらく経過したあとにWDTによるリセットが発生してしまうため、ループの中でFreeRtos::delay_msを呼ぶようにしています。
実際にマイクロマウスなどで使うときはこの無限ループは不要です。
ちなみにWDTは、コンフィギュレーションファイルsdkconfig.defaultsに以下の設定を加えることで無効化することもできます。
ESP32S3に２つのコアがありますが、それぞれ個別に無効化を設定できます。
# CPU0のWDTを無効化
CONFIG_ESP_TASK_WDT_CHECK_IDLE_TASK_CPU0=n
# CPU1のWDTを無効化
CONFIG_ESP_TASK_WDT_CHECK_IDLE_TASK_CPU1=n
use std::panic::{self, PanicInfo};
fn fram_panic_handler(info: &PanicInfo) {
if let Some(location) = info.location() {
fprintln!(
"Panic occurred in file '{}' at line {}",
location.file(),
location.line()
);
println!(
"Panic occurred in file '{}' at line {}",
location.file(),
location.line()
);
} else {
fprintln!("Panic occurred but can't get location information...");
println!("Panic occurred but can't get location information...");
}
fprintln!("{}", info);
println!("{}", info);
// panicが発生したら再起動せずに停止
loop {
// ウォッチドッグタイマによるリセットを防ぐ
esp_idf_hal::delay::FreeRtos::delay_ms(1000);
}
}
pub fn set_panic_handler() {
panic::set_hook(Box::new(fram_panic_handler));
}logクレートは、ログ記録を容易にするための軽量な抽象レイヤーを提供するものです。
このクレートは、異なるログ記録フレームワーク間の互換性を可能にし、複数のログレベル（error, warning、info、debug、trace）をサポートします。
logクレートを使用すると、アプリケーション内の任意のポイントでログメッセージを生成し、選択したバックエンド（例えばenv_logger）にそれらを転送できます。
今回はこのログバックエンドを自作し、転送先をFRAMにします。
ログバックエンドを作るにはlog::Logトレイトのenable,log,flushの３つの関数を実装する必要があります。
use log::{Level, Metadata, Record};
pub struct FramLogger;
impl log::Log for FramLogger {
fn enabled(&self, metadata: &Metadata) -> bool {
metadata.level() <= Level::Info
}
fn log(&self, record: &Record) {
if self.enabled(record.metadata()) {
fprintln!("{} - {}", record.level(), record.args());
println!("{} - {}", record.level(), record.args());
}
}
fn flush(&self) {}
}
pub fn set_log(log_level : log::LevelFilter) {
log::set_boxed_logger(Box::new(FramLogger))
.map(|()| log::set_max_level(log_level))
.unwrap();
}早速試してみましょう。main.rsは以下のとおりです。
use esp_idf_hal::peripherals::Peripherals;
#[macro_use] // マクロを使うためのおまじない
pub mod fram_logger;
use crate::fram_logger::fram_print;
fn main() {
esp_idf_svc::sys::link_patches();
// FRAMとpanicハンドラの初期化
let mut peripherals = Peripherals::take().unwrap();
let _ = fram_logger::init(&mut peripherals);
let _ = fram_logger::set_panic_handler();
let _ = fram_logger::set_log(log::LevelFilter::Info);
// 前回のログを表示
fram_logger::show_log();
// ログを書き込む
log::info!("FRAM logger test");
log::info!("Info test");
log::error!("Error test");
// 意図的なpanic
let array: [u8; 3] = [1, 2, 3];
for i in 0..5 {
log::info!("array[{}] = {}", i, array[i]); // i = 3でpanic
}
}以下のコマンドで書き込み/実行します。
cargo espflash /dev/ttyXXX --monitor
一度実行したあと、マウスを再起動します。 するとFRAMに保存されたログが送られてきます。
Log - - - - - - - - - - - - - - -
INFO - FRAM logger test
INFO - Info test
ERROR - Error test
INFO - array[0] = 1
INFO - array[1] = 2
INFO - array[2] = 3
Panic occurred in file 'src/main.rs' at line 28
panicked at src/main.rs:28:41:
index out of bounds: the len is 3 but the index is 3
- - - - - - - - - - - - - - - - -
log::info!やlog::error!で出力したログや、範囲外アクセスがどこでなぜ発生したのかが保存されていることがわかります。
ESP32 + Rustの組み合わせでマイクロマウスを作る人が増えてくれると嬉しいです。
明日はニシキさんの「マシン紹介および特有課題の解決方法」です。