282 lines
9.1 KiB
C++
Executable File
282 lines
9.1 KiB
C++
Executable File
#include <atomic>
|
|
#include <chrono>
|
|
#include <iostream>
|
|
#include <string>
|
|
#include <thread>
|
|
|
|
#include <boost/program_options.hpp>
|
|
#include <VmbC/VmbC.h>
|
|
|
|
#include "Camera.h"
|
|
#include "MQTT.h"
|
|
#include "Parser.h"
|
|
#include "timing.h"
|
|
|
|
namespace po = boost::program_options;
|
|
|
|
/* ------------------------------------------------------------------ */
|
|
/* Application State */
|
|
/* ------------------------------------------------------------------ */
|
|
|
|
struct AppState {
|
|
std::atomic<bool> running{true};
|
|
|
|
bool motorctl_info_out{false};
|
|
bool init_gimbal{false};
|
|
bool start_gimbal{false};
|
|
bool trigger_after_stopping{false};
|
|
|
|
double imagerate{0.1};
|
|
int ctl_code{0};
|
|
std::string mqtt_hdg{"0"};
|
|
|
|
std::string fwt_name{"Dev"};
|
|
Parser parse_cmd;
|
|
Timer loop_timer;
|
|
};
|
|
|
|
static AppState g_state;
|
|
|
|
/* ------------------------------------------------------------------ */
|
|
/* Helpers */
|
|
/* ------------------------------------------------------------------ */
|
|
|
|
static void readInput() {
|
|
std::string input;
|
|
while (g_state.running) {
|
|
std::getline(std::cin, input);
|
|
if (input == "exit") {
|
|
g_state.running = false;
|
|
break;
|
|
}
|
|
std::cout << "Received input: " << input << "\n";
|
|
g_state.parse_cmd.parse_input(input);
|
|
}
|
|
}
|
|
|
|
static void parseCommandLine(int argc, char* argv[], bool& init, bool& start) {
|
|
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");
|
|
|
|
po::variables_map vm;
|
|
try {
|
|
po::store(po::parse_command_line(argc, argv, desc), vm);
|
|
po::notify(vm);
|
|
|
|
if (vm.count("help")) {
|
|
std::cout << desc << "\n";
|
|
std::exit(0);
|
|
}
|
|
|
|
if (vm.count("init")) {
|
|
init = vm["init"].as<bool>();
|
|
std::cout << "find endstops: " << init << "\n";
|
|
} else {
|
|
std::cout << "init was not set.\n";
|
|
}
|
|
|
|
if (vm.count("start")) {
|
|
start = vm["start"].as<bool>();
|
|
std::cout << "starting gimbal: " << start << "\n";
|
|
} else {
|
|
std::cout << "Not starting Gimbal automatically.\n";
|
|
}
|
|
} catch (const std::exception& e) {
|
|
std::cerr << "ARGS Error: " << e.what() << "\n";
|
|
std::exit(1);
|
|
} catch (...) {
|
|
std::cerr << "Unknown ARGS error!\n";
|
|
std::exit(1);
|
|
}
|
|
}
|
|
|
|
static void initGimbal(SerialPort& serial) {
|
|
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));
|
|
serial.get_controller_info();
|
|
serial.sendCommand("y");
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(500));
|
|
serial.sendCommand("ud80");
|
|
std::cout << "Gimbal init finished." << std::endl;
|
|
}
|
|
|
|
static void printMotorInfo(const motor_info& info) {
|
|
std::cout << "_________________________________\n"
|
|
<< "Encoder act: " << info.Xenc << "\n"
|
|
<< "HDG: " << info.hdg << "\n"
|
|
<< "Encoder err: " << info.Xerr << "\n"
|
|
<< "sgt_stat/sgt_val/not_moving: " << info.sgt_stat
|
|
<< " / " << info.sgt_val << " / " << info.is_moving << "\n"
|
|
<< "Driver Status: " << info.control_status << "\n"
|
|
<< "Deviation Warning: " << info.deviation_warn << "\n"
|
|
<< "temp/humid/fan_pwm(0-255): " << info.temp
|
|
<< " / " << info.humid << " / " << info.fan_pwm << "\n";
|
|
}
|
|
|
|
static void processCommands(SerialPort& serial, VimbaHandler& cam) {
|
|
parser_data cmd_data = g_state.parse_cmd.get_parser_data();
|
|
CMD_eval eval;
|
|
InputCommands cmds = eval.eval(g_state.parse_cmd);
|
|
|
|
if (cmds == no_cmd) return;
|
|
|
|
switch (cmds) {
|
|
case startcamera:
|
|
std::cout << "starting Camera" << std::endl;
|
|
cam.Start();
|
|
break;
|
|
case stopcamera:
|
|
std::cout << "stopping Camera" << std::endl;
|
|
cam.Stop();
|
|
break;
|
|
case setcamera:
|
|
cam.evaluateCommand(cmd_data.option, cmd_data.command_val);
|
|
break;
|
|
case setmotorcontrol:
|
|
std::cout << "send command: " << cmd_data.option << std::endl;
|
|
serial.sendCommand(cmd_data.option);
|
|
break;
|
|
case setimagerate:
|
|
g_state.imagerate = cmd_data.command_val;
|
|
std::cout << "imagerate is: " << g_state.imagerate << std::endl;
|
|
break;
|
|
case setdebug:
|
|
g_state.motorctl_info_out = !g_state.motorctl_info_out;
|
|
std::cout << "debug is: " << g_state.motorctl_info_out << std::endl;
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
static void handleCameraTrigger(
|
|
SerialPort& serial,
|
|
const motor_info& act_info,
|
|
VimbaHandler& cam)
|
|
{
|
|
if (!cam.cam_started) return;
|
|
|
|
const double interval_ms = 1000.0 / g_state.imagerate;
|
|
|
|
if (g_state.ctl_code == 0) {
|
|
if (g_state.loop_timer.ElapsedMillis() > interval_ms && act_info.is_moving == 1) {
|
|
g_state.loop_timer.Reset();
|
|
g_state.trigger_after_stopping = true;
|
|
serial.sendCommand("p");
|
|
}
|
|
if (g_state.trigger_after_stopping && g_state.loop_timer.ElapsedMillis() > 100 && act_info.is_moving == 1) {
|
|
if (cam.TriggerCamera()) {
|
|
std::cout << "trigger camera" << std::endl;
|
|
g_state.trigger_after_stopping = false;
|
|
}
|
|
}
|
|
} else if (g_state.ctl_code == 1) {
|
|
if (g_state.loop_timer.ElapsedMillis() > interval_ms && act_info.is_moving == 1) {
|
|
g_state.loop_timer.Reset();
|
|
g_state.trigger_after_stopping = true;
|
|
serial.sendCommand("kd" + g_state.mqtt_hdg);
|
|
std::cout << "ctl " << g_state.ctl_code << std::endl;
|
|
std::cout << "hdg " << g_state.mqtt_hdg << std::endl;
|
|
}
|
|
if (g_state.trigger_after_stopping && g_state.loop_timer.ElapsedMillis() > 100 && act_info.is_moving == 1) {
|
|
if (cam.TriggerCamera()) {
|
|
std::cout << "trigger camera" << std::endl;
|
|
g_state.trigger_after_stopping = false;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/* ------------------------------------------------------------------ */
|
|
/* Main */
|
|
/* ------------------------------------------------------------------ */
|
|
|
|
int main(int argc, char* argv[]) {
|
|
/* ---- Parse CLI arguments ---- */
|
|
bool init = false, start = false;
|
|
parseCommandLine(argc, argv, init, start);
|
|
g_state.init_gimbal = init;
|
|
g_state.start_gimbal = start;
|
|
|
|
/* ---- Serial communication ---- */
|
|
SerialPort serial("/dev/ttyACM0", 115200);
|
|
std::thread io_thread;
|
|
try {
|
|
serial.startReading();
|
|
io_thread = std::thread([&serial]() { serial.run(); });
|
|
} catch (const std::exception& e) {
|
|
std::cerr << "Serial Exception: " << e.what() << std::endl;
|
|
}
|
|
|
|
/* ---- MQTT connection ---- */
|
|
MQTTClient mqtt_client("172.16.42.1", g_state.fwt_name, g_state.fwt_name);
|
|
const std::string mqtt_STATUS = "GGS/FWT/" + g_state.fwt_name + "/StatusCode";
|
|
|
|
mqtt_client.connect_client();
|
|
while (!mqtt_client.connected) {
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(50));
|
|
}
|
|
mqtt_client.publish(mqtt_STATUS, "0");
|
|
|
|
/* ---- Input thread ---- */
|
|
std::thread inputThread(readInput);
|
|
|
|
/* ---- Camera handler ---- */
|
|
motor_info act_info;
|
|
VimbaHandler cam_handler(NULL, &mqtt_client, &act_info);
|
|
|
|
/* ---- Gimbal initialization ---- */
|
|
if (g_state.init_gimbal) {
|
|
initGimbal(serial);
|
|
}
|
|
|
|
/* ---- Main loop ---- */
|
|
while (g_state.running) {
|
|
/* Commands from stdin */
|
|
processCommands(serial, cam_handler);
|
|
|
|
/* Auto-start camera if requested */
|
|
if (g_state.start_gimbal) {
|
|
std::cout << "starting Camera" << std::endl;
|
|
cam_handler.Start();
|
|
g_state.start_gimbal = false;
|
|
}
|
|
|
|
/* Motor controller info */
|
|
act_info = serial.get_controller_info();
|
|
if (g_state.motorctl_info_out) {
|
|
printMotorInfo(act_info);
|
|
}
|
|
|
|
/* MQTT inbound messages */
|
|
mqtt_sub_data mqtt_in = mqtt_client.callback.get_sub_data();
|
|
if (mqtt_in.ctl_avail) {
|
|
g_state.ctl_code = mqtt_in.control_code;
|
|
mqtt_client.publish(mqtt_STATUS, std::to_string(g_state.ctl_code));
|
|
}
|
|
if (mqtt_in.hdg_avail) {
|
|
g_state.mqtt_hdg = mqtt_in.target_heading;
|
|
}
|
|
|
|
/* Gimbal control and camera trigger */
|
|
handleCameraTrigger(serial, act_info, cam_handler);
|
|
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
|
}
|
|
|
|
/* ---- Cleanup ---- */
|
|
serial.stop();
|
|
io_thread.join();
|
|
inputThread.join();
|
|
|
|
return 0;
|
|
}
|