Eric Leal / Mbed OS AHRS_to_BT_OS6

A sample program to demo the use of the ICM20948 IMU with AHRS modified to be configured in the main.

Dependencies:   Madgwick ICM20948

main.cpp@2:aa4dff412cf7, 2021-12-02 (annotated)

Committer:
ericleal
Date:
Thu Dec 02 15:45:11 2021 +0000
Revision:
2:aa4dff412cf7
Parent:
0:533538484381 
Updated library to OS 6.15

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ericleal 0:533538484381 1 /* mbed Microcontroller Library
ericleal 0:533538484381 2 * Copyright (c) 2019 ARM Limited
ericleal 0:533538484381 3 * SPDX-License-Identifier: Apache-2.0
ericleal 0:533538484381 4 */
ericleal 0:533538484381 5 #include "mbed.h"
ericleal 0:533538484381 6 #include "Madgwick.h"
ericleal 0:533538484381 7 #include "ICM20948.h"
ericleal 0:533538484381 8
ericleal 0:533538484381 9 #define accel_div 1
ericleal 0:533538484381 10 #define accel_lp 0
ericleal 0:533538484381 11 #define accel_lp_en 1
ericleal 0:533538484381 12 #define accel_fs 1
ericleal 0:533538484381 13 #define gyro_div 1
ericleal 0:533538484381 14 #define gyro_lp 0
ericleal 0:533538484381 15 #define gyro_lp_en 1
ericleal 0:533538484381 16 #define gyro_fs 1
ericleal 0:533538484381 17
ericleal 0:533538484381 18 #define accel_conf (accel_lp_en | (accel_fs << 1) | (accel_lp << 3))
ericleal 0:533538484381 19 #define gyro_conf (gyro_lp_en | (gyro_fs << 1) | (gyro_lp << 3))
ericleal 0:533538484381 20
ericleal 0:533538484381 21 #define ahrs_beta 0.1f
ericleal 0:533538484381 22
ericleal 0:533538484381 23 using namespace std::chrono; //needed to do time in us
ericleal 0:533538484381 24
ericleal 0:533538484381 25 BufferedSerial bluetooth(PB_6, PB_7, 115200); //(TX, RX, baud)
ericleal 0:533538484381 26 ICM20948 imu(PB_4, PA_7); //(SDA, SCL)
ericleal 0:533538484381 27 Madgwick AHRS(ahrs_beta);
ericleal 0:533538484381 28 Timer t;
ericleal 0:533538484381 29
ericleal 0:533538484381 30 int main()
ericleal 0:533538484381 31 {
ericleal 0:533538484381 32 DigitalOut led(LED1);
ericleal 0:533538484381 33 int code;
ericleal 0:533538484381 34 //IMU stuff
ericleal 0:533538484381 35 printf("hello IMU!\r\n");
ericleal 0:533538484381 36 led = 1;
ericleal 0:533538484381 37 code = imu.whoAmI();
ericleal 0:533538484381 38 printf("%d \r\n", code);
ericleal 0:533538484381 39 led = !led;
ericleal 0:533538484381 40 imu.init(accel_conf, accel_div, gyro_conf, gyro_div);
ericleal 0:533538484381 41 led = !led;
ericleal 0:533538484381 42 printf("IMU initialized\r\n");
ericleal 0:533538484381 43
ericleal 0:533538484381 44 int send_counter = 0;
ericleal 0:533538484381 45 t.start();
ericleal 0:533538484381 46
ericleal 0:533538484381 47 while (true) {
ericleal 0:533538484381 48
ericleal 0:533538484381 49 imu.getAccGyro();
ericleal 0:533538484381 50 float time = static_cast<float>(duration_cast<microseconds>(t.elapsed_time()).count()) / 1000000.0f;
ericleal 0:533538484381 51 AHRS.update_attitude(imu.getGX(), imu.getGY(), imu.getGZ(), imu.getAX(), imu.getAY(), imu.getAZ(), time);
ericleal 0:533538484381 52
ericleal 0:533538484381 53 t.reset();
ericleal 0:533538484381 54 unsigned int _roll = static_cast<int>(AHRS.getRoll() * 100.0f + 32767.0f);
ericleal 0:533538484381 55 unsigned int _pitch = static_cast<int>(AHRS.getPitch() * 100.0f + 32767.0f);
ericleal 0:533538484381 56 unsigned int _yaw = static_cast<int>(AHRS.getYaw() * 100.0f + 32767.0f);
ericleal 0:533538484381 57
ericleal 0:533538484381 58 char out_buffer[7];
ericleal 0:533538484381 59 out_buffer[0] = 15;
ericleal 0:533538484381 60 out_buffer[1] = (_roll >> 8);
ericleal 0:533538484381 61 out_buffer[2] = (_roll & 0xFF);
ericleal 0:533538484381 62 out_buffer[3] = (_pitch >> 8);
ericleal 0:533538484381 63 out_buffer[4] = (_pitch & 0xFF);
ericleal 0:533538484381 64 out_buffer[5] = (_yaw >> 8);
ericleal 0:533538484381 65 out_buffer[6] = (_yaw & 0xFF);
ericleal 0:533538484381 66 int write_counter;
ericleal 0:533538484381 67 write_counter++;
ericleal 0:533538484381 68 if (write_counter > 5) {
ericleal 0:533538484381 69 bluetooth.write(out_buffer, 7);
ericleal 0:533538484381 70 write_counter = 0;
ericleal 0:533538484381 71 }
ericleal 0:533538484381 72
ericleal 0:533538484381 73 }
ericleal 0:533538484381 74 }