diff --git a/flix/cli.ino b/flix/cli.ino index c6be7cd..80e5d95 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, AUTO; extern float loopRate, dt; extern double t; extern uint16_t channels[16]; extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlArmed, controlMode; +extern int mode; extern bool armed; const char* motd = @@ -34,6 +36,7 @@ const char* motd = "imu - show IMU data\n" "arm - arm the drone (when no armed switch)\n" "disarm - disarm the drone (when no armed switch)\n" +"stab/acro/auto - set mode (when no mode switch)\n" "rc - show RC data\n" "mot - show motor output\n" "log - dump in-RAM log\n" @@ -116,6 +119,12 @@ void doCommand(String str, bool echo = false) { armed = true; } else if (command == "disarm") { armed = false; + } else if (command == "stab") { + mode = STAB; + } else if (command == "acro") { + mode = ACRO; + } else if (command == "auto") { + mode = AUTO; } else if (command == "rc") { print("channels: "); for (int i = 0; i < 16; i++) { diff --git a/flix/control.ino b/flix/control.ino index b23c786..b837ccd 100644 --- a/flix/control.ino +++ b/flix/control.ino @@ -34,7 +34,8 @@ #define TILT_MAX radians(30) #define RATES_D_LPF_ALPHA 0.2 // cutoff frequency ~ 40 Hz -enum { MANUAL, ACRO, STAB, AUTO } mode = STAB; +const int MANUAL = 0, ACRO = 1, STAB = 2, AUTO = 3; // flight modes +int mode = STAB; bool armed = false; PID rollRatePID(ROLLRATE_P, ROLLRATE_I, ROLLRATE_D, ROLLRATE_I_LIM, RATES_D_LPF_ALPHA); diff --git a/flix/mavlink.ino b/flix/mavlink.ino index b7f0ae7..7cbd853 100644 --- a/flix/mavlink.ino +++ b/flix/mavlink.ino @@ -226,8 +226,8 @@ void handleMavlink(const void *_msg) { } if (m.command == MAV_CMD_DO_SET_MODE) { - if (!(m.param2 >= 0 && m.param2 <= AUTO)) return; - mode = static_cast(m.param2); + if (!(m.param2 >= 0 && m.param2 <= AUTO)) return; // incorrect mode + mode = m.param2; mavlink_msg_command_ack_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &ack, m.command, MAV_RESULT_ACCEPTED, UINT8_MAX, 0, msg.sysid, msg.compid); sendMessage(&ack); }