From c8e5e08b03b23ede7ae3407b59ea50d366202886 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sat, 3 Jan 2026 13:28:18 +0300 Subject: [PATCH] Move all global variable declarations to the appropriate subsystems As it makes the subsystems code easier to understand. Declare the most used variables in main sketch file as forward declarations. Make all control input zero by default (except controlMode). Minor changes. --- flix/cli.ino | 3 ++- flix/estimate.ino | 4 ++++ flix/flix.ino | 17 +++++++---------- flix/imu.ino | 5 ++++- flix/mavlink.ino | 5 ++--- flix/motors.ino | 3 ++- flix/rc.ino | 17 ++++++++++------- flix/time.ino | 2 ++ flix/vector.h | 1 - gazebo/flix.h | 21 +++++++++------------ 10 files changed, 42 insertions(+), 36 deletions(-) diff --git a/flix/cli.ino b/flix/cli.ino index b92f56d..145b80e 100644 --- a/flix/cli.ino +++ b/flix/cli.ino @@ -11,7 +11,7 @@ extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRO extern const int RAW, ACRO, STAB, AUTO; extern float t, dt, loopRate; extern uint16_t channels[16]; -extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode; +extern float controlTime; extern int mode; extern bool armed; @@ -132,6 +132,7 @@ void doCommand(String str, bool echo = false) { } print("\nroll: %g pitch: %g yaw: %g throttle: %g mode: %g\n", controlRoll, controlPitch, controlYaw, controlThrottle, controlMode); + print("time: %.1f\n", controlTime); print("mode: %s\n", getModeName()); print("armed: %d\n", armed); } else if (command == "wifi") { diff --git a/flix/estimate.ino b/flix/estimate.ino index cd8dcb9..a873b9a 100644 --- a/flix/estimate.ino +++ b/flix/estimate.ino @@ -8,6 +8,10 @@ #include "lpf.h" #include "util.h" +Vector rates; // estimated angular rates, rad/s +Quaternion attitude; // estimated attitude +bool landed; + float accWeight = 0.003; LowPassFilter ratesFilter(0.2); // cutoff frequency ~ 40 Hz diff --git a/flix/flix.ino b/flix/flix.ino index 8c0f260..8036b60 100644 --- a/flix/flix.ino +++ b/flix/flix.ino @@ -9,16 +9,13 @@ #define WIFI_ENABLED 1 -float t = NAN; // current step time, s -float dt; // time delta from previous step, s -float controlRoll, controlPitch, controlYaw, controlThrottle; // pilot's inputs, range [-1, 1] -float controlMode = NAN; -Vector gyro; // gyroscope data -Vector acc; // accelerometer data, m/s/s -Vector rates; // filtered angular rates, rad/s -Quaternion attitude; // estimated attitude -bool landed; // are we landed and stationary -float motors[4]; // normalized motors thrust in range [0..1] +extern float t, dt; +extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode; +extern Vector gyro, acc; +extern Vector rates; +extern Quaternion attitude; +extern bool landed; +extern float motors[4]; void setup() { Serial.begin(115200); diff --git a/flix/imu.ino b/flix/imu.ino index 7c482f1..22cf3e3 100644 --- a/flix/imu.ino +++ b/flix/imu.ino @@ -12,9 +12,12 @@ MPU9250 imu(SPI); Vector imuRotation(0, 0, -PI / 2); // imu orientation as Euler angles +Vector gyro; // gyroscope output, rad/s +Vector gyroBias; + +Vector acc; // accelerometer output, m/s/s Vector accBias; Vector accScale(1, 1, 1); -Vector gyroBias; void setupIMU() { print("Setup IMU\n"); diff --git a/flix/mavlink.ino b/flix/mavlink.ino index 4436c92..cb122fc 100644 --- a/flix/mavlink.ino +++ b/flix/mavlink.ino @@ -12,12 +12,11 @@ #define MAVLINK_RATE_SLOW 1 #define MAVLINK_RATE_FAST 10 +extern float controlTime; + bool mavlinkConnected = false; String mavlinkPrintBuffer; -extern float controlTime; -extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode; - void processMavlink() { sendMavlink(); receiveMavlink(); diff --git a/flix/motors.ino b/flix/motors.ino index 6b2af65..3dde15c 100644 --- a/flix/motors.ino +++ b/flix/motors.ino @@ -17,7 +17,8 @@ #define PWM_MIN 0 #define PWM_MAX 1000000 / PWM_FREQUENCY -// Motors array indexes: +float motors[4]; // normalized motor thrusts in range [0..1] + const int MOTOR_REAR_LEFT = 0; const int MOTOR_REAR_RIGHT = 1; const int MOTOR_FRONT_RIGHT = 2; diff --git a/flix/rc.ino b/flix/rc.ino index 21c03ae..5702316 100644 --- a/flix/rc.ino +++ b/flix/rc.ino @@ -6,13 +6,16 @@ #include #include "util.h" -SBUS rc(Serial2); // NOTE: Use RC(Serial2, 16, 17) if you use the old UART2 pins +SBUS rc(Serial2); uint16_t channels[16]; // raw rc channels -float controlTime; // time of the last controls update float channelZero[16]; // calibration zero values float channelMax[16]; // calibration max values +float controlRoll, controlPitch, controlYaw, controlThrottle; // pilot's inputs, range [-1, 1] +float controlMode = NAN; // +float controlTime; // time of the last controls update (0 when no RC) + // Channels mapping (using float to store in parameters): float rollChannel = NAN, pitchChannel = NAN, throttleChannel = NAN, yawChannel = NAN, modeChannel = NAN; @@ -38,11 +41,11 @@ void normalizeRC() { controls[i] = mapf(channels[i], channelZero[i], channelMax[i], 0, 1); } // Update control values - controlRoll = rollChannel >= 0 ? controls[(int)rollChannel] : NAN; - controlPitch = pitchChannel >= 0 ? controls[(int)pitchChannel] : NAN; - controlYaw = yawChannel >= 0 ? controls[(int)yawChannel] : NAN; - controlThrottle = throttleChannel >= 0 ? controls[(int)throttleChannel] : NAN; - controlMode = modeChannel >= 0 ? controls[(int)modeChannel] : NAN; + controlRoll = rollChannel >= 0 ? controls[(int)rollChannel] : 0; + controlPitch = pitchChannel >= 0 ? controls[(int)pitchChannel] : 0; + controlYaw = yawChannel >= 0 ? controls[(int)yawChannel] : 0; + controlThrottle = throttleChannel >= 0 ? controls[(int)throttleChannel] : 0; + controlMode = modeChannel >= 0 ? controls[(int)modeChannel] : NAN; // mode switch should not have affect if not set } void calibrateRC() { diff --git a/flix/time.ino b/flix/time.ino index b57b5bc..f3ba3d1 100644 --- a/flix/time.ino +++ b/flix/time.ino @@ -3,6 +3,8 @@ // Time related functions +float t = NAN; // current time, s +float dt; // time delta with the previous step, s float loopRate; // Hz void step() { diff --git a/flix/vector.h b/flix/vector.h index 272e1db..efc3be0 100644 --- a/flix/vector.h +++ b/flix/vector.h @@ -35,7 +35,6 @@ public: z = NAN; } - float norm() const { return sqrt(x * x + y * y + z * z); } diff --git a/gazebo/flix.h b/gazebo/flix.h index e7d3a2c..91d72c4 100644 --- a/gazebo/flix.h +++ b/gazebo/flix.h @@ -12,17 +12,15 @@ #define WIFI_ENABLED 1 -float t = NAN; -float dt; -float motors[4]; -float controlRoll, controlPitch, controlYaw, controlThrottle = NAN; -float controlMode = NAN; -Vector acc; -Vector gyro; -Vector rates; -Quaternion attitude; -bool landed; -Vector imuRotation; +extern float t, dt; +extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode; +extern Vector rates; +extern Quaternion attitude; +extern bool landed; +extern float motors[4]; + +Vector gyro, acc, imuRotation; +Vector accBias, gyroBias, accScale(1, 1, 1); // declarations void step(); @@ -75,4 +73,3 @@ void calibrateAccel() { print("Skip accel calibrating\n"); }; void printIMUCalibration() { print("cal: N/A\n"); }; void printIMUInfo() {}; void printWiFiInfo() {}; -Vector accBias, gyroBias, accScale(1, 1, 1);