Kenji Arai / Mbed OS BMX055_Madgwick

BMX055 data by Madgwick Filter sends to PC and shows it by Python program

Dependencies:   MadgwickFilter BMX055

main.cpp@1:a0ca9f62e81f, 2020-02-07 (annotated)

Committer:
kenjiArai
Date:
Fri Feb 07 04:28:46 2020 +0000
Revision:
1:a0ca9f62e81f
Parent:
0:141e4576190a 
BMX055 data by Madgwick Filter sends to PC and shows it by Python program

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kenjiArai 0:141e4576190a 1 /*
kenjiArai 1:a0ca9f62e81f 2 * Mbed Application program / BMX055(Gyro+Acc+Mag) + Madgwick Filter
kenjiArai 1:a0ca9f62e81f 3 * tested on Nucleo-F446RE
kenjiArai 0:141e4576190a 4 *
kenjiArai 1:a0ca9f62e81f 5 * Copyright (c) 2020 Kenji Arai / JH1PJL
kenjiArai 0:141e4576190a 6 * http://www.page.sannet.ne.jp/kenjia/index.html
kenjiArai 0:141e4576190a 7 * http://mbed.org/users/kenjiArai/
kenjiArai 1:a0ca9f62e81f 8 * Created: February 7th, 2020
kenjiArai 1:a0ca9f62e81f 9 * Revised: February 7th, 2020
kenjiArai 0:141e4576190a 10 */
kenjiArai 0:141e4576190a 11
kenjiArai 0:141e4576190a 12 // Include --------------------------------------------------------------------
kenjiArai 0:141e4576190a 13 #include "mbed.h"
kenjiArai 0:141e4576190a 14 #include "BMX055.h"
kenjiArai 0:141e4576190a 15 #include "Quaternion.hpp"
kenjiArai 0:141e4576190a 16 #include "MadgwickFilter.hpp"
kenjiArai 0:141e4576190a 17
kenjiArai 0:141e4576190a 18 // Definition -----------------------------------------------------------------
kenjiArai 0:141e4576190a 19 enum {
kenjiArai 0:141e4576190a 20 ALL_DATA = 0,
kenjiArai 0:141e4576190a 21 ACC_DATA,
kenjiArai 0:141e4576190a 22 GYR_DATA,
kenjiArai 0:141e4576190a 23 MAG_DATA,
kenjiArai 0:141e4576190a 24 IMU_DATA
kenjiArai 0:141e4576190a 25 };
kenjiArai 0:141e4576190a 26
kenjiArai 0:141e4576190a 27 #define LOOP_TIME 5
kenjiArai 0:141e4576190a 28 #define OUT_ALL_CYC (1000 / LOOP_TIME)
kenjiArai 0:141e4576190a 29 #define OUT_ACC_CYC (200 / LOOP_TIME)
kenjiArai 0:141e4576190a 30 #define OUT_IMU_CYC (200 / LOOP_TIME)
kenjiArai 0:141e4576190a 31
kenjiArai 0:141e4576190a 32 // Object ---------------------------------------------------------------------
kenjiArai 1:a0ca9f62e81f 33 DigitalIn mode_sw(PC_8, PullUp);
kenjiArai 0:141e4576190a 34 I2C i2c(I2C_SDA, I2C_SCL);
kenjiArai 0:141e4576190a 35 BMX055 imu(i2c);
kenjiArai 1:a0ca9f62e81f 36 RawSerial pc(USBTX, USBRX, 115200);
kenjiArai 1:a0ca9f62e81f 37 Timer t;
kenjiArai 0:141e4576190a 38
kenjiArai 0:141e4576190a 39 // RAM ------------------------------------------------------------------------
kenjiArai 0:141e4576190a 40
kenjiArai 0:141e4576190a 41 // ROM / Constant data --------------------------------------------------------
kenjiArai 0:141e4576190a 42 const double DEG_TO_RAD = 0.01745329251994329576923690768489f;//PI / 180.0;
kenjiArai 0:141e4576190a 43
kenjiArai 0:141e4576190a 44 // Function prototypes --------------------------------------------------------
kenjiArai 1:a0ca9f62e81f 45 void print_revision(void);
kenjiArai 0:141e4576190a 46
kenjiArai 0:141e4576190a 47 //------------------------------------------------------------------------------
kenjiArai 0:141e4576190a 48 // Control Program
kenjiArai 0:141e4576190a 49 //------------------------------------------------------------------------------
kenjiArai 0:141e4576190a 50 int main()
kenjiArai 0:141e4576190a 51 {
kenjiArai 0:141e4576190a 52 const BMX055_TypeDef bmx055_my_parameters = {
kenjiArai 0:141e4576190a 53 // ACC
kenjiArai 0:141e4576190a 54 ACC_2G,
kenjiArai 0:141e4576190a 55 ACC_BW500Hz,
kenjiArai 0:141e4576190a 56 // GYR
kenjiArai 0:141e4576190a 57 GYR_500DPS,
kenjiArai 0:141e4576190a 58 GYR_200Hz23Hz,
kenjiArai 0:141e4576190a 59 // MAG
kenjiArai 0:141e4576190a 60 MAG_ODR20Hz
kenjiArai 0:141e4576190a 61 };
kenjiArai 0:141e4576190a 62
kenjiArai 0:141e4576190a 63 BMX055_ACCEL_TypeDef acc;
kenjiArai 0:141e4576190a 64 BMX055_GYRO_TypeDef gyr;
kenjiArai 0:141e4576190a 65 BMX055_MAGNET_TypeDef mag;
kenjiArai 1:a0ca9f62e81f 66
kenjiArai 0:141e4576190a 67 bool state = imu.chip_ready();
kenjiArai 0:141e4576190a 68 if (state){
kenjiArai 0:141e4576190a 69 pc.printf("ACC+GYR+MAG are ready!\r\n");
kenjiArai 1:a0ca9f62e81f 70 } else {
kenjiArai 0:141e4576190a 71 pc.printf("ACC+GYR+MAG are NOT ready!\r\n");
kenjiArai 0:141e4576190a 72 }
kenjiArai 0:141e4576190a 73 imu.set_parameter(&bmx055_my_parameters);
kenjiArai 0:141e4576190a 74
kenjiArai 0:141e4576190a 75 MadgwickFilter attitude(0.1);
kenjiArai 0:141e4576190a 76 Quaternion myQ;
kenjiArai 0:141e4576190a 77
kenjiArai 0:141e4576190a 78 uint32_t output_mode = ALL_DATA;
kenjiArai 0:141e4576190a 79 uint32_t n = 0;
kenjiArai 0:141e4576190a 80 while(true) {
kenjiArai 0:141e4576190a 81 t.reset();
kenjiArai 0:141e4576190a 82 t.start();
kenjiArai 0:141e4576190a 83 imu.get_accel(&acc);
kenjiArai 0:141e4576190a 84 imu.get_gyro(&gyr);
kenjiArai 0:141e4576190a 85 imu.get_magnet(&mag);
kenjiArai 0:141e4576190a 86 if (mode_sw == 1) {
kenjiArai 0:141e4576190a 87 attitude.MadgwickAHRSupdate(
kenjiArai 0:141e4576190a 88 gyr.x*DEG_TO_RAD, gyr.y*DEG_TO_RAD, gyr.z*DEG_TO_RAD,
kenjiArai 0:141e4576190a 89 acc.x, acc.y, acc.z,
kenjiArai 0:141e4576190a 90 0.0f, 0.0f, 0.0f
kenjiArai 0:141e4576190a 91 );
kenjiArai 0:141e4576190a 92 } else {
kenjiArai 0:141e4576190a 93 attitude.MadgwickAHRSupdate(
kenjiArai 0:141e4576190a 94 gyr.x*DEG_TO_RAD, gyr.y*DEG_TO_RAD, gyr.z*DEG_TO_RAD,
kenjiArai 0:141e4576190a 95 acc.x, acc.y, acc.z,
kenjiArai 0:141e4576190a 96 -mag.x, -mag.y, -mag.z
kenjiArai 0:141e4576190a 97 );
kenjiArai 0:141e4576190a 98 }
kenjiArai 1:a0ca9f62e81f 99 if (pc.readable()){
kenjiArai 0:141e4576190a 100 char c = pc.getc();
kenjiArai 0:141e4576190a 101 if (c == '1'){
kenjiArai 0:141e4576190a 102 output_mode = ACC_DATA;
kenjiArai 0:141e4576190a 103 } else if (c == '2'){
kenjiArai 0:141e4576190a 104 output_mode = IMU_DATA;
kenjiArai 0:141e4576190a 105 } else if (c == '3'){
kenjiArai 0:141e4576190a 106 output_mode = GYR_DATA;
kenjiArai 0:141e4576190a 107 } else if (c == '4'){
kenjiArai 0:141e4576190a 108 output_mode = MAG_DATA;
kenjiArai 0:141e4576190a 109 } else if (c == 'R'){
kenjiArai 0:141e4576190a 110 NVIC_SystemReset(); // Reset
kenjiArai 1:a0ca9f62e81f 111 } else {
kenjiArai 0:141e4576190a 112 output_mode = ALL_DATA;
kenjiArai 0:141e4576190a 113 }
kenjiArai 0:141e4576190a 114 }
kenjiArai 0:141e4576190a 115 attitude.getAttitude(&myQ);
kenjiArai 0:141e4576190a 116 switch (output_mode){
kenjiArai 0:141e4576190a 117 case ACC_DATA:
kenjiArai 0:141e4576190a 118 if ((n % OUT_ACC_CYC) == 0){
kenjiArai 0:141e4576190a 119 pc.printf("%+4.3f,%+4.3f,%+4.3f\r\n",
kenjiArai 0:141e4576190a 120 acc.x, acc.y, acc.z);
kenjiArai 0:141e4576190a 121 }
kenjiArai 0:141e4576190a 122 break;
kenjiArai 0:141e4576190a 123 case IMU_DATA:
kenjiArai 0:141e4576190a 124 if ((n % OUT_IMU_CYC) == 0){
kenjiArai 0:141e4576190a 125 pc.printf("%+4.3f,%+4.3f,%+4.3f,%+4.3f\r\n",
kenjiArai 0:141e4576190a 126 myQ.w, myQ.x, myQ.y, myQ.z);
kenjiArai 0:141e4576190a 127 }
kenjiArai 0:141e4576190a 128 break;
kenjiArai 0:141e4576190a 129 case GYR_DATA:
kenjiArai 0:141e4576190a 130 if ((n % OUT_ACC_CYC) == 0){
kenjiArai 0:141e4576190a 131 pc.printf("%+4.3f,%+4.3f,%+4.3f\r\n",
kenjiArai 0:141e4576190a 132 gyr.x, gyr.y, gyr.z);
kenjiArai 0:141e4576190a 133 }
kenjiArai 0:141e4576190a 134 break;
kenjiArai 0:141e4576190a 135 case MAG_DATA:
kenjiArai 0:141e4576190a 136 if ((n % OUT_ACC_CYC) == 0){
kenjiArai 0:141e4576190a 137 pc.printf("%+4.3f,%+4.3f,%+4.3f\r\n",
kenjiArai 0:141e4576190a 138 mag.x, mag.y, mag.z);
kenjiArai 0:141e4576190a 139 }
kenjiArai 0:141e4576190a 140 break;
kenjiArai 0:141e4576190a 141 default:
kenjiArai 0:141e4576190a 142 case ALL_DATA:
kenjiArai 0:141e4576190a 143 if ((n % OUT_ALL_CYC) == 0){
kenjiArai 0:141e4576190a 144 pc.printf("//ACC: x=%+4.3f y=%+4.3f z=%+4.3f //",
kenjiArai 0:141e4576190a 145 acc.x, acc.y, acc.z);
kenjiArai 0:141e4576190a 146 pc.printf("GYR: x=%+3.2f y=%+3.2f z=%+3.2f //",
kenjiArai 0:141e4576190a 147 gyr.x, gyr.y, gyr.z);
kenjiArai 0:141e4576190a 148 pc.printf("MAG: x=%+3.2f y=%+3.2f z=%+3.2f ,",
kenjiArai 0:141e4576190a 149 mag.x, mag.y, mag.z);
kenjiArai 0:141e4576190a 150 pc.printf(" passed %.1f sec\r\n",
kenjiArai 0:141e4576190a 151 float(n * LOOP_TIME) / 1000.0f);
kenjiArai 0:141e4576190a 152 }
kenjiArai 0:141e4576190a 153 break;
kenjiArai 0:141e4576190a 154 }
kenjiArai 0:141e4576190a 155 ++n;
kenjiArai 0:141e4576190a 156 uint32_t passed_time = t.read_ms();
kenjiArai 0:141e4576190a 157 if (passed_time < (LOOP_TIME -1)){
kenjiArai 1:a0ca9f62e81f 158 #if (MBED_MAJOR_VERSION == 2)
kenjiArai 0:141e4576190a 159 wait_ms(LOOP_TIME - t.read_ms());
kenjiArai 1:a0ca9f62e81f 160 #elif (MBED_MAJOR_VERSION == 5)
kenjiArai 1:a0ca9f62e81f 161 ThisThread::sleep_for(LOOP_TIME - t.read_ms());
kenjiArai 1:a0ca9f62e81f 162 #endif
kenjiArai 0:141e4576190a 163 }
kenjiArai 0:141e4576190a 164 }
kenjiArai 0:141e4576190a 165 }