File content as of revision 1:a0ca9f62e81f:

/*
 * Mbed Application program / BMX055(Gyro+Acc+Mag) + Madgwick Filter
 *      tested on Nucleo-F446RE
 *
 * Copyright (c) 2020 Kenji Arai / JH1PJL
 *  http://www.page.sannet.ne.jp/kenjia/index.html
 *  http://mbed.org/users/kenjiArai/
 *      Created:    February   7th, 2020
 *      Revised:    February   7th, 2020
 */

//  Include --------------------------------------------------------------------
#include "mbed.h"
#include "BMX055.h"
#include "Quaternion.hpp"
#include "MadgwickFilter.hpp"

//  Definition -----------------------------------------------------------------
enum {
    ALL_DATA = 0,
    ACC_DATA,
    GYR_DATA,
    MAG_DATA,
    IMU_DATA
};

#define     LOOP_TIME       5
#define     OUT_ALL_CYC     (1000 / LOOP_TIME)
#define     OUT_ACC_CYC     (200 / LOOP_TIME)
#define     OUT_IMU_CYC     (200 / LOOP_TIME)

//  Object ---------------------------------------------------------------------
DigitalIn   mode_sw(PC_8, PullUp);
I2C         i2c(I2C_SDA, I2C_SCL);
BMX055      imu(i2c);
RawSerial   pc(USBTX, USBRX, 115200);
Timer       t;
    
//  RAM ------------------------------------------------------------------------

//  ROM / Constant data --------------------------------------------------------
const double DEG_TO_RAD = 0.01745329251994329576923690768489f;//PI / 180.0;

//  Function prototypes --------------------------------------------------------
void print_revision(void);

//------------------------------------------------------------------------------
//  Control Program
//------------------------------------------------------------------------------
int main()
{
    const BMX055_TypeDef bmx055_my_parameters = {
        // ACC
        ACC_2G,
        ACC_BW500Hz,
        // GYR
        GYR_500DPS,
        GYR_200Hz23Hz,
        // MAG
        MAG_ODR20Hz
    };

    BMX055_ACCEL_TypeDef  acc;
    BMX055_GYRO_TypeDef   gyr;
    BMX055_MAGNET_TypeDef mag;

    bool state = imu.chip_ready();
    if (state){
        pc.printf("ACC+GYR+MAG are ready!\r\n");
    } else {
        pc.printf("ACC+GYR+MAG are NOT ready!\r\n");
    }
    imu.set_parameter(&bmx055_my_parameters);

    MadgwickFilter attitude(0.1);
    Quaternion myQ;

    uint32_t output_mode = ALL_DATA;
    uint32_t n = 0;
    while(true) {
        t.reset();
        t.start();
        imu.get_accel(&acc);
        imu.get_gyro(&gyr);
        imu.get_magnet(&mag);
        if (mode_sw == 1) {
            attitude.MadgwickAHRSupdate(
                gyr.x*DEG_TO_RAD, gyr.y*DEG_TO_RAD, gyr.z*DEG_TO_RAD,
                acc.x, acc.y, acc.z,
                0.0f, 0.0f, 0.0f
            );
        } else {
            attitude.MadgwickAHRSupdate(
                gyr.x*DEG_TO_RAD, gyr.y*DEG_TO_RAD, gyr.z*DEG_TO_RAD,
                acc.x, acc.y, acc.z,
                -mag.x, -mag.y, -mag.z
            );
        }
        if (pc.readable()){ 
            char c = pc.getc();
            if (c == '1'){
                output_mode = ACC_DATA;
            } else if (c == '2'){
                output_mode = IMU_DATA;
            } else if (c == '3'){
                output_mode = GYR_DATA;
            } else if (c == '4'){
                output_mode = MAG_DATA;
            } else if (c == 'R'){
                NVIC_SystemReset(); // Reset
            } else {
                output_mode = ALL_DATA;
            }
        }
        attitude.getAttitude(&myQ);
        switch (output_mode){
            case ACC_DATA:
                if ((n % OUT_ACC_CYC) == 0){
                    pc.printf("%+4.3f,%+4.3f,%+4.3f\r\n",
                                 acc.x, acc.y, acc.z);
                }
                break;
            case IMU_DATA:
                if ((n % OUT_IMU_CYC) == 0){
                    pc.printf("%+4.3f,%+4.3f,%+4.3f,%+4.3f\r\n",
                                myQ.w, myQ.x, myQ.y, myQ.z);
                }
                break;
            case GYR_DATA:
                if ((n % OUT_ACC_CYC) == 0){
                    pc.printf("%+4.3f,%+4.3f,%+4.3f\r\n",
                                 gyr.x, gyr.y, gyr.z);
                }
                break;
            case MAG_DATA:
                if ((n % OUT_ACC_CYC) == 0){
                    pc.printf("%+4.3f,%+4.3f,%+4.3f\r\n",
                                 mag.x, mag.y, mag.z);
                }
                break;
            default:
            case ALL_DATA:
                if ((n % OUT_ALL_CYC) == 0){
                    pc.printf("//ACC: x=%+4.3f y=%+4.3f z=%+4.3f //",
                                 acc.x, acc.y, acc.z);
                    pc.printf("GYR: x=%+3.2f y=%+3.2f z=%+3.2f //",
                                 gyr.x, gyr.y, gyr.z);
                    pc.printf("MAG: x=%+3.2f y=%+3.2f z=%+3.2f ,",
                                 mag.x, mag.y, mag.z);
                    pc.printf(" passed %.1f sec\r\n",
                                 float(n * LOOP_TIME) / 1000.0f);
                }
                break;
        }
        ++n;
        uint32_t passed_time = t.read_ms();
        if (passed_time < (LOOP_TIME -1)){
#if (MBED_MAJOR_VERSION == 2)
            wait_ms(LOOP_TIME - t.read_ms());
#elif (MBED_MAJOR_VERSION == 5)
            ThisThread::sleep_for(LOOP_TIME - t.read_ms());
#endif
        }
    }
}