Enable Serial input and output in simulator, refactor

flix/motors.ino

@@ -128,4 +128,5 @@ static float thrustToMotorInput(uint8_t n, float thrust)
 			return mapff(thrust, 0, 1, motorThrust[n][i-1], motorThrust[n][i]);
 		}
 	}
+	return 1;
 }

gazebo/Arduino.h

@@ -0,0 +1,150 @@
+// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
+// Repository: https://github.com/okalachev/flix
+// Distributed under the MIT License (https://opensource.org/licenses/MIT)
+
+// Partial implementation of Arduino API for simulation
+
+// https://github.com/UFABCRocketDesign/Simulator
+
+#pragma once
+
+#include <iostream>
+#include <cmath>
+#include <string>
+#include <stdio.h>
+#include <unistd.h>
+#include <sys/poll.h>
+
+using std::cout;
+using std::max;
+using std::min;
+using std::isfinite;
+
+#define PI 3.1415926535897932384626433832795
+#define DEG_TO_RAD 0.017453292519943295769236907684886
+#define RAD_TO_DEG 57.295779513082320876798154814105
+
+#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
+
+long map(long x, long in_min, long in_max, long out_min, long out_max) {
+	const long run = in_max - in_min;
+	const long rise = out_max - out_min;
+	const long delta = x - in_min;
+	return (delta * rise) / run + out_min;
+}
+
+class __FlashStringHelper;
+
+// Arduino String partial implementation
+// https://www.arduino.cc/reference/en/language/variables/data-types/stringobject/
+class String: public std::string {
+public:
+	long toInt() { return atol(this->c_str()); }
+	float toFloat() { return atof(this->c_str()); }
+	bool isEmpty() { return this->empty(); }
+};
+
+class Print;
+
+class Printable {
+public:
+	virtual size_t printTo(Print& p) const = 0;
+};
+
+class Print {
+public:
+	size_t printf(const char *format, ...)
+	{
+		va_list args;
+		va_start(args, format);
+		size_t result = vprintf(format, args);
+		va_end(args);
+		return result;
+	}
+
+	size_t print(float n, int digits = 2)
+	{
+		return printf("%.*f", digits, n);
+	}
+
+	size_t println(float n, int digits = 2)
+	{
+		return printf("%.*f\n", digits, n);
+	}
+
+	size_t print(const char* s)
+	{
+		return printf("%s", s);
+	}
+
+	size_t println()
+	{
+		return print("\n");
+	}
+
+	size_t println(const char* s)
+	{
+		return printf("%s\n", s);
+	}
+
+	size_t println(const Printable& p)
+	{
+		return p.printTo(*this) + print("\n");
+	}
+
+	size_t print(const String& s)
+	{
+		return printf("%s", s.c_str());
+	}
+
+	size_t println(const std::string& s)
+	{
+		return printf("%s\n", s.c_str());
+	}
+
+	size_t println(const String& s)
+	{
+		return printf("%s\n", s.c_str());
+	}
+};
+
+class HardwareSerial: public Print {
+public:
+	void begin(unsigned long baud) {
+		// server is running in background by default, so doesn't have access to stdin
+		// https://github.com/gazebosim/gazebo-classic/blob/d45feeb51f773e63960616880b0544770b8d1ad7/gazebo/gazebo_main.cc#L216
+		// set foreground process group to current process group to allow reading from stdin
+		// https://stackoverflow.com/questions/58918188/why-is-stdin-not-propagated-to-child-process-of-different-process-group
+		signal(SIGTTOU, SIG_IGN);
+		tcsetpgrp(STDIN_FILENO, getpgrp());
+		signal(SIGTTOU, SIG_DFL);
+	};
+
+	int available() {
+		// to implement for Windows, see https://stackoverflow.com/a/71992965/6850197
+		struct pollfd pfd = { .fd = STDIN_FILENO, .events = POLLIN };
+		return poll(&pfd, 1, 0) > 0;
+	}
+
+	int read() {
+		if (available()) {
+			char c;
+			::read(STDIN_FILENO, &c, 1); // use raw read to avoid C++ buffering
+			// https://stackoverflow.com/questions/45238997/does-getchar-function-has-its-own-buffer-to-store-remaining-input
+			return c;
+		}
+		return -1;
+	}
+};
+
+HardwareSerial Serial, Serial2;
+
+void delay(uint32_t ms) {
+	std::this_thread::sleep_for(std::chrono::milliseconds(ms));
+}
+
+unsigned long __micros;
+
+unsigned long micros() {
+	return __micros;
+}

gazebo/CMakeLists.txt

@@ -12,5 +12,7 @@ set(FLIX_SOURCE_DIR ../flix)
 include_directories(${FLIX_SOURCE_DIR})
 file(GLOB_RECURSE FLIX_INO_FILES ${FLIX_SOURCE_DIR}/*.ino)
 
+set(CMAKE_BUILD_TYPE RelWithDebInfo)
 add_library(flix SHARED flix.cpp)
 target_link_libraries(flix ${GAZEBO_LIBRARIES} ${SDL2_LIBRARIES})
+target_include_directories(flix PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

gazebo/MPU9250.h

@@ -0,0 +1,17 @@
+// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
+// Repository: https://github.com/okalachev/flix
+// Distributed under the MIT License (https://opensource.org/licenses/MIT)
+
+// Mocks of some MPU9250 library functions
+
+#pragma once
+
+class MPU9250 {
+public:
+	float getAccelBiasX_mss() { return 0; }
+	float getAccelBiasY_mss() { return 0; }
+	float getAccelBiasZ_mss() { return 0; }
+	float getGyroBiasX_rads() { return 0; }
+	float getGyroBiasY_rads() { return 0; }
+	float getGyroBiasZ_rads() { return 0; }
+};

gazebo/arduino.hpp

@@ -1,86 +0,0 @@
-// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
-// Repository: https://github.com/okalachev/flix
-
-#include <iostream>
-#include <cmath>
-#include <string>
-
-using std::cout;
-using std::max;
-using std::min;
-using std::isfinite;
-
-// #define PI 3.1415926535897932384626433832795
-#define DEG_TO_RAD 0.017453292519943295769236907684886
-#define RAD_TO_DEG 57.295779513082320876798154814105
-
-#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
-
-class Print;
-
-class Printable {
-public:
-	virtual size_t printTo(Print& p) const = 0;
-};
-
-class Print {
-public:
-	size_t print(float n, int digits = 2)
-	{
-		cout << n;
-		return 0; // TODO:
-	}
-
-	size_t println(float n, int digits = 2)
-	{
-		print(n, digits);
-		cout << std::endl;
-		return 0;
-	}
-
-	size_t print(const char* s)
-	{
-		cout << s;
-		return 0;
-	}
-
-	size_t println(const char* s)
-	{
-		print(s);
-		cout << std::endl;
-		return 0;
-	}
-
-	size_t println(const Printable& p)
-	{
-		p.printTo(*this);
-		cout << std::endl;
-		return 0;
-	}
-
-	// int available()
-	// {
-	// 	std::string s;
-	// 	s << std::cin;
-	// 	return s.length();
-	// }
-
-	// char read()
-	// {
-	// 	char c;
-	// 	s >> c;
-	// 	return c;
-	// }
-};
-
-class HardwareSerial: public Print {};
-
-HardwareSerial Serial, Serial2;
-
-// gazebo::common::Time curTime = this->dataPtr->model->GetWorld()->SimTime();
-
-// unsigned long micros()
-// {
-// 	// return (unsigned long) (esp_timer_get_time());
-// 	TODO:
-// }

gazebo/flix.cpp

@@ -34,12 +34,15 @@
 #include <iostream>
 #include <fstream>
 
-#include "arduino.hpp"
+#include "Arduino.h"
 #include "flix.hpp"
-#include "turnigy.hpp"
 #include "util.ino"
+#include "time.ino"
+#include "turnigy.hpp"
 #include "estimate.ino"
 #include "control.ino"
+#include "log.ino"
+#include "cli.ino"
 
 using ignition::math::Vector3d;
 using ignition::math::Pose3d;
@@ -52,13 +55,17 @@ Pose3d flu2frd(const Pose3d& p)
 	                              p.Rot().W(), p.Rot().X(), -p.Rot().Y(), -p.Rot().Z());
 }
 
+Vector flu2frd(const Vector3d& v)
+{
+	return Vector(v.X(), -v.Y(), -v.Z());
+}
+
 class ModelFlix : public ModelPlugin
 {
 private:
 	physics::ModelPtr model, estimateModel;
 	physics::LinkPtr body;
 	sensors::ImuSensorPtr imu;
-	common::Time _stepTime;
 	event::ConnectionPtr updateConnection, resetConnection;
 	transport::NodePtr nodeHandle;
 	transport::PublisherPtr motorPub[4];
@@ -78,13 +85,6 @@ public:
 
 		this->estimateModel = model->GetWorld()->ModelByName("flix_estimate");
 
-		// auto scene = rendering::get_scene();
-		// motorVisual[0] = scene->GetVisual("motor0");
-		// motorVisual[1] = scene->GetVisual("motor1");
-		// motorVisual[2] = scene->GetVisual("motor2");
-		// motorVisual[3] = scene->GetVisual("motor3");
-		// motorVisual[0] = model->GetVisual("motor0");
-
 		this->updateConnection = event::Events::ConnectWorldUpdateBegin(
 			std::bind(&ModelFlix::OnUpdate, this));
 
@@ -93,6 +93,8 @@ public:
 
 		initNode();
 
+		Serial.begin(0);
+
 		gzmsg << "Flix plugin loaded" << endl;
 	}
 
@@ -105,92 +107,28 @@ public:
 
 	void OnUpdate()
 	{
-		// this->model->SetLinearVel(ignition::math::Vector3d(.3, 0, 0));
+		__micros = model->GetWorld()->SimTime().Double() * 1000000;
+		step();
 
-		// this->model->GetLink("body")->AddForce(ignition::math::Vector3d(0.0, 0.0, 1.96));
+		// read imu
-		// this->model->GetLink("body")->SetTorque(1.0, ignition::math::Vector3d(1.0, 0.0, 0.0);
+		rates = flu2frd(imu->AngularVelocity());
+		acc = flu2frd(imu->LinearAcceleration());
 
-		// this->model->GetLink("motor0")->AddForce(ignition::math::Vector3d(0.0, 0.0, 1.96/4));
+		// read rc
-		// this->model->GetLink("motor1")->AddForce(ignition::math::Vector3d(0.0, 0.0, 1.96/4));
+		joystickGet();
-		// this->model->GetLink("motor2")->AddForce(ignition::math::Vector3d(0.0, 0.0, 1.96/4));
-		// this->model->GetLink("motor3")->AddForce(ignition::math::Vector3d(0.0, 0.0, 1.96/4))
-
-		// === GROUNDTRUTH ORIENTATION ===
-		// auto pose = flu2frd(this->model->WorldPose());
-		// attitude = Quaternion(pose.Rot().W(), pose.Rot().X(), pose.Rot().Y(), pose.Rot().Z());
-		// auto vel = this->model->RelativeAngularVel();
-		// rates = Vector(vel.X(), -vel.Y(), -vel.Z()); // flu to frd
-
-		// === GROUNDTRUTH POSITION ===
-		// auto pose = flu2frd(this->model->WorldPose());
-		// position = Vector(pose.Pos().X(), pose.Pos().Y(), pose.Pos().Z());
-		// auto vel = this->model->RelativeLinearVel();
-		// velocity = Vector(vel.X(), -vel.Y(), -vel.Z());
-
-		/// == IMU ===
-		auto imuRates = imu->AngularVelocity();
-		rates = Vector(imuRates.X(), -imuRates.Y(), -imuRates.Z()); // flu to frd
-		auto imuAccel = imu->LinearAcceleration();
-		acc = Vector(imuAccel.X(), -imuAccel.Y(), -imuAccel.Z()); // flu to frd
-
-		// gazebo::common::Time curTime = this->dataPtr->model->GetWorld()->SimTime();
-
-		turnigyGet();
 		controls[RC_CHANNEL_MODE] = 1; // 0 acro, 1 stab
 		controls[RC_CHANNEL_AUX] = 1; // armed
-		// std::cout << "yaw: " << yaw << " pitch: " << pitch << " roll: " << roll << " throttle: " << throttle << std::endl;
-
-		if (std::isnan(dt)) {
-			// first step
-			dt = 0;
-		} else {
-			dt = (this->model->GetWorld()->SimTime() - _stepTime).Double();
-		}
-		_stepTime = this->model->GetWorld()->SimTime();
-		stepTime = _stepTime.Double() * 1000000;
-		// std::cout << "dt: " << dt << std::endl;
 
 		estimate();
 
-		// fix yaw
+		// correct yaw to the actual yaw
 		attitude.setYaw(-this->model->WorldPose().Yaw());
-		// Serial.print("attitude: "); Serial.println(attitude);
-		// controlMission();
-		// controlPosition();
-
-		// auto pose = flu2frd(this->model->WorldPose());
 
 		control();
+		parseInput();
+
 		applyMotorsThrust();
 		updateEstimatePose();
-		// autotune();
-
-		/*
-		working:
-		const double max_thrust = 2.2;
-		double thrust = max_thrust * throttle;
-
-		// rate setpoint
-		double rx = roll * 0.1;
-		double ry = -pitch * 0.1;
-		double rz = yaw * 0.1;
-
-		// this->model->GetLink("body")->AddForce(ignition::math::Vector3d(0.0, 0.0, 2.5 * throttle));
-
-		const double d = 0.035355;
-		double front_left = thrust + ry + rx;
-		double front_right = thrust + ry - rx;
-		double rear_left = thrust - ry + rx;
-		double rear_right = thrust - ry - rx;
-
-		if (throttle < 0.1) return;
-
-		this->body->AddLinkForce(ignition::math::Vector3d(0.0, 0.0, front_left), ignition::math::Vector3d(d, d, 0.0));
-		this->body->AddLinkForce(ignition::math::Vector3d(0.0, 0.0, front_right), ignition::math::Vector3d(d, -d, 0.0));
-		this->body->AddLinkForce(ignition::math::Vector3d(0.0, 0.0, rear_left), ignition::math::Vector3d(-d, d, 0.0));
-		this->body->AddLinkForce(ignition::math::Vector3d(0.0, 0.0, rear_right), ignition::math::Vector3d(-d, -d, 0.0));
-		*/
-
 		publishTopics();
 		logData();
 	}
@@ -221,8 +159,6 @@ public:
 		if (motors[MOTOR_REAR_LEFT] < 0.001) mrl = 0;
 		if (motors[MOTOR_REAR_RIGHT] < 0.001) mrr = 0;
 
-		// const float scale0 = 1.0, scale1 = 1.0, scale2 = 1.0, scale3 = 1.0;
-
 		// TODO: min_thrust
 
 		body->AddLinkForce(Vector3d(0.0, 0.0, scale0 * maxThrust * abs(mfl)), Vector3d(d, d, 0.0));

gazebo/flix.hpp

@@ -1,41 +1,31 @@
 // Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
 // Repository: https://github.com/okalachev/flix
 
+// Declarations of some functions and variables in Arduino code
+
 #include <cmath>
 #include "vector.hpp"
 #include "quaternion.hpp"
 
-#define ONE_G 9.807f
-
 #define RC_CHANNELS 6
-// #define RC_CHANNEL_THROTTLE 2
-// #define RC_CHANNEL_YAW 3
-// #define RC_CHANNEL_PITCH 1
-// #define RC_CHANNEL_ROLL 0
-// #define RC_CHANNEL_AUX 4
-// #define RC_CHANNEL_MODE 5
 
 #define MOTOR_REAR_LEFT 0
 #define MOTOR_FRONT_LEFT 3
 #define MOTOR_FRONT_RIGHT 2
 #define MOTOR_REAR_RIGHT 1
 
+uint32_t startTime;
+uint32_t stepTime;
+uint32_t steps;
+float stepsPerSecond;
+float dt;
+float motors[4];
+int16_t channels[16]; // raw rc channels WARNING: unsigned on hardware
+float controls[RC_CHANNELS];
 Vector acc;
 Vector rates;
 Quaternion attitude;
-float dt = NAN;
+bool calibrating;
-float motors[4]; // normalized motors thrust in range [-1..1]
-int16_t channels[16]; // raw rc channels WARNING: unsigned in real life
-float controls[RC_CHANNELS]; // normalized controls in range [-1..1] ([0..1] for thrust)
-uint32_t stepTime;
-
-// util
-float mapf(long x, long in_min, long in_max, float out_min, float out_max);
-float mapff(float x, float in_min, float in_max, float out_min, float out_max);
-// float hypot3(float x, float y, float z);
-
-// rc
-void normalizeRC();
 
 // control
 void control();
@@ -47,5 +37,12 @@ static void controlRate();
 void desaturate(float& a, float& b, float& c, float& d);
 static void indicateSaturation();
 
+// cli
+static void showTable();
+static void cliTestMotor(uint8_t n);
+
 // mocks
-void setLED(bool on) {}
+void setLED(bool on) {};
+void calibrateGyro() {};
+void fullMotorTest(int n) {};
+void sendMotors() {};

gazebo/turnigy.hpp

@@ -12,23 +12,25 @@ static const int16_t channelNeutralMax[] = {-1032,	-258,	-27348,	3353,	0, 0};
 
 static const int16_t channelMax[] =        {27090,	27090,	27090,	27090, 0, 0};
 
+#define RC_CHANNEL_ROLL 0
+#define RC_CHANNEL_PITCH 1
 #define RC_CHANNEL_THROTTLE 2
 #define RC_CHANNEL_YAW 3
-#define RC_CHANNEL_PITCH 1
-#define RC_CHANNEL_ROLL 0
 #define RC_CHANNEL_AUX 4
 #define RC_CHANNEL_MODE 5
 
 static SDL_Joystick *joystick;
 
-bool turnigyInitialized = false, warnShown = false;
+bool joystickInitialized = false, warnShown = false;
 
-void turnigyInit()
+void normalizeRC();
+
+void joystickInit()
 {
 	SDL_Init(SDL_INIT_JOYSTICK);
 	joystick = SDL_JoystickOpen(0);
 	if (joystick != NULL) {
-		turnigyInitialized = true;
+		joystickInitialized = true;
 		gzmsg << "Joystick initialized: " << SDL_JoystickNameForIndex(0) << endl;
 	} else if (!warnShown) {
 		gzwarn << "Joystick not found, begin waiting for joystick..." << endl;
@@ -36,10 +38,10 @@ void turnigyInit()
 	}
 }
 
-void turnigyGet()
+void joystickGet()
 {
-	if (!turnigyInitialized) {
+	if (!joystickInitialized) {
-		turnigyInit();
+		joystickInit();
 		return;
 	}
 
@@ -48,20 +50,16 @@ void turnigyGet()
 	for (uint8_t i = 0; i < 4; i++) {
 		channels[i] = SDL_JoystickGetAxis(joystick, i);
 	}
+	channels[RC_CHANNEL_MODE] = SDL_JoystickGetButton(joystick, 0) ? 1 : 0;
+	controls[RC_CHANNEL_MODE] = channels[RC_CHANNEL_MODE];
 
 	normalizeRC();
 }
 
 void normalizeRC() {
-	for (uint8_t i = 0; i < RC_CHANNELS; i++) {
+	for (uint8_t i = 0; i < 4; i++) {
 		if (channels[i] >= channelNeutralMin[i] && channels[i] <= channelNeutralMax[i]) {
-			controls[i] = 0; // make neutral position strictly equal to 0
+			controls[i] = 0;
-		// } else if (channels[i] > channelNeutralMax[i]) {
-		// 	controls[i] = mapf(channels[i], channelNeutralMax[i], channelMax[i], 0, 1);
-		// } else {
-		// 	float channelMin = channelNeutralMin[i] - (channelMax[i] - channelNeutralMax[i]);
-		// 	controls[i] = mapf(channels[i], channelNeutralMin[i], channelMin, -1, 0);
-		// }
 		} else {
 			controls[i] = mapf(channels[i], (channelNeutralMin[i] + channelNeutralMax[i]) / 2, channelMax[i], 0, 1);
 		}