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

@@ -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") {

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<Vector> ratesFilter(0.2); // cutoff frequency ~ 40 Hz
 

flix/flix.ino

@@ -9,16 +9,13 @@
 
 #define WIFI_ENABLED 1
 
-float t = NAN; // current step time, s
+extern float t, dt;
-float dt; // time delta from previous step, s
+extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode;
-float controlRoll, controlPitch, controlYaw, controlThrottle; // pilot's inputs, range [-1, 1]
+extern Vector gyro, acc;
-float controlMode = NAN;
+extern Vector rates;
-Vector gyro; // gyroscope data
+extern Quaternion attitude;
-Vector acc; // accelerometer data, m/s/s
+extern bool landed;
-Vector rates; // filtered angular rates, rad/s
+extern float motors[4];
-Quaternion attitude; // estimated attitude
-bool landed; // are we landed and stationary
-float motors[4]; // normalized motors thrust in range [0..1]
 
 void setup() {
 	Serial.begin(115200);

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");

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();

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;

flix/rc.ino

@@ -6,13 +6,16 @@
 #include <SBUS.h>
 #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;
+	controlRoll = rollChannel >= 0 ? controls[(int)rollChannel] : 0;
-	controlPitch = pitchChannel >= 0 ? controls[(int)pitchChannel] : NAN;
+	controlPitch = pitchChannel >= 0 ? controls[(int)pitchChannel] : 0;
-	controlYaw = yawChannel >= 0 ? controls[(int)yawChannel] : NAN;
+	controlYaw = yawChannel >= 0 ? controls[(int)yawChannel] : 0;
-	controlThrottle = throttleChannel >= 0 ? controls[(int)throttleChannel] : NAN;
+	controlThrottle = throttleChannel >= 0 ? controls[(int)throttleChannel] : 0;
-	controlMode = modeChannel >= 0 ? controls[(int)modeChannel] : NAN;
+	controlMode = modeChannel >= 0 ? controls[(int)modeChannel] : NAN; // mode switch should not have affect if not set
 }
 
 void calibrateRC() {

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() {

flix/vector.h

@@ -35,7 +35,6 @@ public:
 		z = NAN;
 	}
 
-
 	float norm() const {
 		return sqrt(x * x + y * y + z * z);
 	}

gazebo/flix.h

@@ -12,17 +12,15 @@
 
 #define WIFI_ENABLED 1
 
-float t = NAN;
+extern float t, dt;
-float dt;
+extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode;
-float motors[4];
+extern Vector rates;
-float controlRoll, controlPitch, controlYaw, controlThrottle = NAN;
+extern Quaternion attitude;
-float controlMode = NAN;
+extern bool landed;
-Vector acc;
+extern float motors[4];
-Vector gyro;
+
-Vector rates;
+Vector gyro, acc, imuRotation;
-Quaternion attitude;
+Vector accBias, gyroBias, accScale(1, 1, 1);
-bool landed;
-Vector imuRotation;
 
 // 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);