chakokuのブログ(rev4)

テック・コミック・DTM・・・ごくまれにチャリ

MicroPython用の拡張ライブラリをCで作る

当初、STEVAL-DRONE01の制御プログラムをRUSTで開発しようとしたが、コンパイル言語で試行錯誤するのに疲れて、MicroPythonで作ることにした。STEVAL-DRONE01のFlashメモリが小さいので、MicroPythonの機能を極限まで削減してビルドした結果、STEVAL-DRONE01用にビルドしたMicroPythonではファイルシステムが使えない。だから、、毎回PCからCopy&PasteでSTEVAL-DRONE01上のMicroPythonにプログラムを送る必要がある。これもなかなか手間であり、FIXしたプログラムはMicroPythonのネイティブコードに組み込ませることで、Copy&Pasteする量が減らせられる。
そのためには、FIXしたコードを拡張ライブラリに移す必要がある。詳細には理解できていないが、解説記事等を読んで調べた結果、所定の書き方に従うことで、モジュールやクラスが定義できることが分かった。ソースコードの裏側ではマクロがいろいろ動いているようなのだが、マクロの細かい動作までは調べていない。だから、、少し書き方を間違えると致命的に大量にエラーが出る。一旦エラーが出ると、マクロがどう動いているのか?が分かっていないので直しようがない。だから、、コンパイルが通ったコードは凍結して、そこからブランチで試作、試作がエラーを起こしたら、凍結コードに戻って再度ブランチというやり方でコードを作った(何しろエラーメッセージを見てもなぜundefined symbolになるのか理解できず、ソースコードをどう直したらいいのか見当がつかない)。試行錯誤により、stevalライブラリと、STM32_LEDクラスのひな型まではできた。拡張ライブラリのソースコードは以下

#include "py/runtime.h"


extern const mp_obj_type_t stm32_led_type;

/* ------------------------------------------------- */
/*   Module: stdrone                                 */
/* ------------------------------------------------- */

STATIC const mp_rom_map_elem_t stdrone_module_globals_table[] = {
    { MP_ROM_QSTR(MP_QSTR___name__), MP_ROM_QSTR(MP_QSTR_stdrone) },
    { MP_ROM_QSTR(MP_QSTR_STM32_LED), MP_ROM_PTR(&stm32_led_type) },
};

STATIC MP_DEFINE_CONST_DICT(stdrone_module_globals, stdrone_module_globals_table);

const mp_obj_module_t stdrone_user_cmodule = {
    .base = { &mp_type_module },
    .globals = (mp_obj_dict_t *)&stdrone_module_globals,
};

MP_REGISTER_MODULE(MP_QSTR_stdrone, stdrone_user_cmodule, 1);


/* ------------------------------------------------- */
/*    Class: STM32_LED                               */
/*    init: STM32_LED()  //number: 0/1               */
/*    method:                                        */
/*               set(mode)   mode:0/1                */
/*               on()        ret:1                   */
/*               off()       ret:0                   */
/*               status()    ret:status:1/0          */
/* ------------------------------------------------- */


typedef struct _stm32_led_obj_t {
    mp_obj_base_t base;
} stm32_led_obj_t;

STATIC const stm32_led_obj_t stm32_led = {{&stm32_led_type}};

STATIC mp_obj_t stm32_led_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *all_args) {

    // parse arguments
    return MP_OBJ_FROM_PTR(&stm32_led);
}

STATIC mp_obj_t stm32_led_set(mp_obj_t self_in,mp_obj_t arg1) {
  uint8_t value = 1;
  // write code  here
  return mp_obj_new_int(value);
}
STATIC MP_DEFINE_CONST_FUN_OBJ_2(stm32_led_set_obj, stm32_led_set);


STATIC mp_obj_t stm32_led_on(mp_obj_t self_in) {
  uint8_t value = 1;
  // write code  here
  return mp_obj_new_int(value);
}
STATIC MP_DEFINE_CONST_FUN_OBJ_1(stm32_led_on_obj, stm32_led_on);


STATIC mp_obj_t stm32_led_off(mp_obj_t self_in) {
  uint8_t value = 0;
  // write code  here
  return mp_obj_new_int(value);
}
STATIC MP_DEFINE_CONST_FUN_OBJ_1(stm32_led_off_obj, stm32_led_off);

STATIC mp_obj_t stm32_led_status(mp_obj_t self_in) {
  uint8_t value = 0;
  // write code  here
  return mp_obj_new_int(value);
}
STATIC MP_DEFINE_CONST_FUN_OBJ_1(stm32_led_status_obj, stm32_led_status);


STATIC const mp_rom_map_elem_t stm32_led_locals_dict_table[] = {
    { MP_ROM_QSTR(MP_QSTR_set), MP_ROM_PTR(&stm32_led_set_obj) },
    { MP_ROM_QSTR(MP_QSTR_on), MP_ROM_PTR(&stm32_led_on_obj) },
    { MP_ROM_QSTR(MP_QSTR_off), MP_ROM_PTR(&stm32_led_off_obj) },
    { MP_ROM_QSTR(MP_QSTR_status), MP_ROM_PTR(&stm32_led_status_obj) },
};

STATIC MP_DEFINE_CONST_DICT(stm32_led_locals_dict, stm32_led_locals_dict_table);

const mp_obj_type_t stm32_led_type = {
    { &mp_type_type },
    .name = MP_QSTR_LED,
    .make_new = stm32_led_make_new,
    .locals_dict = (mp_obj_dict_t *)&stm32_led_locals_dict,
};

本当は、DRONE上にはLEDが2つあって、どちらのLEDを制御するのか、インスタンス生成時の初期化メソッドで指定したいのだが、一旦上記で凍結。LEDを制御するためのコードはまだ書いていない。ひな型まで。
上記ソースをモジュールとして組み込んでビルドした動作結果は以下

MicroPython v1.16-243-g8c4ba575f-dirty on 2021-11-03; linux version
Use Ctrl-D to exit, Ctrl-E for paste mode
>>> help('modules')
__main__          stdrone           uheapq            uselect
_thread           termios           uio               usocket
btree             uarray            ujson             ussl
builtins          ubinascii         umachine          ustruct
cmath             ucollections      uos               usys
ffi               ucryptolib        upip              utime
gc                uctypes           upip_utarfile     utimeq
math              uerrno            urandom           uwebsocket
micropython       uhashlib          ure               uzlib
Plus any modules on the filesystem

>>> import stdrone

>>> dir(stdrone)
['__class__', '__name__', 'STM32_LED']

>>> led = stdrone.STM32_LED()

>>> dir(led)
['__class__', 'set', 'off', 'on', 'status']

>>> led.on()
1
>>> led.off()
0
>>> led.status()
0
>>>

■備考
MicroPythonの拡張モジュールを開発するには、あらかじめ用意されたマクロを使いながらモジュールやクラスを定義するのだが、、この順番を勝手に入れ替えるとエラーになる。正しい順番は以下

STATIC mp_obj_t stm32_led_status(mp_obj_t self_in) {
  uint8_t value = 0;
  // write code  here
  return mp_obj_new_int(value);
}
STATIC MP_DEFINE_CONST_FUN_OBJ_1(stm32_led_status_obj, stm32_led_status);

これを順番入れ替えて勝手に以下とすると、、

STATIC MP_DEFINE_CONST_FUN_OBJ_1(stm32_led_status_obj, stm32_led_status);
STATIC mp_obj_t stm32_led_status(mp_obj_t self_in) {
  uint8_t value = 0;
  // write code  here
  return mp_obj_new_int(value);
}

シンボルが見つかりませんのエラーになる。。なぜなんだろうか??C言語にシンボルの定義順番なんてあるのだろうか??コンパイルの最後にラベルに対してリンカでアドレス解決するはずなのに。。原因が理解できず

                 from ../../../mymodule/mymodule1/stdrone.c:1:
../../../mymodule/mymodule1/stdrone.c:71:56: error: ‘stm32_led_status’ undeclared here (not in a function); did you mean ‘stm32_led_status_obj’?
   71 | STATIC MP_DEFINE_CONST_FUN_OBJ_1(stm32_led_status_obj, stm32_led_status);
      |                                                        ^~~~~~~~~~~~~~~~
../../py/obj.h:337:42: note: in definition of macro ‘MP_DEFINE_CONST_FUN_OBJ_1’
  337 |     {{&mp_type_fun_builtin_1}, .fun._1 = fun_name}
      |                                          ^~~~~~~~
../../../mymodule/mymodule1/stdrone.c:72:17: error: ‘stm32_led_status’ defined but not used [-Werror=unused-function]
   72 | STATIC mp_obj_t stm32_led_status(mp_obj_t self_in) {
      |                 ^~~~~~~~~~~~~~~~
cc1: all warnings being treated as errors
make: *** [../../py/mkrules.mk:77: build-standard/mymodule1/stdrone.o] Error 1

Rustでドローン制御プログラムを書いてるけど、TurnAroundTimeが長くてしんどい->試作はMicroPythonに変更

タイトルの通りで、Rustで試行錯誤するのはコンパイル、ロード、デバッグの時間がかかり、細かいエラーも出る状況で、コンパイル言語による試作がつらくてテンションが下がっている(ドローンのプロペラの風切り音がうるさすぎるというのもある)。当初の方針を変更して、制御プログラムの初期段階の試作はMicroPythonで行うことにする。
MicroPythonを再度ビルドする

$ uname -a
Linux DESKTOP-TRNV8F8 5.10.16.3-microsoft-standard-WSL2 #1 SMP Fri Apr 2 22:23:49 UTC 2021 x86_64 x86_64 x86_64 GNU/Linux
$ pwd
/home/<uid>/lang/up/stm32/src/micropython/ports/stm32
$ make BOARD=NUCLEO_F401CC V=1 DEBUG=1

arm-none-eabi-size build-NUCLEO_F401CC/firmware.elf
       text    data     bss     dec        hex        filename
 237472      12   20184  257668   3ee84     build-NUCLEO_F401CC/firmware.elf
GEN build-NUCLEO_F401CC/firmware0.bin
arm-none-eabi-objcopy -O binary -j .isr_vector build-NUCLEO_F401CC/firmware.elf build-NUCLEO_F401CC/firmware0.bin
GEN build-NUCLEO_F401CC/firmware1.bin
arm-none-eabi-objcopy -O binary -j .text -j .data -j .ARM build-NUCLEO_F401CC/firmware.elf build-NUCLEO_F401CC/firmware1.bin
GEN build-NUCLEO_F401CC/firmware.dfu
python3 ../../tools/dfu.py -D 0x0483:0xDF11 -b 0x08000000:build-NUCLEO_F401CC/firmware0.bin -b 0x08020000:build-NUCLEO_F401CC/firmware1.bin build-NUCLEO_F401CC/firmware.dfu
GEN build-NUCLEO_F401CC/firmware.hex
arm-none-eabi-objcopy -O ihex build-NUCLEO_F401CC/firmware.elf build-NUCLEO_F401CC/firmware.hex

バイナリサイズは251KBか? Flashが256KBなので、ぎりぎり収まっている。
できたら、センサは3-WireによるSPI制御なので、SPIドライバを改修して、3-Wireモードで動くようにしたい。Cで動かさないとセンサ部分の性能が出ないと思われるので。 サイズ削減のためにFileIOやFlash上のファイルシステムは使えないので、プログラムは毎回Copy&Paste等でDRONEに送り付ける必要がある。
OpenOCD起動

./bin-x64/openocd.exe -f ./scripts/interface/stlink-v2.cfg -f ./scripts/target/stm32f4x.cfg

GDBからOpenOCDに接続してファームをロード

$ pwd
/home/<uid>/lang/up/stm32/src/micropython/ports/stm32/build-NUCLEO_F401CC

$ /usr/local/GNUToolsARMEmbedded/4.8_2013q4/bin/arm-none-eabi-gdb.exe firmware.elf
GNU gdb (GNU Tools for ARM Embedded Processors) 7.6.0.20131129-cvs
Copyright (C) 2013 Free Software Foundation, Inc.

(gdb) target remote localhost:3333
Remote debugging using localhost:3333
0x00000000 in ?? ()

(gdb) monitor reset halt
Unable to match requested speed 2000 kHz, using 1800 kHz
Unable to match requested speed 2000 kHz, using 1800 kHz
adapter speed: 1800 kHz
target halted due to debug-request, current mode: Thread
xPSR: 0x01000000 pc: 0x08000400 msp: 0x20010000

(gdb) load
Loading section .isr_vector, size 0x1184 lma 0x8000000
Loading section .text, size 0x38e14 lma 0x8005000
Loading section .ARM, size 0x8 lma 0x803de14
Loading section .data, size 0xc lma 0x803de1c
Start address 0x802d968, load size 237484
Transfer rate: 32 KB/sec, 11308 bytes/write.

一応焼けた。

>>>
MPY: soft reboot
MicroPython v1.16-243-g8c4ba575f-dirty on 2021-10-30; STEVAL-DRONE01 with STM32F401CC
Type "help()" for more information.

>>> help()
Welcome to MicroPython!
For online help please visit http://micropython.org/help/.

Quick overview of commands for the board:
  pyb.info()    -- print some general information
  pyb.delay(n)  -- wait for n milliseconds
  pyb.millis()  -- get number of milliseconds        Switch methods: (), callback(f)
  pyb.LED(n)    -- create an LED object for  0bthardware random number
  pyb.Servo(n)  -- create Servo object for servo n (n=1, -itrrupt a running program
  CTRL-D        -- on a blank line, do a soft reset of

>>> help("modules")
__main__          math              uctypes           urandom
_onewire          micropython       uerrno            uselect
builtins          pyb               uio               ustruct
cmath             uarray            umachine          usys
gc                ucollect

>>> dir(machine)
['__class__', '__name__', 'ADC', 'DEEPSLEEP_RESET', 'HARD_RESET', 'I2C', 'PWRON_RESET', 'Pin', 'RTC', 'SOFT_RESET', 'SPI', 'Signal', 'SoftI2C', 'SoftSPI', 'Timer', 'UART', 'WDT', 'WDT_RESET', 'bootloader', 'deepsleep', 'disable_irq', 'enable_irq', 'freq', 'idle', 'info', 'lightsleep', 'mem16', 'mem32', 'mem8', 'reset', 'reset_cause', 'sleep', 'soft_reset',

>>> dir(pyb)
['__class__', '__name__', 'main', 'ADC', 'ADCAll', 'ExtInt', 'LED', 'Pin', 'RTC', 'SPI', 'Switch', 'Timer', 'UART', 'country', 'dht_readinto', 'disable_irq', 'enable_irq', 'fault_debug', 'repl_info', 'repl_uart', 'wfi']

PWMはどこに行ったのだろうか。。

MicroPythonのManualによると、、pyb.Timerを使ってPWMを構成するようだ。

pyb.Pin と pyb.Timer を参照:

from pyb import Pin, Timer
p = Pin('X1')                            # X1 は TIM2, CH1 を持ちます
tim = Timer(2, freq=1000)
ch = tim.channel(1, Timer.PWM, pin=p)
ch.pulse_width_percent(50)

pyboard 用クイックリファレンス — MicroPython 1.17 ドキュメント

MicroPythonのPinクラス、Timerクラスを組み合わせてPWMにより4基のモータを制御するサンプル

from pyb import Pin, Timer

pin_m1 = Pin(Pin.board.PB6) 
motor1 = Timer(4, freq=500).channel(1, Timer.PWM, pin=pin_m1)
motor1.pulse_width_percent(0)

pin_m2 = Pin(Pin.board.PB7) 
motor2 = Timer(4, freq=500).channel(2, Timer.PWM, pin=pin_m2)
motor2.pulse_width_percent(0)

pin_m3 = Pin(Pin.board.PB8) 
motor3 = Timer(4, freq=500).channel(3, Timer.PWM, pin=pin_m3)
motor3.pulse_width_percent(0)

pin_m4 = Pin(Pin.board.PB9) 
motor4 = Timer(4, freq=500).channel(4, Timer.PWM, pin=pin_m4)
motor4.pulse_width_percent(0)

motor4.pulse_width_percent(0)は0%なので、これを10等にすると10%の出力になる。
PinとTimerの組み合わせは決まっており、ハードウエア仕様書か、stm32f401_af.csvを参照して正しい組み合わせを指定する必要あり。間違っているとAFエラーになる(例:ValueError: Pin(B5) doesn't have an af for Timer(1))。


Python/MicroPythonには自作モジュールを追加する仕組みがあり、それを試してみる。
ユーザ開発用サンプルのexamples/usercmoduleがあるので、これをmicropythonのソースツリーの外側に配置して、make時にUSER_C_MODULESとして指定して組み込んでみる。

$ make BOARD=NUCLEO_F401CC USER_C_MODULES=../../../mymodule V=1 DEBUG=1

sampleソースを修正して、stdroneモジュールに修正、再ビルド。実行した結果、stdroneモジュールが見えている

$ make BOARD=NUCLEO_F401CC USER_C_MODULES=../../../mymodule V=1 DEBUG=1
>>> import stdrone
>>> dir(stdrone)
['__class__', '__name__', 'add_ints']
>>> stdrone.add_ints(1,2)
3

今の状態だと、stdroneモジュール内に関数が直接置かれている状態なので、、この状態からSPI3W(SPI 3Wireモード)クラス等を作って、init / read / write 等に仕立てる必要がある。

Zigbeeの透過モードでシリアル通信させてみる

Zigbeeによる通信は透過モード*1APIモードがあるらしい。透過モードはシリアルの全二重のような通信であり、一方のAPIモードは通信パケットを作ってXBeeに投げ込むらしい。APIモードの方がより高度な通信ができるのだが、パケットを作る手間とCRC計算も必要と思われ、仕様理解とCRC計算がやっかいなので、まずは透過モードで通信させる。
通信させるにはXBeeを2つ用意して、片方はZigbee Coordinator AT、片方は、Zigbee Router ATでセットアップする(セットアップは専用のConfigツール(X-CTUを使う))。PAN ID(会議室番号のようなものかと)も、両方のXBeeで合わせる必要がある。初期値は0x00なのだが、好みで、0xF012と設定してみる。この状態で、Xbeeはシリアル接続可能となり9600bpsでマイコンやPCと接続ができる。

[PC]--USB--[USB/Serial]<--Serial-->[XBee]<----(Zigbeeプロトコル)---->[XBee]<---Serial--->[MicroController]

当初、Routeに設定したXBeeからCoordinatorのXbeeにデータは送れるのだが、CoordinatorからRouterにデータが送れなかった。Zigbeeの透過モードは片側通信なのか??と思って調べると、、Coordinator ATの設定の際、通信相手を指定する必要があるらしかった。この作業が抜けていた。抜けていても通信できるがその場合はCoordinatorからブロードキャストでRouterに送る必要があるらしい。それは無駄なので、、通信相手となるのRouterのデバイスAddressをCoordinator側に設定した。これにより全二重の通信は可能になった。
XBeeのシリアル通信のボーレートはデフォルトで9600bpsらしいが、変えることが可能であり、119200まで上げた。テストでは通信できたので、これをSTEVAL-DRONE01のシリアルポートに接続すれば、、無線で制御が可能になる。はず。。
ZigbeeWiFi等と干渉するらしくどれだけ使えるのか分かりませんが、透過モードなら簡単に導入できるので、BLEプロトコルが作りこめるまでは、まずはZibgeeで通信したらいいかと思っています。
以下はX-CTUの画面、通信先のアドレス(High/Low)を設定する必要あり(赤枠の部分)

DRONEから出力されるSerialに対してXBeeを使ってPCと接続、PCからDRONEが制御可能。実際に飛行するまでにはユニバーサル基板を何かに置き換える必要あり。しかし、、姿勢制御は技術的な課題が多くて難しそうだ(段々とモチベーションが下がってきた)。

■参考URL
CoordinatorからRouterに通信できないというQA(これを見て原因が分かりました)
Can I send data from Coordinator to router in AT mode - Digi Forum

Zigbee通信プロトコルの仕様名で、XBeeは通信デバイスの名称と理解しています。

*1:ATモードと言っていいのかどうか

BLEデバイス、SPBTLE-RFを使ってみたいが、、GATT/GAPを実装しないとダメな感じか。。->XBeeを使うことにする

STEVAL-DRONE01との通信はUSB/Serialを使っている。これはまぁ楽なんだけど、飛行物に有線だといつかはじゃまになる。せっかくだから、BLEで通信させたい。そのためのSPBTLE-RFが搭載されている。
しかし、SPBTLE-RFはデバイスの仕様書はあるけど、UsersManualが見当たらない。ST-Microから提供されているのはSDKであり、ライブラリを使ってくれということのようだ。SPBTLE-RF用にMicroPythonのドライバを書いた人も居て、そういう人達のソースを見てどうやったら使えるのかを理解したい。本当に単純な1バイトのReadWriteでいいのだけど。

GitHub - dmazzella/uble: Lightweight Bluetooth Low Energy driver written in pure python for micropython

SPBTLE-RF/src at main · stm32duino/SPBTLE-RF · GitHub

https://www.st.com/content/my_st_com/en/products/embedded-software/evaluation-tool-software/stsw-bnrglp-dk.license=1634292172847.product=STSW-BNRGLP-DK.version=1.1.0.html

面倒だったら、BLE/Serialの様なのチップを載せてしまうか。。ESP32でBLE通信させるか。ESP32は消費電力が大きそうなのだが。

Bluetooth Low Energyのデータ通信プロトコルの基本 | フィールドデザイン
X-NUCLEO-BNRG2A1

■追記

BLEはプロトコルが複数階層なので、、もうちょっと楽に扱えそうなZigBeeを取り付けようかと。。昔勉強しようと思って買ったZigBeeの通信基板(XBee)がまだ残ってるので、これを使ってシリアル通信をしてみよう。

Digi社より、XCTUというツールを落とす
https://hub.digi.com/support/products/xctu/?path=/support/asset/xctu-installer/
参考にしているサイト、技術情報のまとめ、ありがとうございます。
XBee ZigBee 基本編 ワイヤレス通信を始めよう〜XBeeの使い方〜 by ボクにもわかる地上デジタル

XBee3ではMicroPythonが走るらしい
マイコンの実験:XBee3(MicroPython)の実験1

ST Micro社のサンプルコードを参考にSTEVAL-DRONE01のPWM制御部を改良ー>少なくとも浮上はした

HALを使わず周辺I/Oの制御もフルスクラッチで作ってきたSTEVAL-DRONE01だったが最終的に浮上すらしないという状況に陥った。ST Micro社が提供しているDroneサンプルコードを使って動かすとかなり危険に浮上して飛んだ。だから、、基板のGNDが不安定とかではなく制御の問題であると分かった。
サンプルコードを読み解いたが、特別な設定は行っておらず、GPIOもPWMもごく素直な実装であった。ただ違っていたのは、PWMのパラメータであり、サンプルコードでは、PWMの動作周波数は1MHzでカウンタは2000が設定されていた。だから、PWMの周期は500Hzで動作する仕様であった。この実装がモータ出力に関係するかもしれないと思い、俺ライブラリも、同じ設定を入れた(これはではもっと低い周波数で動かしていた)。結果、DRONEは浮上した。深夜あまりにうるさいので、出力は最大まで上げていないけど、浮上する程度にはモータが回せている。なぜ、PWM周波数を上げるとモータのパワーが上がるのかが全く分かっていないけど、、ハードウエアとしては飛ぶことが分かったので、ソフト実装にまた取り組める。
かなり汚い絵ですが、浮上テストしている動画は以下。
youtu.be
PWMのduty:30%ぐらいでほぼ飛びそうなぐらいのパワーです。これ以上出力を上げると深夜でうるさいし、制御不能によりひっくり返りそうなので、これぐらいにしておきます。

どれだけ参考になるか分からんが、、16MHzのクロックに対してプリスケーラで1/16によりPWMの動作周波数を1MHzに落として、カウンタを2000で動かす実装例(Rustで書いています)

  //--------------------------------
  // set Timer4 (TIM4) CH2
  //--------------------------------

  // enable Counter (CE) AND ARPE
  addr = (TIM4_BASE + TIM_CR1_OFFSET) as *mut u32;  // 0
  set_mode = 0x1 << TIM_CEN_BIT;               // UE_BIT = 0
  set_mode |= 0x1 << TIM_ARPE_BIT;            // UE_BIT = 7
  unsafe { 
	let current = core::ptr::read_volatile(addr) ;
        let val = current | set_mode;
	core::ptr::write_volatile(addr, val) ;
  }

  // Set  Prescalar ()
  addr = (TIM4_BASE + TIM_PSC_OFFSET) as *mut u32;  // 0x28
  let div = 16 - 1;  // divied to 16MHz -> 1MHz by prescalar
  unsafe { 
	let current = core::ptr::read_volatile(addr) ;
        let val =  div;  
	core::ptr::write_volatile(addr, val) ;
  }

  // Set ARR ()
  addr = (TIM4_BASE + TIM_ARR_OFFSET) as *mut u32;  // 0x2c
  let auto_reload = ARR_MAX;               //  set value of AUTO-RELOAD to 2000
  unsafe { 
	let current = core::ptr::read_volatile(addr) ;
        let val:u32 = auto_reload;
	core::ptr::write_volatile(addr, val) ;
  }

  // Set  CCR
  addr = (TIM4_BASE + TIM_CCR1_OFFSET) as *mut u32;  //  0x34
  let cc_val = 0;        // duty 0%
  unsafe { 
	let current = core::ptr::read_volatile(addr) ;
        let val = cc_val;
	core::ptr::write_volatile(addr, val) ;
  }

  // set PWM MODE1 
  addr = (TIM4_BASE + TIM_CCMR1_OFFSET) as *mut u32;  // 0
  set_mode = 0b110 << TIM_OC1M_BIT_FLD;              // OCM1_BIT_FIELD = 4
  set_mode |= 1 << TIM_OC1PE_BIT;                    // TIM_OC1PE = 3
  mask = !(0b111 << TIM_OC1M_BIT_FLD | 1 << TIM_OC1PE_BIT);
  unsafe { 
	let current = core::ptr::read_volatile(addr) ;
        let mut val = current & mask;
        val |= set_mode;
	core::ptr::write_volatile(addr, val) ;
  }

  // enable PWM
  addr = (TIM4_BASE + TIM_CCER_OFFSET) as *mut u32;  // 0x20
  set_mode = 1 << TIM_CC1E_BIT;                    // TIM_CC1E_BIT = 0
  //set_mode = 3;
  unsafe { 
	let current = core::ptr::read_volatile(addr) ;
        let val = current | set_mode;
	core::ptr::write_volatile(addr, val) ;
  }

なぜPWMの周波数を上げるとモータの出力が上がるのか??まったく理解できないので、オシロで波形確認したり、参考文献を読んで調べる予定です。パワーが落ちない理由がまったく分からず、晴れやかな気持ちには到底なれないけど、、飛ぶことは分かったので制御プログラムの開発を続けられる。(IoT勉強会の準備も必要だが。。)

離陸しないDrone(STEVAL-DRONE01)をどうするか。。->サンプルコードだと飛行した->原因は自分のプログラムのまずさであった!!

無茶でも飛び上がったら制御プログラムを書こうかという気にもなるが、どうやっても離陸しないDroneを相手にこれからどうしたものか。。やれることとしては、、(1)LiPoバッテリーをフル充電して飛ぶかどうかを確認、(2)ST Microが提供しているサンプルプログラムを入れてみて、それで飛ぶかどうかを確認、、ぐらいかと。特に、バッテリーの蓄電状態については、フルだと1mぐらい飛ぶが、時間が経つと地面を這うような感じというブログの記事もあって、満タンでないとまともに飛ばないらしい。
基本設計がST Microというのと、基板のパターンが細かくて修正するのもほぼ不可能なので、STEVAL-DRONE01をこのまま使い続けるのはちょっと難しいと思っています。勉強を兼ねて、スマフォからBT接続でコントロールできるサンプルをビルドして、それを使ってどんな動きを見て終わりにしようかと思っています*1

■サンプルコードのビルド

Atollic TrueSTUDIOでビルドするらしい。TrueSTUDIOをST MicroからDL(gccでもビルドできるんだろうけど、環境設定がややこしい。深く理解していないとハマりポイント多いだろうし)
「A powerful eclipse-based C/C++ integrated development tool for your STM32 projects」

GithubからDLしたソース一式をTrueSTUDIOに取り込んでビルドしてみたけど、、ヘッダがないと怒られる。実体はあるのだけど、なぜ無いと言われるのか。include pathがうまくいってなさそうなのだが、Projectが複雑でよくわからん。

■今後の取り組み

キットに内蔵されてきたモータのうち、白黒ケーブルでつなぐ方は仕様書通りにはつながらない。仕様書通りに接続すると、回転が逆になる。これは他の人も書いていて、ロットのレベルで間違って販売されているように思う。あと、、浮上するための電池もほぼ満充電でないとダメなのではなかろうか。やれることとしては、、同じ仕様のモータを買って交換してどう動くのかを見る、容量の大きいLiPOに交換する、MOS FETによるモータ制御の部分だけ自作して外付けするか。。。
ST-Microが運営しているSTEVAL-DRONE01の掲示板もあるようなので、そっちも見てみる予定(みんな本当に浮上しているのかどうか)。

ST-Microのサイトを再度見ていると、配線は間違ってましたのErattaが出ていました。。
https://www.st.com/resource/en/errata_sheet/es0527-motor-wiring-inversion-on-certain-batches-stmicroelectronics.pdf

もう一つのErattaは、Frame holes toleranceということで、取り付け穴?に関する連絡か??
https://www.st.com/resource/en/errata_sheet/es0466-board-limitations-stmicroelectronics.pdf

STEVAL-DRONE01のフォーラム(掲示板)
https://community.st.com/s/group/0F90X000000AXtISAW/drone-zone

掲示版を見たけど、ビルドできないという質問が大半で、そもそも浮上しないというネタは見つけられなかった。CubeIDEでビルドしている人がいるようなので、サンプルコードをCubeIDE*2でビルドしてみる。

CubeIDEで、他のプロジェクトを取り込み(的英語)で取り込んでビルドしたらエラーなしで一発でビルドできた。バイナリは以下のパスに生成されたようである。

ST_Drone_FCU_F401-master/ST_Drone_FCU_F401-master/STM32 FW Project/Official release - 170120/CubeIDE/STEVAL-FCU/Debug
$ ls -ltr
total 5293
-rwxr-xr-x  1  なし 1805380 Oct 10 19:29 'ToyDrone Configuration.elf'
-rwxr-xr-x  1 なし 2274426 Oct 10 19:29 'ToyDrone Configuration.list'
-rwxr-xr-x  1  なし 1322138 Oct 10 19:29 'ToyDrone Configuration.map'
-rwxr-xr-x  1 なし    2394 Oct 10 19:29  makefile
-rwxr-xr-x  1  なし    3268 Oct 10 19:29  objects.list
-rwxr-xr-x  1  なし     231 Oct 10 19:29  objects.mk
-rwxr-xr-x  1  なし     743 Oct 10 19:29  sources.mk
drwxr-xr-x+ 1  なし       0 Oct 10 19:29  Application
drwxr-xr-x+ 1  なし       0 Oct 10 19:29  Drivers
drwxr-xr-x+ 1  なし       0 Oct 10 19:29  Middlewares

elf形式を焼けばいいのだろう。ST-Link Utilityを使ってみる。ST-Link Utilityでターゲット接続に失敗するので、OpenOCD+GDBでやくことに。以下はGDB側のコマンド

$ /usr/local/GNUToolsARMEmbedded/4.8_2013q4/bin/arm-none-eabi-gdb.exe    ToyDrone.elf
(gdb) target remote localhost:3333
Remote debugging using localhost:3333
0x00000000 in ?? ()

(gdb) monitor reset halt
Unable to match requested speed 2000 kHz, using 1800 kHz
Unable to match requested speed 2000 kHz, using 1800 kHz
adapter speed: 1800 kHz
target halted due to debug-request, current mode: Thread
xPSR: 0x01000000 pc: 0x08000400 msp: 0x20010000

(gdb) monitor arm semihosting enable
semihosting is enabled

(gdb) load
Loading section .isr_vector, size 0x194 lma 0x8000000
Loading section .text, size 0x17790 lma 0x80001a0
Loading section .rodata, size 0x808 lma 0x8017930
Loading section .ARM, size 0x8 lma 0x8018138
Loading section .init_array, size 0x4 lma 0x8018140
Loading section .fini_array, size 0x4 lma 0x8018144
Loading section .data, size 0x304 lma 0x8018148
Start address 0x80061e8, load size 99392
Transfer rate: 22 KB/sec, 7099 bytes/write.

(gdb) cont
Continuing.

正常に焼けたようで、Drone側はLEDが激しく点滅している。BLEのペアリングをしてくれということか。。 iOS用の制御アプリをDLしてスマフォにインストール (ST BLE Droneというアプリ)
(1)BLEペアリング
(2)キャリブレーション
(3)飛行?

キャリブレーションをどうやるのが正しいのか、マニュアルを見てもイマイチ分からなかったので、、Armで無理やり飛ばしてみた。すると、ハチャメチャではあるがちゃんと浮上した。ということは、、DRONEは浮上性能は持っていて、自分のモータ制御方法が間違っているということか。うーん、こんなことではいかん。しかし、何を間違えるとパワーが出ないのか全く分からない。ソースが階層複雑で、PWM制御をどうやっているのか読み解けるか不明。とりあえず、、この正解設定の周辺I/Oのレジスタ設定ぐらいは取ってみようかと。
GPIOの設定は以下の通りで特別驚く設定はない

(gdb) p/x  *(0x40020400) 0x890aa59d  PB6,7,8,9はAlternateモード
(gdb) p/x  *(0x40020404) 0x0
(gdb) p/x  *(0x40020408) 0xcf000ac3  PB6,7,8,9はSpeed設定なし
(gdb) p/x  *(0x4002040C) 0x11        PB6,7,8,9はPullUpなし
(gdb) p/x  *(0x40020410) 0x380c
(gdb) p/x  *(0x40020414) 0x1004
(gdb) p/x  *(0x40020418) 0x0
(gdb) p/x  *(0x4002041C) 0x0
(gdb) p/x  *(0x40020420) 0x22000000  PB6,7は、AF2/AF2
(gdb) p/x  *(0x40020424) 0x50500022  PB8,9は、AF2/AF2

タイマの設定は以下だが、PWMなので簡単には読み解けない

(gdb) p/x *(0x40000800) 0x1
(gdb) p/x *(0x40000804) 0x0
(gdb) p/x *(0x40000808) 0x0
(gdb) p/x *(0x4000080c) 0x0
(gdb) p/x *(0x40000810) 0x1f
(gdb) p/x *(0x40000814) 0x0
(gdb) p/x *(0x40000818) 0x6868
(gdb) p/x *(0x4000081C) 0x6868
(gdb) p/x *(0x40000820) 0x1111
(gdb) p/x *(0x40000824) 0x61f
(gdb) p/x *(0x40000828) 0x54
(gdb) p/x *(0x4000082C) 0x7cf
(gdb) p/x *(0x40000830) 0x0
(gdb) p/x *(0x40000834) 0x0
(gdb) p/x *(0x40000838) 0x0
(gdb) p/x *(0x4000083C) 0x0
(gdb) p/x *(0x40000840) 0x0
(gdb) p/x *(0x40000844) 0x0
(gdb) p/x *(0x40000848) 0x0
(gdb) p/x *(0x4000084C) 0x1
(gdb) p/x *(0x40000850) 0x0

motor.cのコメントは以下。これを読んでも普通にPWMで制御してますという程度にしか読めないのだが。。

/*                                  Note
 * The PWM is handled by TIM4. 
 * In case of DC motor configuration:
 * - the master clock for TIM4 is 1MHz
 * - the counter counts up to 2000, result in 2ms of PWM period (500Hz)
 * - the PWM pulse width data can to 0~1999, coresponding to 0~100% duty cycle
*/

/*
 * Setup the driving power for 4 motors. p1~p4 data range is 0~1999, which equals
 * to 0~100% duty cycle (for DC motor configuration)
 */

motor.cにデバッグコードを仕込んで、自分でDutyを変えてどう動くのか見てみるか。。
CubeIDEを使ってmain.cとmotor.cを読むと、飛行制御はともかく、モータ制御はシンプルな実装であることが分かった。自分がやれるとしたら、、main.cとmotor.c関連を抜き出して、最小構成のモータ制御プログラムを作ってみることかと。HALを使ってるので、ソース自体はシンプルに仕上がっていて、main.cは1300行程度だ。すばらしい。GPIOの設定はレジスタで見る限り大きな違いはないので、PWMの使い方が違っていそうだ。4基のモータが同時に電源供給されているのか、時分割で1/4ずつ供給されているのか。。あたりが気になる(それぐらいならオシロでも分かるけど)。

モータ制御はTIMER4でPWMを動かしているのだが、以下が定義。これを読んで、レジスタダンプと比較したら、Timerをどう設定しているのかが分かるはず。。

/* TIM4 init function */
void MX_TIM4_Init(void)
{

  TIM_ClockConfigTypeDef sClockSourceConfig;
  TIM_MasterConfigTypeDef sMasterConfig;
  TIM_OC_InitTypeDef sConfigOC;

  htim4.Instance = TIM4;
  #ifdef MOTOR_DC
    htim4.Init.Prescaler = 84;                                    /* DC motor configuration - Freq 494Hz*/
    htim4.Init.CounterMode = TIM_COUNTERMODE_UP;
    htim4.Init.Period = 1999;                  
  #endif
  #ifdef MOTOR_ESC
    htim4.Init.Prescaler = 100;                                    /* ESC motor configuration - Freq 400Hz*/
    htim4.Init.CounterMode = TIM_COUNTERMODE_UP;
    htim4.Init.Period = 2075;                                   
  #endif
                                       
  htim4.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  HAL_TIM_Base_Init(&htim4);

  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
  HAL_TIM_ConfigClockSource(&htim4, &sClockSourceConfig);

  HAL_TIM_PWM_Init(&htim4);

  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  HAL_TIMEx_MasterConfigSynchronization(&htim4, &sMasterConfig);

  sConfigOC.OCMode = TIM_OCMODE_PWM1;
  sConfigOC.Pulse = 0;
  sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
  sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
  HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_1);
  HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_2);
  HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_3);
  HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_4);

}

モータのパワーはPWMのDuty変えて行っているが、これも以下のようなシンプルな実装。タイマのCCRに比率を入れるだけ。

void set_motor_pwm(MotorControlTypeDef *motor_pwm)
{
  if (motor_pwm->motor1_pwm >= MOTOR_MAX_PWM_VALUE)
    htim4.Instance->CCR1 = MOTOR_MAX_PWM_VALUE;
  else if (motor_pwm->motor1_pwm <= MOTOR_MIN_PWM_VALUE)
    htim4.Instance->CCR1 = MOTOR_MIN_PWM_VALUE;
  else
    htim4.Instance->CCR1 = (uint32_t) motor_pwm->motor1_pwm; 
  
  if (motor_pwm->motor2_pwm >= MOTOR_MAX_PWM_VALUE)
    htim4.Instance->CCR2 = MOTOR_MAX_PWM_VALUE;
  else if (motor_pwm->motor2_pwm <= MOTOR_MIN_PWM_VALUE)
    htim4.Instance->CCR2 = MOTOR_MIN_PWM_VALUE;
  else
    htim4.Instance->CCR2 = (uint32_t) motor_pwm->motor2_pwm;
  
  if (motor_pwm->motor3_pwm >= MOTOR_MAX_PWM_VALUE)
    htim4.Instance->CCR3 = MOTOR_MAX_PWM_VALUE;
  else if (motor_pwm->motor3_pwm <= MOTOR_MIN_PWM_VALUE)
    htim4.Instance->CCR3 = MOTOR_MIN_PWM_VALUE;
  else
    htim4.Instance->CCR3 = (uint32_t) motor_pwm->motor3_pwm;
  
  if (motor_pwm->motor4_pwm >= MOTOR_MAX_PWM_VALUE)
    htim4.Instance->CCR4 = MOTOR_MAX_PWM_VALUE;
  else if (motor_pwm->motor4_pwm <= MOTOR_MIN_PWM_VALUE)
    htim4.Instance->CCR4 = MOTOR_MIN_PWM_VALUE;
  else
    htim4.Instance->CCR4 = (uint32_t) motor_pwm->motor4_pwm;
}

ソースを読む限り、特別なテクニック使わず普通に実装されているようなのだが。。PWMの細かい所を読み解かないと、なぜパワーが落ちないのか?が分からない。
今は日曜の9:30でこれ以上追いかけると寝られなくなるので、、今日はここまで*3*4


■メモ
CubeIDEで調査用の別プログラムを作る手順・・・workspaceをあらたに作る、他のProjectをimportで取り込む、ビルドする、デバッグ用に書き換える。同じようにやっているつもりが、今度はシンボルが不明とエラーが出た。さすがに遅いので今日は終わり。 しかし、、PWMの設定次第と思うけど、モータ制御は奥が深いっす

*1:1月開催予定のIoT勉強会の準備もそろそろやらないと・・なので

*2:ビルドはCubeIDEで行う。CubeMXもあるのだが、CubeMXはConfig専用?

*3:ずっとDroneのPGやっていて久しぶりにヨメにめちゃ怒られた

*4:家の事をほっといて自分の興味のあることに逃げ込んで!! 次から次へと!!!etc

なぜモータを2基動かすと電圧が下がるのか?? 定電圧電源なのに??ー>GNDパターンに十分が電流が流せられないからではないかと推測

DRONEのモータはFETでOn/Offが行われているが、1基だけ動かす分には元気に回っているが、2基以上を動かすと出力が落ちる。落ちっぷりは、複数で回しても1基の時と同じぐらいしか出力が出ていない。テストのやりかたのまずさ(VBATは5Vでなく3.7V)もあって、改めてLiPoバッテリーで駆動してテストをやり直した結果も同じであった。なぜなのだろうか。。Dutyのミスかと思って波形を見ながら出力を変えたところ、100%出力では波形も正しく100%比率になっていた。だから、、うっかりDutyが間違っていたわけではない。モータ(一台分)を抵抗に置き換えて、抵抗の両端にかかる電圧を測ってみた。すると、1基の場合は5Vの電圧がかかっていたが、PWM->FETで2基動かすと、3V?ぐらいまで落ちている。なぜ3Vなのか?あたかも、3.3Vがどこかから入ってきているかのような電圧なのだが。。原因が分からず。。
FETのスイッチングによりモータ(正確にはダミー抵抗)が1基だけOnしている時の電圧(テストは間違っているが、5V)

FETのスイッチングにより、2基Onしている時の電圧。3Vぐらいまで下がっている。

FETによるモータ制御回路の写し。3.3Vが間違って流入することはないのだが。。VBATは大容量の電源からつないでいるので、簡単に電圧が落ちるとは考えにくいのだが。。 FET自体抵抗があるとのことで、、1基でOnしている時に計測される電圧がそもそも偶然に高くて、本来は、2基の時の電圧が正しい電圧か??

■今後の取り組み
2基目のモータが電圧を下げている要因であると仮定して、原因がモータのせいなのかを切り分けるため、モータの代わりに十分大きなダミー抵抗を使って、FETでOn/Offをする。それでもやっぱり電圧が下がったら、設計上、どこかから流入しているというか、もしくはGNDが浮いている(プルアップ側につながっている。。。???)のではなかろうか。 そもそもFETってどう動いているのか十分理解できていない。

■原因調査
モータの代わりに300Kぐらいのダミー抵抗に取り換えて、2系統動かすと電圧がどのように変化するかを確かめた。
以下は300Kのダミー抵抗(写真にしたところであまり情報はありませんが・・)

ダミー抵抗の状態でPWM制御によるFETでのOn/Offをやってみても、電位に変化はなくきれいな波形が出ている。

これに対して、モータが接続されているFETをOn/Offさせると、以下のようにGNDに落ちない状況が発生

これまでのテストで自分の回路設計の理解不十分もあって誤解していたのは、、FETによる操作は3.7Vに引き上げる操作ではなくて、0Vに引き下げる操作である。だから、、モータを駆動する電圧が下がるというのは間違いで、モータのマイナス側(FET経由でGND接続)を0Vまで落とせないというのが正しいのでした。一般的にGNDが不安定になるのは、GNDパターンの設計がうまくいっていない場合が多いと理解しており、今回モータを回し始めるとGNDに落とせないのは、GNDの取り回しが細くて、十分な電流が流せられないからではないかと判断。これが仕様なのだとすると、そもそもモータは4基同時に回す設計ではなくて、最大推力を出す場合も4基あるモータを1/4周期ずつ時間をずらして電流を流す設計だったのかも。。詳細わかりませんが。STEVAL-DRONE01を実際に飛ばした記事がInterfaceに掲載されていたので、もう一度読み直すと、PWMを使ってモータをどのように制御したのかが分かるかもしれない。なお、ST-Microが出しているDRONE用評価プログラムをGIthubで参照しましたが、ソフトウエア階層があまりに複雑で、どのようにモータを制御しているのか??とうてい読み取れませんでした。
しかし、、普段は3.3Vのデジタル回路しか触ってない(さらに言うと自分は電気屋ではなくて、なんちゃってのソフト屋)ので、こういうモータ制御(パワー制御?)は分からないことが多いです。勉強になるな~~

■追記
GNDパターンが問題だなどと言っているが、、、ひょっとしてPWM制御のまずさに起因しているといけないので、PWM経由でなく、GPIOから単純にOn/Offした場合も上記のようにGNDが浮くようなことになるのか?も試す予定。 Interface誌を見直したけど、PWM制御でモータを4基同時に回してはいけない等は記載がなかった。しかし、当たり前のように飛んでいる(専門誌にプロの人が書いてるから飛ばないわけがないか。。)

■追記
PWM制御のまずさが関係しているかもと思って、GPIOから直接FETをOn/Offでもやはり同じであった。だから、、4基を同時に動かせないのか、あるいは、LiPOバッテリーがヘタっているのか。。ST-Microが提供しているサンプル版のDrone制御プログラムをビルド、インストールしてどんな風に動くのかを見た方がいいのかも。

■ご参考記事
FETを使ってスイッチするための解説記事(分かりやすい記事ですThanks)
FETを電源スイッチの代わりに使用する方法メモ – Memoteki
MEMOTEKI様[FETを電源スイッチの代わりに使用する方法メモ]