Making an ESP32-based quadcopter from scratch
Python arduinoautopilotcontrol-theorydroneesp32flight-controlflight-controllermavlinkquadcopterquadcopter-firmwareuav
Enable Serial input and output in simulator, refactor
Oleg Kalachevaaa8f70166a6
diff --git a/flix/motors.ino b/flix/motors.inoindex 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.hnew file mode 100644index 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.txtindex 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.hnew file mode 100644index 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.hppdeleted file mode 100644index 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.cppindex 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 stabcontrols[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 yawattitude.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_thrustbody->AddLinkForce(Vector3d(0.0, 0.0, scale0 * maxThrust * abs(mfl)), Vector3d(d, d, 0.0));diff --git a/gazebo/flix.hpp b/gazebo/flix.hppindex 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;// controlvoid 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.hppindex 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 5static 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);}