From 82276ddb92ec02e89775b94d7fbb94d1c4fb6782 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 24 May 2023 10:56:59 +0300 Subject: [PATCH] Cleanups --- flix/cli.ino | 4 +--- flix/flix.ino | 2 -- flix/imu.ino | 23 +++++++++-------------- flix/mavlink.ino | 2 +- gazebo/MPU9250.h | 17 ----------------- gazebo/flix.cpp | 5 ----- 6 files changed, 11 insertions(+), 42 deletions(-) delete mode 100644 gazebo/MPU9250.h diff --git a/flix/cli.ino b/flix/cli.ino index 4c306b7..ca297b9 100644 --- a/flix/cli.ino +++ b/flix/cli.ino @@ -1,7 +1,6 @@ // Copyright (c) 2023 Oleg Kalachev // Repository: https://github.com/okalachev/flix -#include #include "pid.h" static String command; @@ -9,7 +8,6 @@ static String value; static bool parsingCommand = true; extern PID rollRatePID, pitchRatePID, yawRatePID, rollPID, pitchPID; -extern MPU9250 IMU; const char* motd = "\nWelcome to\n" @@ -68,7 +66,7 @@ static void doCommand() } else if (command == "psq") { Serial.printf("qx: %f qy: %f qz: %f qw: %f\n", attitude.x, attitude.y, attitude.z, attitude.w); } else if (command == "imu") { - Serial.printf("gyro bias %f %f %f\n", IMU.getGyroBiasX_rads(), IMU.getGyroBiasY_rads(), IMU.getGyroBiasZ_rads()); + printIMUCal(); } else if (command == "rc") { Serial.printf("RAW throttle %d yaw %d pitch %d roll %d aux %d mode %d\n", channels[RC_CHANNEL_THROTTLE], channels[RC_CHANNEL_YAW], channels[RC_CHANNEL_PITCH], diff --git a/flix/flix.ino b/flix/flix.ino index 94a9e6d..13bd19f 100644 --- a/flix/flix.ino +++ b/flix/flix.ino @@ -21,7 +21,6 @@ #define MOTOR_FRONT_RIGHT 2 #define MOTOR_REAR_RIGHT 1 -uint32_t startTime; // system startup time uint32_t stepTime; // current step time uint32_t steps; // total steps count float stepsPerSecond; // steps per last second @@ -33,7 +32,6 @@ float motors[4]; // normalized motors thrust in range [-1..1] Vector rates; // angular rates, rad/s Vector acc; // accelerometer data, m/s/s Quaternion attitude; // estimated attitude -bool calibrating; // flag we're calibrating void setup() { diff --git a/flix/imu.ino b/flix/imu.ino index 9c74121..763fe8e 100644 --- a/flix/imu.ino +++ b/flix/imu.ino @@ -55,17 +55,12 @@ bool readIMU() static void calibrateGyro() { - calibrating = true; Serial.println("Calibrating gyro, stand still"); delay(500); int status = IMU.calibrateGyro(); Serial.printf("Calibration status: %d\n", status); - Serial.print("Gyro bias: "); - Serial.print(IMU.getGyroBiasX_rads(), 10); Serial.print(" "); - Serial.print(IMU.getGyroBiasY_rads(), 10); Serial.print(" "); - Serial.println(IMU.getGyroBiasZ_rads(), 10); IMU.setSrd(0); - calibrating = false; + printIMUCal(); } static void calibrateAccel() @@ -82,14 +77,7 @@ static void calibrateAccel() IMU.calibrateAccel(); Serial.println("Cal accel: upside down"); delay(300); IMU.calibrateAccel(); - Serial.print("Accel bias: "); - Serial.print(IMU.getAccelBiasX_mss(), 10); Serial.print(" "); - Serial.print(IMU.getAccelBiasY_mss(), 10); Serial.print(" "); - Serial.println(IMU.getAccelBiasZ_mss(), 10); - Serial.print("Accel scale: "); - Serial.print(IMU.getAccelScaleFactorX(), 10); Serial.print(" "); - Serial.print(IMU.getAccelScaleFactorY(), 10); Serial.print(" "); - Serial.println(IMU.getAccelScaleFactorZ(), 10); + printIMUCal(); } static void loadAccelCal() @@ -106,6 +94,13 @@ static void loadGyroCal() IMU.setGyroBiasZ_rads(0.0163032326); } +void printIMUCal() +{ + Serial.printf("gyro bias: %f %f %f\n", IMU.getGyroBiasX_rads(), IMU.getGyroBiasY_rads(), IMU.getGyroBiasZ_rads()); + Serial.printf("accel bias: %f %f %f\n", IMU.getAccelBiasX_mss(), IMU.getAccelBiasY_mss(), IMU.getAccelBiasZ_mss()); + Serial.printf("accel scale: %f %f %f\n", IMU.getAccelScaleFactorX(), IMU.getAccelScaleFactorY(), IMU.getAccelScaleFactorZ()); +} + // Accel bias: 0.0463809967 0.0463809967 0.1486964226 // Accel scale: 0.9986976385 0.9993721247 1.0561490059 diff --git a/flix/mavlink.ino b/flix/mavlink.ino index df7191a..d4c74d0 100644 --- a/flix/mavlink.ino +++ b/flix/mavlink.ino @@ -21,7 +21,7 @@ void sendMavlink() mavlink_msg_heartbeat_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC, MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_SAFETY_ARMED, - 0, calibrating ? MAV_STATE_CALIBRATING : MAV_STATE_STANDBY); + 0, MAV_STATE_STANDBY); sendMessage(&msg); // params test diff --git a/gazebo/MPU9250.h b/gazebo/MPU9250.h deleted file mode 100644 index 9f118d9..0000000 --- a/gazebo/MPU9250.h +++ /dev/null @@ -1,17 +0,0 @@ -// Copyright (c) 2023 Oleg Kalachev -// Repository: https://github.com/okalachev/flix -// Distributed under the MIT License (https://opensource.org/licenses/MIT) - -// Mocks of some MPU9250 library functions - -#pragma once - -class MPU9250 { -public: - float getAccelBiasX_mss() { return 0; } - float getAccelBiasY_mss() { return 0; } - float getAccelBiasZ_mss() { return 0; } - float getGyroBiasX_rads() { return 0; } - float getGyroBiasY_rads() { return 0; } - float getGyroBiasZ_rads() { return 0; } -}; diff --git a/gazebo/flix.cpp b/gazebo/flix.cpp index c80bd07..0fb6177 100644 --- a/gazebo/flix.cpp +++ b/gazebo/flix.cpp @@ -140,11 +140,6 @@ public: const double maxThrust = 0.03 * ONE_G; // 30 g, https://www.youtube.com/watch?v=VtKI4Pjx8Sk // 65 mm prop ~40 g - // std::cout << "fr: " << motors[MOTOR_FRONT_RIGHT] - // << " fl: " << motors[MOTOR_FRONT_LEFT] - // << " rr: " << motors[MOTOR_REAR_RIGHT] - // << " rl: " << motors[MOTOR_REAR_LEFT] << std::endl; - const float scale0 = 1.0, scale1 = 1.1, scale2 = 0.9, scale3 = 1.05; const float minThrustRel = 0;