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
- Committer:
- ericleal
- Date:
- 2021-12-02
- Revision:
- 2:aa4dff412cf7
- Parent:
- 0:533538484381
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; } } }