636 lines
14 KiB
C++
636 lines
14 KiB
C++
// Copyright (C) 2026 Hector van der Aa <hector@h3cx.dev>
|
|
// Copyright (C) 2026 Association Exergie <association.exergie@gmail.com>
|
|
// SPDX-License-Identifier: GPL-3.0-or-later
|
|
|
|
#include "cmd.h"
|
|
|
|
#include <string.h>
|
|
#include "data/track_store.h"
|
|
#include "data/config_store.h"
|
|
#include "data/general_store.h"
|
|
#include "base/router.h"
|
|
|
|
char *Cmd::trimArg(char *input) {
|
|
if (input == nullptr) {
|
|
return nullptr;
|
|
}
|
|
|
|
while (*input == ' ' || *input == '\t' || *input == '\r' || *input == '\n') {
|
|
input++;
|
|
}
|
|
|
|
if (*input == '\0') {
|
|
return input;
|
|
}
|
|
|
|
char *end = input + strlen(input) - 1;
|
|
while (end >= input &&
|
|
(*end == ' ' || *end == '\t' || *end == '\r' || *end == '\n')) {
|
|
*end = '\0';
|
|
end--;
|
|
}
|
|
|
|
return input;
|
|
}
|
|
|
|
unsigned short Cmd::splitArgs(char *input, char *argv[], unsigned short max_args) {
|
|
unsigned short argc = 0;
|
|
char *p = input;
|
|
char *token_start = input;
|
|
|
|
if (input == nullptr || argv == nullptr || max_args == 0) {
|
|
return 0;
|
|
}
|
|
|
|
if (*input == '\0') {
|
|
return 0;
|
|
}
|
|
|
|
while (argc < max_args) {
|
|
if (*p == ',' || *p == '\0') {
|
|
char separator = *p;
|
|
*p = '\0';
|
|
|
|
argv[argc] = trimArg(token_start);
|
|
argc++;
|
|
|
|
if (separator == '\0') {
|
|
break;
|
|
}
|
|
|
|
token_start = p + 1;
|
|
}
|
|
|
|
p++;
|
|
}
|
|
|
|
return argc;
|
|
}
|
|
|
|
Cmd::CommandId Cmd::parseCommandName(const char *input) {
|
|
if (input == nullptr) {
|
|
return Unknown;
|
|
}
|
|
|
|
if (strcmp(input, "REBOOT") == 0) {
|
|
return Reboot;
|
|
}
|
|
|
|
if (strcmp(input, "CFG_DUMP") == 0) {
|
|
return ConfigDump;
|
|
}
|
|
|
|
if (strcmp(input, "TRACK_PUT") == 0) {
|
|
return PutTrack;
|
|
}
|
|
|
|
if (strcmp(input, "TRACK_DELETE") == 0) {
|
|
return DeleteTrack;
|
|
}
|
|
|
|
if (strcmp(input, "TRACK_DUMP") == 0) {
|
|
return DumpTrack;
|
|
}
|
|
|
|
if (strcmp(input, "CFG_RESET") == 0) {
|
|
return ConfigReset;
|
|
}
|
|
|
|
if (strcmp(input, "TRACK_AUTODETECT") == 0) {
|
|
return TrackAutodetect;
|
|
}
|
|
|
|
if (strcmp(input, "DISPLAY_GPS_DEBUG") == 0) {
|
|
return DisplayGpsDebug;
|
|
}
|
|
|
|
if (strcmp(input, "DISPLAY_DRIVER_PRIMARY") == 0) {
|
|
return DisplayDriverPrimary;
|
|
}
|
|
|
|
if (strcmp(input, "BATTERY_CAL") == 0) {
|
|
return BatteryCal;
|
|
}
|
|
|
|
if (strcmp(input, "BATTERY_PRINT_VBAT") == 0) {
|
|
return BatteryPrintVbat;
|
|
}
|
|
|
|
if (strcmp(input, "BATTERY_SET_LOW") == 0) {
|
|
return BatterySetLow;
|
|
}
|
|
|
|
if (strcmp(input, "THERMO_SET_LOW") == 0) {
|
|
return ThermoSetLow;
|
|
}
|
|
|
|
if (strcmp(input, "THERMO_SET_HIGH") == 0) {
|
|
return ThermoSetHigh;
|
|
}
|
|
|
|
return Unknown;
|
|
}
|
|
|
|
int Cmd::parseTrackSlotId(const char *id_str, unsigned short &id_out) {
|
|
if (id_str == nullptr || id_str[0] == '\0') {
|
|
return 1;
|
|
}
|
|
|
|
id_out = strtoul(id_str, nullptr, 10);
|
|
if (id_out < 1 || id_out > 8) {
|
|
return 1;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
int Cmd::dumpTrackSlot(unsigned short id) {
|
|
VehicleConfig config;
|
|
configGlobalRead(config);
|
|
bool occupied = config.track_slot_occupied_[id - 1];
|
|
|
|
TrackData track_data;
|
|
int result = trackGlobalRead(id, track_data);
|
|
if (result != 0) {
|
|
#ifdef ERROR
|
|
if (logger_ != nullptr) {
|
|
logger_->error("Track slot " + String(id) + " has no valid track data");
|
|
}
|
|
#endif
|
|
return 1;
|
|
}
|
|
|
|
#ifdef INFO
|
|
if (logger_ != nullptr) {
|
|
logger_->info("Track dump for slot " + String(id));
|
|
logger_->info(String("\tOccupied flag: ") + String(occupied));
|
|
logger_->info(String("\tID: ") + String(track_data.id_));
|
|
logger_->info(String("\tName: ") + String(track_data.name_));
|
|
logger_->info(String("\tPoint A lat: ") + String(track_data.point_a_.lat_, 6));
|
|
logger_->info(String("\tPoint A lng: ") + String(track_data.point_a_.lng_, 6));
|
|
logger_->info(String("\tPoint B lat: ") + String(track_data.point_b_.lat_, 6));
|
|
logger_->info(String("\tPoint B lng: ") + String(track_data.point_b_.lng_, 6));
|
|
}
|
|
#endif
|
|
|
|
return 0;
|
|
}
|
|
|
|
int Cmd::handleRebootCommand(unsigned short argc) {
|
|
if (argc != 1) {
|
|
#ifdef ERROR
|
|
if (logger_ != nullptr) {
|
|
logger_->error("REBOOT expects no arguments");
|
|
}
|
|
#endif
|
|
return 1;
|
|
}
|
|
|
|
#ifdef INFO
|
|
if (logger_ != nullptr) {
|
|
logger_->info("Rebooting");
|
|
}
|
|
#endif
|
|
delay(200);
|
|
wdt_enable(WDTO_15MS);
|
|
while (true) {
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int Cmd::handleDumpConfigCommand(unsigned short argc) {
|
|
if (argc != 1) {
|
|
#ifdef ERROR
|
|
if (logger_ != nullptr) {
|
|
logger_->error("DUMPCFG expects no arguments");
|
|
}
|
|
#endif
|
|
return 1;
|
|
}
|
|
|
|
#ifdef INFO
|
|
if (logger_ != nullptr) {
|
|
logger_->dumpConfig();
|
|
}
|
|
#endif
|
|
return 0;
|
|
}
|
|
|
|
int Cmd::handleTrackPutCommand(unsigned short argc, char *argv[]) {
|
|
if (argc != 7) {
|
|
#ifdef ERROR
|
|
if (logger_ != nullptr) {
|
|
logger_->error("TRACK_PUT expects 6 arguments");
|
|
}
|
|
#endif
|
|
return 1;
|
|
}
|
|
|
|
TrackData new_track;
|
|
if (parseTrackSlotId(argv[1], new_track.id_) != 0) {
|
|
#ifdef ERROR
|
|
if (logger_ != nullptr) {
|
|
logger_->error(String("ID out of range: ") + String(argv[1]));
|
|
}
|
|
#endif
|
|
return 1;
|
|
}
|
|
|
|
strncpy(new_track.name_, argv[2], sizeof(new_track.name_) - 1);
|
|
new_track.name_[sizeof(new_track.name_) - 1] = '\0';
|
|
|
|
LatLng point_a;
|
|
point_a.lat_ = strtod(argv[3], nullptr);
|
|
point_a.lng_ = strtod(argv[4], nullptr);
|
|
new_track.point_a_ = point_a;
|
|
|
|
LatLng point_b;
|
|
point_b.lat_ = strtod(argv[5], nullptr);
|
|
point_b.lng_ = strtod(argv[6], nullptr);
|
|
new_track.point_b_ = point_b;
|
|
|
|
#ifdef INFO
|
|
if (logger_ != nullptr) {
|
|
logger_->info("Loading new track");
|
|
logger_->info(String("ID: ") + String(new_track.id_));
|
|
logger_->info(String("Name: ") + new_track.name_);
|
|
logger_->info(String("Point A lat: ") + String(new_track.point_a_.lat_));
|
|
logger_->info(String("Point A lng: ") + String(new_track.point_a_.lng_));
|
|
logger_->info(String("Point B lat: ") + String(new_track.point_b_.lat_));
|
|
logger_->info(String("Point B lng: ") + String(new_track.point_b_.lng_));
|
|
}
|
|
#endif
|
|
|
|
trackTempGlobalWrite(new_track);
|
|
return router::send(module::Config, task::ConfigWriteTempTrack);
|
|
}
|
|
|
|
int Cmd::handleTrackDeleteCommand(unsigned short argc, char *argv[]) {
|
|
if (argc != 2) {
|
|
#ifdef ERROR
|
|
if (logger_ != nullptr) {
|
|
logger_->error("TRACK_DELETE expects 1 argument");
|
|
}
|
|
#endif
|
|
return 1;
|
|
}
|
|
|
|
unsigned short id;
|
|
if (parseTrackSlotId(argv[1], id) != 0) {
|
|
#ifdef ERROR
|
|
if (logger_ != nullptr) {
|
|
logger_->error(String("ID out of range: ") + String(argv[1]));
|
|
}
|
|
#endif
|
|
return 1;
|
|
}
|
|
|
|
return router::send(module::Config, task::ConfigTrackDelete, id);
|
|
}
|
|
|
|
int Cmd::handleTrackDumpCommand(unsigned short argc, char *argv[]) {
|
|
if (argc != 2) {
|
|
#ifdef ERROR
|
|
if (logger_ != nullptr) {
|
|
logger_->error("TRACK_DUMP expects 1 argument");
|
|
}
|
|
#endif
|
|
return 1;
|
|
}
|
|
|
|
unsigned short id;
|
|
if (parseTrackSlotId(argv[1], id) != 0) {
|
|
#ifdef ERROR
|
|
if (logger_ != nullptr) {
|
|
logger_->error(String("ID out of range: ") + String(argv[1]));
|
|
}
|
|
#endif
|
|
return 1;
|
|
}
|
|
|
|
return this->dumpTrackSlot(id);
|
|
}
|
|
|
|
int Cmd::handleConfigResetCommand(unsigned short argc) {
|
|
if (argc != 1) {
|
|
#ifdef ERROR
|
|
if (logger_ != nullptr) {
|
|
logger_->error("CFG_RESET expects no arguments");
|
|
}
|
|
#endif
|
|
return 1;
|
|
}
|
|
|
|
#ifdef INFO
|
|
if (logger_ != nullptr) {
|
|
logger_->info("Resetting config");
|
|
}
|
|
#endif
|
|
return router::send(module::Config, task::ConfigReset);
|
|
}
|
|
|
|
int Cmd::handleTrackAutodetectCommand(unsigned short argc) {
|
|
if (argc != 1) {
|
|
#ifdef ERROR
|
|
if (logger_ != nullptr) {
|
|
logger_->error("TRACK_AUTODETECT expects no arguments");
|
|
}
|
|
#endif
|
|
return 1;
|
|
}
|
|
|
|
#ifdef INFO
|
|
if (logger_ != nullptr) {
|
|
logger_->info("Detecting track");
|
|
}
|
|
#endif
|
|
return router::send(module::Config, task::ConfigTrackDetect, 1);
|
|
}
|
|
|
|
int Cmd::handleDisplayGpsDebug(unsigned short argc) {
|
|
if (argc != 1) {
|
|
#ifdef ERROR
|
|
if (logger_ != nullptr) {
|
|
logger_->error("DISPLAY_GPS_DEBUG expects no arguments");
|
|
}
|
|
#endif
|
|
return 1;
|
|
}
|
|
|
|
#ifdef INFO
|
|
if (logger_ != nullptr) {
|
|
logger_->info("Switching to GPS_DEBUG display");
|
|
}
|
|
#endif
|
|
return router::send(module::Lcd, task::DisplayGpsDebug);
|
|
}
|
|
|
|
int Cmd::handleDisplayDriverPrimary(unsigned short argc) {
|
|
if (argc != 1) {
|
|
#ifdef ERROR
|
|
if (logger_ != nullptr) {
|
|
logger_->error("DISPLAY_DRIVER_PRIMARY expects no arguments");
|
|
}
|
|
#endif
|
|
return 1;
|
|
}
|
|
|
|
#ifdef INFO
|
|
if (logger_ != nullptr) {
|
|
logger_->info("Switching to DRIVER_PRIMARY display");
|
|
}
|
|
#endif
|
|
return router::send(module::Lcd, task::DisplayDriverPrimary);
|
|
}
|
|
|
|
int Cmd::handleBatteryCal(unsigned short argc, char *argv[]) {
|
|
if (argc != 2) {
|
|
#ifdef ERROR
|
|
if (logger_ != nullptr) {
|
|
logger_->error("BATTERY_CAL expects 1 argument");
|
|
}
|
|
#endif
|
|
return 1;
|
|
}
|
|
double vbat = strtod(argv[1], nullptr);
|
|
uint32_t task_data;
|
|
memcpy(&task_data, &vbat, sizeof(uint32_t));
|
|
#ifdef INFO
|
|
if (logger_ != nullptr) {
|
|
logger_->info("Calibrating VBAT");
|
|
}
|
|
#endif
|
|
router::send(module::Battery, task::BatteryCal, task_data);
|
|
return 0;
|
|
}
|
|
|
|
int Cmd::handleBatteryPrintVbat(unsigned short argc) {
|
|
if (argc != 1) {
|
|
#ifdef ERROR
|
|
if (logger_ != nullptr) {
|
|
logger_->error("BATTERY_PRINT_VBAT expects no arguments");
|
|
}
|
|
#endif
|
|
return 1;
|
|
}
|
|
|
|
#ifdef INFO
|
|
double vbat;
|
|
vbatGlobalRead(vbat);
|
|
if (logger_ != nullptr) {
|
|
logger_->info("VBAT: " + String(vbat));
|
|
}
|
|
#endif
|
|
return 0;
|
|
}
|
|
|
|
int Cmd::handleBatterySetLow(unsigned short argc, char* argv[]) {
|
|
if (argc != 2) {
|
|
#ifdef ERROR
|
|
if (logger_ != nullptr) {
|
|
logger_->error("BATTERY_SET_LOW expects 1 argument");
|
|
}
|
|
#endif
|
|
return 1;
|
|
}
|
|
double low = strtod(argv[1], nullptr);
|
|
uint32_t task_data;
|
|
memcpy(&task_data, &low, sizeof(uint32_t));
|
|
#ifdef INFO
|
|
if (logger_ != nullptr) {
|
|
logger_->info("Setting warning level for VBAT");
|
|
}
|
|
#endif
|
|
router::send(module::Config, task::ConfigVbatSetLow, task_data);
|
|
return 0;
|
|
}
|
|
|
|
int Cmd::handleThermoSetLow(unsigned short argc, char* argv[]) {
|
|
if (argc != 2) {
|
|
#ifdef ERROR
|
|
if (logger_ != nullptr) {
|
|
logger_->error("THERMO_SET_LOW expects 1 argument");
|
|
}
|
|
#endif
|
|
return 1;
|
|
}
|
|
double low = strtod(argv[1], nullptr);
|
|
uint32_t task_data;
|
|
memcpy(&task_data, &low, sizeof(uint32_t));
|
|
#ifdef INFO
|
|
if (logger_ != nullptr) {
|
|
logger_->info("Setting low level for TENG");
|
|
}
|
|
#endif
|
|
router::send(module::Config, task::ConfigTengSetLow, task_data);
|
|
return 0;
|
|
}
|
|
|
|
int Cmd::handleThermoSetHigh(unsigned short argc, char* argv[]) {
|
|
if (argc != 2) {
|
|
#ifdef ERROR
|
|
if (logger_ != nullptr) {
|
|
logger_->error("THERMO_SET_HIGH expects 1 argument");
|
|
}
|
|
#endif
|
|
return 1;
|
|
}
|
|
double low = strtod(argv[1], nullptr);
|
|
uint32_t task_data;
|
|
memcpy(&task_data, &low, sizeof(uint32_t));
|
|
#ifdef INFO
|
|
if (logger_ != nullptr) {
|
|
logger_->info("Setting high level for TENG");
|
|
}
|
|
#endif
|
|
router::send(module::Config, task::ConfigTengSetLow, task_data);
|
|
return 0;
|
|
}
|
|
|
|
int Cmd::handleUnknownCommand(unsigned short argc, char *argv[]) {
|
|
#ifdef ERROR
|
|
if (logger_ != nullptr) {
|
|
if (argc > 0 && argv != nullptr && argv[0] != nullptr && argv[0][0] != '\0') {
|
|
logger_->error(String("Unknown command: ") + String(argv[0]));
|
|
} else {
|
|
logger_->error("Unknown command");
|
|
}
|
|
}
|
|
#endif
|
|
return 1;
|
|
}
|
|
|
|
int Cmd::dispatchCommand(CommandId command, unsigned short argc, char *argv[]) {
|
|
switch (command) {
|
|
case Reboot:
|
|
return this->handleRebootCommand(argc);
|
|
|
|
case ConfigDump:
|
|
return this->handleDumpConfigCommand(argc);
|
|
|
|
case PutTrack:
|
|
return this->handleTrackPutCommand(argc, argv);
|
|
|
|
case DeleteTrack:
|
|
return this->handleTrackDeleteCommand(argc, argv);
|
|
|
|
case DumpTrack:
|
|
return this->handleTrackDumpCommand(argc, argv);
|
|
|
|
case ConfigReset:
|
|
return this->handleConfigResetCommand(argc);
|
|
|
|
case TrackAutodetect:
|
|
return this->handleTrackAutodetectCommand(argc);
|
|
|
|
case DisplayGpsDebug:
|
|
return this->handleDisplayGpsDebug(argc);
|
|
|
|
case DisplayDriverPrimary:
|
|
return this->handleDisplayDriverPrimary(argc);
|
|
|
|
case BatteryCal:
|
|
return this->handleBatteryCal(argc, argv);
|
|
|
|
case BatteryPrintVbat:
|
|
return this->handleBatteryPrintVbat(argc);
|
|
|
|
case BatterySetLow:
|
|
return this->handleBatterySetLow(argc, argv);
|
|
|
|
case ThermoSetLow:
|
|
return this->handleThermoSetLow(argc, argv);
|
|
|
|
case ThermoSetHigh:
|
|
return this->handleThermoSetHigh(argc, argv);
|
|
|
|
case Unknown:
|
|
default:
|
|
return this->handleUnknownCommand(argc, argv);
|
|
}
|
|
}
|
|
|
|
int Cmd::tryParse() {
|
|
#ifdef DEBUG
|
|
if (logger_ != nullptr) {
|
|
logger_->debug("Attempting to parse command");
|
|
}
|
|
#endif
|
|
|
|
char *argvp[MAX_ARGS];
|
|
unsigned short argc = splitArgs(buffer_, argvp, MAX_ARGS);
|
|
|
|
if (argc == 0 || argvp[0] == nullptr || argvp[0][0] == '\0') {
|
|
#ifdef ERROR
|
|
if (logger_ != nullptr) {
|
|
logger_->error("Empty command");
|
|
}
|
|
#endif
|
|
return 1;
|
|
}
|
|
|
|
CommandId command = parseCommandName(argvp[0]);
|
|
return dispatchCommand(command, argc, argvp);
|
|
}
|
|
|
|
int Cmd::push(const Task &task) {
|
|
return queue_.push(task);
|
|
}
|
|
|
|
Cmd::Cmd(HardwareSerial *data_stream)
|
|
: data_stream_(data_stream), logger_(nullptr) {}
|
|
|
|
Cmd::Cmd(HardwareSerial *data_stream, SystemLogger *logger)
|
|
: data_stream_(data_stream), logger_(logger) {}
|
|
|
|
Cmd::~Cmd() {}
|
|
|
|
int Cmd::init() {
|
|
data_stream_->begin(baud_rate_);
|
|
return 0;
|
|
}
|
|
|
|
int Cmd::parseTask(unsigned long timeout_ms) {
|
|
unsigned long start = millis();
|
|
|
|
while (data_stream_->available()) {
|
|
if ((unsigned long)(millis() - start) >= timeout_ms) {
|
|
return 1;
|
|
}
|
|
|
|
char current_char = data_stream_->read();
|
|
|
|
if (current_char == '<') {
|
|
buffer_open_ = true;
|
|
index_ = 0;
|
|
continue;
|
|
}
|
|
|
|
if (!buffer_open_) {
|
|
continue;
|
|
}
|
|
|
|
if (current_char == '>') {
|
|
buffer_[index_] = '\0';
|
|
buffer_open_ = false;
|
|
return this->tryParse();
|
|
}
|
|
|
|
if (index_ >= sizeof(buffer_) - 1) {
|
|
buffer_open_ = false;
|
|
index_ = 0;
|
|
#ifdef ERROR
|
|
if (logger_ != nullptr) {
|
|
logger_->error("Command parser buffer overflow");
|
|
}
|
|
#endif
|
|
return 1;
|
|
}
|
|
|
|
buffer_[index_] = current_char;
|
|
index_++;
|
|
}
|
|
|
|
return 0;
|
|
}
|