Add parameters for list of modes bound to rc switch

Parameters: CTL_FLT_MODE_0, CTL_FLT_MODE_1, CTL_FLT_MODE_2.
Also fix a bug with incorrect choosing the mode from controlMode.

flix/control.ino

@@ -52,6 +52,7 @@ PID pitchPID(PITCH_P, PITCH_I, PITCH_D);
 PID yawPID(YAW_P, 0, 0);
 Vector maxRate(ROLLRATE_MAX, PITCHRATE_MAX, YAWRATE_MAX);
 float tiltMax = TILT_MAX;
+int flightModes[] = {STAB, STAB, STAB}; // map for rc mode switch
 
 extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
 extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode;
@@ -65,9 +66,9 @@ void control() {
 }
 
 void interpretControls() {
-	if (controlMode < 0.25) mode = STAB;
-	if (controlMode < 0.75) mode = STAB;
-	if (controlMode > 0.75) mode = STAB;
+	if (controlMode < 0.25) mode = flightModes[0];
+	else if (controlMode < 0.75) mode = flightModes[1];
+	else if (controlMode > 0.75) mode = flightModes[2];
 
 	if (mode == AUTO) return; // pilot is not effective in AUTO mode
 

flix/parameters.ino

@@ -49,6 +49,9 @@ Parameter parameters[] = {
 	{"CTL_R_RATE_MAX", &maxRate.x},
 	{"CTL_Y_RATE_MAX", &maxRate.z},
 	{"CTL_TILT_MAX", &tiltMax},
+	{"CTL_FLT_MODE_0", &flightModes[0]},
+	{"CTL_FLT_MODE_1", &flightModes[1]},
+	{"CTL_FLT_MODE_2", &flightModes[2]},
 	// imu
 	{"IMU_ROT_ROLL", &imuRotation.x},
 	{"IMU_ROT_PITCH", &imuRotation.y},

gazebo/flix.h

@@ -9,6 +9,7 @@
 #include "quaternion.h"
 #include "Arduino.h"
 #include "wifi.h"
+#include "lpf.h"
 
 extern float t, dt;
 extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode;
@@ -19,6 +20,7 @@ extern float motors[4];
 
 Vector gyro, acc, imuRotation;
 Vector accBias, gyroBias, accScale(1, 1, 1);
+LowPassFilter<Vector> gyroBiasFilter(0);
 
 // declarations
 void step();

gazebo/wifi.h

@@ -11,9 +11,10 @@
 #include <sys/poll.h>
 #include <gazebo/gazebo.hh>
 
-#define WIFI_UDP_PORT 14580
-#define WIFI_UDP_REMOTE_PORT 14550
-#define WIFI_UDP_REMOTE_ADDR "255.255.255.255"
+int wifiMode = 1; // mock
+int udpLocalPort = 14580;
+int udpRemotePort = 14550;
+const char *udpRemoteIP = "255.255.255.255";
 
 int wifiSocket;
 
@@ -22,22 +23,22 @@ void setupWiFi() {
 	sockaddr_in addr; // local address
 	addr.sin_family = AF_INET;
 	addr.sin_addr.s_addr = INADDR_ANY;
-	addr.sin_port = htons(WIFI_UDP_PORT);
+	addr.sin_port = htons(udpLocalPort);
 	if (bind(wifiSocket, (sockaddr *)&addr, sizeof(addr))) {
-		gzerr << "Failed to bind WiFi UDP socket on port " << WIFI_UDP_PORT << std::endl;
+		gzerr << "Failed to bind WiFi UDP socket on port " << udpLocalPort << std::endl;
 		return;
 	}
 	int broadcast = 1;
 	setsockopt(wifiSocket, SOL_SOCKET, SO_BROADCAST, &broadcast, sizeof(broadcast)); // enable broadcast
-	gzmsg << "WiFi UDP socket initialized on port " << WIFI_UDP_PORT << " (remote port " << WIFI_UDP_REMOTE_PORT << ")" << std::endl;
+	gzmsg << "WiFi UDP socket initialized on port " << udpLocalPort << " (remote port " << udpRemotePort << ")" << std::endl;
 }
 
 void sendWiFi(const uint8_t *buf, int len) {
 	if (wifiSocket == 0) setupWiFi();
 	sockaddr_in addr; // remote address
 	addr.sin_family = AF_INET;
-	addr.sin_addr.s_addr = inet_addr(WIFI_UDP_REMOTE_ADDR);
-	addr.sin_port = htons(WIFI_UDP_REMOTE_PORT);
+	addr.sin_addr.s_addr = inet_addr(udpRemoteIP);
+	addr.sin_port = htons(udpRemotePort);
 	sendto(wifiSocket, buf, len, 0, (sockaddr *)&addr, sizeof(addr));
 }