339 lines
13 KiB
C++
339 lines
13 KiB
C++
#include <exception>
|
|
#include <iostream>
|
|
#include "cxxopts.hpp"
|
|
#include <thread>
|
|
#include <string>
|
|
#include <atomic>
|
|
#include <VmbC/VmbC.h>
|
|
#include <boost/program_options.hpp>
|
|
#include "Parser.h"
|
|
#include "Camera.h"
|
|
#include "MQTT.h"
|
|
#include "timing.h"
|
|
#include "fgc/Config.h"
|
|
#include "fgc/Paths.h"
|
|
|
|
namespace po = boost::program_options;
|
|
|
|
std::atomic<bool> running(true);
|
|
Timer loop_timer;
|
|
bool motorctl_info_out=false;
|
|
bool init_gimbal = false;
|
|
bool start_gimbal = false;
|
|
bool start_demo = false;
|
|
bool trigger_after_stopping = false;
|
|
double imagerate = 0.1;
|
|
int ctl_code = 0;
|
|
std::string mqtt_hdg = "0";
|
|
Parser parse_cmd;
|
|
std::string fwt_name = "";
|
|
std::string server_ip = "";
|
|
mqtt_sub_data mqtt_in;
|
|
MQTTClient* mqtt_client = nullptr;
|
|
std::vector<std::string> camera_id_vec; //= { "192.168.11.101" , "192.168.11.102", "192.168.11.103" }; //{ "DEV_1AB22C04F7A9" }; //{ "192.168.11.101" , "192.168.11.102", "192.168.11.103" };
|
|
|
|
void readInput() {
|
|
std::string input;
|
|
while (running) {
|
|
if (!std::getline(std::cin, input)) {
|
|
// stdin geschlossen oder EOF (z.B. unter systemd)
|
|
// Einfach Thread schlafen legen und nichts tun
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(500));
|
|
continue;
|
|
}
|
|
if (input.empty()) continue; // leere Zeilen ignorieren
|
|
if (input == "exit") {
|
|
running = false;
|
|
break;
|
|
}
|
|
std::cout << "Received input: " << input << "\n";
|
|
parse_cmd.parse_input(input);
|
|
}
|
|
}
|
|
|
|
int main(int argc, char* argv[])
|
|
{
|
|
// Resolve the config file from a search order (no hardcoded paths):
|
|
// --config (added in a later phase) -> $FGC_CONFIG -> ./config.ini
|
|
// -> <exe dir>/config.ini -> $XDG_CONFIG_HOME/fire_gimbal_control/config.ini
|
|
auto config_path = fgc::paths::resolveConfigPath();
|
|
if (!config_path) {
|
|
std::cerr << "No config.ini found. Searched:\n";
|
|
for (const auto& p : fgc::paths::configSearchPaths())
|
|
std::cerr << " - " << p << "\n";
|
|
std::cerr << "Copy config/config.example.ini to one of these locations "
|
|
"or set $FGC_CONFIG." << std::endl;
|
|
return 1;
|
|
}
|
|
|
|
fgc::AppConfig cfg;
|
|
try {
|
|
cfg = fgc::ConfigLoader::loadFromFile(*config_path);
|
|
} catch (const std::exception& e) {
|
|
std::cerr << "Config error (" << *config_path << "): " << e.what() << std::endl;
|
|
return 1;
|
|
}
|
|
|
|
// Bridge typed config into the existing globals/locals.
|
|
camera_id_vec = cfg.camera.ids;
|
|
fwt_name = cfg.general.tower_name;
|
|
imagerate = cfg.image_rate();
|
|
motorctl_info_out = cfg.general.debug;
|
|
server_ip = cfg.network.broker_ip;
|
|
std::string mqtt_user = cfg.network.mqtt_user;
|
|
std::string mqtt_pw = cfg.network.mqtt_pw;
|
|
|
|
std::cout << "Loaded config: " << *config_path << "\n";
|
|
std::cout << camera_id_vec.size() << " camera(s) configured.\n";
|
|
std::cout << "tower_name: " << fwt_name << "\n";
|
|
std::cout << "image_interval: " << cfg.general.image_interval << " s\n";
|
|
std::cout << "debug: " << cfg.general.debug << "\n";
|
|
std::cout << "broker: " << server_ip << std::endl;
|
|
|
|
|
|
//args
|
|
// Declare the supported options
|
|
po::options_description desc("Allowed options");
|
|
desc.add_options()
|
|
("help,h", "produce help message")
|
|
("init,i", po::value<bool>(), "find endstops")
|
|
("start,s", po::value<bool>(), "start gimbal system automatically")
|
|
("demo,d", po::value<bool>(), "demo mode (simulation)");
|
|
po::variables_map vm;
|
|
try {
|
|
// Parse command-line options
|
|
po::store(po::parse_command_line(argc, argv, desc), vm);
|
|
po::notify(vm);
|
|
|
|
if (vm.count("help")) {
|
|
std::cout << desc << "\n";
|
|
return 0;
|
|
}
|
|
|
|
if (vm.count("init")) {
|
|
std::cout << "find endstops " << vm["init"].as<bool>() << ".\n";
|
|
init_gimbal = true;
|
|
}
|
|
else {
|
|
std::cout << "init was not set.\n"; init_gimbal = false;
|
|
}
|
|
|
|
if (vm.count("start")) {
|
|
std::cout << "starting gimbal is: " << vm["start"].as<bool>() << ".\n";
|
|
start_gimbal = true;
|
|
}
|
|
else {
|
|
std::cout << "Not starting Gimbal automatically.\n"; start_gimbal = false;
|
|
}
|
|
|
|
if (vm.count("demo")) {
|
|
std::cout << "demomode is: " << vm["demo"].as<bool>() << ".\n";
|
|
start_demo = true;
|
|
}
|
|
else {
|
|
std::cout << "demomode is not set.\n"; start_demo = false;
|
|
}
|
|
}
|
|
catch (std::exception& e) {
|
|
std::cerr << "ARGS Error: " << e.what() << "\n";
|
|
return 1;
|
|
}
|
|
catch (...) {
|
|
std::cerr << "Unknown ARGS error!" << "\n";
|
|
return 1;
|
|
}
|
|
//main vars
|
|
SerialPort serial(cfg.serial.device, cfg.serial.baud);
|
|
std::thread io_thread;
|
|
|
|
const std::string mqtt_STATUS = "GGS/FWT/" + fwt_name + "/StatusCode";
|
|
mqtt_client =new MQTTClient(server_ip, fwt_name, fwt_name, mqtt_user, mqtt_pw);
|
|
mqtt_client->connect_client();
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(500));
|
|
if (!mqtt_client->connected) {
|
|
std::cerr << "MQTT not connected, exit!" << "\n";
|
|
return 1;
|
|
}
|
|
mqtt_client->publish(mqtt_STATUS, "0");
|
|
//cmd parser
|
|
/*
|
|
try {
|
|
cxxopts::Options options("Firewatch Gimbal Software", "Base Gimbal Software C++");
|
|
|
|
options.add_options()
|
|
("h,help", "Print usage")
|
|
("d,debug", "Enable debugging")
|
|
("f,file", "File name", cxxopts::value<std::string>())
|
|
("i,integer", "An integer", cxxopts::value<int>()->default_value("10"))
|
|
;
|
|
|
|
auto result = options.parse(argc, argv);
|
|
if (result.count("help")) {
|
|
std::cout << options.help() << std::endl;
|
|
return 0;
|
|
}
|
|
if (result.count("debug")) {
|
|
std::cout << "Debugging enabled" << std::endl;
|
|
}
|
|
if (result.count("file")) {
|
|
std::cout << "File: " << result["file"].as<std::string>() << std::endl;
|
|
}
|
|
std::cout << "Integer: " << result["integer"].as<int>() << std::endl;
|
|
}
|
|
catch (const cxxopts::exceptions::exception& e) {
|
|
std::cerr << "Error parsing options: " << e.what() << std::endl;
|
|
return 1;
|
|
}
|
|
*/
|
|
|
|
//serial communication
|
|
try {
|
|
// Start reading from the serial port
|
|
serial.startReading();
|
|
// Run Boost.Asio's I/O service in a separate thread
|
|
io_thread=std::thread([&serial]() {
|
|
serial.run();
|
|
});
|
|
// Send commands in the main thread or from another thread
|
|
}
|
|
catch (const std::exception& e) {
|
|
std::cerr << "Serial Exception: " << e.what() << std::endl;
|
|
}
|
|
|
|
//start cin thread
|
|
std::thread inputThread(readInput);
|
|
const std::chrono::milliseconds interval(100);
|
|
motor_info act_info;
|
|
//Camera Obj
|
|
VimbaHandler cam_handler(camera_id_vec, mqtt_client, &act_info, start_demo);
|
|
cam_handler.SetTowerName(fwt_name);
|
|
cam_handler.SetOutputDir(cfg.paths.output_dir);
|
|
//init Gimbal
|
|
if (init_gimbal) {
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(500));
|
|
serial.sendCommand("r");
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(500));
|
|
serial.sendCommand("e");
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(500));
|
|
serial.sendCommand("q");
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(60000));
|
|
act_info = serial.get_controller_info();
|
|
//while (act_info.is_moving==0) {
|
|
// std::this_thread::sleep_for(std::chrono::milliseconds(500));
|
|
// act_info = serial.get_controller_info();
|
|
// if (act_info.is_moving == 1) {
|
|
// std::this_thread::sleep_for(std::chrono::milliseconds(2000));
|
|
// act_info = serial.get_controller_info();
|
|
// }
|
|
//}
|
|
serial.sendCommand("y");
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(500));
|
|
serial.sendCommand("ud56"); // set number of imtervals per turn
|
|
std::cout << "Gimbal init finished." << std::endl;
|
|
}
|
|
//loop
|
|
while (running) {
|
|
//check parser and evaluate commands
|
|
parser_data cmd_data = parse_cmd.get_parser_data();
|
|
CMD_eval eval;
|
|
InputCommands cmds;
|
|
|
|
cmds=eval.eval(parse_cmd);
|
|
if (cmds != no_cmd) {
|
|
if (cmds == startcamera) {
|
|
std::cout << "starting Camera" << std::endl;
|
|
cam_handler.Start();
|
|
}
|
|
else if (cmds == stopcamera) {
|
|
std::cout << "stopping Camera" << std::endl;
|
|
cam_handler.Stop();
|
|
}
|
|
else if (cmds == setcamera) {
|
|
cam_handler.evaluateCommand(cmd_data.option, cmd_data.command_val);
|
|
}
|
|
else if (cmds == setmotorcontrol) {
|
|
std::cout << "send command: " << cmd_data.option << std::endl;
|
|
serial.sendCommand(cmd_data.option);
|
|
}
|
|
else if (cmds == setimagerate) {
|
|
imagerate = cmd_data.command_val;
|
|
std::cout << "imagerate is: " << imagerate << std::endl;
|
|
}
|
|
else if (cmds == setdebug) {
|
|
motorctl_info_out = !motorctl_info_out;
|
|
std::cout << "debug is: " << motorctl_info_out << std::endl;
|
|
}
|
|
}if (start_gimbal) {
|
|
std::cout << "starting Camera" << std::endl;
|
|
cam_handler.Start();
|
|
start_gimbal = false;
|
|
}
|
|
act_info= serial.get_controller_info();
|
|
if(motorctl_info_out)
|
|
std::cout << "_________________________________\nEncoder act: "<< act_info.Xenc << "\nHDG: " << act_info.hdg << "\nEncoder err: " << act_info.Xerr << "\nsgt_stat/sgt_val/not_moving: " << act_info.sgt_stat <<" / "<<act_info.sgt_val << " / " << act_info.is_moving << "\nDriver Status: " << act_info.control_status << "\nDeviation Warning: " << act_info.deviation_warn << "\ntemp/humid/fan_pwm(0-255): " << act_info.temp << " / " << act_info.humid << " / " << act_info.fan_pwm << std::endl;
|
|
|
|
//mqtt
|
|
mqtt_in = mqtt_client->callback.get_sub_data();
|
|
if (mqtt_in.ctl_avail) {
|
|
ctl_code = mqtt_in.control_code;
|
|
mqtt_client->publish(mqtt_STATUS, std::to_string(ctl_code));
|
|
}
|
|
if (mqtt_in.hdg_avail) {
|
|
mqtt_hdg = mqtt_in.target_heading;
|
|
}
|
|
//gimbal control
|
|
if (ctl_code == 0) {
|
|
if (loop_timer.ElapsedMillis() > (1000 / imagerate) && cam_handler.cam_started) {
|
|
if (act_info.is_moving == 1) {
|
|
loop_timer.Reset();
|
|
trigger_after_stopping = true;
|
|
serial.sendCommand("p");
|
|
//if (cam_handler.TriggerCamera()) {
|
|
//}
|
|
//wait for stop and trigger
|
|
}
|
|
}
|
|
if (trigger_after_stopping && loop_timer.ElapsedMillis() > 100) {
|
|
if (act_info.is_moving == 1) {
|
|
if (cam_handler.TriggerCamera()) {
|
|
std::cout << "Temperature:" << act_info.temp << ", Humid:" << act_info.humid << ", FAN RPM:" << act_info.fan_pwm << std::endl;
|
|
trigger_after_stopping = false;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
else if (ctl_code == 1) {
|
|
|
|
if (loop_timer.ElapsedMillis() > (1000 / imagerate)&& cam_handler.cam_started) {
|
|
if (act_info.is_moving == 1) {
|
|
loop_timer.Reset();
|
|
trigger_after_stopping = true;
|
|
serial.sendCommand("kd"+mqtt_hdg);
|
|
std::cout << "ctl" << ctl_code << std::endl;
|
|
std::cout << "hdg" << mqtt_hdg << std::endl;
|
|
//if (cam_handler.TriggerCamera()) {
|
|
//}
|
|
//wait for stop and trigger
|
|
}
|
|
}
|
|
if (trigger_after_stopping && loop_timer.ElapsedMillis() > 100) {
|
|
if (act_info.is_moving == 1) {
|
|
if (cam_handler.TriggerCamera()) {
|
|
trigger_after_stopping = false;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
|
}
|
|
//stop serial thread
|
|
serial.stop();
|
|
io_thread.join();
|
|
//stop cin thread
|
|
inputThread.join();
|
|
// Clean up
|
|
delete mqtt_client;
|
|
mqtt_client = nullptr;
|
|
return 0;
|
|
}
|