mirror of
https://github.com/okalachev/flix.git
synced 2026-01-10 04:57:17 +00:00
Rename manual mode to raw mode
Make it callable from the console. Increase the coefficient. Corresponding change in pyflix. pyflix@0.11.
This commit is contained in:
@@ -211,9 +211,9 @@ The default mode is *STAB*. In this mode, the drone stabilizes its attitude (ori
|
|||||||
|
|
||||||
In this mode, the pilot controls the angular rates. This control method is difficult to fly and mostly used in FPV racing.
|
In this mode, the pilot controls the angular rates. This control method is difficult to fly and mostly used in FPV racing.
|
||||||
|
|
||||||
#### MANUAL
|
#### RAW
|
||||||
|
|
||||||
Manual mode disables all the stabilization, and the pilot inputs are passed directly to the motors. This mode is intended for testing and demonstration purposes only, and basically the drone **cannot fly in this mode**.
|
*RAW* mode disables all the stabilization, and the pilot inputs are mixed directly to the motors. The IMU sensor is not involved. This mode is intended for testing and demonstration purposes only, and basically the drone **cannot fly in this mode**.
|
||||||
|
|
||||||
#### AUTO
|
#### AUTO
|
||||||
|
|
||||||
|
|||||||
@@ -8,7 +8,7 @@
|
|||||||
#include "util.h"
|
#include "util.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 ACRO, STAB, AUTO;
|
extern const int RAW, ACRO, STAB, AUTO;
|
||||||
extern float t, dt, loopRate;
|
extern float t, dt, loopRate;
|
||||||
extern uint16_t channels[16];
|
extern uint16_t channels[16];
|
||||||
extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode;
|
extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode;
|
||||||
@@ -35,7 +35,7 @@ const char* motd =
|
|||||||
"imu - show IMU data\n"
|
"imu - show IMU data\n"
|
||||||
"arm - arm the drone\n"
|
"arm - arm the drone\n"
|
||||||
"disarm - disarm the drone\n"
|
"disarm - disarm the drone\n"
|
||||||
"stab/acro/auto - set mode\n"
|
"raw/stab/acro/auto - set mode\n"
|
||||||
"rc - show RC data\n"
|
"rc - show RC data\n"
|
||||||
"wifi - show Wi-Fi info\n"
|
"wifi - show Wi-Fi info\n"
|
||||||
"mot - show motor output\n"
|
"mot - show motor output\n"
|
||||||
@@ -117,6 +117,8 @@ void doCommand(String str, bool echo = false) {
|
|||||||
armed = true;
|
armed = true;
|
||||||
} else if (command == "disarm") {
|
} else if (command == "disarm") {
|
||||||
armed = false;
|
armed = false;
|
||||||
|
} else if (command == "raw") {
|
||||||
|
mode = RAW;
|
||||||
} else if (command == "stab") {
|
} else if (command == "stab") {
|
||||||
mode = STAB;
|
mode = STAB;
|
||||||
} else if (command == "acro") {
|
} else if (command == "acro") {
|
||||||
|
|||||||
@@ -34,7 +34,7 @@
|
|||||||
#define TILT_MAX radians(30)
|
#define TILT_MAX radians(30)
|
||||||
#define RATES_D_LPF_ALPHA 0.2 // cutoff frequency ~ 40 Hz
|
#define RATES_D_LPF_ALPHA 0.2 // cutoff frequency ~ 40 Hz
|
||||||
|
|
||||||
const int MANUAL = 0, ACRO = 1, STAB = 2, AUTO = 3; // flight modes
|
const int RAW = 0, ACRO = 1, STAB = 2, AUTO = 3; // flight modes
|
||||||
int mode = STAB;
|
int mode = STAB;
|
||||||
bool armed = false;
|
bool armed = false;
|
||||||
|
|
||||||
@@ -65,7 +65,6 @@ void control() {
|
|||||||
}
|
}
|
||||||
|
|
||||||
void interpretControls() {
|
void interpretControls() {
|
||||||
// NOTE: put ACRO or MANUAL modes there if you want to use them
|
|
||||||
if (controlMode < 0.25) mode = STAB;
|
if (controlMode < 0.25) mode = STAB;
|
||||||
if (controlMode < 0.75) mode = STAB;
|
if (controlMode < 0.75) mode = STAB;
|
||||||
if (controlMode > 0.75) mode = STAB;
|
if (controlMode > 0.75) mode = STAB;
|
||||||
@@ -93,10 +92,10 @@ void interpretControls() {
|
|||||||
ratesTarget.z = -controlYaw * maxRate.z; // positive yaw stick means clockwise rotation in FLU
|
ratesTarget.z = -controlYaw * maxRate.z; // positive yaw stick means clockwise rotation in FLU
|
||||||
}
|
}
|
||||||
|
|
||||||
if (mode == MANUAL) { // passthrough mode
|
if (mode == RAW) { // direct torque control
|
||||||
attitudeTarget.invalidate(); // skip attitude control
|
attitudeTarget.invalidate(); // skip attitude control
|
||||||
ratesTarget.invalidate(); // skip rate control
|
ratesTarget.invalidate(); // skip rate control
|
||||||
torqueTarget = Vector(controlRoll, controlPitch, -controlYaw) * 0.01;
|
torqueTarget = Vector(controlRoll, controlPitch, -controlYaw) * 0.1;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -157,7 +156,7 @@ void controlTorque() {
|
|||||||
|
|
||||||
const char* getModeName() {
|
const char* getModeName() {
|
||||||
switch (mode) {
|
switch (mode) {
|
||||||
case MANUAL: return "MANUAL";
|
case RAW: return "RAW";
|
||||||
case ACRO: return "ACRO";
|
case ACRO: return "ACRO";
|
||||||
case STAB: return "STAB";
|
case STAB: return "STAB";
|
||||||
case AUTO: return "AUTO";
|
case AUTO: return "AUTO";
|
||||||
|
|||||||
@@ -40,7 +40,7 @@ class Flix:
|
|||||||
|
|
||||||
_connection_timeout = 3
|
_connection_timeout = 3
|
||||||
_print_buffer: str = ''
|
_print_buffer: str = ''
|
||||||
_modes = ['MANUAL', 'ACRO', 'STAB', 'AUTO']
|
_modes = ['RAW', 'ACRO', 'STAB', 'AUTO']
|
||||||
|
|
||||||
def __init__(self, system_id: int=1, wait_connection: bool=True):
|
def __init__(self, system_id: int=1, wait_connection: bool=True):
|
||||||
if not (0 <= system_id < 256):
|
if not (0 <= system_id < 256):
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
[project]
|
[project]
|
||||||
name = "pyflix"
|
name = "pyflix"
|
||||||
version = "0.10"
|
version = "0.11"
|
||||||
description = "Python API for Flix drone"
|
description = "Python API for Flix drone"
|
||||||
authors = [{ name="Oleg Kalachev", email="okalachev@gmail.com" }]
|
authors = [{ name="Oleg Kalachev", email="okalachev@gmail.com" }]
|
||||||
license = "MIT"
|
license = "MIT"
|
||||||
|
|||||||
Reference in New Issue
Block a user