117 lines
3.4 KiB
C++
117 lines
3.4 KiB
C++
#include "fgc/OperatingMode.h"
|
|
|
|
#include "fgc/Geometry.h"
|
|
|
|
#include <cctype>
|
|
#include <cmath>
|
|
#include <sstream>
|
|
|
|
namespace fgc {
|
|
|
|
namespace {
|
|
|
|
std::string lower(std::string s) {
|
|
for (char& c : s) c = static_cast<char>(std::tolower(static_cast<unsigned char>(c)));
|
|
return s;
|
|
}
|
|
|
|
std::vector<std::string> words(const std::string& line) {
|
|
std::istringstream in(line);
|
|
std::vector<std::string> out;
|
|
for (std::string w; in >> w;) out.push_back(lower(w));
|
|
return out;
|
|
}
|
|
|
|
// A whole-token number: "30" and "-4.5" yes, "30x" and "" no.
|
|
bool number(const std::string& s, double& out) {
|
|
try {
|
|
size_t used = 0;
|
|
out = std::stod(s, &used);
|
|
return used == s.size() && std::isfinite(out);
|
|
} catch (...) {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
} // namespace
|
|
|
|
const char* modeName(Mode m) {
|
|
switch (m) {
|
|
case Mode::Auto: return "auto";
|
|
case Mode::Pan: return "pan";
|
|
case Mode::Manual: return "manual";
|
|
}
|
|
return "?";
|
|
}
|
|
|
|
std::vector<double> panHeadings(const PanSpec& pan, double step) {
|
|
if (step <= 0.0 || pan.span < step) return {wrap360(pan.heading)};
|
|
const int intervals = static_cast<int>(std::ceil(pan.span / step - 1e-9));
|
|
const double spacing = pan.span / intervals;
|
|
const double left = pan.heading - pan.span / 2.0;
|
|
std::vector<double> out;
|
|
for (int i = 0; i <= intervals; ++i) out.push_back(wrap360(left + i * spacing));
|
|
return out;
|
|
}
|
|
|
|
ModeRequest parseModeCommand(const std::vector<std::string>& tokens) {
|
|
ModeRequest r;
|
|
if (tokens.size() < 2) {
|
|
r.valid = true;
|
|
r.query = true;
|
|
return r;
|
|
}
|
|
const std::string which = lower(tokens[1]);
|
|
if (which == "auto" || which == "manual") {
|
|
if (tokens.size() > 2) {
|
|
r.error = "'mode " + which + "' takes no arguments";
|
|
return r;
|
|
}
|
|
r.mode = which == "auto" ? Mode::Auto : Mode::Manual;
|
|
r.valid = true;
|
|
return r;
|
|
}
|
|
if (which != "pan") {
|
|
r.error = "unknown mode '" + tokens[1] + "' (auto, pan or manual)";
|
|
return r;
|
|
}
|
|
if (tokens.size() < 4 || tokens.size() > 5) {
|
|
r.error = "usage: mode pan <heading> <span> [<pitch>]";
|
|
return r;
|
|
}
|
|
PanSpec p;
|
|
if (!number(tokens[2], p.heading)) {
|
|
r.error = "mode pan: bad heading '" + tokens[2] + "'";
|
|
return r;
|
|
}
|
|
if (!number(tokens[3], p.span) || p.span < 0.0 || p.span > 360.0) {
|
|
r.error = "mode pan: the span must be 0 to 360 degrees (got '" + tokens[3] + "')";
|
|
return r;
|
|
}
|
|
if (tokens.size() == 5 && !number(tokens[4], p.pitch)) {
|
|
r.error = "mode pan: bad pitch '" + tokens[4] + "'";
|
|
return r;
|
|
}
|
|
p.heading = wrap360(p.heading);
|
|
r.mode = Mode::Pan;
|
|
r.pan = p;
|
|
r.valid = true;
|
|
return r;
|
|
}
|
|
|
|
bool allowedIn(Mode mode, const std::string& line) {
|
|
if (mode == Mode::Manual) return true;
|
|
const auto w = words(line);
|
|
if (w.empty()) return true;
|
|
const std::string& verb = w[0];
|
|
const std::string sub = w.size() > 1 ? w[1] : "";
|
|
if (verb == "help" || verb == "mode" || verb == "cancel" || verb == "exit" ||
|
|
verb == "debug" || verb == "trace" || verb == "thermal")
|
|
return true;
|
|
if (verb == "gimbal") return sub == "status" || sub == "dump";
|
|
if (verb == "imu") return sub == "status" || sub == "config";
|
|
return false;
|
|
}
|
|
|
|
} // namespace fgc
|