From 0661aecccfb9b6ff038a77d561bac82831224122 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sun, 4 Feb 2024 14:42:57 +0300 Subject: [PATCH 01/39] Remove unneeded INVERT_SERIAL define --- flix/rc.ino | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/flix/rc.ino b/flix/rc.ino index ce7e439..014fe6a 100644 --- a/flix/rc.ino +++ b/flix/rc.ino @@ -5,8 +5,6 @@ #include -#define INVERT_SERIAL true // false if external SBUS invertor is used - // NOTE: use `cr` command and replace with the actual values int channelNeutral[] = {995, 883, 200, 972, 512, 512}; int channelMax[] = {1651, 1540, 1713, 1630, 1472, 1472}; @@ -16,7 +14,7 @@ SBUS RC(Serial2); void setupRC() { Serial.println("Setup RC"); RC.begin(); - Serial2.setRxInvert(INVERT_SERIAL); + Serial2.setRxInvert(true); // SBUS uses inverted signal } void readRC() { From 31d382dd86f9ccbf0793cc23d1bcc83e65b5faae Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 6 Feb 2024 10:49:48 +0300 Subject: [PATCH 02/39] Simplify motors pwm calculation using unified value for all motors --- flix/motors.ino | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/flix/motors.ino b/flix/motors.ino index 937d8e4..1f21bfd 100644 --- a/flix/motors.ino +++ b/flix/motors.ino @@ -9,16 +9,13 @@ #define MOTOR_1_PIN 13 #define MOTOR_2_PIN 14 #define MOTOR_3_PIN 15 - #define PWM_FREQUENCY 200 #define PWM_RESOLUTION 8 - #define PWM_NEUTRAL 1500 - -const uint16_t pwmMin[] = {1600, 1600, 1600, 1600}; -const uint16_t pwmMax[] = {2300, 2300, 2300, 2300}; -const uint16_t pwmReverseMin[] = {1390, 1440, 1440, 1440}; -const uint16_t pwmReverseMax[] = {1100, 1100, 1100, 1100}; +#define PWM_MIN 1600 +#define PWM_MAX 2300 +#define PWM_REVERSE_MIN 1440 +#define PWM_REVERSE_MAX 1100 void setupMotors() { Serial.println("Setup Motors"); @@ -43,9 +40,9 @@ uint16_t getPWM(float val, int n) { if (val == 0) { return PWM_NEUTRAL; } else if (val > 0) { - return mapff(val, 0, 1, pwmMin[n], pwmMax[n]); + return mapff(val, 0, 1, PWM_MIN, PWM_MAX); } else { - return mapff(val, 0, -1, pwmReverseMin[n], pwmReverseMax[n]); + return mapff(val, 0, -1, PWM_REVERSE_MIN, PWM_REVERSE_MAX); } } From 410fccf0154ffbf7e874cf63297d2eff215d2cbc Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 6 Feb 2024 13:50:56 +0300 Subject: [PATCH 03/39] Fix vector, quaternion, pid and lpf libraries curly braces code style --- flix/lpf.h | 9 +++------ flix/pid.h | 6 ++---- flix/quaternion.h | 51 ++++++++++++++++------------------------------- flix/vector.h | 42 +++++++++++++------------------------- 4 files changed, 36 insertions(+), 72 deletions(-) diff --git a/flix/lpf.h b/flix/lpf.h index 937078f..56c231e 100644 --- a/flix/lpf.h +++ b/flix/lpf.h @@ -13,8 +13,7 @@ public: LowPassFilter(float alpha): alpha(alpha) {}; - T update(const T input) - { + T update(const T input) { if (alpha == 1) { // filter disabled return input; } @@ -26,13 +25,11 @@ public: return output = output * (1 - alpha) + input * alpha; } - void setCutOffFrequency(float cutOffFreq, float dt) - { + void setCutOffFrequency(float cutOffFreq, float dt) { alpha = 1 - exp(-2 * PI * cutOffFreq * dt); } - void reset() - { + void reset() { initialized = false; } diff --git a/flix/pid.h b/flix/pid.h index a330349..cdfbcae 100644 --- a/flix/pid.h +++ b/flix/pid.h @@ -21,8 +21,7 @@ public: PID(float p, float i, float d, float windup = 0, float dAlpha = 1) : p(p), i(i), d(d), windup(windup), lpf(dAlpha) {}; - float update(float error, float dt) - { + float update(float error, float dt) { integral += error * dt; if (isfinite(prevError) && dt > 0) { @@ -38,8 +37,7 @@ public: return p * error + constrain(i * integral, -windup, windup) + d * derivative; // PID } - void reset() - { + void reset() { prevError = NAN; integral = 0; derivative = 0; diff --git a/flix/quaternion.h b/flix/quaternion.h index 115eeab..8aee072 100644 --- a/flix/quaternion.h +++ b/flix/quaternion.h @@ -15,8 +15,7 @@ public: Quaternion(float w, float x, float y, float z): w(w), x(x), y(y), z(z) {}; - static Quaternion fromAxisAngle(float a, float b, float c, float angle) - { + static Quaternion fromAxisAngle(float a, float b, float c, float angle) { float halfAngle = angle * 0.5; float sin2 = sin(halfAngle); float cos2 = cos(halfAngle); @@ -24,16 +23,14 @@ public: return Quaternion(cos2, a * sinNorm, b * sinNorm, c * sinNorm); } - static Quaternion fromAngularRates(const Vector& rates) - { + static Quaternion fromAngularRates(const Vector& rates) { if (rates.zero()) { return Quaternion(); } return Quaternion::fromAxisAngle(rates.x, rates.y, rates.z, rates.norm()); } - static Quaternion fromEulerZYX(float x, float y, float z) - { + static Quaternion fromEulerZYX(float x, float y, float z) { float cx = cos(x / 2); float cy = cos(y / 2); float cz = cos(z / 2); @@ -48,8 +45,7 @@ public: cx * cy * sz - sx * sy * cz); } - static Quaternion fromBetweenVectors(Vector u, Vector v) - { + static Quaternion fromBetweenVectors(Vector u, Vector v) { float dot = u.x * v.x + u.y * v.y + u.z * v.z; float w1 = u.y * v.z - u.z * v.y; float w2 = u.z * v.x - u.x * v.z; @@ -64,24 +60,21 @@ public: return ret; } - void toAxisAngle(float& a, float& b, float& c, float& angle) - { + void toAxisAngle(float& a, float& b, float& c, float& angle) { angle = acos(w) * 2; a = x / sin(angle / 2); b = y / sin(angle / 2); c = z / sin(angle / 2); } - Vector toEulerZYX() const - { + Vector toEulerZYX() const { return Vector( atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y)), asin(2 * (w * y - z * x)), atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z))); } - float getYaw() const - { + float getYaw() const { // https://github.com/ros/geometry2/blob/589caf083cae9d8fae7effdb910454b4681b9ec1/tf2/include/tf2/impl/utils.h#L122 float yaw; float sqx = x * x; @@ -101,15 +94,13 @@ public: return yaw; } - void setYaw(float yaw) - { + void setYaw(float yaw) { // TODO: optimize? Vector euler = toEulerZYX(); (*this) = Quaternion::fromEulerZYX(euler.x, euler.y, yaw); } - Quaternion& operator *= (const Quaternion& q) - { + Quaternion& operator *= (const Quaternion& q) { Quaternion ret( w * q.w - x * q.x - y * q.y - z * q.z, w * q.x + x * q.w + y * q.z - z * q.y, @@ -118,8 +109,7 @@ public: return (*this = ret); } - Quaternion operator * (const Quaternion& q) - { + Quaternion operator * (const Quaternion& q) { return Quaternion( w * q.w - x * q.x - y * q.y - z * q.z, w * q.x + x * q.w + y * q.z - z * q.y, @@ -127,8 +117,7 @@ public: w * q.z + z * q.w + x * q.y - y * q.x); } - Quaternion inversed() const - { + Quaternion inversed() const { float normSqInv = 1 / (w * w + x * x + y * y + z * z); return Quaternion( w * normSqInv, @@ -137,13 +126,11 @@ public: -z * normSqInv); } - float norm() const - { + float norm() const { return sqrt(w * w + x * x + y * y + z * z); } - void normalize() - { + void normalize() { float n = norm(); w /= n; x /= n; @@ -151,27 +138,23 @@ public: z /= n; } - Vector conjugate(const Vector& v) - { + Vector conjugate(const Vector& v) { Quaternion qv(0, v.x, v.y, v.z); Quaternion res = (*this) * qv * inversed(); return Vector(res.x, res.y, res.z); } - Vector conjugateInversed(const Vector& v) - { + Vector conjugateInversed(const Vector& v) { Quaternion qv(0, v.x, v.y, v.z); Quaternion res = inversed() * qv * (*this); return Vector(res.x, res.y, res.z); } - inline Vector rotate(const Vector& v) - { + inline Vector rotate(const Vector& v) { return conjugateInversed(v); } - inline bool finite() const - { + inline bool finite() const { return isfinite(w) && isfinite(x) && isfinite(y) && isfinite(z); } diff --git a/flix/vector.h b/flix/vector.h index 5ed57a5..2f25e42 100644 --- a/flix/vector.h +++ b/flix/vector.h @@ -13,76 +13,62 @@ public: Vector(float x, float y, float z): x(x), y(y), z(z) {}; - float norm() const - { + float norm() const { return sqrt(x * x + y * y + z * z); } - bool zero() const - { + bool zero() const { return x == 0 && y == 0 && z == 0; } - void normalize() - { + void normalize() { float n = norm(); x /= n; y /= n; z /= n; } - Vector operator * (const float b) const - { + Vector operator * (const float b) const { return Vector(x * b, y * b, z * b); } - Vector operator / (const float b) const - { + Vector operator / (const float b) const { return Vector(x / b, y / b, z / b); } - Vector operator + (const Vector& b) const - { + Vector operator + (const Vector& b) const { return Vector(x + b.x, y + b.y, z + b.z); } - Vector operator - (const Vector& b) const - { + Vector operator - (const Vector& b) const { return Vector(x - b.x, y - b.y, z - b.z); } - inline bool operator == (const Vector& b) const - { + inline bool operator == (const Vector& b) const { return x == b.x && y == b.y && z == b.z; } - inline bool operator != (const Vector& b) const - { + inline bool operator != (const Vector& b) const { return !(*this == b); } - inline bool finite() const - { + inline bool finite() const { return isfinite(x) && isfinite(y) && isfinite(z); } - static float dot(const Vector& a, const Vector& b) - { + static float dot(const Vector& a, const Vector& b) { return a.x * b.x + a.y * b.y + a.z * b.z; } - static Vector cross(const Vector& a, const Vector& b) - { + static Vector cross(const Vector& a, const Vector& b) { return Vector(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x); } - static float angleBetweenVectors(const Vector& a, const Vector& b) - { + static float angleBetweenVectors(const Vector& a, const Vector& b) { return acos(constrain(dot(a, b) / (a.norm() * b.norm()), -1, 1)); } - static Vector angularRatesBetweenVectors(const Vector& u, const Vector& v) - { + static Vector angularRatesBetweenVectors(const Vector& u, const Vector& v) { Vector direction = cross(u, v); direction.normalize(); float angle = angleBetweenVectors(u, v); From ba6e63b50b224bccf88739de90a071cff1f14371 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 6 Feb 2024 21:02:20 +0300 Subject: [PATCH 04/39] Correctly set output parameters of simulated SBUS::read, minor name fix --- flix/rc.ino | 6 +++--- gazebo/SBUS.h | 4 +++- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/flix/rc.ino b/flix/rc.ino index 014fe6a..8927a21 100644 --- a/flix/rc.ino +++ b/flix/rc.ino @@ -18,9 +18,9 @@ void setupRC() { } void readRC() { - bool failSafe, lostFrame; - if (RC.read(channels, &failSafe, &lostFrame)) { - if (failSafe) { return; } // TODO: + bool failsafe, lostFrame; + if (RC.read(channels, &failsafe, &lostFrame)) { + if (failsafe) { return; } // TODO: if (lostFrame) { return; } normalizeRC(); } diff --git a/gazebo/SBUS.h b/gazebo/SBUS.h index 307a8ba..afc7559 100644 --- a/gazebo/SBUS.h +++ b/gazebo/SBUS.h @@ -9,7 +9,9 @@ class SBUS { public: SBUS(HardwareSerial& bus) {}; void begin() {}; - bool read(int16_t* channels, bool* failsafe, bool* lostFrame) { // NOTE: on the hardware channels is uint16_t + bool read(int16_t *channels, bool *failsafe, bool *lostFrame) { // NOTE: on the hardware channels is uint16_t + *failsafe = false; + *lostFrame = false; return joystickGet(); }; }; From 33319db1fa1f6ba56e4cf9ca9dc430a85591a643 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 7 Feb 2024 10:49:31 +0300 Subject: [PATCH 05/39] Make rates LPF cut-off frequency equal to 40 Hz --- flix/control.ino | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flix/control.ino b/flix/control.ino index 5bc327b..225e592 100644 --- a/flix/control.ino +++ b/flix/control.ino @@ -32,7 +32,7 @@ #define YAWRATE_MAX radians(360) #define MAX_TILT radians(30) -#define RATES_LFP_ALPHA 0.8 // cutoff frequency ~ 250 Hz +#define RATES_LFP_ALPHA 0.2 // cutoff frequency ~ 40 Hz #define RATES_D_LPF_ALPHA 0.2 // cutoff frequency ~ 40 Hz enum { MANUAL, ACRO, STAB } mode = STAB; From bf803cf345fed5ea817f928496ab8d7cca51de6e Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sat, 10 Feb 2024 14:12:09 +0300 Subject: [PATCH 06/39] Display MAVLink remote port in simulator --- gazebo/wifi.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gazebo/wifi.h b/gazebo/wifi.h index fe0a8d5..5d55b03 100644 --- a/gazebo/wifi.h +++ b/gazebo/wifi.h @@ -25,7 +25,7 @@ void setupWiFi() { bind(wifiSocket, (sockaddr *)&addr, sizeof(addr)); int broadcast = 1; setsockopt(wifiSocket, SOL_SOCKET, SO_BROADCAST, &broadcast, sizeof(broadcast)); // enable broadcast - gzmsg << "WiFi UDP socket initialized on port " << WIFI_UDP_PORT_LOCAL << std::endl; + gzmsg << "WiFi UDP socket initialized on port " << WIFI_UDP_PORT_LOCAL << " (remote port " << WIFI_UDP_PORT_REMOTE << ")" << std::endl; } void sendWiFi(const uint8_t *buf, int len) { From e0db3bee383c0c4488297291a885d6dadf0c320e Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Fri, 16 Feb 2024 01:13:32 +0300 Subject: [PATCH 07/39] Read mode stick using axis read in simulation --- gazebo/joystick.h | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/gazebo/joystick.h b/gazebo/joystick.h index 04db142..6cf9ce4 100644 --- a/gazebo/joystick.h +++ b/gazebo/joystick.h @@ -8,15 +8,15 @@ #include // simulation calibration overrides, NOTE: use `cr` command and replace with the actual values -const int channelNeutralOverride[] = {-258, -258, -27349, 0, 0, 1}; -const int channelMaxOverride[] = {27090, 27090, 27090, 27090, 0, 1}; +const int channelNeutralOverride[] = {-258, -258, -27349, 0, 5417, 0}; +const int channelMaxOverride[] = {27090, 27090, 27090, 27090, 27090, 1}; #define RC_CHANNEL_ROLL 0 #define RC_CHANNEL_PITCH 1 #define RC_CHANNEL_THROTTLE 2 #define RC_CHANNEL_YAW 3 -#define RC_CHANNEL_ARMED 4 -#define RC_CHANNEL_MODE 5 +#define RC_CHANNEL_ARMED 5 +#define RC_CHANNEL_MODE 4 SDL_Joystick *joystick; bool joystickInitialized = false, warnShown = false; @@ -49,10 +49,8 @@ bool joystickGet() { SDL_JoystickUpdate(); - for (uint8_t i = 0; i < 4; i++) { + for (uint8_t i = 0; i < 8; 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]; return true; } From 4eec63adfa84157893b6f5fbd85535fb25c8bad5 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sat, 17 Feb 2024 22:17:27 +0300 Subject: [PATCH 08/39] Add info about input group for joystick usage in building instructions --- docs/build.md | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/docs/build.md b/docs/build.md index 4a5caf9..ad46ab2 100644 --- a/docs/build.md +++ b/docs/build.md @@ -36,7 +36,13 @@ cd flix sudo apt-get update && sudo apt-get install build-essential libsdl2-dev ``` -4. Run the simulation: +4. Add your user to the `input` group to enable joystick support (you need to re-login after this command): + + ```bash + sudo usermod -a -G input $USER + ``` + +5. Run the simulation: ```bash make simulator From 455729fdb4ac41c4dc4efd5621a484d09d0fba18 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sun, 18 Feb 2024 01:23:33 +0300 Subject: [PATCH 09/39] Improve log download: remove empty records, sort by timestamp To make Plotjuggler not to warn about unsorted records everytime --- flix/log.ino | 1 + tools/grab_log.py | 11 ++++++++--- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/flix/log.ino b/flix/log.ino index e412cb4..7892254 100644 --- a/flix/log.ino +++ b/flix/log.ino @@ -44,6 +44,7 @@ void dumpLog() { Serial.printf("t,rates.x,rates.y,rates.z,ratesTarget.x,ratesTarget.y,ratesTarget.z," "attitude.x,attitude.y,attitude.z,attitudeTarget.x,attitudeTarget.y,attitudeTarget.z,thrustTarget\n"); for (int i = 0; i < LOG_SIZE; i++) { + if (logBuffer[i][0] == 0) continue; // skip empty records for (int j = 0; j < LOG_COLUMNS - 1; j++) { Serial.printf("%f,", logBuffer[i][j]); } diff --git a/tools/grab_log.py b/tools/grab_log.py index 620ea08..c1e1a94 100755 --- a/tools/grab_log.py +++ b/tools/grab_log.py @@ -9,8 +9,7 @@ PORT = os.environ['PORT'] DIR = os.path.dirname(os.path.realpath(__file__)) dev = serial.Serial(port=PORT, baudrate=115200, timeout=0.5) - -log = open(f'{DIR}/log/{datetime.datetime.now().isoformat()}.csv', 'wb') +lines = [] print('Downloading log...') count = 0 @@ -19,8 +18,14 @@ while True: line = dev.readline() if not line: break - log.write(line) + lines.append(line) count += 1 print(f'\r{count} lines', end='') +# sort by timestamp +header = lines.pop(0) +lines.sort(key=lambda line: float(line.split(b',')[0])) + +log = open(f'{DIR}/log/{datetime.datetime.now().isoformat()}.csv', 'wb') +log.writelines([header] + lines) print(f'\nWritten {os.path.relpath(log.name, os.curdir)}') From 85182ac2b82504e419f3bbd83a8da08abf2e81ba Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 20 Feb 2024 04:47:13 +0300 Subject: [PATCH 10/39] =?UTF-8?q?Use=20more=20correct=20implementation=20o?= =?UTF-8?q?f=20toEulerZYX=20fixing=20some=20yaw=20issues=20We=20actually?= =?UTF-8?q?=20need=20to=20use=20Tait=E2=80=93Bryan=20Z-Y-X=20angles,=20not?= =?UTF-8?q?=20classic=20Euler's?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- flix/quaternion.h | 26 ++++++++++++++++++++++---- 1 file changed, 22 insertions(+), 4 deletions(-) diff --git a/flix/quaternion.h b/flix/quaternion.h index 8aee072..c535d63 100644 --- a/flix/quaternion.h +++ b/flix/quaternion.h @@ -68,10 +68,28 @@ public: } Vector toEulerZYX() const { - return Vector( - atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y)), - asin(2 * (w * y - z * x)), - atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z))); + // https://github.com/ros/geometry2/blob/589caf083cae9d8fae7effdb910454b4681b9ec1/tf2/include/tf2/impl/utils.h#L87 + Vector euler; + float sqx = x * x; + float sqy = y * y; + float sqz = z * z; + float sqw = w * w; + // Cases derived from https://orbitalstation.wordpress.com/tag/quaternion/ + float sarg = -2 * (x * z - w * y) / (sqx + sqy + sqz + sqw); /* normalization added from urdfom_headers */ + if (sarg <= -0.99999) { + euler.x = 0; + euler.y = -0.5 * PI; + euler.z = -2 * atan2(y, x); + } else if (sarg >= 0.99999) { + euler.x = 0; + euler.y = 0.5 * PI; + euler.z = 2 * atan2(y, x); + } else { + euler.x = atan2(2 * (y * z + w * x), sqw - sqx - sqy + sqz); + euler.y = asin(sarg); + euler.z = atan2(2 * (x * y + w * z), sqw + sqx - sqy - sqz); + } + return euler; } float getYaw() const { From 5ec6b5e6651f41b2c704e1c15cdc6b7166668ac9 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 20 Feb 2024 04:51:59 +0300 Subject: [PATCH 11/39] Make fromEulerZYX accept Vector instead of x, y, z --- flix/control.ino | 4 ++-- flix/quaternion.h | 19 +++++++++---------- 2 files changed, 11 insertions(+), 12 deletions(-) diff --git a/flix/control.ino b/flix/control.ino index 225e592..3f97a7f 100644 --- a/flix/control.ino +++ b/flix/control.ino @@ -89,10 +89,10 @@ void interpretRC() { } else if (mode == STAB) { yawMode = controls[RC_CHANNEL_YAW] == 0 ? YAW : YAW_RATE; - attitudeTarget = Quaternion::fromEulerZYX( + attitudeTarget = Quaternion::fromEulerZYX(Vector( controls[RC_CHANNEL_ROLL] * MAX_TILT, -controls[RC_CHANNEL_PITCH] * MAX_TILT, - attitudeTarget.getYaw()); + attitudeTarget.getYaw())); ratesTarget.z = controls[RC_CHANNEL_YAW] * YAWRATE_MAX; } else if (mode == MANUAL) { diff --git a/flix/quaternion.h b/flix/quaternion.h index c535d63..07d8504 100644 --- a/flix/quaternion.h +++ b/flix/quaternion.h @@ -30,13 +30,13 @@ public: return Quaternion::fromAxisAngle(rates.x, rates.y, rates.z, rates.norm()); } - static Quaternion fromEulerZYX(float x, float y, float z) { - float cx = cos(x / 2); - float cy = cos(y / 2); - float cz = cos(z / 2); - float sx = sin(x / 2); - float sy = sin(y / 2); - float sz = sin(z / 2); + static Quaternion fromEulerZYX(const Vector& euler) { + float cx = cos(euler.x / 2); + float cy = cos(euler.y / 2); + float cz = cos(euler.z / 2); + float sx = sin(euler.x / 2); + float sy = sin(euler.y / 2); + float sz = sin(euler.z / 2); return Quaternion( cx * cy * cz + sx * sy * sz, @@ -99,9 +99,7 @@ public: float sqy = y * y; float sqz = z * z; float sqw = w * w; - double sarg = -2 * (x * z - w * y) / (sqx + sqy + sqz + sqw); - if (sarg <= -0.99999) { yaw = -2 * atan2(y, x); } else if (sarg >= 0.99999) { @@ -115,7 +113,8 @@ public: void setYaw(float yaw) { // TODO: optimize? Vector euler = toEulerZYX(); - (*this) = Quaternion::fromEulerZYX(euler.x, euler.y, yaw); + euler.z = yaw; + (*this) = Quaternion::fromEulerZYX(euler); } Quaternion& operator *= (const Quaternion& q) { From 5b6ef9c50e1310d847a11cbb9196e27cd487c86b Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 21 Feb 2024 18:24:35 +0300 Subject: [PATCH 12/39] Add warning about shaft diameter for the motors --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 1b49a0e..ad11385 100644 --- a/README.md +++ b/README.md @@ -49,7 +49,7 @@ You can also check a user contributed [variant of complete circuit diagram](http |ESP32 Mini|Microcontroller board||1| |GY-91|IMU+LDO+barometer board||1| |K100|Quadcopter frame||1| -|8520 3.7V brushed motor|Motor||4| +|8520 3.7V brushed motor (**shaft 0.8mm!**)|Motor||4| |Hubsan 55 mm| Propeller||4| |2.7A 1S Dual Way Micro Brush ESC|Motor ESC||4| |KINGKONG TINY X8|RC transmitter||1| From ab2f99ab59d7f42d1c5d6919bf63a53a0e75deda Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Thu, 22 Feb 2024 03:09:12 +0300 Subject: [PATCH 13/39] Simplify making user modes for control, add USER mode --- flix/control.ino | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/flix/control.ino b/flix/control.ino index 3f97a7f..ce0f0d6 100644 --- a/flix/control.ino +++ b/flix/control.ino @@ -35,7 +35,7 @@ #define RATES_LFP_ALPHA 0.2 // cutoff frequency ~ 40 Hz #define RATES_D_LPF_ALPHA 0.2 // cutoff frequency ~ 40 Hz -enum { MANUAL, ACRO, STAB } mode = STAB; +enum { MANUAL, ACRO, STAB, USER } mode = STAB; enum { YAW, YAW_RATE } yawMode = YAW; bool armed = false; @@ -68,6 +68,8 @@ void control() { } void interpretRC() { + armed = controls[RC_CHANNEL_THROTTLE] >= 0.05 && controls[RC_CHANNEL_ARMED] >= 0.5; + // NOTE: put ACRO or MANUAL modes there if you want to use them if (controls[RC_CHANNEL_MODE] < 0.25) { mode = STAB; @@ -77,7 +79,6 @@ void interpretRC() { mode = STAB; } - armed = controls[RC_CHANNEL_THROTTLE] >= 0.05 && controls[RC_CHANNEL_ARMED] >= 0.5; thrustTarget = controls[RC_CHANNEL_THROTTLE]; if (mode == ACRO) { From 1c9b10a674acf809329786bcab49bd968809e63e Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sat, 24 Feb 2024 15:28:41 +0300 Subject: [PATCH 14/39] Use default recommended chip-select pin (GPIO5) for SPI Update link to the schematics #3 to the most recent version --- README.md | 2 +- flix/imu.ino | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index ad11385..f72552d 100644 --- a/README.md +++ b/README.md @@ -38,7 +38,7 @@ Simulation in Gazebo using a plugin that runs original Arduino code is implement Flix schematics -You can also check a user contributed [variant of complete circuit diagram](https://github.com/okalachev/flix/issues/3#issue-2066079898) of the drone. +You can also check a user contributed [variant of complete circuit diagram](https://miro.com/app/board/uXjVN-dTjoo=/) of the drone. *\* — SBUS inverter is not needed as ESP32 supports [software pin inversion](https://github.com/bolderflight/sbus#inverted-serial).* diff --git a/flix/imu.ino b/flix/imu.ino index acbade5..9d0107b 100644 --- a/flix/imu.ino +++ b/flix/imu.ino @@ -6,10 +6,9 @@ #include #include -#define IMU_CS_PIN 4 // chip-select pin for IMU SPI connection #define LOAD_GYRO_CAL false -MPU9250 IMU(SPI, IMU_CS_PIN); +MPU9250 IMU(SPI, SS); void setupIMU() { Serial.println("Setup IMU, stand still"); From 34a81536c252529bf7b59d725ff027acc05e8977 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sat, 2 Mar 2024 15:37:38 +0300 Subject: [PATCH 15/39] Fix reverse motors pwm --- flix/motors.ino | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/flix/motors.ino b/flix/motors.ino index 1f21bfd..91fc367 100644 --- a/flix/motors.ino +++ b/flix/motors.ino @@ -14,8 +14,8 @@ #define PWM_NEUTRAL 1500 #define PWM_MIN 1600 #define PWM_MAX 2300 -#define PWM_REVERSE_MIN 1440 -#define PWM_REVERSE_MAX 1100 +#define PWM_REVERSE_MIN 1100 +#define PWM_REVERSE_MAX 1440 void setupMotors() { Serial.println("Setup Motors"); From aeec8e34eb59f991351b0ef9f9b616e3fa7f57eb Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sun, 3 Mar 2024 21:08:43 +0300 Subject: [PATCH 16/39] Add auto-center throttle setting notice to QGC usage documentation --- docs/build.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/build.md b/docs/build.md index ad46ab2..34b3319 100644 --- a/docs/build.md +++ b/docs/build.md @@ -73,7 +73,7 @@ cd flix ### Flight -Use USB remote control or QGroundControl mobile app (with *Virtual Joystick* setting enabled) to control the drone. +Use USB remote control or QGroundControl mobile app (with *Virtual Joystick* setting enabled) to control the drone. *Auto-Center Throttle* setting **should be disabled**. ## Firmware From d752cce0ccc6b5199467b99f887264bd88c39ddb Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 12 Mar 2024 00:36:53 +0300 Subject: [PATCH 17/39] Fix accel calibration upside down wait time --- flix/imu.ino | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flix/imu.ino b/flix/imu.ino index 9d0107b..f5754ff 100644 --- a/flix/imu.ino +++ b/flix/imu.ino @@ -65,7 +65,7 @@ void calibrateAccel() { IMU.calibrateAccel(); Serial.println("Cal accel: place on left side"); delay(3000); IMU.calibrateAccel(); - Serial.println("Cal accel: upside down"); delay(300); + Serial.println("Cal accel: upside down"); delay(3000); IMU.calibrateAccel(); printIMUCal(); } From 2cf1c7abb3d671275a7f1914e16d991ca707a49a Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Fri, 15 Mar 2024 10:38:48 +0300 Subject: [PATCH 18/39] Use FlixPeriph library for IMU, implement own IMU calibration --- Makefile | 2 +- docs/build.md | 2 +- flix/flix.ino | 3 +- flix/imu.ino | 144 ++++++++++++++++++++++++++++++-------------------- flix/vector.h | 10 ++++ 5 files changed, 100 insertions(+), 61 deletions(-) diff --git a/Makefile b/Makefile index 58e5f7d..f1434a8 100644 --- a/Makefile +++ b/Makefile @@ -16,7 +16,7 @@ dependencies .dependencies: arduino-cli core install esp32:esp32@2.0.11 --config-file arduino-cli.yaml arduino-cli lib update-index arduino-cli lib install "Bolder Flight Systems SBUS"@1.0.1 - arduino-cli lib install "Bolder Flight Systems MPU9250"@1.0.2 + arduino-cli lib install "FlixPeriph" arduino-cli lib install "MAVLink"@2.0.1 touch .dependencies diff --git a/docs/build.md b/docs/build.md index 34b3319..12e1bfd 100644 --- a/docs/build.md +++ b/docs/build.md @@ -83,7 +83,7 @@ Use USB remote control or QGroundControl mobile app (with *Virtual Joystick* set 2. Install ESP32 core using [Boards Manager](https://docs.arduino.cc/learn/starting-guide/cores). 3. Install the following libraries using [Library Manager](https://docs.arduino.cc/software/ide-v2/tutorials/ide-v2-installing-a-library) (**versions are significant**): * `Bolder Flight Systems SBUS`, version 1.0.1. - * `Bolder Flight Systems MPU9250`, version 1.0.2. + * `FlixPeriph`, version 1.0.0. * `MAVLink`, version 2.0.1. 4. Clone the project using git or [download the source code as a ZIP archive](https://codeload.github.com/okalachev/flix/zip/refs/heads/master). 5. Open the downloaded Arduino sketch `flix/flix.ino` in Arduino IDE. diff --git a/flix/flix.ino b/flix/flix.ino index e01da35..5e4358b 100644 --- a/flix/flix.ino +++ b/flix/flix.ino @@ -51,8 +51,7 @@ void setup() { } void loop() { - if (!readIMU()) return; - + readIMU(); step(); readRC(); estimate(); diff --git a/flix/imu.ino b/flix/imu.ino index f5754ff..5576e80 100644 --- a/flix/imu.ino +++ b/flix/imu.ino @@ -6,86 +6,116 @@ #include #include -#define LOAD_GYRO_CAL false +#define ONE_G 9.80665 -MPU9250 IMU(SPI, SS); +// NOTE: use 'ca' command to calibrate the accelerometer and put the values here +Vector accBias(0, 0, 0); +Vector accScale(1, 1, 1); + +MPU9250 IMU(SPI, 4); +Vector gyroBias; void setupIMU() { - Serial.println("Setup IMU, stand still"); - - auto status = IMU.begin(); - if (status < 0) { + Serial.println("Setup IMU"); + bool status = IMU.begin(); + if (!status) { while (true) { - Serial.printf("IMU begin error: %d\n", status); + Serial.println("IMU begin error"); delay(1000); } } - - if (LOAD_GYRO_CAL) loadGyroCal(); - loadAccelCal(); - - IMU.setSrd(0); // set sample rate to 1000 Hz - // NOTE: very important, without the above the rate would be terrible 50 Hz + calibrateGyro(); } -bool readIMU() { - if (IMU.readSensor() < 0) { - Serial.println("IMU read error"); - return false; - } +void configureIMU() { + IMU.setAccelRange(IMU.ACCEL_RANGE_4G); + IMU.setGyroRange(IMU.GYRO_RANGE_500DPS); + IMU.setDlpfBandwidth(IMU.DLPF_BANDWIDTH_184HZ); + IMU.setSrd(0); // set sample rate to 1000 Hz +} - auto lastRates = rates; - - rates.x = IMU.getGyroX_rads(); - rates.y = IMU.getGyroY_rads(); - rates.z = IMU.getGyroZ_rads(); - acc.x = IMU.getAccelX_mss(); - acc.y = IMU.getAccelY_mss(); - acc.z = IMU.getAccelZ_mss(); - - return rates != lastRates; +void readIMU() { + IMU.waitForData(); + IMU.getGyro(rates.x, rates.y, rates.z); + IMU.getAccel(acc.x, acc.y, acc.z); + // apply scale and bias + acc = (acc - accBias) / accScale; + rates = rates - gyroBias; } void calibrateGyro() { + const int samples = 1000; Serial.println("Calibrating gyro, stand still"); - int status = IMU.calibrateGyro(); - Serial.printf("Calibration status: %d\n", status); - IMU.setSrd(0); + IMU.setGyroRange(IMU.GYRO_RANGE_250DPS); // the most sensitive mode + + gyroBias = Vector(0, 0, 0); + for (int i = 0; i < samples; i++) { + IMU.waitForData(); + IMU.getGyro(rates.x, rates.y, rates.z); + gyroBias = gyroBias + rates; + } + gyroBias = gyroBias / samples; + printIMUCal(); + configureIMU(); } void calibrateAccel() { - Serial.println("Cal accel: place level"); delay(3000); - IMU.calibrateAccel(); - Serial.println("Cal accel: place nose up"); delay(3000); - IMU.calibrateAccel(); - Serial.println("Cal accel: place nose down"); delay(3000); - IMU.calibrateAccel(); - Serial.println("Cal accel: place on right side"); delay(3000); - IMU.calibrateAccel(); - Serial.println("Cal accel: place on left side"); delay(3000); - IMU.calibrateAccel(); - Serial.println("Cal accel: upside down"); delay(3000); - IMU.calibrateAccel(); + Serial.println("Calibrating accelerometer"); + IMU.setAccelRange(IMU.ACCEL_RANGE_2G); + IMU.setDlpfBandwidth(IMU.DLPF_BANDWIDTH_20HZ); + IMU.setSrd(19); + Serial.setTimeout(60000); + Serial.print("Place level [enter] "); Serial.readStringUntil('\n'); + calibrateAccelOnce(); + Serial.print("Place nose up [enter] "); Serial.readStringUntil('\n'); + calibrateAccelOnce(); + Serial.print("Place nose down [enter] "); Serial.readStringUntil('\n'); + calibrateAccelOnce(); + Serial.print("Place on right side [enter] "); Serial.readStringUntil('\n'); + calibrateAccelOnce(); + Serial.print("Place on left side [enter] "); Serial.readStringUntil('\n'); + calibrateAccelOnce(); + Serial.print("Place upside down [enter] "); Serial.readStringUntil('\n'); + calibrateAccelOnce(); printIMUCal(); + IMU.setAccelRange(IMU.ACCEL_RANGE_16G); + IMU.setDlpfBandwidth(IMU.DLPF_BANDWIDTH_184HZ); + IMU.setSrd(0); } -void loadAccelCal() { - // NOTE: this should be changed to the actual values - IMU.setAccelCalX(-0.0048542023, 1.0008112192); - IMU.setAccelCalY(0.0521845818, 0.9985780716); - IMU.setAccelCalZ(0.5754694939, 1.0045746565); -} +void calibrateAccelOnce() { + const int samples = 100; + static Vector accMax(-INFINITY, -INFINITY, -INFINITY); + static Vector accMin(INFINITY, INFINITY, INFINITY); -void loadGyroCal() { - // NOTE: this should be changed to the actual values - IMU.setGyroBiasX_rads(-0.0185128022); - IMU.setGyroBiasY_rads(-0.0262369743); - IMU.setGyroBiasZ_rads(0.0163032326); + // Compute the average of the accelerometer readings + acc = Vector(0, 0, 0); + for (int i = 0; i < samples; i++) { + IMU.waitForData(); + Vector sample; + IMU.getAccel(sample.x, sample.y, sample.z); + acc = acc + sample; + } + acc = acc / samples; + + // Update the maximum and minimum values + if (acc.x > accMax.x) accMax.x = acc.x; + if (acc.y > accMax.y) accMax.y = acc.y; + if (acc.z > accMax.z) accMax.z = acc.z; + if (acc.x < accMin.x) accMin.x = acc.x; + if (acc.y < accMin.y) accMin.y = acc.y; + if (acc.z < accMin.z) accMin.z = acc.z; + Serial.printf("acc %f %f %f\n", acc.x, acc.y, acc.z); + Serial.printf("max %f %f %f\n", accMax.x, accMax.y, accMax.z); + Serial.printf("min %f %f %f\n", accMin.x, accMin.y, accMin.z); + // Compute scale and bias + accScale = (accMax - accMin) / 2 / ONE_G; + accBias = (accMax + accMin) / 2; } void printIMUCal() { - Serial.printf("gyro bias: %f %f %f\n", IMU.getGyroBiasX_rads(), IMU.getGyroBiasY_rads(), IMU.getGyroBiasZ_rads()); - Serial.printf("accel bias: %f %f %f\n", IMU.getAccelBiasX_mss(), IMU.getAccelBiasY_mss(), IMU.getAccelBiasZ_mss()); - Serial.printf("accel scale: %f %f %f\n", IMU.getAccelScaleFactorX(), IMU.getAccelScaleFactorY(), IMU.getAccelScaleFactorZ()); + Serial.printf("gyro bias: %f %f %f\n", gyroBias.x, gyroBias.y, gyroBias.z); + Serial.printf("accel bias: %f %f %f\n", accBias.x, accBias.y, accBias.z); + Serial.printf("accel scale: %f %f %f\n", accScale.x, accScale.y, accScale.z); } diff --git a/flix/vector.h b/flix/vector.h index 2f25e42..7602ac5 100644 --- a/flix/vector.h +++ b/flix/vector.h @@ -44,6 +44,16 @@ public: return Vector(x - b.x, y - b.y, z - b.z); } + // Element-wise multiplication + Vector operator * (const Vector& b) const { + return Vector(x * b.x, y * b.y, z * b.z); + } + + // Element-wise division + Vector operator / (const Vector& b) const { + return Vector(x / b.x, y / b.y, z / b.z); + } + inline bool operator == (const Vector& b) const { return x == b.x && y == b.y && z == b.z; } From 32f29dc1a4cf78230d0eedc31302c988476c4305 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Fri, 15 Mar 2024 13:14:28 +0300 Subject: [PATCH 19/39] Use default SPI CS pin for IMU --- flix/imu.ino | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flix/imu.ino b/flix/imu.ino index 5576e80..99d2ffe 100644 --- a/flix/imu.ino +++ b/flix/imu.ino @@ -12,7 +12,7 @@ Vector accBias(0, 0, 0); Vector accScale(1, 1, 1); -MPU9250 IMU(SPI, 4); +MPU9250 IMU(SPI); Vector gyroBias; void setupIMU() { From f782f647cb3efef27a21df7a4040de99437ce607 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Fri, 15 Mar 2024 13:26:59 +0300 Subject: [PATCH 20/39] Correctly restore IMU settings after accel calibration --- flix/imu.ino | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/flix/imu.ino b/flix/imu.ino index 99d2ffe..8f34fe8 100644 --- a/flix/imu.ino +++ b/flix/imu.ino @@ -62,9 +62,10 @@ void calibrateGyro() { void calibrateAccel() { Serial.println("Calibrating accelerometer"); - IMU.setAccelRange(IMU.ACCEL_RANGE_2G); + IMU.setAccelRange(IMU.ACCEL_RANGE_2G); // the most sensitive mode IMU.setDlpfBandwidth(IMU.DLPF_BANDWIDTH_20HZ); IMU.setSrd(19); + Serial.setTimeout(60000); Serial.print("Place level [enter] "); Serial.readStringUntil('\n'); calibrateAccelOnce(); @@ -78,10 +79,9 @@ void calibrateAccel() { calibrateAccelOnce(); Serial.print("Place upside down [enter] "); Serial.readStringUntil('\n'); calibrateAccelOnce(); + printIMUCal(); - IMU.setAccelRange(IMU.ACCEL_RANGE_16G); - IMU.setDlpfBandwidth(IMU.DLPF_BANDWIDTH_184HZ); - IMU.setSrd(0); + configureIMU(); } void calibrateAccelOnce() { From 646fa46f6bbd1b811bd005975b3b3319256a0322 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sun, 17 Mar 2024 02:29:37 +0300 Subject: [PATCH 21/39] Use FlixPeriph library for SBUS --- Makefile | 1 - docs/build.md | 5 ++--- flix/flix.ino | 2 +- flix/rc.ino | 10 ++++------ gazebo/SBUS.h | 15 +++++++++++---- gazebo/flix.h | 2 +- 6 files changed, 19 insertions(+), 16 deletions(-) diff --git a/Makefile b/Makefile index f1434a8..45a1edf 100644 --- a/Makefile +++ b/Makefile @@ -15,7 +15,6 @@ dependencies .dependencies: arduino-cli core update-index --config-file arduino-cli.yaml arduino-cli core install esp32:esp32@2.0.11 --config-file arduino-cli.yaml arduino-cli lib update-index - arduino-cli lib install "Bolder Flight Systems SBUS"@1.0.1 arduino-cli lib install "FlixPeriph" arduino-cli lib install "MAVLink"@2.0.1 touch .dependencies diff --git a/docs/build.md b/docs/build.md index 12e1bfd..a970289 100644 --- a/docs/build.md +++ b/docs/build.md @@ -81,9 +81,8 @@ Use USB remote control or QGroundControl mobile app (with *Virtual Joystick* set 1. Install [Arduino IDE](https://www.arduino.cc/en/software) (version 2 is recommended). 2. Install ESP32 core using [Boards Manager](https://docs.arduino.cc/learn/starting-guide/cores). -3. Install the following libraries using [Library Manager](https://docs.arduino.cc/software/ide-v2/tutorials/ide-v2-installing-a-library) (**versions are significant**): - * `Bolder Flight Systems SBUS`, version 1.0.1. - * `FlixPeriph`, version 1.0.0. +3. Install the following libraries using [Library Manager](https://docs.arduino.cc/software/ide-v2/tutorials/ide-v2-installing-a-library): + * `FlixPeriph`. * `MAVLink`, version 2.0.1. 4. Clone the project using git or [download the source code as a ZIP archive](https://codeload.github.com/okalachev/flix/zip/refs/heads/master). 5. Open the downloaded Arduino sketch `flix/flix.ino` in Arduino IDE. diff --git a/flix/flix.ino b/flix/flix.ino index 5e4358b..5ab7ffb 100644 --- a/flix/flix.ino +++ b/flix/flix.ino @@ -26,7 +26,7 @@ float t = NAN; // current step time, s float dt; // time delta from previous step, s float loopFreq; // loop frequency, Hz -uint16_t channels[16]; // raw rc channels +int16_t channels[16]; // raw rc channels float controls[RC_CHANNELS]; // normalized controls in range [-1..1] ([0..1] for throttle) Vector rates; // angular rates, rad/s Vector acc; // accelerometer data, m/s/s diff --git a/flix/rc.ino b/flix/rc.ino index 8927a21..69f7574 100644 --- a/flix/rc.ino +++ b/flix/rc.ino @@ -5,7 +5,7 @@ #include -// NOTE: use `cr` command and replace with the actual values +// NOTE: use 'cr' command to calibrate the RC and put the values here int channelNeutral[] = {995, 883, 200, 972, 512, 512}; int channelMax[] = {1651, 1540, 1713, 1630, 1472, 1472}; @@ -14,14 +14,12 @@ SBUS RC(Serial2); void setupRC() { Serial.println("Setup RC"); RC.begin(); - Serial2.setRxInvert(true); // SBUS uses inverted signal } void readRC() { - bool failsafe, lostFrame; - if (RC.read(channels, &failsafe, &lostFrame)) { - if (failsafe) { return; } // TODO: - if (lostFrame) { return; } + if (RC.read()) { + SBUSData data = RC.data(); + memcpy(channels, data.ch, sizeof(channels)); // copy channels data normalizeRC(); } } diff --git a/gazebo/SBUS.h b/gazebo/SBUS.h index afc7559..ca9c9ec 100644 --- a/gazebo/SBUS.h +++ b/gazebo/SBUS.h @@ -5,13 +5,20 @@ #include "joystick.h" +struct SBUSData { + int16_t ch[16]; +}; + class SBUS { public: SBUS(HardwareSerial& bus) {}; void begin() {}; - bool read(int16_t *channels, bool *failsafe, bool *lostFrame) { // NOTE: on the hardware channels is uint16_t - *failsafe = false; - *lostFrame = false; - return joystickGet(); + bool read() { return joystickGet(); }; + SBUSData data() { + SBUSData data; + for (uint8_t i = 0; i < 16; i++) { + data.ch[i] = channels[i]; + } + return data; }; }; diff --git a/gazebo/flix.h b/gazebo/flix.h index a8ce214..193e751 100644 --- a/gazebo/flix.h +++ b/gazebo/flix.h @@ -23,7 +23,7 @@ float t = NAN; float dt; float loopFreq; float motors[4]; -int16_t channels[16]; // raw rc channels WARNING: unsigned on hardware +int16_t channels[16]; // raw rc channels float controls[RC_CHANNELS]; Vector acc; Vector rates; From fff7262d1bb5c3657cab2ba480ca9db5483a8ac4 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sat, 23 Mar 2024 09:20:08 +0300 Subject: [PATCH 22/39] Minor fix for SBUS dummy for simulator --- gazebo/SBUS.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gazebo/SBUS.h b/gazebo/SBUS.h index ca9c9ec..8d56d45 100644 --- a/gazebo/SBUS.h +++ b/gazebo/SBUS.h @@ -11,7 +11,7 @@ struct SBUSData { class SBUS { public: - SBUS(HardwareSerial& bus) {}; + SBUS(HardwareSerial& bus, const bool inv = true) {}; void begin() {}; bool read() { return joystickGet(); }; SBUSData data() { From 28a6bf2230a574b0aed46e5c1f41764d8ce5763e Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sat, 30 Mar 2024 18:23:42 +0300 Subject: [PATCH 23/39] Add info about motors testing commands to intro message --- flix/cli.ino | 1 + 1 file changed, 1 insertion(+) diff --git a/flix/cli.ino b/flix/cli.ino index b4c9a5e..3660534 100644 --- a/flix/cli.ino +++ b/flix/cli.ino @@ -30,6 +30,7 @@ const char* motd = "cr - calibrate RC\n" "cg - calibrate gyro\n" "ca - calibrate accel\n" +"mfr, mfl, mrr, mrl - test appropriate motor\n" "fullmot - full motor test\n" "reset - reset drone's state\n"; From d095b81d7e36045b18438ade2dfd2070aae72fdb Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 2 Apr 2024 22:28:02 +0300 Subject: [PATCH 24/39] Print out loop frequency on imu command --- flix/cli.ino | 1 + 1 file changed, 1 insertion(+) diff --git a/flix/cli.ino b/flix/cli.ino index 3660534..e92df50 100644 --- a/flix/cli.ino +++ b/flix/cli.ino @@ -73,6 +73,7 @@ void doCommand(String& command, String& value) { Serial.printf("gyro: %f %f %f\n", rates.x, rates.y, rates.z); Serial.printf("acc: %f %f %f\n", acc.x, acc.y, acc.z); printIMUCal(); + Serial.printf("frequency: %f\n", loopFreq); } else if (command == "rc") { Serial.printf("Raw: throttle %d yaw %d pitch %d roll %d armed %d mode %d\n", channels[RC_CHANNEL_THROTTLE], channels[RC_CHANNEL_YAW], channels[RC_CHANNEL_PITCH], From fb80b899e0603b7623d528b4f2f17f2d3fed8354 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 9 Apr 2024 02:45:21 +0300 Subject: [PATCH 25/39] Refine Gazebo installation instructions for macOS --- docs/build.md | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/docs/build.md b/docs/build.md index a970289..9ea0fc8 100644 --- a/docs/build.md +++ b/docs/build.md @@ -65,6 +65,13 @@ cd flix brew install sdl2 ``` + Set up your Gazebo environment variables: + + ```bash + echo "source /opt/homebrew/share/gazebo/setup.sh" >> ~/.zshrc + source ~/.zshrc + ``` + 3. Run the simulation: ```bash From 24e856990504b5fe36664dbc5418b9d585cff3f1 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sat, 20 Apr 2024 10:57:32 +0300 Subject: [PATCH 26/39] Make Vector methods arguments more consistent --- flix/vector.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/flix/vector.h b/flix/vector.h index 7602ac5..c1433e1 100644 --- a/flix/vector.h +++ b/flix/vector.h @@ -78,10 +78,10 @@ public: return acos(constrain(dot(a, b) / (a.norm() * b.norm()), -1, 1)); } - static Vector angularRatesBetweenVectors(const Vector& u, const Vector& v) { - Vector direction = cross(u, v); + static Vector angularRatesBetweenVectors(const Vector& a, const Vector& b) { + Vector direction = cross(a, b); direction.normalize(); - float angle = angleBetweenVectors(u, v); + float angle = angleBetweenVectors(a, b); return direction * angle; } From 41a9a95747a4137ef7f7b699e7f98612792d9273 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sat, 20 Apr 2024 14:52:01 +0300 Subject: [PATCH 27/39] Transfer gyro low pass filter to estimate.ino Separate raw gyro data and filtered rates to different variables --- docs/firmware.md | 5 +++-- flix/control.ino | 6 +----- flix/estimate.ino | 9 ++++++++- flix/flix.ino | 3 ++- flix/imu.ino | 8 ++++---- flix/mavlink.ino | 2 +- gazebo/flix.h | 1 + gazebo/simulator.cpp | 2 +- 8 files changed, 21 insertions(+), 15 deletions(-) diff --git a/docs/firmware.md b/docs/firmware.md index 9c74c43..b073d68 100644 --- a/docs/firmware.md +++ b/docs/firmware.md @@ -8,9 +8,10 @@ The main loop is running at 1000 Hz. All the dataflow is happening through globa * `t` *(float)* — current step time, *s*. * `dt` *(float)* — time delta between the current and previous steps, *s*. -* `rates` *(Vector)* — angular rates from the gyroscope, *rad/s*. +* `gyro` *(Vector)* — data from the gyroscope, *rad/s*. * `acc` *(Vector)* — acceleration data from the accelerometer, *m/s2*. -* `attitude` *(Quaternion)* — current estimated attitude (orientation) of drone. +* `rates` *(Vector)* — filtered angular rates, *rad/s*. +* `attitude` *(Quaternion)* — estimated attitude (orientation) of drone. * `controls` *(float[])* — user control inputs from the RC, normalized to [-1, 1] range. * `motors` *(float[])* — motor outputs, normalized to [-1, 1] range; reverse rotation is possible. diff --git a/flix/control.ino b/flix/control.ino index ce0f0d6..dc524bf 100644 --- a/flix/control.ino +++ b/flix/control.ino @@ -32,7 +32,6 @@ #define YAWRATE_MAX radians(360) #define MAX_TILT radians(30) -#define RATES_LFP_ALPHA 0.2 // cutoff frequency ~ 40 Hz #define RATES_D_LPF_ALPHA 0.2 // cutoff frequency ~ 40 Hz enum { MANUAL, ACRO, STAB, USER } mode = STAB; @@ -46,8 +45,6 @@ PID rollPID(ROLL_P, ROLL_I, ROLL_D); PID pitchPID(PITCH_P, PITCH_I, PITCH_D); PID yawPID(YAW_P, 0, 0); -LowPassFilter ratesFilter(RATES_LFP_ALPHA); - Quaternion attitudeTarget; Vector ratesTarget; Vector torqueTarget; @@ -138,8 +135,7 @@ void controlRate() { return; } - Vector ratesFiltered = ratesFilter.update(rates); - Vector error = ratesTarget - ratesFiltered; + Vector error = ratesTarget - rates; // Calculate desired torque, where 0 - no torque, 1 - maximum possible torque torqueTarget.x = rollRatePID.update(error.x, dt); diff --git a/flix/estimate.ino b/flix/estimate.ino index 598a1ec..6824d32 100644 --- a/flix/estimate.ino +++ b/flix/estimate.ino @@ -5,9 +5,13 @@ #include "quaternion.h" #include "vector.h" +#include "lpf.h" #define ONE_G 9.807f #define WEIGHT_ACC 0.5f +#define RATES_LFP_ALPHA 0.2 // cutoff frequency ~ 40 Hz + +LowPassFilter ratesFilter(RATES_LFP_ALPHA); void estimate() { applyGyro(); @@ -16,7 +20,10 @@ void estimate() { } void applyGyro() { - // applying gyro + // filter gyro to get angular rates + rates = ratesFilter.update(gyro); + + // apply rates to attitude attitude *= Quaternion::fromAngularRates(rates * dt); attitude.normalize(); } diff --git a/flix/flix.ino b/flix/flix.ino index 5ab7ffb..92aeb79 100644 --- a/flix/flix.ino +++ b/flix/flix.ino @@ -28,8 +28,9 @@ float dt; // time delta from previous step, s float loopFreq; // loop frequency, Hz int16_t channels[16]; // raw rc channels float controls[RC_CHANNELS]; // normalized controls in range [-1..1] ([0..1] for throttle) -Vector rates; // angular rates, rad/s +Vector gyro; // gyroscope data Vector acc; // accelerometer data, m/s/s +Vector rates; // filtered angular rates, rad/s Quaternion attitude; // estimated attitude float motors[4]; // normalized motors thrust in range [-1..1] diff --git a/flix/imu.ino b/flix/imu.ino index 8f34fe8..24852b2 100644 --- a/flix/imu.ino +++ b/flix/imu.ino @@ -36,11 +36,11 @@ void configureIMU() { void readIMU() { IMU.waitForData(); - IMU.getGyro(rates.x, rates.y, rates.z); + IMU.getGyro(gyro.x, gyro.y, gyro.z); IMU.getAccel(acc.x, acc.y, acc.z); // apply scale and bias acc = (acc - accBias) / accScale; - rates = rates - gyroBias; + gyro = gyro - gyroBias; } void calibrateGyro() { @@ -51,8 +51,8 @@ void calibrateGyro() { gyroBias = Vector(0, 0, 0); for (int i = 0; i < samples; i++) { IMU.waitForData(); - IMU.getGyro(rates.x, rates.y, rates.z); - gyroBias = gyroBias + rates; + IMU.getGyro(gyro.x, gyro.y, gyro.z); + gyroBias = gyroBias + gyro; } gyroBias = gyroBias / samples; diff --git a/flix/mavlink.ino b/flix/mavlink.ino index 64bcd07..d4c6480 100644 --- a/flix/mavlink.ino +++ b/flix/mavlink.ino @@ -55,7 +55,7 @@ void sendMavlink() { mavlink_msg_scaled_imu_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, time, acc.x * 1000, acc.y * 1000, acc.z * 1000, - rates.x * 1000, rates.y * 1000, rates.z * 1000, + gyro.x * 1000, gyro.y * 1000, gyro.z * 1000, 0, 0, 0, 0); sendMessage(&msg); } diff --git a/gazebo/flix.h b/gazebo/flix.h index 193e751..14dc727 100644 --- a/gazebo/flix.h +++ b/gazebo/flix.h @@ -26,6 +26,7 @@ float motors[4]; int16_t channels[16]; // raw rc channels float controls[RC_CHANNELS]; Vector acc; +Vector gyro; Vector rates; Quaternion attitude; diff --git a/gazebo/simulator.cpp b/gazebo/simulator.cpp index f681513..44a4481 100644 --- a/gazebo/simulator.cpp +++ b/gazebo/simulator.cpp @@ -64,7 +64,7 @@ public: step(); // read imu - rates = flu2frd(imu->AngularVelocity()); + gyro = flu2frd(imu->AngularVelocity()); acc = this->accFilter.update(flu2frd(imu->LinearAcceleration())); // read rc From cfb2e60310b170fbde50615fb670fcc3e55784c0 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 1 May 2024 02:33:44 +0300 Subject: [PATCH 28/39] Add correct rules to yml files editor config --- .editorconfig | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.editorconfig b/.editorconfig index 9a231d1..2e6af90 100644 --- a/.editorconfig +++ b/.editorconfig @@ -9,3 +9,7 @@ charset = utf-8 indent_style = tab tab_width = 4 trim_trailing_whitespace = true + +[*.yml] +indent_style = space +indent_size = 2 From 6392c4a97a33fb5a6dde52fbd82b530f01c5bbf4 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 1 May 2024 02:35:30 +0300 Subject: [PATCH 29/39] Update dataflow diagram to reflect newly introduced gyro variable --- docs/img/dataflow.svg | 505 ++++++++++++++++-------------------------- 1 file changed, 186 insertions(+), 319 deletions(-) diff --git a/docs/img/dataflow.svg b/docs/img/dataflow.svg index de00c8d..5413a39 100644 --- a/docs/img/dataflow.svg +++ b/docs/img/dataflow.svg @@ -2,181 +2,19 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - rc.ino - - - rc.ino - - - estimate.ino - - - estimate.ino - - - imu.ino - - - imu.ino - - - control.ino - - - control.ino - - - motors.ino - - - motors.ino - - - - - rates, acc - - - - gyro, acccontrols[16] - - - - controls[16]attitude - - - rates, attitude - - - - - - - - - - - - - motors - - - motors - - - - + transform="matrix(1.3333333,0,0,-1.3333333,0,1440)" /> From a383c83a29c54fdc15c0d046a281a489ce13e965 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Thu, 2 May 2024 21:22:13 +0300 Subject: [PATCH 30/39] Minor update to .editorconfig --- .editorconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.editorconfig b/.editorconfig index 2e6af90..0dd4b38 100644 --- a/.editorconfig +++ b/.editorconfig @@ -10,6 +10,6 @@ indent_style = tab tab_width = 4 trim_trailing_whitespace = true -[*.yml] +[{*.yml,*.yaml,CMakeLists.txt}] indent_style = space indent_size = 2 From 751627913221a924865ad1737d06c753198a825b Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Fri, 10 May 2024 22:28:42 +0300 Subject: [PATCH 31/39] Add requirements.txt for tools --- tools/requirements.txt | 2 ++ 1 file changed, 2 insertions(+) create mode 100644 tools/requirements.txt diff --git a/tools/requirements.txt b/tools/requirements.txt new file mode 100644 index 0000000..7b4771a --- /dev/null +++ b/tools/requirements.txt @@ -0,0 +1,2 @@ +docopt +matplotlib From 28da7baf61763f3c9d883ec946bbdd599f5c32fa Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Fri, 17 May 2024 07:59:18 +0300 Subject: [PATCH 32/39] Add link to Habr article to readme --- README.md | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index f72552d..27f0045 100644 --- a/README.md +++ b/README.md @@ -62,6 +62,8 @@ You can also check a user contributed [variant of complete circuit diagram](http *\* — not needed as ESP32 supports [software pin inversion](https://github.com/bolderflight/sbus#inverted-serial).* -## Telegram-channel +## Materials Subscribe to Telegram-channel on developing the drone and the flight controller (in Russian): https://t.me/opensourcequadcopter. + +Detailed article on Habr.com about the development of the drone (in Russian): https://habr.com/ru/articles/814127/. From b91f4d3b6d12b84b7dbec7d3c8168564100a20ab Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 21 May 2024 10:34:05 +0300 Subject: [PATCH 33/39] Install arduino-cli to /usr/local/bin The Arduino docs probably has mistake offering non-existent ~/local/bin path instead of ~/.local/bin. Some systems lack ~/.local/bin as well, so simply use /usr/local/bin. Also install arduino-cli in CI the same way as in the docs to check them. --- .github/workflows/build.yml | 2 +- docs/build.md | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 704b6f1..eedd6a7 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -12,7 +12,7 @@ jobs: steps: - uses: actions/checkout@v3 - name: Install Arduino CLI - uses: arduino/setup-arduino-cli@v1.1.1 + run: curl -fsSL https://raw.githubusercontent.com/arduino/arduino-cli/master/install.sh | BINDIR=/usr/local/bin sh - name: Build firmware run: make diff --git a/docs/build.md b/docs/build.md index 9ea0fc8..bf53514 100644 --- a/docs/build.md +++ b/docs/build.md @@ -14,7 +14,7 @@ cd flix 1. Install Arduino CLI: ```bash - curl -fsSL https://raw.githubusercontent.com/arduino/arduino-cli/master/install.sh | BINDIR=~/local/bin sh + curl -fsSL https://raw.githubusercontent.com/arduino/arduino-cli/master/install.sh | BINDIR=/usr/local/bin sh ``` 2. Install Gazebo 11: From ad6bc02643749ded136a04970b97d451b4d02e9f Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 21 May 2024 10:49:57 +0300 Subject: [PATCH 34/39] Minor fixes and changes --- .gitignore | 2 +- README.md | 4 ++-- flix/control.ino | 1 + flix/quaternion.h | 1 + gazebo/joystick.h | 4 ++-- 5 files changed, 7 insertions(+), 5 deletions(-) diff --git a/.gitignore b/.gitignore index 0ee1924..f7a56be 100644 --- a/.gitignore +++ b/.gitignore @@ -1,5 +1,5 @@ *.hex *.elf -gazebo/build/ +build/ tools/log/ .dependencies diff --git a/README.md b/README.md index 27f0045..1318881 100644 --- a/README.md +++ b/README.md @@ -57,8 +57,8 @@ You can also check a user contributed [variant of complete circuit diagram](http ||~~SBUS inverter~~*||~~1~~| |3.7 Li-Po 850 MaH 60C|Battery||| ||Battery charger||1| -||Wires, connectors, tape, ...|| -||3D-printed frame parts|| +||Wires, connectors, tape, ...||| +||3D-printed frame parts||| *\* — not needed as ESP32 supports [software pin inversion](https://github.com/bolderflight/sbus#inverted-serial).* diff --git a/flix/control.ino b/flix/control.ino index dc524bf..2d18175 100644 --- a/flix/control.ino +++ b/flix/control.ino @@ -169,6 +169,7 @@ const char* getModeName() { case MANUAL: return "MANUAL"; case ACRO: return "ACRO"; case STAB: return "STAB"; + case USER: return "USER"; default: return "UNKNOWN"; } } diff --git a/flix/quaternion.h b/flix/quaternion.h index 07d8504..c644156 100644 --- a/flix/quaternion.h +++ b/flix/quaternion.h @@ -167,6 +167,7 @@ public: return Vector(res.x, res.y, res.z); } + // Rotate vector by quaternion inline Vector rotate(const Vector& v) { return conjugateInversed(v); } diff --git a/gazebo/joystick.h b/gazebo/joystick.h index 6cf9ce4..5294d77 100644 --- a/gazebo/joystick.h +++ b/gazebo/joystick.h @@ -8,8 +8,8 @@ #include // simulation calibration overrides, NOTE: use `cr` command and replace with the actual values -const int channelNeutralOverride[] = {-258, -258, -27349, 0, 5417, 0}; -const int channelMaxOverride[] = {27090, 27090, 27090, 27090, 27090, 1}; +const int channelNeutralOverride[] = {-258, -258, -27349, 0, -27349, 0}; +const int channelMaxOverride[] = {27090, 27090, 27090, 27090, -5676, 1}; #define RC_CHANNEL_ROLL 0 #define RC_CHANNEL_PITCH 1 From be3d2be9d3244b35ab6b76fe1cf9ac7033f5758c Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 21 May 2024 10:50:47 +0300 Subject: [PATCH 35/39] Fix Vector::angularRatesBetweenVectors return NaNs on opposite vectors --- flix/vector.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/flix/vector.h b/flix/vector.h index c1433e1..b096a5e 100644 --- a/flix/vector.h +++ b/flix/vector.h @@ -80,6 +80,10 @@ public: static Vector angularRatesBetweenVectors(const Vector& a, const Vector& b) { Vector direction = cross(a, b); + if (direction.zero()) { + // vectors are opposite, return any perpendicular vector + return cross(a, Vector(1, 0, 0)); + } direction.normalize(); float angle = angleBetweenVectors(a, b); return direction * angle; From 94d24cbd281fc6e0db20b92f52814389e10bf8c6 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 21 May 2024 10:51:45 +0300 Subject: [PATCH 36/39] Fix PWM values for reverse rotations --- flix/motors.ino | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/flix/motors.ino b/flix/motors.ino index 91fc367..9d35445 100644 --- a/flix/motors.ino +++ b/flix/motors.ino @@ -14,8 +14,8 @@ #define PWM_NEUTRAL 1500 #define PWM_MIN 1600 #define PWM_MAX 2300 -#define PWM_REVERSE_MIN 1100 -#define PWM_REVERSE_MAX 1440 +#define PWM_REVERSE_MIN 1400 +#define PWM_REVERSE_MAX 700 void setupMotors() { Serial.println("Setup Motors"); From 7cfcf5b63bfb99b37c5c2d3bc3e20578c1fa8dc0 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 21 May 2024 10:52:39 +0300 Subject: [PATCH 37/39] Use more natural torqueTarget order in mixer (xyz) --- flix/control.ino | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/flix/control.ino b/flix/control.ino index 2d18175..ae60e4a 100644 --- a/flix/control.ino +++ b/flix/control.ino @@ -149,10 +149,10 @@ void controlTorque() { return; } - motors[MOTOR_FRONT_LEFT] = thrustTarget + torqueTarget.y + torqueTarget.x - torqueTarget.z; - motors[MOTOR_FRONT_RIGHT] = thrustTarget + torqueTarget.y - torqueTarget.x + torqueTarget.z; - motors[MOTOR_REAR_LEFT] = thrustTarget - torqueTarget.y + torqueTarget.x + torqueTarget.z; - motors[MOTOR_REAR_RIGHT] = thrustTarget - torqueTarget.y - torqueTarget.x - torqueTarget.z; + motors[MOTOR_FRONT_LEFT] = thrustTarget + torqueTarget.x + torqueTarget.y - torqueTarget.z; + motors[MOTOR_FRONT_RIGHT] = thrustTarget - torqueTarget.x + torqueTarget.y + torqueTarget.z; + motors[MOTOR_REAR_LEFT] = thrustTarget + torqueTarget.x - torqueTarget.y + torqueTarget.z; + motors[MOTOR_REAR_RIGHT] = thrustTarget - torqueTarget.x - torqueTarget.y - torqueTarget.z; motors[0] = constrain(motors[0], 0, 1); motors[1] = constrain(motors[1], 0, 1); From fbe33eac1b00f53c0231a0b5e7e37f53151d0ade Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Fri, 24 May 2024 14:46:38 +0300 Subject: [PATCH 38/39] Set gyro limits to 2000 DPS by default --- flix/imu.ino | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flix/imu.ino b/flix/imu.ino index 24852b2..45660e8 100644 --- a/flix/imu.ino +++ b/flix/imu.ino @@ -29,7 +29,7 @@ void setupIMU() { void configureIMU() { IMU.setAccelRange(IMU.ACCEL_RANGE_4G); - IMU.setGyroRange(IMU.GYRO_RANGE_500DPS); + IMU.setGyroRange(IMU.GYRO_RANGE_2000DPS); IMU.setDlpfBandwidth(IMU.DLPF_BANDWIDTH_184HZ); IMU.setSrd(0); // set sample rate to 1000 Hz } From 1119c77ccabcf0c368a40245d7391d5457fea74c Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Fri, 24 May 2024 14:47:26 +0300 Subject: [PATCH 39/39] Remove unneeded abs for motors thrust in simulation --- gazebo/simulator.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gazebo/simulator.cpp b/gazebo/simulator.cpp index 44a4481..f7da906 100644 --- a/gazebo/simulator.cpp +++ b/gazebo/simulator.cpp @@ -92,10 +92,10 @@ public: const double maxThrust = 0.03 * ONE_G; // ~30 g, https://youtu.be/VtKI4Pjx8Sk?&t=78 const float scale0 = 1.0, scale1 = 1.1, scale2 = 0.9, scale3 = 1.05; // imitating motors asymmetry - float mfl = scale0 * maxThrust * abs(motors[MOTOR_FRONT_LEFT]); - float mfr = scale1 * maxThrust * abs(motors[MOTOR_FRONT_RIGHT]); - float mrl = scale2 * maxThrust * abs(motors[MOTOR_REAR_LEFT]); - float mrr = scale3 * maxThrust * abs(motors[MOTOR_REAR_RIGHT]); + float mfl = scale0 * maxThrust * motors[MOTOR_FRONT_LEFT]; + float mfr = scale1 * maxThrust * motors[MOTOR_FRONT_RIGHT]; + float mrl = scale2 * maxThrust * motors[MOTOR_REAR_LEFT]; + float mrr = scale3 * maxThrust * motors[MOTOR_REAR_RIGHT]; body->AddLinkForce(Vector3d(0.0, 0.0, mfl), Vector3d(dist, dist, 0.0)); body->AddLinkForce(Vector3d(0.0, 0.0, mfr), Vector3d(dist, -dist, 0.0));