Skip to content

Commit

Permalink
Merge pull request #4 from KTH-SML/safety-merge-lli-imu
Browse files Browse the repository at this point in the history 
IT ALL WORKS LETS GO
  • Loading branch information
@nilskiefer
nilskiefer authored Nov 22, 2023
2 parents e44ef79 + 0b8a805 commit 1d6fdcb
Show file tree
Hide file tree
Showing 4 changed files with 83 additions and 95 deletions.
97 changes: 49 additions & 48 deletions src/external_ic/IMU.h
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
#include <Arduino.h>

// Deps needed to interface with the IMU
#include "sensor_msgs/Imu.h"
#include "sensor_msgs/MagneticField.h"
Expand Down Expand Up @@ -26,61 +27,61 @@ bool setupIMU() {
Serial.println("BN0055 detected");
return true;
}
// Im sorry, this is the way
uint32_t headerCntIMU = 0;
uint32_t headerCntMag = 0;
uint32_t headerCntTemp = 0;
void IMUReadingToMsg(ros::Time time, sensor_msgs::Imu &msg) {
sensors_event_t event;
bno.getEvent(&event);
msg.header.seq = headerCntIMU++;
msg.header.stamp = time;
msg.header.frame_id = "imu";
// ros::Time now,
void IMUReadingToMsg() {

// Header stuff
header.stamp = nh.now();
header.seq++;

MSG_IMU.header = header;
MSG_MAG.header = header;
MSG_TEMP.header = header;

imu::Quaternion quat = bno.getQuat();
msg.orientation.x = (float)quat.x();
msg.orientation.y = (float)quat.y();
msg.orientation.z = (float)quat.z();
msg.orientation.w = (float)quat.w();
imu::Vector<3> gyroVec = bno.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);
msg.angular_velocity.x = (float)gyroVec.x();
msg.angular_velocity.y = (float)gyroVec.y();
msg.angular_velocity.z = (float)gyroVec.z();
imu::Vector<3> accelVec = bno.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL);
msg.linear_acceleration.x = (float)accelVec.x();
msg.linear_acceleration.y = (float)accelVec.y();
msg.linear_acceleration.z = (float)accelVec.z();
float fakeCovariance = 0;
MSG_IMU.orientation.x = quat.x();
MSG_IMU.orientation.y = quat.y();
MSG_IMU.orientation.z = quat.z();
MSG_IMU.orientation.w = quat.w();
imu::Vector<3> Vec;
Vec = bno.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);
MSG_IMU.angular_velocity.x = Vec.x();
MSG_IMU.angular_velocity.y = Vec.y();
MSG_IMU.angular_velocity.z = Vec.z();

Vec = bno.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL);
MSG_IMU.linear_acceleration.x = Vec.x();
MSG_IMU.linear_acceleration.y = Vec.y();
MSG_IMU.linear_acceleration.z = Vec.z();

Vec = bno.getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER);
MSG_MAG.magnetic_field.x = Vec.x();
MSG_MAG.magnetic_field.y = Vec.y();
MSG_MAG.magnetic_field.z = Vec.z();

MSG_TEMP.temperature = bno.getTemp();

int fakeCovariance = 0;
for (int i = 0; i < 9; ++i) {
msg.orientation_covariance[i] = fakeCovariance;
msg.angular_velocity_covariance[i] = fakeCovariance;
msg.linear_acceleration_covariance[i] = fakeCovariance;
}
Serial.printf("IMU: Orientation(%f, %f, %f, %f) Angular Velocity(%f, %f, %f) Linear Acceleration(%f, %f, %f)\n", msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w, msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z, msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z);
}
void MagReadingToMsg(ros::Time time, sensor_msgs::MagneticField &msg) {
sensors_event_t event;
bno.getEvent(&event);
//msg.header.seq = headerCntMag++;
//msg.header.stamp = time;
//msg.header.frame_id = "imu";
//imu::Vector<3> magVec = bno.getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER);
//msg.magnetic_field.x = (float)magVec.x();
//msg.magnetic_field.y = (float)magVec.y();
//msg.magnetic_field.z = (float)magVec.z();
msg.magnetic_field.x = 6;
//float fakeCovariance = 0;
//for (int i = 0; i < 9; ++i) {
// msg.magnetic_field_covariance[i] = fakeCovariance;
//}
Serial.printf("Magnetic Field: (%f, %f, %f)\n", msg.magnetic_field.x, msg.magnetic_field.y, msg.magnetic_field.z);
}
void TempReadingToMsg(ros::Time time, sensor_msgs::Temperature &msg) {
msg.header.seq = headerCntTemp++;
msg.header.stamp = time;
msg.header.frame_id = "t";
msg.temperature = (float)bno.getTemp();
Serial.printf("Temperature: %f\n", msg.temperature);
//nh.spinOnce();
//Serial.println("MSG_IMU: ");
imu_pub.publish(&MSG_IMU);
nh.spinOnce();
//Serial.println("MSG_MAG: ");
imu_mag.publish(&MSG_MAG);
nh.spinOnce();
//Serial.println("MSG_TEMP: ");
imu_temp.publish(&MSG_TEMP);
nh.spinOnce();
//Serial.printf("IMU: Orientation(%f, %f, %f, %f) Angular Velocity(%f, %f, %f) Linear Acceleration(%f, %f, %f)\n", msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w, msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z, msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z);

return;
}

void IMU_DEBUG() {
/* Get a new sensor event */
sensors_event_t event;
Expand Down
46 changes: 18 additions & 28 deletions src/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -309,31 +309,42 @@ void rosSetup() {
nh.initNode();
// NOTE: Putting advertise before subscribe destroys
// EVERYTHING :DDDD~~~~~


nh.subscribe(ctrl_request);
nh.subscribe(emergency_request);

nh.advertise(remote_pub);

nh.advertise(imu_mag);
nh.advertise(imu_pub);

nh.advertise(imu_temp);

nh.advertise(ctrl_actuated_pub);
nh.advertise(encoder_pub);
nh.advertise(debug_pub);

nh.advertise(imu_pub);
nh.advertise(imu_mag);
nh.advertise(imu_temp);


nh.advertise(debug_pub);
nh.negotiateTopics();
}

//! Arduino setup function
void setup() {
Serial.begin(SERIAL_BAUD_RATE);
// Serial.begin(SERIAL_BAUD_RATE);
setupActuation();
/* ROS setup */
rosSetup();

pinMode(LED_BUILTIN, OUTPUT);

Wire1.begin();
setup_gpio();
pwm_reader::setup();
encoders::setup();
setupIMU();

rosSetup();
Serial.println("Setup done");
}

Expand Down Expand Up @@ -383,28 +394,7 @@ void loop() {
encoder_pub.publish(&MSG_ENCODER);
}

IMUReadingToMsg(nh.now(), MSG_IMU);
MagReadingToMsg(nh.now(), MSG_MAG);
TempReadingToMsg(nh.now(), MSG_TEMP);
Serial.println("Publishing IMU");
imu_pub.publish(&MSG_IMU);
Serial.println(sizeof(MSG_IMU)); // Print the size of the structure

Serial.println("Publishing MAG");
Serial.println("Printing MSG_MAG:");
Serial.print("Magnetic Field X: ");
Serial.println(MSG_MAG.magnetic_field.x);

Serial.print("Magnetic Field Y: ");
Serial.println(MSG_MAG.magnetic_field.y);

Serial.print("Magnetic Field Z: ");
Serial.println(MSG_MAG.magnetic_field.z);
Serial.println(sizeof(MSG_MAG)); // Print the size of the structure

imu_mag.publish(&MSG_MAG);
Serial.println("Publishing TEMP");
// imu_temp.publish(&MSG_TEMP);
IMUReadingToMsg();

// PCB LED Logic
buttons::updateButtons();
Expand Down
11 changes: 7 additions & 4 deletions src/settings.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@
const uint32_t SERIAL_BAUD_RATE = 115200;
#else
const uint32_t SERIAL_BAUD_RATE = 250000;
//const uint32_t SERIAL_BAUD_RATE = 115200;
#endif

/*
Expand All @@ -20,11 +21,13 @@ const uint32_t SERIAL_BAUD_RATE = 250000;
const unsigned long SW_TIMEOUT = 200; //!< Duration (ms) from last recieved computer
//!< message when the computer will count as idle

// THIS IS VERY STUPID AND SHOULD BE REMOVED, but cant be removed
// Because ROS
//! Maximum number of ROS subscribers
const uint8_t MAX_ROS_SUBSCRIBERS = 2;
const uint8_t MAX_ROS_SUBSCRIBERS = 50;
//! Maximum number of ROS publishers
const uint8_t MAX_ROS_PUBLISHERS = 5;
const uint8_t MAX_ROS_PUBLISHERS = 50;
//! Maximum number of ROS subscribers
const uint16_t ROS_IN_BUFFER_SIZE = 300;
const uint16_t ROS_OUT_BUFFER_SIZE = 300;
const uint16_t ROS_IN_BUFFER_SIZE = 500;
const uint16_t ROS_OUT_BUFFER_SIZE = 500;
#endif /* SVEA_LLI_SETTINGS */
24 changes: 9 additions & 15 deletions src/svea_teensy.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,6 @@
#include "sensor_msgs/MagneticField.h"
#include "sensor_msgs/Temperature.h"


#include "settings.h"

/*! @file svea_teensy.h*/
Expand Down Expand Up @@ -129,10 +128,6 @@ typedef svea_msgs::lli_ctrl lli_ctrl_in_t; //!< Message type for incomming me
typedef svea_msgs::lli_ctrl lli_ctrl_out_t; //!< Message type for outgoing messages'
typedef svea_msgs::lli_encoder lli_encoder_t; //!< Message type for encoder messages

typedef sensor_msgs::Imu lli_imu_t; //!< Message type for imu messages
typedef sensor_msgs::MagneticField lli_mag_t;
typedef sensor_msgs::Temperature lli_t_t;

/*
* Storage variables
*/
Expand Down Expand Up @@ -188,23 +183,22 @@ lli_ctrl_out_t MSG_ACTUATED; //!< Message sending actuated messages
lli_encoder_t MSG_ENCODER; //!< Message used for outgoing wheel encoder messages
lli_encoder_t MSG_DEBUG;

//BN055 Stuff
lli_imu_t MSG_IMU;
lli_mag_t MSG_MAG;
lli_t_t MSG_TEMP;

// BN055 Stuff
sensor_msgs::Imu MSG_IMU;
sensor_msgs::MagneticField MSG_MAG;
sensor_msgs::Temperature MSG_TEMP;

//!< Message used for misc debugging
ros::Publisher remote_pub("lli/remote", &MSG_REMOTE); //!< Remote message publisher
ros::Publisher ctrl_actuated_pub("lli/ctrl_actuated", &MSG_ACTUATED); //!< Actuated control message publisher
ros::Publisher encoder_pub("lli/encoder", &MSG_ENCODER);
ros::Publisher debug_pub("lli/debug", &MSG_DEBUG);
ros::Publisher debug_pub("lli/debug", &MSG_DEBUG);

ros::Publisher imu_pub("/imu/data", &MSG_IMU);
ros::Publisher imu_mag("/imu/mag", &MSG_MAG);
ros::Publisher imu_temp("/imu/temp", &MSG_TEMP);
ros::Publisher imu_pub("/imu/data", &MSG_IMU);
ros::Publisher imu_mag("/imu/mag", &MSG_MAG);
ros::Publisher imu_temp("/imu/temp", &MSG_TEMP);

//!< Encoder reading publisher
//!< Encoder reading publisher
ros::Subscriber<lli_ctrl_in_t> ctrl_request("lli/ctrl_request", &callbackCtrlRequest); //!< Controll request subscriber
ros::Subscriber<svea_msgs::lli_emergency> emergency_request("lli/emergency", &callbackEmergency); //!< Controll request subscriber

Expand Down

0 comments on commit 1d6fdcb

Please sign in to comment.