Skip to content

armgit / flix

Public Imported from GitHub
0

Making an ESP32-based quadcopter from scratch

Enable Serial input and output in simulator, refactor

Oleg Kalachevaaa8f70166a6
diff --git a/flix/motors.ino b/flix/motors.ino
index 91a5f94..84aa599 100644
--- a/flix/motors.ino
+++ b/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;
}
diff --git a/gazebo/Arduino.h b/gazebo/Arduino.h
new file mode 100644
index 0000000..7826ae6
--- /dev/null
+++ b/gazebo/Arduino.h
@@ -0,0 +1,150 @@
+// Copyright (c) 2023 Oleg Kalachev <[email protected]>
+// 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;
+}
diff --git a/gazebo/CMakeLists.txt b/gazebo/CMakeLists.txt
index c7d6752..dd43bce 100644
--- a/gazebo/CMakeLists.txt
+++ b/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})
diff --git a/gazebo/MPU9250.h b/gazebo/MPU9250.h
new file mode 100644
index 0000000..9f118d9
--- /dev/null
+++ b/gazebo/MPU9250.h
@@ -0,0 +1,17 @@
+// Copyright (c) 2023 Oleg Kalachev <[email protected]>
+// 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; }
+};
diff --git a/gazebo/arduino.hpp b/gazebo/arduino.hpp
deleted file mode 100644
index dc01f29..0000000
--- a/gazebo/arduino.hpp
+++ /dev/null
@@ -1,86 +0,0 @@
-// Copyright (c) 2023 Oleg Kalachev <[email protected]>
-// 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:
-// }
diff --git a/gazebo/flix.cpp b/gazebo/flix.cpp
index e8f6c79..a655add 100644
--- a/gazebo/flix.cpp
+++ b/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));
- // this->model->GetLink("body")->SetTorque(1.0, ignition::math::Vector3d(1.0, 0.0, 0.0);
+ // read imu
+ rates = flu2frd(imu->AngularVelocity());
+ acc = flu2frd(imu->LinearAcceleration());
- // this->model->GetLink("motor0")->AddForce(ignition::math::Vector3d(0.0, 0.0, 1.96/4));
- // this->model->GetLink("motor1")->AddForce(ignition::math::Vector3d(0.0, 0.0, 1.96/4));
- // 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();
+ // read rc
+ joystickGet();
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));
diff --git a/gazebo/flix.hpp b/gazebo/flix.hpp
index 3322edf..120a9f2 100644
--- a/gazebo/flix.hpp
+++ b/gazebo/flix.hpp
@@ -1,41 +1,31 @@
// Copyright (c) 2023 Oleg Kalachev <[email protected]>
// 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;
-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();
+bool calibrating;
// 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() {};
diff --git a/gazebo/turnigy.hpp b/gazebo/turnigy.hpp
index 139087c..3c490f8 100644
--- a/gazebo/turnigy.hpp
+++ b/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 normalizeRC();
-void turnigyInit()
+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) {
- turnigyInit();
+ if (!joystickInitialized) {
+ 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
- // } 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);
- // }
+ controls[i] = 0;
} else {
controls[i] = mapf(channels[i], (channelNeutralMin[i] + channelNeutralMax[i]) / 2, channelMax[i], 0, 1);
}