From 9a75de6adf4e6b271974105000d160412847ae54 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 23 Oct 2024 08:39:54 +0300 Subject: [PATCH] Use FLU as the main coordinate system instead of FRD This simplifies the code a lot --- flix/control.ino | 20 ++++++++++---------- flix/estimate.ino | 4 ++-- flix/imu.ino | 6 +++--- flix/mavlink.ino | 8 +++++++- gazebo/flix.h | 1 + gazebo/simulator.cpp | 9 ++++----- gazebo/util.h | 14 -------------- 7 files changed, 27 insertions(+), 35 deletions(-) delete mode 100644 gazebo/util.h diff --git a/flix/control.ino b/flix/control.ino index ef6eb8a..f6c4a03 100644 --- a/flix/control.ino +++ b/flix/control.ino @@ -81,22 +81,22 @@ void interpretRC() { if (mode == ACRO) { yawMode = YAW_RATE; ratesTarget.x = controls[RC_CHANNEL_ROLL] * ROLLRATE_MAX; - ratesTarget.y = -controls[RC_CHANNEL_PITCH] * PITCHRATE_MAX; // up pitch stick means tilt clockwise in frd - ratesTarget.z = controls[RC_CHANNEL_YAW] * YAWRATE_MAX; + ratesTarget.y = controls[RC_CHANNEL_PITCH] * PITCHRATE_MAX; + ratesTarget.z = -controls[RC_CHANNEL_YAW] * YAWRATE_MAX; // positive yaw stick means clockwise rotation in FLU } else if (mode == STAB) { yawMode = controls[RC_CHANNEL_YAW] == 0 ? YAW : YAW_RATE; attitudeTarget = Quaternion::fromEulerZYX(Vector( controls[RC_CHANNEL_ROLL] * MAX_TILT, - -controls[RC_CHANNEL_PITCH] * MAX_TILT, + controls[RC_CHANNEL_PITCH] * MAX_TILT, attitudeTarget.getYaw())); - ratesTarget.z = controls[RC_CHANNEL_YAW] * YAWRATE_MAX; + ratesTarget.z = -controls[RC_CHANNEL_YAW] * YAWRATE_MAX; // positive yaw stick means clockwise rotation in FLU } else if (mode == MANUAL) { // passthrough mode yawMode = YAW_RATE; - torqueTarget = Vector(controls[RC_CHANNEL_ROLL], -controls[RC_CHANNEL_PITCH], controls[RC_CHANNEL_YAW]) * 0.01; + torqueTarget = Vector(controls[RC_CHANNEL_ROLL], controls[RC_CHANNEL_PITCH], -controls[RC_CHANNEL_YAW]) * 0.01; } if (yawMode == YAW_RATE || !motorsActive()) { @@ -113,7 +113,7 @@ void controlAttitude() { return; } - const Vector up(0, 0, -1); + const Vector up(0, 0, 1); Vector upActual = attitude.rotate(up); Vector upTarget = attitudeTarget.rotate(up); @@ -150,10 +150,10 @@ void controlTorque() { return; } - 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[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); diff --git a/flix/estimate.ino b/flix/estimate.ino index 6824d32..ed6506a 100644 --- a/flix/estimate.ino +++ b/flix/estimate.ino @@ -36,7 +36,7 @@ void applyAcc() { if (!landed) return; // calculate accelerometer correction - Vector up = attitude.rotate(Vector(0, 0, -1)); + Vector up = attitude.rotate(Vector(0, 0, 1)); Vector correction = Vector::angularRatesBetweenVectors(acc, up) * dt * WEIGHT_ACC; // apply correction @@ -45,6 +45,6 @@ void applyAcc() { } void signalizeHorizontality() { - float angle = Vector::angleBetweenVectors(attitude.rotate(Vector(0, 0, -1)), Vector(0, 0, -1)); + float angle = Vector::angleBetweenVectors(attitude.rotate(Vector(0, 0, 1)), Vector(0, 0, 1)); setLED(angle < radians(15)); } diff --git a/flix/imu.ino b/flix/imu.ino index d1d411b..368fa3f 100644 --- a/flix/imu.ino +++ b/flix/imu.ino @@ -2,6 +2,9 @@ // Repository: https://github.com/okalachev/flix // Work with the IMU sensor +// IMU is oriented FLU (front-left-up) style. +// In case of FRD (front-right-down) orientation of the IMU, use this code: +// https://gist.github.com/okalachev/713db47e31bce643dbbc9539d166ce98. #include #include @@ -41,9 +44,6 @@ void readIMU() { // apply scale and bias acc = (acc - accBias) / accScale; gyro = gyro - gyroBias; - // rotate to FRD - rotateData(acc); - rotateData(gyro); } void calibrateGyro() { diff --git a/flix/mavlink.ino b/flix/mavlink.ino index d4c6480..de7602a 100644 --- a/flix/mavlink.ino +++ b/flix/mavlink.ino @@ -38,8 +38,9 @@ void sendMavlink() { lastFast = t; const float zeroQuat[] = {0, 0, 0, 0}; + Quaternion attitudeFRD = FLU2FRD(attitude); // MAVLink uses FRD coordinate system mavlink_msg_attitude_quaternion_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, - time, attitude.w, attitude.x, attitude.y, attitude.z, rates.x, rates.y, rates.z, zeroQuat); + time, attitudeFRD.w, attitudeFRD.x, attitudeFRD.y, attitudeFRD.z, rates.x, rates.y, rates.z, zeroQuat); sendMessage(&msg); mavlink_msg_rc_channels_scaled_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, time, 0, @@ -97,4 +98,9 @@ void handleMavlink(const void *_msg) { } } +// Convert Forward-Left-Up to Forward-Right-Down quaternion +inline Quaternion FLU2FRD(const Quaternion &q) { + return Quaternion(q.w, q.x, -q.y, -q.z); +} + #endif diff --git a/gazebo/flix.h b/gazebo/flix.h index 7debf3e..8b261bd 100644 --- a/gazebo/flix.h +++ b/gazebo/flix.h @@ -49,6 +49,7 @@ void sendMavlink(); void sendMessage(const void *msg); void receiveMavlink(); void handleMavlink(const void *_msg); +inline Quaternion FLU2FRD(const Quaternion &q); // mocks void setLED(bool on) {}; diff --git a/gazebo/simulator.cpp b/gazebo/simulator.cpp index dd5a31a..e67ea10 100644 --- a/gazebo/simulator.cpp +++ b/gazebo/simulator.cpp @@ -17,7 +17,6 @@ #include "Arduino.h" #include "flix.h" -#include "util.h" #include "util.ino" #include "rc.ino" #include "time.ino" @@ -64,9 +63,9 @@ class ModelFlix : public ModelPlugin { __micros = model->GetWorld()->SimTime().Double() * 1000000; step(); - // read imu - gyro = flu2frd(imu->AngularVelocity()); - acc = this->accFilter.update(flu2frd(imu->LinearAcceleration())); + // read virtual imu + gyro = Vector(imu->AngularVelocity().X(), imu->AngularVelocity().Y(), imu->AngularVelocity().Z()); + acc = this->accFilter.update(Vector(imu->LinearAcceleration().X(), imu->LinearAcceleration().Y(), imu->LinearAcceleration().Z())); // read rc readRC(); @@ -76,7 +75,7 @@ class ModelFlix : public ModelPlugin { estimate(); // correct yaw to the actual yaw - attitude.setYaw(-this->model->WorldPose().Yaw()); + attitude.setYaw(this->model->WorldPose().Yaw()); control(); parseInput(); diff --git a/gazebo/util.h b/gazebo/util.h deleted file mode 100644 index 7467a2c..0000000 --- a/gazebo/util.h +++ /dev/null @@ -1,14 +0,0 @@ -#include -#include - -using ignition::math::Vector3d; -using ignition::math::Pose3d; - -Pose3d flu2frd(const Pose3d& p) { - return ignition::math::Pose3d(p.Pos().X(), -p.Pos().Y(), -p.Pos().Z(), - 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()); -}