File content as of revision 2:aa4dff412cf7:

/* mbed Microcontroller Library
 * Copyright (c) 2019 ARM Limited
 * SPDX-License-Identifier: Apache-2.0
 */
#include "mbed.h"
#include "Madgwick.h"
#include "ICM20948.h"

#define accel_div   1
#define accel_lp    0
#define accel_lp_en    1
#define accel_fs    1
#define gyro_div   1
#define gyro_lp    0
#define gyro_lp_en    1
#define gyro_fs    1

#define accel_conf (accel_lp_en | (accel_fs << 1) | (accel_lp << 3))
#define gyro_conf (gyro_lp_en | (gyro_fs << 1) | (gyro_lp << 3))

#define ahrs_beta 0.1f

using namespace std::chrono;    //needed to do time in us

BufferedSerial bluetooth(PB_6, PB_7, 115200);  //(TX, RX, baud)
ICM20948 imu(PB_4, PA_7);   //(SDA, SCL)
Madgwick   AHRS(ahrs_beta);
Timer t;
              
int main()
{
    DigitalOut led(LED1);
    int code;
    //IMU stuff
    printf("hello IMU!\r\n");
    led = 1;
    code = imu.whoAmI();
    printf("%d \r\n", code);
    led = !led;
    imu.init(accel_conf, accel_div, gyro_conf, gyro_div);
    led = !led;
    printf("IMU initialized\r\n");

    int send_counter = 0;
    t.start();

    while (true) {

        imu.getAccGyro();
        float time = static_cast<float>(duration_cast<microseconds>(t.elapsed_time()).count()) / 1000000.0f;
        AHRS.update_attitude(imu.getGX(), imu.getGY(), imu.getGZ(), imu.getAX(), imu.getAY(), imu.getAZ(), time);

        t.reset();
        unsigned int _roll = static_cast<int>(AHRS.getRoll() * 100.0f + 32767.0f);
        unsigned int _pitch = static_cast<int>(AHRS.getPitch() * 100.0f + 32767.0f);
        unsigned int _yaw = static_cast<int>(AHRS.getYaw() * 100.0f + 32767.0f);

        char out_buffer[7];
        out_buffer[0] = 15;
        out_buffer[1] = (_roll >> 8);
        out_buffer[2] = (_roll & 0xFF);
        out_buffer[3] = (_pitch >> 8);
        out_buffer[4] = (_pitch & 0xFF);
        out_buffer[5] = (_yaw >> 8);
        out_buffer[6] = (_yaw & 0xFF);
        int write_counter;
        write_counter++;
        if (write_counter > 5) {
            bluetooth.write(out_buffer, 7);
            write_counter = 0;
        }
        
    }
}