aleks/flix
1
0
Fork 0
mirror of https://github.com/okalachev/flix.git synced 2025-07-27 09:39:33 +00:00
Code Issues Packages Projects Releases Wiki Activity

Use FLU as the main coordinate system instead of FRD

Browse Source 
Corresponding to the IMU orientation in the new version
This commit is contained in:
Oleg Kalachev 2024-10-23 09:30:49 +03:00
parent f46460e53d
commit abcc9b96de
7 changed files with 27 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
flix/control.ino
View File

@ -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);

4
flix/estimate.ino
View File

@ -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));
}

3
flix/imu.ino
View File

@ -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 <SPI.h>
#include <MPU9250.h>

8
flix/mavlink.ino
View File

@ -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

1
gazebo/flix.h
View File

@ -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) {};

9
gazebo/simulator.cpp
View File

@ -17,7 +17,6 @@
#include "Arduino.h"
#include "flix.h"
#include "util.h"
#include "util.ino"
#include "rc.ino"
#include "time.ino"
@ -63,9 +62,9 @@ public:
__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();
@ -75,7 +74,7 @@ public:
estimate();
// correct yaw to the actual yaw
attitude.setYaw(-this->model->WorldPose().Yaw());
attitude.setYaw(this->model->WorldPose().Yaw());
control();
parseInput();

14
gazebo/util.h
View File

@ -1,14 +0,0 @@
#include <ignition/math/Vector3.hh>
#include <ignition/math/Pose3.hh>
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());
}