Merge branch 'master' into cpp

gazebo/Arduino.h

@@ -21,6 +21,8 @@
 #define degrees(rad) ((rad)*RAD_TO_DEG)
 
 #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
+template<typename T> T max(T a, T b) { return a > b ? a : b; }
+template<typename T> T min(T a, T b) { return a < b ? a : b; }
 
 long map(long x, long in_min, long in_max, long out_min, long out_max) {
 	const long run = in_max - in_min;
@@ -149,7 +151,7 @@ public:
 	void setRxInvert(bool invert) {};
 };
 
-HardwareSerial Serial, Serial2;
+HardwareSerial Serial, Serial1, Serial2;
 
 class EspClass {
 public:
@@ -165,6 +167,9 @@ void delay(uint32_t ms) {
 
 bool ledcAttach(uint8_t pin, uint32_t freq, uint8_t resolution) { return true; }
 bool ledcWrite(uint8_t pin, uint32_t duty) { return true; }
+uint32_t ledcChangeFrequency(uint8_t pin, uint32_t freq, uint8_t resolution) { return freq; }
+int8_t digitalPinToAnalogChannel(uint8_t pin) { return -1; }
+uint32_t analogReadMilliVolts(uint8_t pin) { return 0; }
 
 unsigned long __micros;
 unsigned long __resetTime = 0;

gazebo/ESP32_NOW_Serial.h

@@ -0,0 +1,12 @@
+// Dummy file for the simulator
+
+class ESP_NOW_Peer {
+protected:
+	size_t send(const uint8_t *data, int len) { return 0; }
+};
+
+class ESP_NOW_Serial_Class : public ESP_NOW_Peer {
+public:
+	virtual void onSent(bool success) {};
+	virtual size_t write(const uint8_t *data, size_t len) { return 0; };
+};

gazebo/SBUS.h

@@ -13,7 +13,7 @@ class SBUS {
 public:
 	SBUS(HardwareSerial& bus, const bool inv = true) {};
 	SBUS(HardwareSerial& bus, const int8_t rxpin, const int8_t txpin, const bool inv = true) {};
-	void begin() {};
+	void begin(int rxpin = -1, int txpin = -1, bool inv = true, bool fast = false) {};
 	bool read() { return joystickInit(); };
 	SBUSData data() {
 		SBUSData data;

gazebo/flix.h

@@ -9,8 +9,7 @@
 #include "quaternion.h"
 #include "Arduino.h"
 #include "wifi.h"
-
-#define WIFI_ENABLED 1
+#include "lpf.h"
 
 extern float t, dt;
 extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode;
@@ -21,28 +20,33 @@ extern float motors[4];
 
 Vector gyro, acc, imuRotation;
 Vector accBias, gyroBias, accScale(1, 1, 1);
+LowPassFilter<Vector> gyroBiasFilter(0);
 
 // declarations
 void step();
 void computeLoopRate();
 void applyGyro();
 void applyAcc();
+void applyLevel();
 void control();
 void interpretControls();
 void controlAttitude();
 void controlRates();
 void controlTorque();
+void desaturate(float& a, float& b, float& c, float& d);
 const char* getModeName();
 void sendMotors();
+int getDutyCycle(float value);
 bool motorsActive();
 void testMotor(int n);
 void print(const char* format, ...);
 void pause(float duration);
 void doCommand(String str, bool echo);
 void handleInput();
+void setupRC();
 void normalizeRC();
 void calibrateRC();
-void calibrateRCChannel(float *channel, uint16_t zero[16], uint16_t max[16], const char *str);
+void calibrateRCChannel(int*, uint16_t[16], uint16_t[16], const char*);
 void printRCCalibration();
 void printLogHeader();
 void printLogData();
@@ -54,6 +58,7 @@ void handleMavlink(const void *_msg);
 void mavlinkPrint(const char* str);
 void sendMavlinkPrint();
 inline Quaternion fluToFrd(const Quaternion &q);
+void setupPower();
 void failsafe();
 void rcLossFailsafe();
 void descend();
@@ -73,3 +78,4 @@ void calibrateAccel() { print("Skip accel calibrating\n"); };
 void printIMUCalibration() { print("cal: N/A\n"); };
 void printIMUInfo() {};
 void printWiFiInfo() {};
+void configWiFi(bool, const char*, const char*) { print("Skip WiFi config\n"); };

gazebo/simulator.cpp

@@ -60,6 +60,8 @@ public:
 		gyro = Vector(imu->AngularVelocity().X(), imu->AngularVelocity().Y(), imu->AngularVelocity().Z());
 		acc = this->accFilter.update(Vector(imu->LinearAcceleration().X(), imu->LinearAcceleration().Y(), imu->LinearAcceleration().Z()));
 
+		voltage = 4.2f; // dummy voltage value
+
 		readRC();
 		estimate();
 

gazebo/soc/soc.h

@@ -1,4 +1,3 @@
 // Dummy file to make it possible to compile simulator with Flix' util.h
 
-#define WRITE_PERI_REG(addr, val) {}
 #define REG_CLR_BIT(_r, _b) {}

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