diff --git a/.editorconfig b/.editorconfig
index 9a231d1..0dd4b38 100644
--- a/.editorconfig
+++ b/.editorconfig
@@ -9,3 +9,7 @@ charset = utf-8
indent_style = tab
tab_width = 4
trim_trailing_whitespace = true
+
+[{*.yml,*.yaml,CMakeLists.txt}]
+indent_style = space
+indent_size = 2
diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml
index 0d9c0ed..10a4950 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/.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/Makefile b/Makefile
index 7d976bd..1a9ea16 100644
--- a/Makefile
+++ b/Makefile
@@ -15,8 +15,7 @@ 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 "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/README.md b/README.md
index 1b49a0e..1318881 100644
--- a/README.md
+++ b/README.md
@@ -38,7 +38,7 @@ Simulation in Gazebo using a plugin that runs original Arduino code is implement
-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).*
@@ -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|
@@ -57,11 +57,13 @@ 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).*
-## 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/.
diff --git a/docs/build.md b/docs/build.md
index 4a5caf9..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:
@@ -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
@@ -59,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
@@ -67,7 +80,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
@@ -75,9 +88,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.
- * `Bolder Flight Systems MPU9250`, version 1.0.2.
+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/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/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 @@
+ transform="matrix(1.3333333,0,0,-1.3333333,0,1440)" />
diff --git a/flix/cli.ino b/flix/cli.ino
index b4c9a5e..e92df50 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";
@@ -72,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],
diff --git a/flix/control.ino b/flix/control.ino
index 5bc327b..ae60e4a 100644
--- a/flix/control.ino
+++ b/flix/control.ino
@@ -32,10 +32,9 @@
#define YAWRATE_MAX radians(360)
#define MAX_TILT radians(30)
-#define RATES_LFP_ALPHA 0.8 // cutoff frequency ~ 250 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;
@@ -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;
@@ -68,6 +65,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 +76,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) {
@@ -89,10 +87,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) {
@@ -137,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);
@@ -152,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);
@@ -172,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/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 e01da35..92aeb79 100644
--- a/flix/flix.ino
+++ b/flix/flix.ino
@@ -26,10 +26,11 @@
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 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]
@@ -51,8 +52,7 @@ void setup() {
}
void loop() {
- if (!readIMU()) return;
-
+ readIMU();
step();
readRC();
estimate();
diff --git a/flix/imu.ino b/flix/imu.ino
index acbade5..45660e8 100644
--- a/flix/imu.ino
+++ b/flix/imu.ino
@@ -6,87 +6,116 @@
#include
#include
-#define IMU_CS_PIN 4 // chip-select pin for IMU SPI connection
-#define LOAD_GYRO_CAL false
+#define ONE_G 9.80665
-MPU9250 IMU(SPI, IMU_CS_PIN);
+// 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);
+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_2000DPS);
+ 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(gyro.x, gyro.y, gyro.z);
+ IMU.getAccel(acc.x, acc.y, acc.z);
+ // apply scale and bias
+ acc = (acc - accBias) / accScale;
+ gyro = gyro - 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(gyro.x, gyro.y, gyro.z);
+ gyroBias = gyroBias + gyro;
+ }
+ 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(300);
- IMU.calibrateAccel();
+ Serial.println("Calibrating accelerometer");
+ 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();
+ 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();
+ configureIMU();
}
-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/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/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/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/flix/motors.ino b/flix/motors.ino
index 937d8e4..9d35445 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 1400
+#define PWM_REVERSE_MAX 700
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);
}
}
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..c644156 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,22 +23,20 @@ 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)
- {
- 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,
@@ -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,33 +60,46 @@ 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
- {
- 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)));
+ Vector toEulerZYX() const {
+ // 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
- {
+ float getYaw() const {
// https://github.com/ros/geometry2/blob/589caf083cae9d8fae7effdb910454b4681b9ec1/tf2/include/tf2/impl/utils.h#L122
float yaw;
float sqx = x * x;
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) {
@@ -101,15 +110,14 @@ public:
return yaw;
}
- void setYaw(float yaw)
- {
+ 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)
- {
+ 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 +126,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 +134,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 +143,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 +155,24 @@ 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)
- {
+ // Rotate vector by quaternion
+ 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/rc.ino b/flix/rc.ino
index ce7e439..69f7574 100644
--- a/flix/rc.ino
+++ b/flix/rc.ino
@@ -5,9 +5,7 @@
#include
-#define INVERT_SERIAL true // false if external SBUS invertor is used
-
-// 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};
@@ -16,14 +14,12 @@ SBUS RC(Serial2);
void setupRC() {
Serial.println("Setup RC");
RC.begin();
- Serial2.setRxInvert(INVERT_SERIAL);
}
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/flix/vector.h b/flix/vector.h
index 5ed57a5..b096a5e 100644
--- a/flix/vector.h
+++ b/flix/vector.h
@@ -13,79 +13,79 @@ 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
- {
+ // 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;
}
- 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)
- {
- Vector direction = cross(u, v);
+ 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(u, v);
+ float angle = angleBetweenVectors(a, b);
return direction * angle;
}
diff --git a/gazebo/SBUS.h b/gazebo/SBUS.h
index 307a8ba..8d56d45 100644
--- a/gazebo/SBUS.h
+++ b/gazebo/SBUS.h
@@ -5,11 +5,20 @@
#include "joystick.h"
+struct SBUSData {
+ int16_t ch[16];
+};
+
class SBUS {
public:
- SBUS(HardwareSerial& bus) {};
+ SBUS(HardwareSerial& bus, const bool inv = true) {};
void begin() {};
- bool read(int16_t* channels, bool* failsafe, bool* lostFrame) { // NOTE: on the hardware channels is uint16_t
- 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..14dc727 100644
--- a/gazebo/flix.h
+++ b/gazebo/flix.h
@@ -23,9 +23,10 @@ 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 gyro;
Vector rates;
Quaternion attitude;
diff --git a/gazebo/joystick.h b/gazebo/joystick.h
index 04db142..5294d77 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, -27349, 0};
+const int channelMaxOverride[] = {27090, 27090, 27090, 27090, -5676, 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;
}
diff --git a/gazebo/simulator.cpp b/gazebo/simulator.cpp
index f681513..f7da906 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
@@ -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));
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) {
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)}')
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