mirror of
https://github.com/okalachev/flix.git
synced 2026-01-11 21:46:55 +00:00
Add cli commands for switching mode
Make mode variable int instead of enum, which is more convinient.
This commit is contained in:
@@ -8,10 +8,12 @@
|
|||||||
#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;
|
||||||
extern float loopRate, dt;
|
extern float loopRate, dt;
|
||||||
extern double t;
|
extern double t;
|
||||||
extern uint16_t channels[16];
|
extern uint16_t channels[16];
|
||||||
extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlArmed, controlMode;
|
extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlArmed, controlMode;
|
||||||
|
extern int mode;
|
||||||
|
|
||||||
const char* motd =
|
const char* motd =
|
||||||
"\nWelcome to\n"
|
"\nWelcome to\n"
|
||||||
@@ -31,6 +33,7 @@ const char* motd =
|
|||||||
"ps - show pitch/roll/yaw\n"
|
"ps - show pitch/roll/yaw\n"
|
||||||
"psq - show attitude quaternion\n"
|
"psq - show attitude quaternion\n"
|
||||||
"imu - show IMU data\n"
|
"imu - show IMU data\n"
|
||||||
|
"stab / acro - set mode\n"
|
||||||
"rc - show RC data\n"
|
"rc - show RC data\n"
|
||||||
"mot - show motor output\n"
|
"mot - show motor output\n"
|
||||||
"log - dump in-RAM log\n"
|
"log - dump in-RAM log\n"
|
||||||
@@ -106,6 +109,10 @@ void doCommand(String str, bool echo = false) {
|
|||||||
printIMUInfo();
|
printIMUInfo();
|
||||||
printIMUCalibration();
|
printIMUCalibration();
|
||||||
print("landed: %d\n", landed);
|
print("landed: %d\n", landed);
|
||||||
|
} else if (command == "stab") {
|
||||||
|
mode = STAB;
|
||||||
|
} else if (command == "acro") {
|
||||||
|
mode = ACRO;
|
||||||
} else if (command == "rc") {
|
} else if (command == "rc") {
|
||||||
print("channels: ");
|
print("channels: ");
|
||||||
for (int i = 0; i < 16; i++) {
|
for (int i = 0; i < 16; i++) {
|
||||||
|
|||||||
@@ -34,8 +34,8 @@
|
|||||||
#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
|
||||||
|
|
||||||
enum { MANUAL, ACRO, STAB } mode = STAB;
|
const int MANUAL = 0, ACRO = 1, STAB = 2; // flight modes
|
||||||
enum { YAW, YAW_RATE } yawMode = YAW;
|
int mode = STAB;
|
||||||
bool armed = false;
|
bool armed = false;
|
||||||
|
|
||||||
PID rollRatePID(ROLLRATE_P, ROLLRATE_I, ROLLRATE_D, ROLLRATE_I_LIM, RATES_D_LPF_ALPHA);
|
PID rollRatePID(ROLLRATE_P, ROLLRATE_I, ROLLRATE_D, ROLLRATE_I_LIM, RATES_D_LPF_ALPHA);
|
||||||
|
|||||||
Reference in New Issue
Block a user