diff --git a/src/filters/IMU_filter.cpp b/src/filters/IMU_filter.cpp new file mode 100644 index 00000000..4f31f419 --- /dev/null +++ b/src/filters/IMU_filter.cpp @@ -0,0 +1,244 @@ +#include "IMU_filter.hpp" +// For more detail about imu filter Check https://github.com/OAOJim/Study_Notes/blob/main/IMU_filter.md + +void IMU_filter::init_EKF_6axis(IMU_data data){ + // Set a initial value for the quaternion + float recipNorm = 1.0f/sqrt(data.accel_X * data.accel_X + data.accel_Y * data.accel_Y + data.accel_Z * data.accel_Z); + data.accel_X = data.accel_X * recipNorm; + data.accel_Y = data.accel_Y * recipNorm; + data.accel_Z = data.accel_Z * recipNorm;//This is the unit gravity + float axis_norm = sqrt(data.accel_Y * data.accel_Y + data.accel_X * data.accel_X); + float sin_halfangle = sinf(acosf(data.accel_Z)/2.0f); + if(axis_norm > 1e-6){ // Just some stupid way to initialize the quaternion. Quaternion is not easy to initialize since 2 quaternion can represent the same rotation + x[0] = cosf(data.accel_Z/2.0f); + x[1] = data.accel_Y * sin_halfangle / axis_norm; + x[2] = -data.accel_X * sin_halfangle / axis_norm; + x[3] = 1 - x[0] * x[0] - x[1] * x[1] - x[2] * x[2]; + }else{ + x[0] = 1; + x[1] = 0; + x[2] = 0; + x[3] = 0; + } + // After test. within 0.5 second pitch and roll will get to the right value when IMU facing up; + //Since the weight of Q and R for all element should be the same + Q = 1; // Process noise covariance for model update + R = 1000; // measure noise + P[0] = {1000,0,0,0}; // Initial P matrix + P[1] = {0,1000,0,0}; + P[2] = {0,0,1000,0}; + P[3] = {0,0,0,1000}; +} + +int IMU_filter::step_EKF_6axis(IMU_data data){ + // For unit Quaternions, The seperation of garvity is what q_i, q_j, q_k should be + dt = timer.delta(); + float gravity_now = sqrt(data.accel_X * data.accel_X + data.accel_Y * data.accel_Y + data.accel_Z * data.accel_Z); + float recipNorm = 1.0f/gravity_now; + float unit_accel_X = data.accel_X * recipNorm; + float unit_accel_Y = data.accel_Y * recipNorm; + float unit_accel_Z = data.accel_Z * recipNorm;//This is the unit gravity + + //Helping numbers for F + float helpgx = (data.gyro_X * dt) * 0.5f; + float helpgy = (data.gyro_Y * dt) * 0.5f; + float helpgz = (data.gyro_Z * dt) * 0.5f; + // Predict for x + float x0 = x[0]; + float x1 = x[1]; + float x2 = x[2]; + float x3 = x[3]; + + x[0] = x0 - x1*helpgx - x2*helpgy - x3*helpgz ; + x[1] = x0*helpgx + x1 + x2*helpgz - x3*helpgy ; + x[2] = x0*helpgy - x1*helpgz + x2 + x3*helpgx ; + x[3] = x0*helpgz + x1*helpgy - x2*helpgx + x3 ; + // unit x + recipNorm = 1.0f/sqrt(x[0] * x[0] + x[1] * x[1] + x[2] * x[2] + x[3] * x[3]); + x[0] *= recipNorm; + x[1] *= recipNorm; + x[2] *= recipNorm; + x[3] *= recipNorm; + + // predict for P + float F[4][4] = { + {1, -helpgx, -helpgy, -helpgz}, + {helpgx, 1, helpgz, -helpgy}, + {helpgy, -helpgz, 1, helpgx}, + {helpgz, helpgy, -helpgx, 1} + }; + // [ 1, -(dt*gx)/2, -(dt*gy)/2, -(dt*gz)/2] + // [(dt*gx)/2, 1, (dt*gz)/2, -(dt*gy)/2] + // [(dt*gy)/2, -(dt*gz)/2, 1, (dt*gx)/2] + // [(dt*gz)/2, (dt*gy)/2, -(dt*gx)/2, 1] Jacobian function F state transition model + + float FxP[4][4] = {0}; + + // Calculate FxPxF_transpose + for(int i = 0; i < 4; i++){ + for(int j = 0; j < 4; j++){ + for(int k = 0; k < 4; k++){ + FxP[i][j] += F[i][k] * P[k][j]; + } + } + } + for(int i = 0; i < 4; i++){ + for(int j = 0; j < 4; j++){ + P[i][j] = 0; + for(int k = 0; k < 4; k++){ + P[i][j] += FxP[i][k] * F[j][k]; // P = F*P*F^T + Q + if (i == j){ + P[i][j] += Q * dt; + } + } + } + } + // // Updata + float H[3][4] = { + {-2*x[2], 2*x[3], -2*x[0], 2*x[1]}, + {2*x[1], 2*x[0], 2*x[3], 2*x[2]}, + {2*x[0], -2*x[1], -2*x[2], 2*x[3]} + }; + // [-2*q2, 2*q3, -2*q0, 2*q1] + // [ 2*q1, 2*q0, 2*q3, 2*q2] + // [ 2*q0, -2*q1, -2*q2, 2*q3] Jacobian function H observation model + + float PH_transpose[4][3] = {0}; + float HPH_transpose[3][3] = {0}; + // Calculate HPH_transpose + for(int i = 0; i < 4; i++){ + for(int j = 0; j < 3; j++){ + for(int k = 0; k < 4; k++){ + PH_transpose[i][j] += P[i][k] * H[j][k]; + } + } + } + for(int i = 0; i < 3; i++){ + for(int j = 0; j < 3; j++){ + for(int k = 0; k < 4; k++){ + HPH_transpose[i][j] += H[i][k] * PH_transpose[k][j]; + if (i == j){ + HPH_transpose[i][j] += R; + } + } + } + } + float HPH_transpose_inv[3][3] = {0}; + inverse3x3(HPH_transpose, HPH_transpose_inv); + float K[4][3] = {0}; + for(int i = 0; i < 4; i++){ + for(int j = 0; j < 3; j++){ + for(int k = 0; k < 3; k++){ + K[i][j] += PH_transpose[i][k] * HPH_transpose_inv[k][j]; + } + } + } + // caulculate the error between the predicted and the measured + float g[3] = { + unit_accel_X - 2*(x[1]*x[3] - x[0]*x[2]), + unit_accel_Y - 2*(x[2]*x[3] + x[0]*x[1]), + unit_accel_Z - (1 - 2*(x[1]*x[1] + x[2]*x[2])) + }; + + if (gravity_now > 9.7 && gravity_now < 9.9){ + // Updata x + for(int i = 1; i < 3; i++){ + x[i] += K[i][0] * g[0] + K[i][1] * g[1] + K[i][2] * g[2]; + } + recipNorm = 1.0f/sqrt(x[0] * x[0] + x[1] * x[1] + x[2] * x[2] + x[3] * x[3]); + x[0] *= recipNorm; + x[1] *= recipNorm; + x[2] *= recipNorm; + x[3] *= recipNorm; + // Update P + float KH[4][4] = {0}; + float I[4][4] = {0}; + for(int i = 0; i < 4; i++){ + I[i][i] = 1.0f; + } + + for(int i = 0; i < 4; i++){ + for(int j = 0; j < 4; j++){ + for(int k = 0; k < 3; k++){ + KH[i][j] += K[i][k] * H[k][j]; + } + } + } + float temp2[4][4] = {0}; + for(int i = 0; i < 4; i++){ + for(int j = 0; j < 4; j++){ + for(int k = 0; k < 4; k++){ + temp2[i][j] += (I[i][k] - KH[i][k]) * P[k][j]; + } + } + } + for(int i = 0; i < 4; i++){ + for(int j = 0; j < 4; j++){ + P[i][j] = temp2[i][j]; + } + } + } + // These are the last time data + float temp1 = filtered_data.pitch; + float temp2 = filtered_data.roll; + float temp3 = filtered_data.yaw; + filtered_data = data; // Reset all filtered data to the raw data + filtered_data.pitch = (atan2f(2.0f * (x[0] * x[1] + x[2] * x[3]), 1.0f - 2.0f * (x[1] * x[1] + x[2] * x[2]))); + if(abs(filtered_data.pitch) > M_PI_2) { + filtered_data.roll = -(asinf(2.0f * (x[0]*x[2] - x[1]*x[3]))); + if(filtered_data.roll > 0) { + filtered_data.roll -= M_PI; + } else { + filtered_data.roll += M_PI; + } + }else{ + filtered_data.roll = asinf(2.0f * (x[0]*x[2] - x[1]*x[3])); + } + filtered_data.yaw = atan2f(2.0f*(x[0]*x[3] + x[1]*x[2]), 1.0f - 2.0f*(x[2]*x[2] + x[3]*x[3])); + // Calculate the filtered gyro data + filtered_data.gyro_yaw = (filtered_data.yaw - temp3)/dt; + filtered_data.gyro_roll = (filtered_data.roll - temp2)/dt; + filtered_data.gyro_pitch = (filtered_data.pitch - temp1)/dt; + // Convert to the original + filtered_data.accel_world_X = filtered_data.accel_X * (1-2*(x[2]*x[2] + x[3]*x[3])) + filtered_data.accel_Y * (2*(x[1]*x[2] - x[3]*x[0])) + filtered_data.accel_Z * (2*(x[1]*x[3] + x[0]*x[2])); + filtered_data.accel_world_Y = filtered_data.accel_X * (2*(x[1]*x[2] + x[3]*x[0])) + filtered_data.accel_Y * (1-2*(x[1]*x[1] + x[3]*x[3])) + filtered_data.accel_Z * (2*(x[2]*x[3] - x[1]*x[0])); + filtered_data.accel_world_Z = filtered_data.accel_X * (2*(x[1]*x[3] - x[2]*x[0])) + filtered_data.accel_Y * (2*(x[2]*x[3] + x[1]*x[0])) + filtered_data.accel_Z * (1 - 2*(x[1]*x[1] + x[2]*x[2])) - SENSORS_GRAVITY_EARTH; + return 0; + +} + +IMU_data* IMU_filter::get_filter_data(){ + return &filtered_data; +} + +void IMU_filter::inverse3x3(float mat[3][3], float inv[3][3]) { + float det = mat[0][0] * (mat[1][1] * mat[2][2] - mat[2][1] * mat[1][2]) - + mat[0][1] * (mat[1][0] * mat[2][2] - mat[1][2] * mat[2][0]) + + mat[0][2] * (mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0]); + + if (det == 0) { + // Matrix is not invertible + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + inv[i][j] = mat[i][j]; + + return; + } + + float invDet = 1.0f / det; + + inv[0][0] = (mat[1][1] * mat[2][2] - mat[2][1] * mat[1][2]) * invDet; + inv[0][1] = (mat[0][2] * mat[2][1] - mat[0][1] * mat[2][2]) * invDet; + inv[0][2] = (mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1]) * invDet; + inv[1][0] = (mat[1][2] * mat[2][0] - mat[1][0] * mat[2][2]) * invDet; + inv[1][1] = (mat[0][0] * mat[2][2] - mat[0][2] * mat[2][0]) * invDet; + inv[1][2] = (mat[1][0] * mat[0][2] - mat[0][0] * mat[1][2]) * invDet; + inv[2][0] = (mat[1][0] * mat[2][1] - mat[2][0] * mat[1][1]) * invDet; + inv[2][1] = (mat[2][0] * mat[0][1] - mat[0][0] * mat[2][1]) * invDet; + inv[2][2] = (mat[0][0] * mat[1][1] - mat[1][0] * mat[0][1]) * invDet; +} + + + + + diff --git a/src/filters/IMU_filter.hpp b/src/filters/IMU_filter.hpp new file mode 100644 index 00000000..93190afc --- /dev/null +++ b/src/filters/IMU_filter.hpp @@ -0,0 +1,45 @@ +#ifndef IMU_FILTER_H +#define IMU_FILTER_H +#include "../utils/timing.hpp" +#include "../sensors/IMUSensor.hpp" +/// @brief the IMU filter class that filters the IMU data +class IMU_filter{ + private: + /// @brief the timer for dt + Timer timer; + /// @brief the time step + float dt; + /// @brief the quaternion for the filter + float x[4]; + /// @brief process noise for model update + float Q; + /// @brief process noise for accelerometer + float R; + /// @brief the prediction matrix + std::array, 4> P; + /// @brief the Kalman gain + std::array, 4> K; // Kalman gain + /// @brief the data structure that holds all the IMU data + IMU_data filtered_data = {0}; + /// @brief the function for inverse 3x3 matrix + /// @param mat input + /// @param inv output + void inverse3x3(float mat[3][3], float inv[3][3]); + public: + /// @brief Initalize everything including filter constant + /// @param IMU_data is the IMU data structure that holds all the IMU data + void init_EKF_6axis(IMU_data); + /// @brief Do one step of the EKF filter + /// @param IMU_data is the IMU data structure that holds all the IMU data + /// @return int 0 if successful + int step_EKF_6axis(IMU_data); + + /// @brief Print out data for debugging + void print(); + /// @brief Print out data for a Python 3D visulize function + void serial_data_for_plot(); + /// @brief get the filtered data + /// @return pointer of filtered IMU data structure + IMU_data* get_filter_data(); +}; +#endif \ No newline at end of file diff --git a/src/sensors/ICM20649.cpp b/src/sensors/ICM20649.cpp index 440a1049..6100dd03 100644 --- a/src/sensors/ICM20649.cpp +++ b/src/sensors/ICM20649.cpp @@ -53,23 +53,14 @@ bool ICM20649::read() { // could increase efficiency by specifying which values we need, and only assigning values to the object's members from that. // However, getEvent will read all values from the sensor regardless, and assigning these values is very fast - accel_X = accel.acceleration.x; - accel_Y = accel.acceleration.y; - accel_Z = accel.acceleration.z; - gyro_X = gyro.gyro.x; - gyro_Y = gyro.gyro.y; - gyro_Z = gyro.gyro.z; + data.accel_X = accel.acceleration.x; + data.accel_Y = accel.acceleration.y; + data.accel_Z = accel.acceleration.z; + data.gyro_X = gyro.gyro.x; + data.gyro_Y = gyro.gyro.y; + data.gyro_Z = gyro.gyro.z; - temperature = temp.temperature; - - //copy the values to the data struct - icm_sensor_data.accel_X = accel_X; - icm_sensor_data.accel_Y = accel_Y; - icm_sensor_data.accel_Z = accel_Z; - icm_sensor_data.gyro_X = gyro_X; - icm_sensor_data.gyro_Y = gyro_Y; - icm_sensor_data.gyro_Z = gyro_Z; - icm_sensor_data.temperature = temperature; + data.temperature = temp.temperature; return true; } @@ -104,3 +95,4 @@ void ICM20649::set_gyro_range(int gyro_rate_range) { break; } } + diff --git a/src/sensors/ICM20649.hpp b/src/sensors/ICM20649.hpp index efbdc348..7931a9dc 100644 --- a/src/sensors/ICM20649.hpp +++ b/src/sensors/ICM20649.hpp @@ -17,26 +17,6 @@ /// @brief SCL/SCK (SPI Clock) pin for software-SPI mode #define ICM_SCK 27 -/// @brief Structure for the ICM sensor. -struct ICMSensorData { - /// Sensor ID. - uint8_t id; - /// Acceleration in X-axis. - float accel_X; - /// Acceleration in Y-axis. - float accel_Y; - /// Acceleration in Z-axis. - float accel_Z; - /// Gyroscope reading in X-axis. - float gyro_X; - /// Gyroscope reading in Y-axis. - float gyro_Y; - /// Gyroscope reading in Z-axis. - float gyro_Z; - /// Temperature reading. - float temperature; -}; - /// @brief Sensor access for an ICM20649 IMU Sensor. Child of the abstract IMUSensor class. /// @note supports I2C and SPI communication. /// @see Adafruit library this class utilizes: https://adafruit.github.io/Adafruit_ICM20X/html/class_adafruit___i_c_m20_x.html @@ -84,9 +64,6 @@ class ICM20649 : public IMUSensor { /// @brief The selected communication protocol CommunicationProtocol protocol; - - ///ICM sensor data. - ICMSensorData icm_sensor_data; }; diff --git a/src/sensors/IMUSensor.cpp b/src/sensors/IMUSensor.cpp index d716545e..0b68ea0d 100644 --- a/src/sensors/IMUSensor.cpp +++ b/src/sensors/IMUSensor.cpp @@ -1,6 +1,19 @@ #include "IMUSensor.hpp" -// default implementation for printing data +IMU_data IMUSensor::get_data(){ + return data; +} + +void IMUSensor::fix_raw_data(){ + data.accel_X *= scale_accel; + data.accel_Y *= scale_accel; + data.accel_Z *= scale_accel; + data.gyro_X -= offset_X; + data.gyro_Y -= offset_Y; + data.gyro_Z -= offset_Z; + return; +} + void IMUSensor::print() { // Display the temperature data, measured in Celcius Serial.print("\t\tTemperature "); @@ -23,4 +36,34 @@ void IMUSensor::print() { Serial.print(get_gyro_Z()); Serial.println(" radians/s "); Serial.println(); +} + +void IMUSensor::calibration_all(){ + Serial.println("Calibrating IMU..."); + float sum_accel_x = 0; + float sum_accel_y = 0; + float sum_accel_z = 0; + float sum_x = 0; + float sum_y = 0; + float sum_z = 0; + for (int i = 0; i < CALIBRATION_NUM; i++){ + read(); + sum_accel_x += data.accel_X; + sum_accel_y += data.accel_Y; + sum_accel_z += data.accel_Z; + sum_x += data.gyro_X; + sum_y += data.gyro_Y; + sum_z += data.gyro_Z; + } + float x = sum_accel_x/CALIBRATION_NUM; + float y = sum_accel_y/CALIBRATION_NUM; + float z = sum_accel_z/CALIBRATION_NUM; + scale_accel = SENSORS_GRAVITY_EARTH/sqrt((x*x) + (y*y) + (z*z)); + set_offsets(sum_x / CALIBRATION_NUM, sum_y / CALIBRATION_NUM, sum_z / CALIBRATION_NUM); + data.pitch_bias = atan2(y,sqrt(x*x + z*z)); + data.roll_bias = atan2(-x,z); + data.accel_X *= scale_accel; + data.accel_Y *= scale_accel; + data.accel_Z *= scale_accel; + Serial.println("Calibrating Finish..."); } \ No newline at end of file diff --git a/src/sensors/IMUSensor.hpp b/src/sensors/IMUSensor.hpp index aa0157e5..64a8e625 100644 --- a/src/sensors/IMUSensor.hpp +++ b/src/sensors/IMUSensor.hpp @@ -6,6 +6,47 @@ #include #include "Sensor.hpp" +#define CALIBRATION_NUM 100000 + +/// @brief the data structure that holds all the IMU data +struct IMU_data{ +/// @brief raw acceleration value (m/s^2) +float accel_X = 0; +/// @brief raw acceleration value (m/s^2) +float accel_Y = 0; +/// @brief raw acceleration value (m/s^2) +float accel_Z = 0; +/// @brief raw gyroscope value (rad/s) +float gyro_X = 0; +/// @brief raw gyroscope value (rad/s) +float gyro_Y = 0; +/// @brief raw gyroscope value (rad/s) +float gyro_Z = 0; +/// @brief Temperature in Celcius +float temperature = 0; +/// @brief Angle (rad) +float pitch = 0; +/// @brief Angle (rad) +float roll = 0; +/// @brief Angle (rad) +float yaw = 0; +/// @brief Acceleration in world frame (m/s) +float accel_world_X = 0; +/// @brief Acceleration in world frame (m/s) +float accel_world_Y = 0; +/// @brief Acceleration in world frame (m/s) +float accel_world_Z = 0; +/// @brief angular velocity (rad/s) +float gyro_pitch = 0; +/// @brief angular velocity (rad/s) +float gyro_roll = 0; +/// @brief angular velocity (rad/s) +float gyro_yaw = 0; +/// @brief Bias for roll angle (rad) +float roll_bias = 0; +/// @brief Bias for pitch angle (rad) +float pitch_bias = 0; +}; /// @brief Abstract parent class for all IMUSensors, which give acceleration and gyroscope data. class IMUSensor : public Sensor { @@ -14,39 +55,40 @@ class IMUSensor : public Sensor { IMUSensor() : Sensor(SensorType::ICM) {} - /// @brief read values from the sensor. Call this to update sensor data before accessing them from the getters. + /// @brief read values from the sensor. Call this to update sensor data before accessing them from the getters. /// @return true if successful, false if no data available virtual bool read() = 0; /// @brief Get the temperature of the sensor /// @return temperature in Celcius - inline float get_temperature() { return temperature; }; - + inline float get_temperature() { return data.temperature; }; - //POSSIBLE BUG REASON: the accelerations are not corrected for the offset possibly being the reason for the weird values. /// @brief Get the acceleration of the sensor in its local x axis. /// @return acceleration m/s^2 - inline float get_accel_X() { return accel_X; }; + inline float get_accel_X() { return data.accel_X; }; /// @brief Get the acceleration of the sensor in its local y axis /// @return acceleration m/s^2 - inline float get_accel_Y() { return accel_Y; }; + inline float get_accel_Y() { return data.accel_Y; }; /// @brief Get the acceleration of the sensor in its local z axis /// @return acceleration m/s^2 - inline float get_accel_Z() { return accel_Z; }; + inline float get_accel_Z() { return data.accel_Z; }; /// @brief Get the change in gyroscope orientation relative to the x axis /// @return gryoscope x in radians/s - inline float get_gyro_X() { return gyro_X - offset_X; }; + inline float get_gyro_X() { return data.gyro_X; }; /// @brief Get the change in gyroscope orientation relative to the y axis /// @return gyroscope y in radians/s - inline float get_gyro_Y() { return gyro_Y - offset_Y; }; + inline float get_gyro_Y() { return data.gyro_Y;}; /// @brief Get the change in gyroscope orientation relative to the z axis /// @return gyroscope z in radians/s - inline float get_gyro_Z() { return gyro_Z - offset_Z; }; + inline float get_gyro_Z() { return data.gyro_Z;}; + /// @brief get the data structure that holds all the IMU data + /// @return the IMU data structure + IMU_data get_data(); /// @brief Set offsets that we calculate during calibration /// @param x offset in x @@ -57,10 +99,18 @@ class IMUSensor : public Sensor { offset_Y = y; offset_Z = z; } + /// @brief set the scale of the accelerometer + /// @param a the scale of the accelerometer + inline void set_scale(float a){ + scale_accel = a; + } + /// @brief add bias to gyro and scale to accel + void fix_raw_data(); /// @brief Print out all IMU data to Serial for debugging purposes void print(); - + /// @brief Calibrate the IMU sensor + void calibration_all(); protected: // sensor events to read from @@ -74,30 +124,16 @@ class IMUSensor : public Sensor { sensors_event_t temp; // acceleration values assignmed after read() - /// @brief acceleration x value - float accel_X = 0; - /// @brief acceleration y value - float accel_Y = 0; - /// @brief acceleration z value - float accel_Z = 0; - - // gyroscope values assigned after read() - - /// @brief gyroscope x value - float gyro_X = 0; - /// @brief gyroscope y value - float gyro_Y = 0; - /// @brief gyroscope z value - float gyro_Z = 0; + /// @brief data set + IMU_data data; /// @brief offset x value float offset_X = 0; /// @brief offset y value float offset_Y = 0; /// @brief offset z value float offset_Z = 0; - - /// @brief temperature value - float temperature = 0; + /// @brief scale of the accelerometer + float scale_accel = 1; }; #endif \ No newline at end of file diff --git a/src/sensors/LSM6DSOX.cpp b/src/sensors/LSM6DSOX.cpp index 703c7e95..021bffbe 100644 --- a/src/sensors/LSM6DSOX.cpp +++ b/src/sensors/LSM6DSOX.cpp @@ -23,13 +23,13 @@ bool LSM6DSOX::read() { // could increase efficiency by specifying which values we need, and only assigning values to the object's members from that. // However, getEvent will read all values from the sensor regardless, and assigning these values is very fast - accel_X = accel.acceleration.x; - accel_Y = accel.acceleration.y; - accel_Z = accel.acceleration.z; - gyro_X = gyro.gyro.x; - gyro_Y = gyro.gyro.y; - gyro_Z = gyro.gyro.z; - - temperature = temp.temperature; + data.accel_X = accel.acceleration.x; + data.accel_Y = accel.acceleration.y; + data.accel_Z = accel.acceleration.z; + data.gyro_X = gyro.gyro.x; + data.gyro_Y = gyro.gyro.y; + data.gyro_Z = gyro.gyro.z; + + data.temperature = temp.temperature; return true; } \ No newline at end of file diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp index 81238e77..1e161420 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -46,9 +46,8 @@ void SensorManager::init(const Config* config_data) { icm_sensors[i] = new ICM20649(); icm_sensors[i]->init(icm_sensors[i]->CommunicationProtocol::SPI); icm_sensors[i]->set_gyro_range(4000); - + icm_sensors[i]->calibration_all(); } - calibrate_imus(); // initialize rev encoders for (int i = 0; i < rev_sensor_count; i++) { @@ -78,7 +77,8 @@ void SensorManager::read() { } for (int i = 0; i < icm_sensor_count; i++) { icm_sensors[i]->read(); - // icm_sensors[i]->print(); + icm_sensors[i]->fix_raw_data(); + icm_sensors[i]->print(); } for (int i = 0; i < rev_sensor_count; i++) { @@ -126,31 +126,6 @@ D200LD14P* SensorManager::get_lidar_sensor(int index) { } } -void SensorManager::calibrate_imus() { - Serial.println("Calibrating IMU's..."); - float sum_x = 0; - float sum_y = 0; - float sum_z = 0; - - float sum_accel_x = 0; - float sum_accel_y = 0; - float sum_accel_z = 0; - - for (int i = 0; i < NUM_IMU_CALIBRATION; i++) { - icm_sensors[0]->read(); - sum_x += icm_sensors[0]->get_gyro_X(); - sum_y += icm_sensors[0]->get_gyro_Y(); - sum_z += icm_sensors[0]->get_gyro_Z(); - - sum_accel_x += icm_sensors[0]->get_accel_X(); - sum_accel_y += icm_sensors[0]->get_accel_Y(); - sum_accel_z += icm_sensors[0]->get_accel_Z(); - } - - Serial.printf("Calibrated offsets: %f, %f, %f\n", sum_x / NUM_IMU_CALIBRATION, sum_y / NUM_IMU_CALIBRATION, sum_z / NUM_IMU_CALIBRATION); - icm_sensors[0]->set_offsets(sum_x / NUM_IMU_CALIBRATION, sum_y / NUM_IMU_CALIBRATION, sum_z / NUM_IMU_CALIBRATION); -} - int SensorManager::get_num_sensors(SensorType sensor_type) { switch (sensor_type) { case SensorType::BUFFENC: diff --git a/src/sensors/SensorManager.hpp b/src/sensors/SensorManager.hpp index 4057ec95..ce9569f6 100644 --- a/src/sensors/SensorManager.hpp +++ b/src/sensors/SensorManager.hpp @@ -74,9 +74,6 @@ class SensorManager { return ref; } - /// @brief Calibrate the IMUs - void calibrate_imus(); - // TODO: Create function that will create a struct to work with the new comms private: