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

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

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