2 Commits

Author SHA1 Message Date
Oleg Kalachev
2308bd4d8f Use Serial1 for for rc on esp32-c3 2026-01-23 03:18:41 +03:00
Oleg Kalachev
8153644578 Add esp32-c3 build to ci 2026-01-23 03:04:57 +03:00
25 changed files with 102 additions and 121 deletions

View File

@@ -25,6 +25,8 @@ jobs:
path: flix/build path: flix/build
- name: Build firmware for ESP32-S3 - name: Build firmware for ESP32-S3
run: make BOARD=esp32:esp32:esp32s3 run: make BOARD=esp32:esp32:esp32s3
- name: Build firmware for ESP32-C3
run: make BOARD=esp32:esp32:esp32c3
- name: Check c_cpp_properties.json - name: Check c_cpp_properties.json
run: tools/check_c_cpp_properties.py run: tools/check_c_cpp_properties.py

View File

@@ -138,10 +138,10 @@ You can see a user-contributed [variant of complete circuit diagram](https://mir
|Motor|Position|Direction|Prop type|Motor wires|GPIO| |Motor|Position|Direction|Prop type|Motor wires|GPIO|
|-|-|-|-|-|-| |-|-|-|-|-|-|
|Motor 0|Rear left|Counter-clockwise|B|Black & White|GPIO12 *(TDI)*| |Motor 0|Rear left|Counter-clockwise|B|Black & White|GPIO12 (*TDI*)|
|Motor 1|Rear right|Clockwise|A|Blue & Red|GPIO13 *(TCK)*| |Motor 1|Rear right|Clockwise|A|Blue & Red|GPIO13 (*TCK*)|
|Motor 2|Front right|Counter-clockwise|B|Black & White|GPIO14 *(TMS)*| |Motor 2|Front right|Counter-clockwise|B|Black & White|GPIO14 (*TMS*)|
|Motor 3|Front left|Clockwise|A|Blue & Red|GPIO15 *(TD0)*| |Motor 3|Front left|Clockwise|A|Blue & Red|GPIO15 (*TD0*)|
Clockwise motors have blue & red wires and correspond to propeller type A (marked on the propeller). Clockwise motors have blue & red wires and correspond to propeller type A (marked on the propeller).
Counter-clockwise motors have black & white wires correspond to propeller type B. Counter-clockwise motors have black & white wires correspond to propeller type B.

View File

@@ -41,10 +41,10 @@ Motors connection table:
|Motor|Position|Direction|Prop type|Motor wires|GPIO| |Motor|Position|Direction|Prop type|Motor wires|GPIO|
|-|-|-|-|-|-| |-|-|-|-|-|-|
|Motor 0|Rear left|Counter-clockwise|B|Black & White|GPIO12 *(TDI)*| |Motor 0|Rear left|Counter-clockwise|B|Black & White|GPIO12 (*TDI*)|
|Motor 1|Rear right|Clockwise|A|Blue & Red|GPIO13 *(TCK)*| |Motor 1|Rear right|Clockwise|A|Blue & Red|GPIO13 (*TCK*)|
|Motor 2|Front right|Counter-clockwise|B|Black & White|GPIO14 *(TMS)*| |Motor 2|Front right|Counter-clockwise|B|Black & White|GPIO14 (*TMS*)|
|Motor 3|Front left|Clockwise|A|Blue & Red|GPIO15 *(TD0)*| |Motor 3|Front left|Clockwise|A|Blue & Red|GPIO15 (*TD0*)|
## Motors tightening ## Motors tightening

View File

@@ -110,7 +110,7 @@ float angle = Vector::angleBetween(a, b); // 1.57 (90 градусов)
#### Скалярное произведение #### Скалярное произведение
Скалярное произведение векторов *(dot product)* — это произведение длин двух векторов на косинус угла между ними. В математике оно обозначается знаком `·` или слитным написанием векторов. Интуитивно, результат скалярного произведения показывает, насколько два вектора *сонаправлены*. Скалярное произведение векторов (*dot product*) — это произведение длин двух векторов на косинус угла между ними. В математике оно обозначается знаком `·` или слитным написанием векторов. Интуитивно, результат скалярного произведения показывает, насколько два вектора *сонаправлены*.
В Flix используется статический метод `Vector::dot()`: В Flix используется статический метод `Vector::dot()`:
@@ -124,7 +124,7 @@ float dotProduct = Vector::dot(a, b); // 32
#### Векторное произведение #### Векторное произведение
Векторное произведение *(cross product)* позволяет найти вектор, перпендикулярный двум другим векторам. В математике оно обозначается знаком `×`, а в прошивке используется статический метод `Vector::cross()`: Векторное произведение (*cross product*) позволяет найти вектор, перпендикулярный двум другим векторам. В математике оно обозначается знаком `×`, а в прошивке используется статический метод `Vector::cross()`:
```cpp ```cpp
Vector a(1, 2, 3); Vector a(1, 2, 3);
@@ -144,9 +144,9 @@ Vector crossProduct = Vector::cross(a, b); // -3, 6, -3
В прошивке углы Эйлера сохраняются в обычный объект `Vector` (хоть и, строго говоря, не являются вектором): В прошивке углы Эйлера сохраняются в обычный объект `Vector` (хоть и, строго говоря, не являются вектором):
* Угол по крену *(roll)* — `vector.x`. * Угол по крену (*roll*) — `vector.x`.
* Угол по тангажу *(pitch)* — `vector.y`. * Угол по тангажу (*pitch*) — `vector.y`.
* Угол по рысканию *(yaw)* — `vector.z`. * Угол по рысканию (*yaw*) — `vector.z`.
Особенности углов Эйлера: Особенности углов Эйлера:
@@ -162,8 +162,8 @@ Vector crossProduct = Vector::cross(a, b); // -3, 6, -3
Помимо углов Эйлера, любую ориентацию в трехмерном пространстве можно представить в виде вращения вокруг некоторой оси на некоторый угол. В геометрии это доказывается, как **теорема вращения Эйлера**. В таком представлении ориентация задается двумя величинами: Помимо углов Эйлера, любую ориентацию в трехмерном пространстве можно представить в виде вращения вокруг некоторой оси на некоторый угол. В геометрии это доказывается, как **теорема вращения Эйлера**. В таком представлении ориентация задается двумя величинами:
* **Ось вращения** *(axis)* — единичный вектор, определяющий ось вращения. * **Ось вращения** (*axis*) — единичный вектор, определяющий ось вращения.
* **Угол поворота** *(angle* или *θ)* — угол, на который нужно повернуть объект вокруг этой оси. * **Угол поворота** (*angle* или *θ*) — угол, на который нужно повернуть объект вокруг этой оси.
В Flix ось вращения задается объектом `Vector`, а угол поворота — числом типа `float` в радианах: В Flix ось вращения задается объектом `Vector`, а угол поворота — числом типа `float` в радианах:
@@ -177,7 +177,7 @@ float angle = radians(45);
### Вектор вращения ### Вектор вращения
Если умножить вектор *axis* на угол поворота *θ*, то получится **вектор вращения** *(rotation vector)*. Этот вектор играет важную роль в алгоритмах управления ориентацией летательного аппарата. Если умножить вектор *axis* на угол поворота *θ*, то получится **вектор вращения** (*rotation vector*). Этот вектор играет важную роль в алгоритмах управления ориентацией летательного аппарата.
Вектор вращения обладает замечательным свойством: если угловые скорости объекта (в собственной системе координат) в каждый момент времени совпадают с компонентами этого вектора, то за единичное время объект придет к заданной этим вектором ориентации. Это свойство позволяет использовать вектор вращения для управления ориентацией объекта посредством управления угловыми скоростями. Вектор вращения обладает замечательным свойством: если угловые скорости объекта (в собственной системе координат) в каждый момент времени совпадают с компонентами этого вектора, то за единичное время объект придет к заданной этим вектором ориентации. Это свойство позволяет использовать вектор вращения для управления ориентацией объекта посредством управления угловыми скоростями.
@@ -198,7 +198,7 @@ Vector rotation = radians(45) * Vector(1, 2, 3);
<a href="https://github.com/okalachev/flix/blob/master/flix/quaternion.h"><code>quaternion.h</code></a>.<br> <a href="https://github.com/okalachev/flix/blob/master/flix/quaternion.h"><code>quaternion.h</code></a>.<br>
</div> </div>
Вектор вращения удобен, но еще удобнее использовать **кватернион**. В Flix кватернионы задаются объектами `Quaternion` из библиотеки `quaternion.h`. Кватернион состоит из четырех значений: *w*, *x*, *y*, *z* и рассчитывается из вектора оси вращения *(axis)* и угла поворота *(θ)* по формуле: Вектор вращения удобен, но еще удобнее использовать **кватернион**. В Flix кватернионы задаются объектами `Quaternion` из библиотеки `quaternion.h`. Кватернион состоит из четырех значений: *w*, *x*, *y*, *z* и рассчитывается из вектора оси вращения (*axis*) и угла поворота (*θ*) по формуле:
\\[ q = \left( \begin{array}{c} w \\\\ x \\\\ y \\\\ z \end{array} \right) = \left( \begin{array}{c} \cos\left(\frac{\theta}{2}\right) \\\\ axis\_x \cdot \sin\left(\frac{\theta}{2}\right) \\\\ axis\_y \cdot \sin\left(\frac{\theta}{2}\right) \\\\ axis\_z \cdot \sin\left(\frac{\theta}{2}\right) \end{array} \right) \\] \\[ q = \left( \begin{array}{c} w \\\\ x \\\\ y \\\\ z \end{array} \right) = \left( \begin{array}{c} \cos\left(\frac{\theta}{2}\right) \\\\ axis\_x \cdot \sin\left(\frac{\theta}{2}\right) \\\\ axis\_y \cdot \sin\left(\frac{\theta}{2}\right) \\\\ axis\_z \cdot \sin\left(\frac{\theta}{2}\right) \end{array} \right) \\]

View File

@@ -177,7 +177,7 @@ imu.setDLPF(imu.DLPF_MAX);
## Калибровка гироскопа ## Калибровка гироскопа
Как и любое измерительное устройство, гироскоп вносит искажения в измерения. Наиболее простая модель этих искажений делит их на статические смещения *(bias)* и случайный шум *(noise)*: Как и любое измерительное устройство, гироскоп вносит искажения в измерения. Наиболее простая модель этих искажений делит их на статические смещения (*bias*) и случайный шум (*noise*):
\\[ gyro_{xyz}=rates_{xyz}+bias_{xyz}+noise \\] \\[ gyro_{xyz}=rates_{xyz}+bias_{xyz}+noise \\]

Binary file not shown.

Before

Width:  |  Height:  |  Size: 62 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 48 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 50 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 17 KiB

View File

@@ -13,10 +13,10 @@ Do the following:
Do the following: Do the following:
* **Check the battery voltage**. Use a multimeter to measure the battery voltage. It should be in range of 3.7-4.2 V. * **Check the battery voltage**. Use a multimeter to measure the battery voltage. It should be in range of 3.7-4.2 V.
* **Check if there are some startup errors**. Connect the ESP32 to the computer and check the Serial Monitor output. Use the Reset button to make sure you see the whole ESP32 startup output. * **Check if there are some startup errors**. Connect the ESP32 to the computer and check the Serial Monitor output. Use the Reset button to make sure you see the whole ESP32 output.
* **Check the baudrate is correct**. If you see garbage characters in the Serial Monitor, make sure the baudrate is set to 115200. * **Check the baudrate is correct**. If you see garbage characters in the Serial Monitor, make sure the baudrate is set to 115200.
* **Make sure correct IMU model is chosen**. If using ICM-20948/MPU-6050 board, change `MPU9250` to `ICM20948`/`MPU6050` in the `imu.ino` file. * **Make sure correct IMU model is chosen**. If using ICM-20948/MPU-6050 board, change `MPU9250` to `ICM20948`/`MPU6050` in the `imu.ino` file.
* **Check if the console is working**. Perform `help` command in Serial Monitor. You should see the list of available commands. You can also access the console using QGroundControl *(Vehicle Setup**Analyze Tools**MAVLink Console)*. * **Check if the CLI is working**. Perform `help` command in Serial Monitor. You should see the list of available commands. You can also access the CLI using QGroundControl (*Vehicle Setup**Analyze Tools**MAVLink Console*).
* **Configure QGroundControl correctly before connecting to the drone** if you use it to control the drone. Go to the settings and enable *Virtual Joystick*. *Auto-Center Throttle* setting **should be disabled**. * **Configure QGroundControl correctly before connecting to the drone** if you use it to control the drone. Go to the settings and enable *Virtual Joystick*. *Auto-Center Throttle* setting **should be disabled**.
* **If QGroundControl doesn't connect**, you might need to disable the firewall and/or VPN on your computer. * **If QGroundControl doesn't connect**, you might need to disable the firewall and/or VPN on your computer.
* **Check the IMU is working**. Perform `imu` command and check its output: * **Check the IMU is working**. Perform `imu` command and check its output:

View File

@@ -134,20 +134,6 @@ Before flight you need to calibrate the accelerometer:
1. Access the console using QGroundControl (recommended) or Serial Monitor. 1. Access the console using QGroundControl (recommended) or Serial Monitor.
2. Type `ca` command there and follow the instructions. 2. Type `ca` command there and follow the instructions.
### Setup motors
If using non-default motor pins, set the pin numbers using the parameters: `MOTOR_PIN_FL`, `MOTOR_PIN_FR`, `MOTOR_PIN_RL`, `MOTOR_PIN_RR` (front-left, front-right, rear-left, rear-right respectively).
If using brushless motors and ESCs:
1. Set the appropriate PWM using the parameters: `MOT_PWM_STOP`, `MOT_PWM_MIN`, and `MOT_PWM_MAX` (1000, 1000, and 2000 is typical).
2. Decrease the PWM frequency using the `MOT_PWM_FREQ` parameter (400 is typical).
Reboot the drone to apply the changes.
> [!CAUTION]
> **Remove the props when configuring the motors!** If improperly configured, you may not be able to stop them.
### Check everything works ### Check everything works
1. Check the IMU is working: perform `imu` command and check its output: 1. Check the IMU is working: perform `imu` command and check its output:
@@ -264,8 +250,8 @@ You can configure the Wi-Fi using parameters and console commands.
The Wi-Fi mode is chosen using `WIFI_MODE` parameter in QGroundControl or in the console: The Wi-Fi mode is chosen using `WIFI_MODE` parameter in QGroundControl or in the console:
* `0` — Wi-Fi is disabled. * `0` — Wi-Fi is disabled.
* `1` — Access Point mode *(AP)* the drone creates a Wi-Fi network. * `1` — Access Point mode (*AP*) the drone creates a Wi-Fi network.
* `2` — Client mode *(STA)* — the drone connects to an existing Wi-Fi network. * `2` — Client mode (*STA*) the drone connects to an existing Wi-Fi network.
* `3` — *ESP-NOW (not implemented yet)*. * `3` — *ESP-NOW (not implemented yet)*.
> [!WARNING] > [!WARNING]

View File

@@ -4,17 +4,6 @@ This page contains user-built drones based on the Flix project. Publish your pro
--- ---
Author: **Arkadiy "Arky" Matsekh**, Foucault Dynamics, Gold Coast, Australia.<br>
The drone was built for the University of Queensland industry-led Master's capstone project.
**Flight video:**
<a href="https://drive.google.com/file/d/1NNYSVXBY-w0JjCo07D8-PgnVq3ca9plj/view?usp=sharing"><img height=300 src="img/user/arkymatsekh/video.jpg"></a>
<img src="img/user/arkymatsekh/1.jpg" height=150> <img src="img/user/arkymatsekh/2.jpg" height=150> <img src="img/user/arkymatsekh/3.jpg" height=150>
---
Author: [goldarte](https://t.me/goldarte).<br> Author: [goldarte](https://t.me/goldarte).<br>
<img src="img/user/goldarte/1.jpg" height=150> <img src="img/user/goldarte/2.jpg" height=150> <img src="img/user/goldarte/1.jpg" height=150> <img src="img/user/goldarte/2.jpg" height=150>

View File

@@ -6,7 +6,6 @@
#include "pid.h" #include "pid.h"
#include "vector.h" #include "vector.h"
#include "util.h" #include "util.h"
#include "lpf.h"
extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT; extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
extern const int RAW, ACRO, STAB, AUTO; extern const int RAW, ACRO, STAB, AUTO;
@@ -15,7 +14,6 @@ extern uint16_t channels[16];
extern float controlTime; extern float controlTime;
extern int mode; extern int mode;
extern bool armed; extern bool armed;
extern LowPassFilter<Vector> gyroBiasFilter;
const char* motd = const char* motd =
"\nWelcome to\n" "\nWelcome to\n"
@@ -94,7 +92,7 @@ void doCommand(String str, bool echo = false) {
} else if (command == "p") { } else if (command == "p") {
bool success = setParameter(arg0.c_str(), arg1.toFloat()); bool success = setParameter(arg0.c_str(), arg1.toFloat());
if (success) { if (success) {
print("%s = %g\n", arg0.c_str(), getParameter(arg0.c_str())); print("%s = %g\n", arg0.c_str(), arg1.toFloat());
} else { } else {
print("Parameter not found: %s\n", arg0.c_str()); print("Parameter not found: %s\n", arg0.c_str());
} }
@@ -180,7 +178,6 @@ void doCommand(String str, bool echo = false) {
#endif #endif
} else if (command == "reset") { } else if (command == "reset") {
attitude = Quaternion(); attitude = Quaternion();
gyroBiasFilter.reset();
} else if (command == "reboot") { } else if (command == "reboot") {
ESP.restart(); ESP.restart();
} else { } else {

View File

@@ -52,7 +52,6 @@ PID pitchPID(PITCH_P, PITCH_I, PITCH_D);
PID yawPID(YAW_P, 0, 0); PID yawPID(YAW_P, 0, 0);
Vector maxRate(ROLLRATE_MAX, PITCHRATE_MAX, YAWRATE_MAX); Vector maxRate(ROLLRATE_MAX, PITCHRATE_MAX, YAWRATE_MAX);
float tiltMax = TILT_MAX; float tiltMax = TILT_MAX;
int flightModes[] = {STAB, STAB, STAB}; // map for rc mode switch
extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT; extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode; extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode;
@@ -66,9 +65,9 @@ void control() {
} }
void interpretControls() { void interpretControls() {
if (controlMode < 0.25) mode = flightModes[0]; if (controlMode < 0.25) mode = STAB;
else if (controlMode < 0.75) mode = flightModes[1]; if (controlMode < 0.75) mode = STAB;
else if (controlMode > 0.75) mode = flightModes[2]; if (controlMode > 0.75) mode = STAB;
if (mode == AUTO) return; // pilot is not effective in AUTO mode if (mode == AUTO) return; // pilot is not effective in AUTO mode

View File

@@ -7,6 +7,7 @@
#include "quaternion.h" #include "quaternion.h"
#include "util.h" #include "util.h"
extern float t, dt; extern float t, dt;
extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode; extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode;
extern Vector gyro, acc; extern Vector gyro, acc;

View File

@@ -19,8 +19,6 @@ Vector acc; // accelerometer output, m/s/s
Vector accBias; Vector accBias;
Vector accScale(1, 1, 1); Vector accScale(1, 1, 1);
LowPassFilter<Vector> gyroBiasFilter(0.001);
void setupIMU() { void setupIMU() {
print("Setup IMU\n"); print("Setup IMU\n");
imu.begin(); imu.begin();
@@ -52,6 +50,8 @@ void readIMU() {
void calibrateGyroOnce() { void calibrateGyroOnce() {
static Delay landedDelay(2); static Delay landedDelay(2);
if (!landedDelay.update(landed)) return; // calibrate only if definitely stationary if (!landedDelay.update(landed)) return; // calibrate only if definitely stationary
static LowPassFilter<Vector> gyroBiasFilter(0.001);
gyroBias = gyroBiasFilter.update(gyro); gyroBias = gyroBiasFilter.update(gyro);
} }

View File

@@ -14,6 +14,15 @@ public:
LowPassFilter(float alpha): alpha(alpha) {}; LowPassFilter(float alpha): alpha(alpha) {};
T update(const T input) { T update(const T input) {
if (alpha == 1) { // filter disabled
return input;
}
if (!initialized) {
output = input;
initialized = true;
}
return output += alpha * (input - output); return output += alpha * (input - output);
} }
@@ -22,6 +31,9 @@ public:
} }
void reset() { void reset() {
output = T(); // set to zero initialized = false;
} }
private:
bool initialized = false;
}; };

View File

@@ -142,7 +142,7 @@ void handleMavlink(const void *_msg) {
// send ack // send ack
mavlink_message_t msg; mavlink_message_t msg;
mavlink_msg_param_value_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, mavlink_msg_param_value_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg,
m.param_id, getParameter(name), MAV_PARAM_TYPE_REAL32, parametersCount(), 0); // index is unknown m.param_id, m.param_value, MAV_PARAM_TYPE_REAL32, parametersCount(), 0); // index is unknown
sendMessage(&msg); sendMessage(&msg);
} }

View File

@@ -1,18 +1,23 @@
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com> // Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
// Repository: https://github.com/okalachev/flix // Repository: https://github.com/okalachev/flix
// PWM control for motors // Motors output control using MOSFETs
// In case of using ESCs, change PWM_STOP, PWM_MIN and PWM_MAX to appropriate values in μs, decrease PWM_FREQUENCY (to 400)
#include "util.h" #include "util.h"
float motors[4]; // normalized motor thrusts in range [0..1] #define MOTOR_0_PIN 12 // rear left
#define MOTOR_1_PIN 13 // rear right
#define MOTOR_2_PIN 14 // front right
#define MOTOR_3_PIN 15 // front left
int motorPins[4] = {12, 13, 14, 15}; // default pin numbers #define PWM_FREQUENCY 78000
int pwmFrequency = 78000; #define PWM_RESOLUTION 10
int pwmResolution = 10; #define PWM_STOP 0
int pwmStop = 0; #define PWM_MIN 0
int pwmMin = 0; #define PWM_MAX 1000000 / PWM_FREQUENCY
int pwmMax = -1; // -1 means duty cycle mode
float motors[4]; // normalized motor thrusts in range [0..1]
const int MOTOR_REAR_LEFT = 0; const int MOTOR_REAR_LEFT = 0;
const int MOTOR_REAR_RIGHT = 1; const int MOTOR_REAR_RIGHT = 1;
@@ -21,30 +26,30 @@ const int MOTOR_FRONT_LEFT = 3;
void setupMotors() { void setupMotors() {
print("Setup Motors\n"); print("Setup Motors\n");
// configure pins // configure pins
for (int i = 0; i < 4; i++) { ledcAttach(MOTOR_0_PIN, PWM_FREQUENCY, PWM_RESOLUTION);
ledcAttach(motorPins[i], pwmFrequency, pwmResolution); ledcAttach(MOTOR_1_PIN, PWM_FREQUENCY, PWM_RESOLUTION);
} ledcAttach(MOTOR_2_PIN, PWM_FREQUENCY, PWM_RESOLUTION);
ledcAttach(MOTOR_3_PIN, PWM_FREQUENCY, PWM_RESOLUTION);
sendMotors(); sendMotors();
print("Motors initialized\n"); print("Motors initialized\n");
} }
void sendMotors() {
for (int i = 0; i < 4; i++) {
ledcWrite(motorPins[i], getDutyCycle(motors[i]));
}
}
int getDutyCycle(float value) { int getDutyCycle(float value) {
value = constrain(value, 0, 1); value = constrain(value, 0, 1);
if (pwmMax >= 0) { // pwm mode float pwm = mapf(value, 0, 1, PWM_MIN, PWM_MAX);
float pwm = mapf(value, 0, 1, pwmMin, pwmMax); if (value == 0) pwm = PWM_STOP;
if (value == 0) pwm = pwmStop; float duty = mapf(pwm, 0, 1000000 / PWM_FREQUENCY, 0, (1 << PWM_RESOLUTION) - 1);
float duty = mapf(pwm, 0, 1000000 / pwmFrequency, 0, (1 << pwmResolution) - 1); return round(duty);
return round(duty); }
} else { // duty cycle mode
return round(value * ((1 << pwmResolution) - 1)); void sendMotors() {
} ledcWrite(MOTOR_0_PIN, getDutyCycle(motors[0]));
ledcWrite(MOTOR_1_PIN, getDutyCycle(motors[1]));
ledcWrite(MOTOR_2_PIN, getDutyCycle(motors[2]));
ledcWrite(MOTOR_3_PIN, getDutyCycle(motors[3]));
} }
bool motorsActive() { bool motorsActive() {

View File

@@ -49,9 +49,6 @@ Parameter parameters[] = {
{"CTL_R_RATE_MAX", &maxRate.x}, {"CTL_R_RATE_MAX", &maxRate.x},
{"CTL_Y_RATE_MAX", &maxRate.z}, {"CTL_Y_RATE_MAX", &maxRate.z},
{"CTL_TILT_MAX", &tiltMax}, {"CTL_TILT_MAX", &tiltMax},
{"CTL_FLT_MODE_0", &flightModes[0]},
{"CTL_FLT_MODE_1", &flightModes[1]},
{"CTL_FLT_MODE_2", &flightModes[2]},
// imu // imu
{"IMU_ROT_ROLL", &imuRotation.x}, {"IMU_ROT_ROLL", &imuRotation.x},
{"IMU_ROT_PITCH", &imuRotation.y}, {"IMU_ROT_PITCH", &imuRotation.y},
@@ -62,20 +59,9 @@ Parameter parameters[] = {
{"IMU_ACC_SCALE_X", &accScale.x}, {"IMU_ACC_SCALE_X", &accScale.x},
{"IMU_ACC_SCALE_Y", &accScale.y}, {"IMU_ACC_SCALE_Y", &accScale.y},
{"IMU_ACC_SCALE_Z", &accScale.z}, {"IMU_ACC_SCALE_Z", &accScale.z},
{"IMU_GYRO_BIAS_A", &gyroBiasFilter.alpha},
// estimate // estimate
{"EST_ACC_WEIGHT", &accWeight}, {"EST_ACC_WEIGHT", &accWeight},
{"EST_RATES_LPF_A", &ratesFilter.alpha}, {"EST_RATES_LPF_A", &ratesFilter.alpha},
// motors
{"MOT_PIN_FL", &motorPins[MOTOR_FRONT_LEFT]},
{"MOT_PIN_FR", &motorPins[MOTOR_FRONT_RIGHT]},
{"MOT_PIN_RL", &motorPins[MOTOR_REAR_LEFT]},
{"MOT_PIN_RR", &motorPins[MOTOR_REAR_RIGHT]},
{"MOT_PWM_FREQ", &pwmFrequency},
{"MOT_PWM_RES", &pwmResolution},
{"MOT_PWM_STOP", &pwmStop},
{"MOT_PWM_MIN", &pwmMin},
{"MOT_PWM_MAX", &pwmMax},
// rc // rc
{"RC_ZERO_0", &channelZero[0]}, {"RC_ZERO_0", &channelZero[0]},
{"RC_ZERO_1", &channelZero[1]}, {"RC_ZERO_1", &channelZero[1]},

View File

@@ -6,7 +6,11 @@
#include <SBUS.h> #include <SBUS.h>
#include "util.h" #include "util.h"
#ifdef ESP32C3
SBUS rc(Serial1);
#else
SBUS rc(Serial2); SBUS rc(Serial2);
#endif
uint16_t channels[16]; // raw rc channels uint16_t channels[16]; // raw rc channels
float channelZero[16]; // calibration zero values float channelZero[16]; // calibration zero values

View File

@@ -9,7 +9,6 @@
#include "quaternion.h" #include "quaternion.h"
#include "Arduino.h" #include "Arduino.h"
#include "wifi.h" #include "wifi.h"
#include "lpf.h"
extern float t, dt; extern float t, dt;
extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode; extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode;
@@ -20,7 +19,6 @@ extern float motors[4];
Vector gyro, acc, imuRotation; Vector gyro, acc, imuRotation;
Vector accBias, gyroBias, accScale(1, 1, 1); Vector accBias, gyroBias, accScale(1, 1, 1);
LowPassFilter<Vector> gyroBiasFilter(0);
// declarations // declarations
void step(); void step();
@@ -34,7 +32,6 @@ void controlRates();
void controlTorque(); void controlTorque();
const char* getModeName(); const char* getModeName();
void sendMotors(); void sendMotors();
int getDutyCycle(float value);
bool motorsActive(); bool motorsActive();
void testMotor(int n); void testMotor(int n);
void print(const char* format, ...); void print(const char* format, ...);

View File

@@ -11,10 +11,9 @@
#include <sys/poll.h> #include <sys/poll.h>
#include <gazebo/gazebo.hh> #include <gazebo/gazebo.hh>
int wifiMode = 1; // mock #define WIFI_UDP_PORT 14580
int udpLocalPort = 14580; #define WIFI_UDP_REMOTE_PORT 14550
int udpRemotePort = 14550; #define WIFI_UDP_REMOTE_ADDR "255.255.255.255"
const char *udpRemoteIP = "255.255.255.255";
int wifiSocket; int wifiSocket;
@@ -23,22 +22,22 @@ void setupWiFi() {
sockaddr_in addr; // local address sockaddr_in addr; // local address
addr.sin_family = AF_INET; addr.sin_family = AF_INET;
addr.sin_addr.s_addr = INADDR_ANY; addr.sin_addr.s_addr = INADDR_ANY;
addr.sin_port = htons(udpLocalPort); addr.sin_port = htons(WIFI_UDP_PORT);
if (bind(wifiSocket, (sockaddr *)&addr, sizeof(addr))) { if (bind(wifiSocket, (sockaddr *)&addr, sizeof(addr))) {
gzerr << "Failed to bind WiFi UDP socket on port " << udpLocalPort << std::endl; gzerr << "Failed to bind WiFi UDP socket on port " << WIFI_UDP_PORT << std::endl;
return; return;
} }
int broadcast = 1; int broadcast = 1;
setsockopt(wifiSocket, SOL_SOCKET, SO_BROADCAST, &broadcast, sizeof(broadcast)); // enable broadcast setsockopt(wifiSocket, SOL_SOCKET, SO_BROADCAST, &broadcast, sizeof(broadcast)); // enable broadcast
gzmsg << "WiFi UDP socket initialized on port " << udpLocalPort << " (remote port " << udpRemotePort << ")" << std::endl; gzmsg << "WiFi UDP socket initialized on port " << WIFI_UDP_PORT << " (remote port " << WIFI_UDP_REMOTE_PORT << ")" << std::endl;
} }
void sendWiFi(const uint8_t *buf, int len) { void sendWiFi(const uint8_t *buf, int len) {
if (wifiSocket == 0) setupWiFi(); if (wifiSocket == 0) setupWiFi();
sockaddr_in addr; // remote address sockaddr_in addr; // remote address
addr.sin_family = AF_INET; addr.sin_family = AF_INET;
addr.sin_addr.s_addr = inet_addr(udpRemoteIP); addr.sin_addr.s_addr = inet_addr(WIFI_UDP_REMOTE_ADDR);
addr.sin_port = htons(udpRemotePort); addr.sin_port = htons(WIFI_UDP_REMOTE_PORT);
sendto(wifiSocket, buf, len, 0, (sockaddr *)&addr, sizeof(addr)); sendto(wifiSocket, buf, len, 0, (sockaddr *)&addr, sizeof(addr));
} }

View File

@@ -13,7 +13,7 @@ lines = []
print('Downloading log...') print('Downloading log...')
count = 0 count = 0
dev.write('log dump\n'.encode()) dev.write('log\n'.encode())
while True: while True:
line = dev.readline() line = dev.readline()
if not line: if not line:

View File

@@ -92,17 +92,17 @@ Full list of events:
|-----|-----------|----------------| |-----|-----------|----------------|
|`connected`|Connected to the drone|| |`connected`|Connected to the drone||
|`disconnected`|Connection is lost|| |`disconnected`|Connection is lost||
|`armed`|Armed state update|Armed state *(bool)*| |`armed`|Armed state update|Armed state (*bool*)|
|`mode`|Flight mode update|Flight mode *(str)*| |`mode`|Flight mode update|Flight mode (*str*)|
|`landed`|Landed state update|Landed state *(bool)*| |`landed`|Landed state update|Landed state (*bool*)|
|`print`|The drone prints text to the console|Text| |`print`|The drone prints text to the console|Text|
|`attitude`|Attitude update|Attitude quaternion *(list)*| |`attitude`|Attitude update|Attitude quaternion (*list*)|
|`attitude_euler`|Attitude update|Euler angles *(list)*| |`attitude_euler`|Attitude update|Euler angles (*list*)|
|`rates`|Angular rates update|Angular rates *(list)*| |`rates`|Angular rates update|Angular rates (*list*)|
|`channels`|Raw RC channels update|Raw RC channels *(list)*| |`channels`|Raw RC channels update|Raw RC channels (*list*)|
|`motors`|Motor outputs update|Motor outputs *(list)*| |`motors`|Motor outputs update|Motor outputs (*list*)|
|`acc`|Accelerometer update|Accelerometer output *(list)*| |`acc`|Accelerometer update|Accelerometer output (*list*)|
|`gyro`|Gyroscope update|Gyroscope output *(list)*| |`gyro`|Gyroscope update|Gyroscope output (*list*)|
|`mavlink`|Received MAVLink message|Message object| |`mavlink`|Received MAVLink message|Message object|
|`mavlink.<message_name>`|Received specific MAVLink message|Message object| |`mavlink.<message_name>`|Received specific MAVLink message|Message object|
|`mavlink.<message_id>`|Received specific MAVLink message|Message object| |`mavlink.<message_id>`|Received specific MAVLink message|Message object|
@@ -277,3 +277,7 @@ logger = logging.getLogger('flix')
logger.setLevel(logging.DEBUG) # be more verbose logger.setLevel(logging.DEBUG) # be more verbose
logger.setLevel(logging.WARNING) # be less verbose logger.setLevel(logging.WARNING) # be less verbose
``` ```
## Stability
The library is in development stage. The API is not stable.