Loading files
This file is pinned to the selected release. Download the original to open it in your design tool.
f2dec78b1This file is pinned to the selected release. Download the original to open it in your design tool.
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
// Repository: https://github.com/okalachev/flix
// Implementation of command line interface
#include "pid.h"
#include "vector.h"
#include "util.h"
extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
extern float loopRate, dt;
extern double t;
extern int rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
const char* motd =
"\nWelcome to\n"
" _______ __ __ ___ ___\n"
"| ____|| | | | \\ \\ / /\n"
"| |__ | | | | \\ V /\n"
"| __| | | | | > <\n"
"| | | `----.| | / . \\\n"
"|__| |_______||__| /__/ \\__\\\n\n"
"Commands:\n\n"
"help - show help\n"
"p - show all parameters\n"
"p <name> - show parameter\n"
"p <name> <value> - set parameter\n"
"preset - reset parameters\n"
"time - show time info\n"
"ps - show pitch/roll/yaw\n"
"psq - show attitude quaternion\n"
"imu - show IMU data\n"
"rc - show RC data\n"
"mot - show motor output\n"
"log - dump in-RAM log\n"
"cr - calibrate RC\n"
"ca - calibrate accel\n"
"mfr, mfl, mrr, mrl - test motor (remove props)\n"
"reset - reset drone's state\n"
"reboot - reboot the drone\n";
void print(const char* format, ...) {
char buf[1000];
va_list args;
va_start(args, format);
vsnprintf(buf, sizeof(buf), format, args);
va_end(args);
Serial.print(buf);
#if WIFI_ENABLED
mavlinkPrint(buf);
#endif
}
void pause(float duration) {
#if ARDUINO
double start = t;
while (t - start < duration) {
step();
handleInput();
#if WIFI_ENABLED
processMavlink();
#endif
}
#else
// Code above won't work in the simulation
delay(duration * 1000);
#endif
}
void doCommand(String str, bool echo = false) {
// parse command
String command, arg0, arg1;
splitString(str, command, arg0, arg1);
// echo command
if (echo && !command.isEmpty()) {
print("> %s\n", str.c_str());
}
// execute command
if (command == "help" || command == "motd") {
print("%s\n", motd);
} else if (command == "p" && arg0 == "") {
printParameters();
} else if (command == "p" && arg0 != "" && arg1 == "") {
print("%s = %g\n", arg0.c_str(), getParameter(arg0.c_str()));
} else if (command == "p") {
bool success = setParameter(arg0.c_str(), arg1.toFloat());
if (success) {
print("%s = %g\n", arg0.c_str(), arg1.toFloat());
} else {
print("Parameter not found: %s\n", arg0.c_str());
}
} else if (command == "preset") {
resetParameters();
} else if (command == "time") {
print("Time: %f\n", t);
print("Loop rate: %f\n", loopRate);
print("dt: %f\n", dt);
} else if (command == "ps") {
Vector a = attitude.toEulerZYX();
print("roll: %f pitch: %f yaw: %f\n", degrees(a.x), degrees(a.y), degrees(a.z));
} else if (command == "psq") {
print("qx: %f qy: %f qz: %f qw: %f\n", attitude.x, attitude.y, attitude.z, attitude.w);
} else if (command == "imu") {
printIMUInfo();
print("gyro: %f %f %f\n", rates.x, rates.y, rates.z);
print("acc: %f %f %f\n", acc.x, acc.y, acc.z);
printIMUCal();
print("rate: %f\n", loopRate);
print("landed: %d\n", landed);
} else if (command == "rc") {
print("Raw: throttle %d yaw %d pitch %d roll %d armed %d mode %d\n",
channels[throttleChannel], channels[yawChannel], channels[pitchChannel],
channels[rollChannel], channels[armedChannel], channels[modeChannel]);
print("Control: throttle %g yaw %g pitch %g roll %g armed %g mode %g\n",
controls[throttleChannel], controls[yawChannel], controls[pitchChannel],
controls[rollChannel], controls[armedChannel], controls[modeChannel]);
print("Mode: %s\n", getModeName());
} else if (command == "mot") {
print("Motors: front-right %g front-left %g rear-right %g rear-left %g\n",
motors[MOTOR_FRONT_RIGHT], motors[MOTOR_FRONT_LEFT], motors[MOTOR_REAR_RIGHT], motors[MOTOR_REAR_LEFT]);
} else if (command == "log") {
dumpLog();
} else if (command == "cr") {
calibrateRC();
} else if (command == "ca") {
calibrateAccel();
} else if (command == "mfr") {
testMotor(MOTOR_FRONT_RIGHT);
} else if (command == "mfl") {
testMotor(MOTOR_FRONT_LEFT);
} else if (command == "mrr") {
testMotor(MOTOR_REAR_RIGHT);
} else if (command == "mrl") {
testMotor(MOTOR_REAR_LEFT);
} else if (command == "reset") {
attitude = Quaternion();
} else if (command == "reboot") {
ESP.restart();
} else if (command == "") {
// do nothing
} else {
print("Invalid command: %s\n", command.c_str());
}
}
void handleInput() {
static bool showMotd = true;
static String input;
if (showMotd) {
print("%s\n", motd);
showMotd = false;
}
while (Serial.available()) {
char c = Serial.read();
if (c == '\n') {
doCommand(input);
input.clear();
} else {
input += c;
}
}
}