無茶でも飛び上がったら制御プログラムを書こうかという気にもなるが、どうやっても離陸しないDroneを相手にこれからどうしたものか。。やれることとしては、、（１）LiPoバッテリーをフル充電して飛ぶかどうかを確認、（２）ST Microが提供しているサンプルプログラムを入れてみて、それで飛ぶかどうかを確認、、ぐらいかと。特に、バッテリーの蓄電状態については、フルだと１ｍぐらい飛ぶが、時間が経つと地面を這うような感じというブログの記事もあって、満タンでないとまともに飛ばないらしい。
基本設計が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の使い方が違っていそうだ。４基のモータが同時に電源供給されているのか、時分割で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の設定次第と思うけど、モータ制御は奥が深いっす
