From b21e81a68b5e853e7ccf6b36e5d598f44e2a1ad6 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 26 Aug 2025 21:55:27 +0300 Subject: [PATCH] Add cli commands for switching mode Make mode variable int instead of enum, which is more convinient. --- flix/cli.ino | 7 +++++++ flix/control.ino | 4 ++-- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/flix/cli.ino b/flix/cli.ino index 92942e1..c022ceb 100644 --- a/flix/cli.ino +++ b/flix/cli.ino @@ -8,10 +8,12 @@ #include "util.h" extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT; +extern const int ACRO, STAB; extern float loopRate, dt; extern double t; extern uint16_t channels[16]; extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlArmed, controlMode; +extern int mode; const char* motd = "\nWelcome to\n" @@ -31,6 +33,7 @@ const char* motd = "ps - show pitch/roll/yaw\n" "psq - show attitude quaternion\n" "imu - show IMU data\n" +"stab / acro - set mode\n" "rc - show RC data\n" "mot - show motor output\n" "log - dump in-RAM log\n" @@ -106,6 +109,10 @@ void doCommand(String str, bool echo = false) { printIMUInfo(); printIMUCalibration(); print("landed: %d\n", landed); + } else if (command == "stab") { + mode = STAB; + } else if (command == "acro") { + mode = ACRO; } else if (command == "rc") { print("channels: "); for (int i = 0; i < 16; i++) { diff --git a/flix/control.ino b/flix/control.ino index 2d5db74..5bd7756 100644 --- a/flix/control.ino +++ b/flix/control.ino @@ -34,8 +34,8 @@ #define TILT_MAX radians(30) #define RATES_D_LPF_ALPHA 0.2 // cutoff frequency ~ 40 Hz -enum { MANUAL, ACRO, STAB } mode = STAB; -enum { YAW, YAW_RATE } yawMode = YAW; +const int MANUAL = 0, ACRO = 1, STAB = 2; // flight modes +int mode = STAB; bool armed = false; PID rollRatePID(ROLLRATE_P, ROLLRATE_I, ROLLRATE_D, ROLLRATE_I_LIM, RATES_D_LPF_ALPHA);