Chawankorn Tiemprasert / Mbed 2 deprecated Ros_STM32_IMU_BNO055

use in class project

Dependencies:   mbed BNO055 ros_lib_kinetic

main.cpp@1:1e40c57efbb1, 2018-12-19 (annotated)

Committer:
chawankorn
Date:
Wed Dec 19 08:24:48 2018 +0000
Revision:
1:1e40c57efbb1
Parent:
0:7f3dd28ad889 
for class project

Who changed what in which revision?

UserRevisionLine numberNew contents of line
chawankorn 0:7f3dd28ad889 1 #include "mbed.h"
chawankorn 0:7f3dd28ad889 2 #include "BNO055.h"
chawankorn 1:1e40c57efbb1 3 #include <ros.h>
chawankorn 1:1e40c57efbb1 4 #include <std_msgs/Float64.h>
chawankorn 0:7f3dd28ad889 5
chawankorn 0:7f3dd28ad889 6 Serial pc(USBTX, USBRX);
chawankorn 0:7f3dd28ad889 7 BNO055 imu(I2C_SDA,I2C_SCL);
chawankorn 0:7f3dd28ad889 8 DigitalOut led(LED1);
chawankorn 0:7f3dd28ad889 9
chawankorn 1:1e40c57efbb1 10 ros::NodeHandle nh;
chawankorn 1:1e40c57efbb1 11
chawankorn 1:1e40c57efbb1 12 std_msgs::Float64 E_roll;
chawankorn 1:1e40c57efbb1 13 ros::Publisher pub_roll("Euler roll", &E_roll);
chawankorn 1:1e40c57efbb1 14
chawankorn 0:7f3dd28ad889 15 int main() {
chawankorn 0:7f3dd28ad889 16 pc.baud(115200);
chawankorn 1:1e40c57efbb1 17 nh.initNode();
chawankorn 1:1e40c57efbb1 18 nh.advertise(pub_roll);
chawankorn 1:1e40c57efbb1 19 //pc.printf("BNO055 Hello World++\r\n\r\n");
chawankorn 0:7f3dd28ad889 20 led = 1;
chawankorn 0:7f3dd28ad889 21 // Reset the BNO055
chawankorn 0:7f3dd28ad889 22 imu.reset();
chawankorn 0:7f3dd28ad889 23 // Check that the BNO055 is connected and flash LED if not
chawankorn 0:7f3dd28ad889 24 if (!imu.check())
chawankorn 0:7f3dd28ad889 25 while (true){
chawankorn 0:7f3dd28ad889 26 led = !led;
chawankorn 0:7f3dd28ad889 27 wait(0.1);
chawankorn 0:7f3dd28ad889 28 }
chawankorn 0:7f3dd28ad889 29 // Display sensor information
chawankorn 1:1e40c57efbb1 30 /*pc.printf("BNO055 found\r\n\r\n");
chawankorn 0:7f3dd28ad889 31 pc.printf("Chip ID: %u\r\n",imu.ID.id);
chawankorn 0:7f3dd28ad889 32 pc.printf("Accelerometer ID: %u\r\n",imu.ID.accel);
chawankorn 0:7f3dd28ad889 33 pc.printf("Gyroscope ID: %u\r\n",imu.ID.gyro);
chawankorn 0:7f3dd28ad889 34 pc.printf("Magnetometer ID: %u\r\n\r\n",imu.ID.mag);
chawankorn 0:7f3dd28ad889 35 pc.printf("Firmware version v%d.%0d\r\n",imu.ID.sw[0],imu.ID.sw[1]);
chawankorn 0:7f3dd28ad889 36 pc.printf("Bootloader version v%d\r\n\r\n",imu.ID.bootload);
chawankorn 0:7f3dd28ad889 37 // Display chip serial number
chawankorn 0:7f3dd28ad889 38 for (int i = 0; i<4; i++){
chawankorn 0:7f3dd28ad889 39 pc.printf("%u.%u.%u.%u\r\n",imu.ID.serial[i*4],imu.ID.serial[i*4+1],imu.ID.serial[i*4+2],imu.ID.serial[i*4+3]);
chawankorn 0:7f3dd28ad889 40 }
chawankorn 1:1e40c57efbb1 41 pc.printf("\r\n");*/
chawankorn 0:7f3dd28ad889 42
chawankorn 1:1e40c57efbb1 43 while (true) {
chawankorn 0:7f3dd28ad889 44 imu.setmode(OPERATION_MODE_NDOF);
chawankorn 0:7f3dd28ad889 45 imu.get_calib();
chawankorn 0:7f3dd28ad889 46 imu.get_angles();
chawankorn 0:7f3dd28ad889 47 imu.get_temp();
chawankorn 0:7f3dd28ad889 48 imu.get_quat();
chawankorn 1:1e40c57efbb1 49
chawankorn 1:1e40c57efbb1 50 float roll_angle;
chawankorn 1:1e40c57efbb1 51 roll_angle = imu.euler.roll;
chawankorn 1:1e40c57efbb1 52 E_roll.data = roll_angle;
chawankorn 1:1e40c57efbb1 53 pub_roll.publish(&E_roll);
chawankorn 0:7f3dd28ad889 54 pc.printf("Temperature\t%d\n", imu.temperature);
chawankorn 1:1e40c57efbb1 55 //pc.printf("Quaternion: w:%f\tx:%f\ty:%f\tz:%f\n", imu.quat.w, imu.quat.x, imu.quat.y, imu.quat.z);
chawankorn 1:1e40c57efbb1 56 pc.printf("%u\t roll:%5.1f\t pitch:%5.1f\t yaw:%5.1f\r\n",imu.calib,imu.euler.roll,imu.euler.pitch,imu.euler.yaw);
chawankorn 1:1e40c57efbb1 57 nh.spinOnce();
chawankorn 0:7f3dd28ad889 58 wait(0.5);
chawankorn 0:7f3dd28ad889 59 }
chawankorn 1:1e40c57efbb1 60 nh.spinOnce();
chawankorn 0:7f3dd28ad889 61 }