main.cpp

00001 /*
00002  * Mbed Application program / BMX055(Gyro+Acc+Mag) + Madgwick Filter
00003  *      tested on Nucleo-F446RE
00004  *
00005  * Copyright (c) 2020 Kenji Arai / JH1PJL
00006  *  http://www.page.sannet.ne.jp/kenjia/index.html
00007  *  http://mbed.org/users/kenjiArai/
00008  *      Created:    February   7th, 2020
00009  *      Revised:    February   7th, 2020
00010  */
00011 
00012 //  Include --------------------------------------------------------------------
00013 #include "mbed.h"
00014 #include "BMX055.h"
00015 #include "Quaternion.hpp"
00016 #include "MadgwickFilter.hpp"
00017 
00018 //  Definition -----------------------------------------------------------------
00019 enum {
00020     ALL_DATA = 0,
00021     ACC_DATA,
00022     GYR_DATA,
00023     MAG_DATA,
00024     IMU_DATA
00025 };
00026 
00027 #define     LOOP_TIME       5
00028 #define     OUT_ALL_CYC     (1000 / LOOP_TIME)
00029 #define     OUT_ACC_CYC     (200 / LOOP_TIME)
00030 #define     OUT_IMU_CYC     (200 / LOOP_TIME)
00031 
00032 //  Object ---------------------------------------------------------------------
00033 DigitalIn   mode_sw(PC_8, PullUp);
00034 I2C         i2c(I2C_SDA, I2C_SCL);
00035 BMX055      imu(i2c);
00036 RawSerial   pc(USBTX, USBRX, 115200);
00037 Timer       t;
00038     
00039 //  RAM ------------------------------------------------------------------------
00040 
00041 //  ROM / Constant data --------------------------------------------------------
00042 const double DEG_TO_RAD = 0.01745329251994329576923690768489f;//PI / 180.0;
00043 
00044 //  Function prototypes --------------------------------------------------------
00045 void print_revision(void);
00046 
00047 //------------------------------------------------------------------------------
00048 //  Control Program
00049 //------------------------------------------------------------------------------
00050 int main()
00051 {
00052     const BMX055_TypeDef bmx055_my_parameters = {
00053         // ACC
00054         ACC_2G,
00055         ACC_BW500Hz,
00056         // GYR
00057         GYR_500DPS,
00058         GYR_200Hz23Hz,
00059         // MAG
00060         MAG_ODR20Hz
00061     };
00062 
00063     BMX055_ACCEL_TypeDef  acc;
00064     BMX055_GYRO_TypeDef   gyr;
00065     BMX055_MAGNET_TypeDef mag;
00066 
00067     bool state = imu.chip_ready();
00068     if (state){
00069         pc.printf("ACC+GYR+MAG are ready!\r\n");
00070     } else {
00071         pc.printf("ACC+GYR+MAG are NOT ready!\r\n");
00072     }
00073     imu.set_parameter(&bmx055_my_parameters);
00074 
00075     MadgwickFilter attitude(0.1);
00076     Quaternion myQ;
00077 
00078     uint32_t output_mode = ALL_DATA;
00079     uint32_t n = 0;
00080     while(true) {
00081         t.reset();
00082         t.start();
00083         imu.get_accel(&acc);
00084         imu.get_gyro(&gyr);
00085         imu.get_magnet(&mag);
00086         if (mode_sw == 1) {
00087             attitude.MadgwickAHRSupdate(
00088                 gyr.x*DEG_TO_RAD, gyr.y*DEG_TO_RAD, gyr.z*DEG_TO_RAD,
00089                 acc.x, acc.y, acc.z,
00090                 0.0f, 0.0f, 0.0f
00091             );
00092         } else {
00093             attitude.MadgwickAHRSupdate(
00094                 gyr.x*DEG_TO_RAD, gyr.y*DEG_TO_RAD, gyr.z*DEG_TO_RAD,
00095                 acc.x, acc.y, acc.z,
00096                 -mag.x, -mag.y, -mag.z
00097             );
00098         }
00099         if (pc.readable()){ 
00100             char c = pc.getc();
00101             if (c == '1'){
00102                 output_mode = ACC_DATA;
00103             } else if (c == '2'){
00104                 output_mode = IMU_DATA;
00105             } else if (c == '3'){
00106                 output_mode = GYR_DATA;
00107             } else if (c == '4'){
00108                 output_mode = MAG_DATA;
00109             } else if (c == 'R'){
00110                 NVIC_SystemReset(); // Reset
00111             } else {
00112                 output_mode = ALL_DATA;
00113             }
00114         }
00115         attitude.getAttitude(&myQ);
00116         switch (output_mode){
00117             case ACC_DATA:
00118                 if ((n % OUT_ACC_CYC) == 0){
00119                     pc.printf("%+4.3f,%+4.3f,%+4.3f\r\n",
00120                                  acc.x, acc.y, acc.z);
00121                 }
00122                 break;
00123             case IMU_DATA:
00124                 if ((n % OUT_IMU_CYC) == 0){
00125                     pc.printf("%+4.3f,%+4.3f,%+4.3f,%+4.3f\r\n",
00126                                 myQ.w, myQ.x, myQ.y, myQ.z);
00127                 }
00128                 break;
00129             case GYR_DATA:
00130                 if ((n % OUT_ACC_CYC) == 0){
00131                     pc.printf("%+4.3f,%+4.3f,%+4.3f\r\n",
00132                                  gyr.x, gyr.y, gyr.z);
00133                 }
00134                 break;
00135             case MAG_DATA:
00136                 if ((n % OUT_ACC_CYC) == 0){
00137                     pc.printf("%+4.3f,%+4.3f,%+4.3f\r\n",
00138                                  mag.x, mag.y, mag.z);
00139                 }
00140                 break;
00141             default:
00142             case ALL_DATA:
00143                 if ((n % OUT_ALL_CYC) == 0){
00144                     pc.printf("//ACC: x=%+4.3f y=%+4.3f z=%+4.3f //",
00145                                  acc.x, acc.y, acc.z);
00146                     pc.printf("GYR: x=%+3.2f y=%+3.2f z=%+3.2f //",
00147                                  gyr.x, gyr.y, gyr.z);
00148                     pc.printf("MAG: x=%+3.2f y=%+3.2f z=%+3.2f ,",
00149                                  mag.x, mag.y, mag.z);
00150                     pc.printf(" passed %.1f sec\r\n",
00151                                  float(n * LOOP_TIME) / 1000.0f);
00152                 }
00153                 break;
00154         }
00155         ++n;
00156         uint32_t passed_time = t.read_ms();
00157         if (passed_time < (LOOP_TIME -1)){
00158 #if (MBED_MAJOR_VERSION == 2)
00159             wait_ms(LOOP_TIME - t.read_ms());
00160 #elif (MBED_MAJOR_VERSION == 5)
00161             ThisThread::sleep_for(LOOP_TIME - t.read_ms());
00162 #endif
00163         }
00164     }
00165 }