🐱
トラ技2019/7月号でカルマンフィルタを学ぶ(2)-傾斜計搭載部品動作確認
目的
前回作成した傾斜計（倒立振子）の動作確認を行います。
ボタンとLEDの動作確認
Lチカでもいいんでしょうけど、せっかくなのでボタン押下とLED点灯の連動をするプログラムを作成します。
まずは、ボタン関連とLED関連のコードを作成しました。
lib/button.h
#include <stdio.h>
extern void init_button_all(void);
extern void init_button1(void);
extern void init_button2(void);
extern uint get_button1_pinnum(void);
extern uint get_button2_pinnum(void);
extern bool button1_pressed(void);
extern bool button2_pressed(void);
lib/button.c
#include <math.h>
#include "pico/stdlib.h"
const uint SW1 = 2;
const uint SW2 = 3;
#define PRESSED false
#define RELEASED true
void init_button1(void) {
gpio_init(SW1);
gpio_set_dir(SW1, GPIO_IN);
gpio_pull_up(SW1);
}
void init_button2(void) {
gpio_init(SW2);
gpio_set_dir(SW2, GPIO_IN);
gpio_pull_up(SW2);
}
void init_button_all(void) {
init_button1();
init_button2();
}
uint get_button1_pinnum(void) {
return SW1;
}
uint get_button2_pinnum(void) {
return SW2;
}
bool button1_pressed(void) {
return (gpio_get(get_button1_pinnum()) == PRESSED);
}
bool button2_pressed(void) {
return (gpio_get(get_button2_pinnum()) == PRESSED);
}
lib/led.h
#include <stdio.h>
void init_led_all(void);
void init_led1(void);
void init_led2(void);
void init_led3(void);
void set_led1(int state);
void set_led2(int state);
void set_led3(int state);
lib/led.c
#include <math.h>
#include "pico/stdlib.h"
const uint LED1 = 4;
const uint LED2 = 5;
const uint LED3 = 6;
void init_led1(void) {
gpio_init(LED1);
gpio_set_dir(LED1, GPIO_OUT);
}
void init_led2(void) {
gpio_init(LED2);
gpio_set_dir(LED2, GPIO_OUT);
}
void init_led3(void) {
gpio_init(LED3);
gpio_set_dir(LED3, GPIO_OUT);
}
void init_led_all(void) {
init_led1();
init_led2();
init_led3();
}
void set_led1(int state) {
gpio_put(LED1, state);
}
void set_led2(int state) {
gpio_put(LED2, state);
}
void set_led3(int state) {
gpio_put(LED3, state);
}
次にボタン押下とLED点灯の連動をするコードです。以前書いたものを流用しています。
LED3は0.5[s]ごとに点灯・消灯をします。
button_led_test.c
#include <stdio.h>
#include <math.h>
#include "pico/stdlib.h"
#include "hardware/timer.h"
#include "common.h"
#include "lib/button.h"
#include "lib/led.h"
#define BOUNCE_TIME 10 // [ms]
static int64_t button1_callback(alarm_id_t id, void *user_data) {
/*
* 割込みが発生して BOUNCE_TIME[ms]後、
* ボタンが押されてたらLEDを点灯
* ボタンが離されてたらLEDを消灯
*/
if (button1_pressed()) {
set_led1(LED_ON);
}
else {
set_led1(LED_OFF);
}
/* 割込み禁止の解除 */
gpio_set_irq_enabled(get_button1_pinnum(), 0x4u | 0x8u, true);
return 0;
}
static int64_t button2_callback(alarm_id_t id, void *user_data) {
/*
* 割込みが発生して BOUNCE_TIME[ms]後、
* ボタンが押されてたらLEDを点灯
* ボタンが離されてたらLEDを消灯
*/
if (button2_pressed()) {
set_led2(LED_ON);
}
else {
set_led2(LED_OFF);
}
/* 割込み禁止の解除 */
gpio_set_irq_enabled(get_button2_pinnum(), 0x4u | 0x8u, true);
return 0;
}
static void push_button_callback(uint gpio, uint32_t events) {
/* アクションがあったボタンからの割込み禁止にする */
gpio_set_irq_enabled(gpio, 0x4u | 0x8u, false);
/* タイマー設定 */
if (gpio == get_button1_pinnum()) {
add_alarm_in_ms(BOUNCE_TIME, button1_callback, NULL, false);
} else if (gpio == get_button2_pinnum()) {
add_alarm_in_ms(BOUNCE_TIME, button2_callback, NULL, false);
}
}
int main() {
init_button_all();
init_led_all();
gpio_set_irq_enabled_with_callback(get_button1_pinnum(), 0x4u | 0x8u, true, push_button_callback);
gpio_set_irq_enabled_with_callback(get_button2_pinnum(), 0x4u | 0x8u, true, push_button_callback);
while (true) {
set_led3(LED_ON);
sleep_ms(500);
set_led3(LED_OFF);
sleep_ms(500);
}
}
IMU(MPU-9250)とUARTの動作確認
ボタン・LEDと同様IMUも呼び出し関数群とテストコードを作りました。
lib/mpu9250.h
extern void mpu9250_init();
extern void mpu9250_reset();
extern void mpu9250_settings();
extern void mpu9250_read_raw(int16_t accel[3], int16_t gyro[3], int16_t *temp);
lib/mpu9250.c
#include <stdio.h>
#include <string.h>
#include "pico/stdlib.h"
#include "pico/binary_info.h"
#include "hardware/i2c.h"
const uint IMU_SCL = 13;
const uint IMU_SDA = 12;
// By default these devices are on bus address 0x68
static int addr = 0x68;
void mpu9250_init() {
i2c_init(i2c0, 400 * 1000);
gpio_set_function(IMU_SDA, GPIO_FUNC_I2C);
gpio_set_function(IMU_SCL, GPIO_FUNC_I2C);
gpio_pull_up(IMU_SDA);
gpio_pull_up(IMU_SCL);
// Make the I2C pins available to picotool
bi_decl(bi_2pins_with_func(IMU_SDA, IMU_SCL, GPIO_FUNC_I2C));
}
void mpu9250_reset() {
uint8_t buf[] = {0x6B, 0x80};
i2c_write_blocking(i2c0, addr, buf, 2, false);
sleep_ms(100);
buf[1] = 0x00;
i2c_write_blocking(i2c0, addr, buf, 2, false);
sleep_ms(100);
}
void mpu9250_settings() {
uint8_t buf[2];
/*
Gyroscope Bandwidth : 8800Hz
Fullscale : +250dps
Accelerometer Bandwidth : 1.13kHz
Fullscale : +-2g
*/
buf[0] = 0x1A; /* Register 26 - Configuration */
buf[1] = 0x00;
i2c_write_blocking(i2c0, addr, buf, 2, false);
sleep_ms(100);
buf[0] = 0x1B; /* Register 27 - Gyroscope Configuration */
buf[1] = 0x00;
i2c_write_blocking(i2c0, addr, buf, 2, false);
sleep_ms(100);
buf[0] = 0x1C; /* Register 28 - Accelerometer Configuration */
buf[1] = 0x00;
i2c_write_blocking(i2c0, addr, buf, 2, false);
sleep_ms(100);
buf[0] = 0x1D; /* Register 29 - Accelerometer Configuration 2 */
buf[1] = 0x00;
i2c_write_blocking(i2c0, addr, buf, 2, false);
sleep_ms(100);
}
void mpu9250_read_raw(int16_t accel[3], int16_t gyro[3], int16_t *temp) {
uint8_t buffer[6];
// Start reading acceleration registers from register 0x3B for 6 bytes
uint8_t val = 0x3B;
i2c_write_blocking(i2c0, addr, &val, 1, true); // true to keep master control of bus
i2c_read_blocking(i2c0, addr, buffer, 6, false);
for (int i = 0; i < 3; i++) {
accel[i] = (buffer[i * 2] << 8 | buffer[(i * 2) + 1]);
}
// Now gyro data from reg 0x43 for 6 bytes
// The register is auto incrementing on each read
val = 0x43;
i2c_write_blocking(i2c0, addr, &val, 1, true);
i2c_read_blocking(i2c0, addr, buffer, 6, false); // False - finished with bus
for (int i = 0; i < 3; i++) {
gyro[i] = (buffer[i * 2] << 8 | buffer[(i * 2) + 1]);
}
// Now temperature from reg 0x41 for 2 bytes
// The register is auto incrementing on each read
val = 0x41;
i2c_write_blocking(i2c0, addr, &val, 1, true);
i2c_read_blocking(i2c0, addr, buffer, 2, false); // False - finished with bus
*temp = buffer[0] << 8 | buffer[1];
}
IMUのテストコードは参考書のinclinometer.cppになるべく近い形にしました。IMUから取得したデータを2.5[ms]ごと(=400[Hz])出力するようにしています。
imu_test.c
#include <stdio.h>
#include <pico/stdlib.h>
#include <math.h>
#include "lib/mpu9250.h"
int sample_num = 100;
float theta_mean;
float theta_variance;
float theta_dot_mean;
float theta_dot_variance;
void get_data(float *theta, float *theta_dot)
{
int16_t accel[3], gyro[3], temp;
mpu9250_read_raw(accel, gyro, &temp);
*theta = atan((float)accel[2] / (float)accel[0]) * 360.0 / 2.0 / M_PI;
*theta_dot = (float)gyro[1] / 131.0; // degree per sec
}
void acc_gyro_init()
{
float theta, theta_dot;
float theta_array[sample_num];
float theta_dot_array[sample_num];
// get data
for (int i = 0; i < sample_num; i++)
{
get_data(&theta, &theta_dot);
theta_array[i] = theta;
theta_dot_array[i] = theta_dot;
sleep_ms(10);
}
// calculate mean
theta_mean = 0;
theta_dot_mean = 0;
for (int i = 0; i < sample_num; i++)
{
theta_mean += theta_array[i];
theta_dot_mean += theta_dot_array[i];
}
theta_mean /= sample_num;
theta_dot_mean /= sample_num;
// calcurate variance
float temp1, temp2;
theta_variance = 0;
theta_dot_variance = 0;
for (int i = 0; i < sample_num; i++)
{
temp1 = theta_array[i] - theta_mean;
theta_variance += temp1 * temp1;
temp2 = theta_dot_array[i] - theta_dot_mean;
theta_dot_variance += temp2 * temp2;
}
theta_variance /= sample_num;
theta_dot_variance /= sample_num;
}
int main() {
stdio_init_all();
mpu9250_init();
mpu9250_reset();
mpu9250_settings();
acc_gyro_init();
printf("Theta,Theta_dot\n");
while (1) {
float theta, theta_dot;
get_data(&theta, &theta_dot);
printf("%f,%f\n", theta, theta_dot);
sleep_us(2500);
}
}
UARTの出力を出力するために、FT234Xのシリアル変換モジュールをつなげ、gtktermで表示させました。
Arduino IDEのシリアルプロッタでデータを可視化させました。
最後に
ひとまず傾斜計に必要なIMUデータとRP2040-Oneで計算した結果を出力する準備ができました。次は参考書に戻り、カルマンフィルタを使った傾斜計のソフトウェアの実装を行います。
Discussion