Initial commit
This commit is contained in:
commit
89be503edb
GPG Key ID:
D4A6C394DF13F1D7
|
|
@ -0,0 +1,8 @@
|
||||||
|
{
|
||||||
|
"permissions": {
|
||||||
|
"allow": [
|
||||||
|
"Bash(git add *)",
|
||||||
|
"Bash(grep -iE \"config\\\\.ini|\\\\.out|\\\\.o$|compile_commands\")"
|
||||||
|
]
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,31 @@
|
||||||
|
# ---- Build output ----
|
||||||
|
/build/
|
||||||
|
/obj/
|
||||||
|
*.o
|
||||||
|
*.out
|
||||||
|
bin/**/*.out
|
||||||
|
|
||||||
|
# CMake
|
||||||
|
CMakeCache.txt
|
||||||
|
CMakeFiles/
|
||||||
|
cmake_install.cmake
|
||||||
|
/_deps/
|
||||||
|
|
||||||
|
# Editor / tooling
|
||||||
|
compile_commands.json
|
||||||
|
.cache/
|
||||||
|
.vscode/
|
||||||
|
*.swp
|
||||||
|
|
||||||
|
# ---- Captured images ----
|
||||||
|
*.jxl
|
||||||
|
bin/x64/Release/RGB/
|
||||||
|
bin/x64/Release/ACR/
|
||||||
|
bin/x64/Release/NIR/
|
||||||
|
# but keep the demo-mode placeholder image (force-tracked)
|
||||||
|
!bin/x64/Release/test_smoke.jxl
|
||||||
|
|
||||||
|
# ---- Secrets / machine-specific config ----
|
||||||
|
# Real configs hold plaintext MQTT credentials. Commit config/config.example.ini instead.
|
||||||
|
config.ini
|
||||||
|
bin/x64/Release/config.ini
|
||||||
|
|
@ -0,0 +1,449 @@
|
||||||
|
#include "Camera.h"
|
||||||
|
#include "JPEG_XL.h"
|
||||||
|
#include "timing.h"
|
||||||
|
#include <iostream>
|
||||||
|
#include <mutex>
|
||||||
|
#include <filesystem>
|
||||||
|
#include <opencv2/opencv.hpp>
|
||||||
|
|
||||||
|
|
||||||
|
class FrameObserver : public IFrameObserver
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
typedef std::function<void(int)> Callback;
|
||||||
|
Callback call;
|
||||||
|
int cam_id = 0;
|
||||||
|
std::atomic<int> settle = { 3 };
|
||||||
|
|
||||||
|
void registerCallback(Callback f) {
|
||||||
|
call = f;
|
||||||
|
}
|
||||||
|
|
||||||
|
FrameObserver(CameraPtr camera) : IFrameObserver(camera) {};
|
||||||
|
|
||||||
|
void FrameReceived(const FramePtr pFrame)
|
||||||
|
{
|
||||||
|
int old_val = settle.fetch_sub(1);
|
||||||
|
std::cout << "[FrameObserver cam=" << cam_id << "] FrameReceived settle=" << old_val << " → new=" << old_val - 1 << std::endl;
|
||||||
|
|
||||||
|
if (old_val > 0) {
|
||||||
|
std::cout << "[FrameObserver cam=" << cam_id << "] DUMP (settling)" << std::endl;
|
||||||
|
m_pCamera->QueueFrame(pFrame);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool bQueueDirectly = true;
|
||||||
|
VmbFrameStatusType eReceiveStatus;
|
||||||
|
|
||||||
|
if (VmbErrorSuccess == pFrame->GetReceiveStatus(eReceiveStatus))
|
||||||
|
{
|
||||||
|
/* ignore any incompletely frame */
|
||||||
|
if (VmbFrameStatusComplete != eReceiveStatus)
|
||||||
|
{
|
||||||
|
VmbUint64_t id;
|
||||||
|
std::string name;
|
||||||
|
std::string serial;
|
||||||
|
pFrame->GetFrameID(id);
|
||||||
|
m_pCamera->GetModel(name);
|
||||||
|
m_pCamera->GetSerialNumber(serial);
|
||||||
|
std::cout << "!!!incomplete frame, rcvd error: " << eReceiveStatus << " id: " << id << " camera: " << name << " + " << serial << std::endl;
|
||||||
|
m_pCamera->QueueFrame(pFrame);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
VmbUint64_t id;
|
||||||
|
pFrame->GetFrameID(id);
|
||||||
|
std::cout << "[FrameObserver cam=" << cam_id << "] rcvd frame ID=" << id << " → ENQUEUE" << std::endl;
|
||||||
|
// Lock the frame queue
|
||||||
|
m_FramesMutex.lock();
|
||||||
|
// Add frame to queue
|
||||||
|
m_Frames.push(pFrame);
|
||||||
|
// Unlock frame queue
|
||||||
|
m_FramesMutex.unlock();
|
||||||
|
// Emit the frame received signal
|
||||||
|
call(cam_id);
|
||||||
|
// callback!
|
||||||
|
bQueueDirectly = false;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
std::cout << "[FrameObserver cam=" << cam_id << "] frame rcvd error" << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
// If any error occurred we queue the frame without notification
|
||||||
|
if (true == bQueueDirectly)
|
||||||
|
{
|
||||||
|
m_pCamera->QueueFrame(pFrame);
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
FramePtr GetFrame() {
|
||||||
|
FramePtr res;
|
||||||
|
// Lock the frame queue
|
||||||
|
m_FramesMutex.lock();
|
||||||
|
// Pop frame from queue
|
||||||
|
if (!m_Frames.empty())
|
||||||
|
{
|
||||||
|
res = m_Frames.front();
|
||||||
|
m_Frames.pop();
|
||||||
|
}
|
||||||
|
// Unlock frame queue
|
||||||
|
m_FramesMutex.unlock();
|
||||||
|
return res;
|
||||||
|
}
|
||||||
|
|
||||||
|
void QueueF(FramePtr& frame) {
|
||||||
|
m_pCamera->QueueFrame(frame);
|
||||||
|
}
|
||||||
|
|
||||||
|
void ClearFrameQueue() {
|
||||||
|
// Lock the frame queue
|
||||||
|
m_FramesMutex.lock();
|
||||||
|
std::queue<FramePtr> empty;
|
||||||
|
std::swap(m_Frames, empty);
|
||||||
|
m_FramesMutex.unlock();
|
||||||
|
}
|
||||||
|
private:
|
||||||
|
std::queue<FramePtr> m_Frames;
|
||||||
|
std::mutex m_FramesMutex;
|
||||||
|
};
|
||||||
|
|
||||||
|
VimbaHandler::VimbaHandler(std::vector<std::string> cameraIds, MQTTClient* mqtt_c, motor_info* motor_i, bool demo) :
|
||||||
|
m_vmbSystem(VmbSystem::GetInstance()), mqtt_client(mqtt_c), gimbal_data(motor_i), demo_flag(demo)
|
||||||
|
{
|
||||||
|
VmbErrorType err = m_vmbSystem.Startup();
|
||||||
|
|
||||||
|
if (err != VmbErrorSuccess)
|
||||||
|
{
|
||||||
|
throw std::runtime_error("Could not start API, err=" + std::to_string(err));
|
||||||
|
}
|
||||||
|
|
||||||
|
//CameraPtrVector cameras;
|
||||||
|
//err = m_vmbSystem.GetCameras(cameras);
|
||||||
|
//if (err != VmbErrorSuccess)
|
||||||
|
//{
|
||||||
|
// m_vmbSystem.Shutdown();
|
||||||
|
// throw std::runtime_error("Could not get cameras, err=" + std::to_string(err));
|
||||||
|
//}
|
||||||
|
//if (cameras.empty())
|
||||||
|
//{
|
||||||
|
// m_vmbSystem.Shutdown();
|
||||||
|
// throw std::runtime_error("No cameras found.");
|
||||||
|
//}
|
||||||
|
|
||||||
|
if (cameraIds.size() > 0)
|
||||||
|
{
|
||||||
|
int id = 0;
|
||||||
|
|
||||||
|
for (std::string cameraId : cameraIds)
|
||||||
|
{
|
||||||
|
CameraPtr cam;
|
||||||
|
err = m_vmbSystem.GetCameraByID(cameraId.c_str(), cam);
|
||||||
|
if (err != VmbErrorSuccess)
|
||||||
|
{
|
||||||
|
m_vmbSystem.Shutdown();
|
||||||
|
throw std::runtime_error("No camera found with ID=" + std::string(cameraId) + ", err = " + std::to_string(err));
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
m_cameras.push_back(cam);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
//else
|
||||||
|
//{
|
||||||
|
// m_camera = cameras[0];
|
||||||
|
//}
|
||||||
|
for (CameraPtr camptr : m_cameras)
|
||||||
|
{
|
||||||
|
err = camptr->Open(VmbAccessModeFull);
|
||||||
|
if (err != VmbErrorSuccess)
|
||||||
|
{
|
||||||
|
m_vmbSystem.Shutdown();
|
||||||
|
throw std::runtime_error("Could not open camera, err=" + std::to_string(err));
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
std::string name;
|
||||||
|
if (camptr->GetName(name) == VmbErrorSuccess)
|
||||||
|
{
|
||||||
|
std::cout << "Opened Camera " << name << std::endl;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
VimbaHandler::~VimbaHandler()
|
||||||
|
{
|
||||||
|
try
|
||||||
|
{
|
||||||
|
Stop();
|
||||||
|
}
|
||||||
|
catch (std::runtime_error& e)
|
||||||
|
{
|
||||||
|
std::cout << e.what() << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
m_vmbSystem.Shutdown();
|
||||||
|
}
|
||||||
|
|
||||||
|
void VimbaHandler::Open()
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
void VimbaHandler::Close()
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
void VimbaHandler::Start()
|
||||||
|
{
|
||||||
|
save_thread_running = true;
|
||||||
|
std::cout << "starting Camera thread" << std::endl;
|
||||||
|
image_saver_thread = std::thread(&VimbaHandler::SaveImage, this);
|
||||||
|
int cam_id = 0;
|
||||||
|
for (CameraPtr cam : m_cameras) {
|
||||||
|
IFrameObserverPtr FO_ptr;
|
||||||
|
ImageQueue8 image_queue;
|
||||||
|
m_ImageQueue_vec.push_back(image_queue);
|
||||||
|
SP_SET(FO_ptr, new FrameObserver(cam));
|
||||||
|
SP_DYN_CAST<FrameObserver>(FO_ptr)->cam_id = cam_id;
|
||||||
|
cam_id++;
|
||||||
|
VmbErrorType err = cam->StartContinuousImageAcquisition(5, FO_ptr);
|
||||||
|
FO_ptr_vec.push_back(FO_ptr);
|
||||||
|
if (err != VmbErrorSuccess)
|
||||||
|
{
|
||||||
|
throw std::runtime_error("Could not start acquisition, err=" + std::to_string(err));
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
|
||||||
|
SP_DYN_CAST<FrameObserver>(FO_ptr)->registerCallback(std::bind(&VimbaHandler::EnqueueToStoreStruct, this, std::placeholders::_1));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
cam_started = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void VimbaHandler::Stop()
|
||||||
|
{
|
||||||
|
for (CameraPtr cam : m_cameras) {
|
||||||
|
VmbErrorType err = cam->StopContinuousImageAcquisition();
|
||||||
|
if (err != VmbErrorSuccess)
|
||||||
|
{
|
||||||
|
throw std::runtime_error("Could not stop acquisition, err=" + std::to_string(err));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
cam_started = false;
|
||||||
|
save_thread_running = false;
|
||||||
|
if (image_saver_thread.joinable()) {
|
||||||
|
image_saver_thread.join();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void VimbaHandler::evaluateCommand(std::string cmd, double val)
|
||||||
|
{
|
||||||
|
if (cmd == "fps" && val > 0)
|
||||||
|
ChangeFramerate(val);
|
||||||
|
else if (cmd == "jxlq" && val > 0)
|
||||||
|
jxlq = val;
|
||||||
|
else if (cmd == "jxle" && val > 0)
|
||||||
|
jxle = val;
|
||||||
|
else if (cmd == "display" && val >= 0)
|
||||||
|
display_image = int(val);
|
||||||
|
}
|
||||||
|
|
||||||
|
void VimbaHandler::EnqueueToStoreStruct(int cam_id)
|
||||||
|
{
|
||||||
|
FramePtr frame = SP_DYN_CAST<FrameObserver>(FO_ptr_vec[cam_id])->GetFrame();
|
||||||
|
|
||||||
|
VmbUint32_t Width;
|
||||||
|
VmbUint32_t Height;
|
||||||
|
VmbUint32_t BufferSize;
|
||||||
|
VmbPixelFormatType PixelFormat;
|
||||||
|
const VmbUchar_t* pBuffer(NULL);
|
||||||
|
|
||||||
|
if (VmbErrorSuccess == frame->GetPixelFormat(PixelFormat)
|
||||||
|
&& VmbErrorSuccess == frame->GetWidth(Width)
|
||||||
|
&& VmbErrorSuccess == frame->GetHeight(Height)
|
||||||
|
&& VmbErrorSuccess == frame->GetBufferSize(BufferSize)
|
||||||
|
&& VmbErrorSuccess == frame->GetBuffer(pBuffer))
|
||||||
|
{
|
||||||
|
NanoUnixTimer time;
|
||||||
|
long long tstamp = time.Stamp_longlong();
|
||||||
|
std::lock_guard<std::mutex> lg(queue_mut);
|
||||||
|
ImageStore8Ptr pFrame;
|
||||||
|
// in case we reached the maximum number of queued frames
|
||||||
|
// take of the oldest and reuse it to store the newly arriving frame
|
||||||
|
|
||||||
|
std::vector<VmbUchar_t> data_in = std::vector<VmbUchar_t>(pBuffer, pBuffer + BufferSize);
|
||||||
|
if (m_ImageQueue_vec[cam_id].size() >= 100)
|
||||||
|
{
|
||||||
|
pFrame = m_ImageQueue_vec[cam_id].front();
|
||||||
|
m_ImageQueue_vec[cam_id].pop();
|
||||||
|
if (!pFrame->equal(Width, Height, PixelFormat))
|
||||||
|
{
|
||||||
|
pFrame.reset();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (pFrame == NULL)
|
||||||
|
{
|
||||||
|
pFrame = ImageStore8Ptr(new image_store_8bit(data_in.data(), data_in.size(), Width, Height, PixelFormat, tstamp, cam_id));
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
pFrame->setData(data_in.data(), data_in.size(), tstamp);
|
||||||
|
}
|
||||||
|
m_ImageQueue_vec[cam_id].push(pFrame);
|
||||||
|
|
||||||
|
SP_DYN_CAST<FrameObserver>(FO_ptr_vec[cam_id])->QueueF(frame);
|
||||||
|
std::cout << "Enqueue finished Camera:" << cam_id << " -- Queue size: " << m_ImageQueue_vec[cam_id].size() << std::endl;
|
||||||
|
if (cam_id == 0)
|
||||||
|
cv_proc.notify_one();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void VimbaHandler::SaveImage()
|
||||||
|
{
|
||||||
|
while (save_thread_running) {
|
||||||
|
std::unique_lock<std::mutex> lock(proc_wait_mut);
|
||||||
|
cv_proc.wait(lock);
|
||||||
|
ImageStore8Ptr pFrame;
|
||||||
|
for (ImageQueue8& imageQueue : m_ImageQueue_vec) {
|
||||||
|
while (imageQueue.size() > 0 && save_jxl)
|
||||||
|
{
|
||||||
|
Timer time;
|
||||||
|
queue_mut.lock();
|
||||||
|
pFrame = imageQueue.front();
|
||||||
|
imageQueue.pop();
|
||||||
|
queue_count_rgb = imageQueue.size();
|
||||||
|
queue_mut.unlock();
|
||||||
|
|
||||||
|
|
||||||
|
int imagetype;
|
||||||
|
int imagechannels;
|
||||||
|
if (pFrame->dataSize() / (pFrame->height() * pFrame->width()) == 1) {
|
||||||
|
imagetype = CV_8UC1;
|
||||||
|
imagechannels = 1;
|
||||||
|
}
|
||||||
|
else if (pFrame->dataSize() / (pFrame->height() * pFrame->width()) == 3) {
|
||||||
|
imagetype = CV_8UC3;
|
||||||
|
imagechannels = 3;
|
||||||
|
}
|
||||||
|
|
||||||
|
cv::Mat img_to_rotate = cv::Mat(pFrame->height(), pFrame->width(), imagetype, pFrame->data());
|
||||||
|
cv::Mat img;
|
||||||
|
cv::rotate(img_to_rotate, img, cv::ROTATE_90_COUNTERCLOCKWISE);
|
||||||
|
if (display_image) {
|
||||||
|
cv::namedWindow("Display Image", cv::WINDOW_NORMAL);
|
||||||
|
cv::resizeWindow("Display Image", img.cols / 4, img.rows / 4);
|
||||||
|
cv::imshow("Display Image", img);
|
||||||
|
cv::waitKey(10);
|
||||||
|
}
|
||||||
|
std::string homedir = getenv("HOME");
|
||||||
|
std::string cameraname;
|
||||||
|
if (pFrame->getCamId() == 0)
|
||||||
|
cameraname = "RGB";
|
||||||
|
if (pFrame->getCamId() == 1)
|
||||||
|
cameraname = "ACR";
|
||||||
|
if (pFrame->getCamId() == 2)
|
||||||
|
cameraname = "NIR";
|
||||||
|
std::string filename = homedir + "/projects/Fire_Gimbal_Control/bin/x64/Release/" + cameraname + "/" + std::to_string(pFrame->getTimestamp()) + ".jxl";
|
||||||
|
std::filesystem::path path(filename);
|
||||||
|
// Extract the directory part of the path
|
||||||
|
std::filesystem::path dir = path.parent_path();
|
||||||
|
// Create directories if they don't exist
|
||||||
|
if (!dir.empty() && !std::filesystem::exists(dir)) {
|
||||||
|
std::filesystem::create_directories(dir);
|
||||||
|
std::cout << "Created directories: " << dir << std::endl;
|
||||||
|
}
|
||||||
|
if (!demo_flag) {
|
||||||
|
JPEGXL jxl_writer(img.cols, img.rows, img.data, imagechannels, (float)jxlq, (int)jxle);
|
||||||
|
jxl_writer.WriteFile(filename.c_str());
|
||||||
|
std::cout << "Camera: " + std::to_string(pFrame->getCamId()) + " -- Compress TIME:" << std::to_string(time.ElapsedMillis()) << " -- SaveQueue size:" << imageQueue.size() << std::endl;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
std::filesystem::copy("test_smoke.jxl", filename);
|
||||||
|
}
|
||||||
|
time.Reset();
|
||||||
|
//cv::imwrite(filename, img, compression_params);
|
||||||
|
std::string net_path = "/mnt/ggs-smb/FirewatchTowers/FWT_Podrosche/";
|
||||||
|
std::string backup_net_path = "/home/ggs/fwt_image_backup/";
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
//try {
|
||||||
|
// std::filesystem::copy_file(filename, net_path + filename);
|
||||||
|
// std::cout << "RGB Upload TIME:" << std::to_string(time.ElapsedMillis()) << std::endl;
|
||||||
|
//}
|
||||||
|
//catch (std::filesystem::filesystem_error& e)
|
||||||
|
//{
|
||||||
|
// std::cout << "Could not copy image to zkms: " << e.what() << '\n';
|
||||||
|
// std::cout << "Copy Backup to LattePanda Sigma" << '\n';
|
||||||
|
// try {
|
||||||
|
// std::filesystem::copy_file(filename, backup_net_path + filename);
|
||||||
|
// }
|
||||||
|
// catch (std::filesystem::filesystem_error& e)
|
||||||
|
// {
|
||||||
|
// std::cout << "Could not copy image to LattePanda Sigma: " << e.what() << '\n';
|
||||||
|
// }
|
||||||
|
//}
|
||||||
|
//std::filesystem::remove(filename);
|
||||||
|
std::string payload = "{ \"fwt\":\"" + fwt_name + "\" ,\"cam\":\"" + cameraname + "\", \"hdg\":" + std::to_string((int)(gimbal_data->hdg * 10)) + ", \"time\":" + std::to_string(pFrame->getTimestamp()) + " }";
|
||||||
|
mqtt_client->publish(mqtt_RGB, payload);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool VimbaHandler::ChangeFramerate(double fr)
|
||||||
|
{
|
||||||
|
VmbErrorType result;
|
||||||
|
for (CameraPtr cam : m_cameras) {
|
||||||
|
FeaturePtr pFeature;
|
||||||
|
result = SP_ACCESS(cam)->GetFeatureByName("AcquisitionFrameRate", pFeature);
|
||||||
|
if (result == VmbErrorSuccess)
|
||||||
|
result = pFeature->SetValue(fr);
|
||||||
|
if (result == VmbErrorSuccess) {
|
||||||
|
std::cout << "camera fps changed: " << fr << std::endl;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (result == VmbErrorSuccess) {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
std::cout << "camera fps change failed with error:" << result << std::endl;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void VimbaHandler::TriggerSettle()
|
||||||
|
{
|
||||||
|
for (auto& fo : FO_ptr_vec) {
|
||||||
|
SP_DYN_CAST<FrameObserver>(fo)->settle.store(3);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool VimbaHandler::TriggerCamera()
|
||||||
|
{
|
||||||
|
TriggerSettle();
|
||||||
|
std::cout << "[TriggerCamera] settle reset to 3, firing 4 triggers (3 settle + 1 real)..." << std::endl;
|
||||||
|
|
||||||
|
FeaturePtr pFeature;
|
||||||
|
VmbErrorType result;
|
||||||
|
|
||||||
|
for (int i = 0; i < 4; i++) {
|
||||||
|
result = SP_ACCESS(m_cameras[0])->GetFeatureByName("TriggerSoftware", pFeature);
|
||||||
|
if (result == VmbErrorSuccess)
|
||||||
|
result = pFeature->RunCommand();
|
||||||
|
int sl = SP_DYN_CAST<FrameObserver>(FO_ptr_vec[0])->settle.load();
|
||||||
|
std::cout << "[TriggerCamera] trigger #" << (i+1) << " settle_before=" << sl << " result=" << result << std::endl;
|
||||||
|
std::this_thread::sleep_for(std::chrono::milliseconds(400));
|
||||||
|
}
|
||||||
|
|
||||||
|
std::cout << "[TriggerCamera] done, 4th frame should be real image" << std::endl;
|
||||||
|
return (result == VmbErrorSuccess);
|
||||||
|
}
|
||||||
|
|
||||||
|
void VimbaHandler::SetTowerName(std::string name)
|
||||||
|
{
|
||||||
|
fwt_name = name;
|
||||||
|
mqtt_RGB = "GGS/FWT/" + fwt_name + "/CamEvent";
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,108 @@
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <VmbCPP/VmbCPP.h>
|
||||||
|
#include <thread>
|
||||||
|
#include <mutex>
|
||||||
|
#include <memory>
|
||||||
|
#include <queue>
|
||||||
|
#include <condition_variable>
|
||||||
|
#include <atomic>
|
||||||
|
#include "MQTT.h"
|
||||||
|
#include "Serial.h"
|
||||||
|
using namespace VmbCPP;
|
||||||
|
|
||||||
|
struct image_store_8bit
|
||||||
|
{
|
||||||
|
private:
|
||||||
|
typedef std::vector<VmbUchar_t> data_vec;
|
||||||
|
data_vec m_Data;
|
||||||
|
VmbUint32_t m_Width; // frame width
|
||||||
|
VmbUint32_t m_Height; // frame height
|
||||||
|
VmbPixelFormat_t m_PixelFormat; // frame pixel format
|
||||||
|
long long m_timestamp;
|
||||||
|
int m_cam_id;
|
||||||
|
public:
|
||||||
|
image_store_8bit(const VmbUchar_t* pBuffer, VmbUint32_t BufferByteSize, VmbUint32_t Width, VmbUint32_t Height, VmbPixelFormatType PixelFormat, long long stamp, int cam_id)
|
||||||
|
: m_Data(pBuffer, pBuffer + BufferByteSize)
|
||||||
|
, m_Width(Width)
|
||||||
|
, m_Height(Height)
|
||||||
|
, m_PixelFormat(PixelFormat)
|
||||||
|
, m_timestamp(stamp)
|
||||||
|
, m_cam_id(cam_id)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
long long getTimestamp() {
|
||||||
|
return(m_timestamp);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool equal(VmbUint32_t Width, VmbUint32_t Height, VmbPixelFormat_t PixelFormat) const
|
||||||
|
{
|
||||||
|
return m_Width == Width
|
||||||
|
&& m_Height == Height
|
||||||
|
&& m_PixelFormat == PixelFormat;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool setData(const VmbUchar_t* Buffer, VmbUint32_t BufferSize, long long stamp)
|
||||||
|
{
|
||||||
|
if (BufferSize == dataSize())
|
||||||
|
{
|
||||||
|
std::copy(Buffer, Buffer + BufferSize, m_Data.begin());
|
||||||
|
m_timestamp = stamp;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
VmbPixelFormat_t pixelFormat() const { return m_PixelFormat; }
|
||||||
|
VmbUint32_t width() const { return m_Width; }
|
||||||
|
VmbUint32_t height() const { return m_Height; }
|
||||||
|
VmbUint32_t dataSize() const { return static_cast<VmbUint32_t>(m_Data.size()); }
|
||||||
|
const VmbUchar_t* data() const { return &*m_Data.begin(); }
|
||||||
|
VmbUchar_t* data() { return &*m_Data.begin(); }
|
||||||
|
int getCamId() { return m_cam_id; }
|
||||||
|
};
|
||||||
|
|
||||||
|
class VimbaHandler
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
VimbaHandler(std::vector<std::string> cameraIds, MQTTClient* mqtt_c, motor_info* motor_i, bool demo);
|
||||||
|
~VimbaHandler();
|
||||||
|
void Open();
|
||||||
|
void Close();
|
||||||
|
void Start();
|
||||||
|
void Stop();
|
||||||
|
void evaluateCommand(std::string cmd, double val);
|
||||||
|
void EnqueueToStoreStruct(int cam_id);
|
||||||
|
void SaveImage();
|
||||||
|
bool ChangeFramerate(double fr);
|
||||||
|
bool TriggerCamera();
|
||||||
|
void TriggerSettle();
|
||||||
|
void SetTowerName(std::string name);
|
||||||
|
|
||||||
|
bool cam_started = false;
|
||||||
|
int queue_count_rgb = 0;
|
||||||
|
private:
|
||||||
|
bool demo_flag = false;
|
||||||
|
double jxlq = 2.0;
|
||||||
|
double jxle = 3.0;
|
||||||
|
std::string fwt_name = "Rietschen";//"Dev"; //TODO: put in config
|
||||||
|
std::string mqtt_RGB = "GGS/FWT/" + fwt_name + "/CamEvent";
|
||||||
|
motor_info* gimbal_data;
|
||||||
|
MQTTClient* mqtt_client;
|
||||||
|
typedef std::shared_ptr<image_store_8bit> ImageStore8Ptr;
|
||||||
|
typedef std::queue<ImageStore8Ptr> ImageQueue8;
|
||||||
|
std::vector <ImageQueue8> m_ImageQueue_vec;
|
||||||
|
std::mutex queue_mut;
|
||||||
|
std::mutex proc_wait_mut;
|
||||||
|
std::condition_variable cv_proc;
|
||||||
|
bool save_thread_running = false;
|
||||||
|
bool save_jxl = true;
|
||||||
|
bool display_image = false;
|
||||||
|
std::thread image_saver_thread;
|
||||||
|
|
||||||
|
std::vector<IFrameObserverPtr> FO_ptr_vec;
|
||||||
|
VmbSystem& m_vmbSystem;
|
||||||
|
CameraPtrVector m_cameras;
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
|
@ -0,0 +1,110 @@
|
||||||
|
#pragma once
|
||||||
|
#include <iostream>
|
||||||
|
#include <cstdio> // for fopen, fclose
|
||||||
|
#include <vector>
|
||||||
|
#include <jxl/codestream_header.h>
|
||||||
|
#include <jxl/color_encoding.h>
|
||||||
|
#include <jxl/encode.h>
|
||||||
|
#include <jxl/encode_cxx.h>
|
||||||
|
#include <jxl/thread_parallel_runner.h>
|
||||||
|
#include <jxl/thread_parallel_runner_cxx.h>
|
||||||
|
#include <jxl/types.h>
|
||||||
|
class JPEGXL
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
JPEGXL(int width, int height, const unsigned char* img, int num_channels, float q, int e, int th = 3) :m_width(width), m_height(height), m_num_channels(num_channels), m_quality(q), m_efford(e)
|
||||||
|
{
|
||||||
|
auto enc = JxlEncoderMake(/*memory_manager=*/nullptr);
|
||||||
|
|
||||||
|
auto runner = JxlThreadParallelRunnerMake(nullptr, th);
|
||||||
|
if (JXL_ENC_SUCCESS != JxlEncoderSetParallelRunner(enc.get(),
|
||||||
|
JxlThreadParallelRunner,
|
||||||
|
runner.get())) {
|
||||||
|
fprintf(stderr, "JxlEncoderSetParallelRunner failed\n");
|
||||||
|
}
|
||||||
|
|
||||||
|
JxlPixelFormat pixel_format = { m_num_channels, JXL_TYPE_UINT8, JXL_NATIVE_ENDIAN, 0 };
|
||||||
|
JxlBasicInfo basic_info;
|
||||||
|
JxlEncoderInitBasicInfo(&basic_info);
|
||||||
|
basic_info.xsize = m_width;
|
||||||
|
basic_info.ysize = m_height;
|
||||||
|
basic_info.alpha_bits = 0;
|
||||||
|
basic_info.bits_per_sample = 8;
|
||||||
|
basic_info.num_color_channels = m_num_channels;
|
||||||
|
basic_info.uses_original_profile = false;
|
||||||
|
|
||||||
|
if (JXL_ENC_SUCCESS != JxlEncoderSetBasicInfo(enc.get(), &basic_info)) {
|
||||||
|
fprintf(stderr, "JxlEncoderSetBasicInfo failed\n");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
JxlColorEncoding color_encoding = {};
|
||||||
|
JxlColorEncodingSetToSRGB(&color_encoding,/*is_gray=*/pixel_format.num_channels < 3);
|
||||||
|
if (JXL_ENC_SUCCESS != JxlEncoderSetColorEncoding(enc.get(), &color_encoding)) {
|
||||||
|
fprintf(stderr, "JxlEncoderSetColorEncoding failed\n");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
JxlEncoderFrameSettings* frame_settings = JxlEncoderFrameSettingsCreate(enc.get(), nullptr);
|
||||||
|
JxlEncoderFrameSettingsSetOption(frame_settings, JXL_ENC_FRAME_SETTING_EFFORT, e);
|
||||||
|
JxlEncoderFrameSettingsSetOption(frame_settings, JXL_ENC_FRAME_SETTING_DECODING_SPEED, 0);
|
||||||
|
|
||||||
|
if (q == 0) {//if lossless
|
||||||
|
JxlEncoderSetFrameDistance(frame_settings, 0);
|
||||||
|
JxlEncoderSetFrameLossless(frame_settings, true);
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
JxlEncoderSetFrameLossless(frame_settings, false);
|
||||||
|
JxlEncoderSetFrameDistance(frame_settings, q);
|
||||||
|
}
|
||||||
|
if (JXL_ENC_SUCCESS != JxlEncoderAddImageFrame(frame_settings, &pixel_format, (void*)img, sizeof(uint8_t) * m_width * m_height * m_num_channels)) {
|
||||||
|
fprintf(stderr, "JxlEncoderAddImageFrame failed\n");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
JxlEncoderCloseInput(enc.get());
|
||||||
|
std::vector<uint8_t>* compressed = &compressed_data;
|
||||||
|
compressed->resize(64);
|
||||||
|
uint8_t* next_out = compressed->data();
|
||||||
|
size_t avail_out = compressed->size() - (next_out - compressed->data());
|
||||||
|
JxlEncoderStatus process_result = JXL_ENC_NEED_MORE_OUTPUT;
|
||||||
|
while (process_result == JXL_ENC_NEED_MORE_OUTPUT) {
|
||||||
|
process_result = JxlEncoderProcessOutput(enc.get(), &next_out, &avail_out);
|
||||||
|
if (process_result == JXL_ENC_NEED_MORE_OUTPUT) {
|
||||||
|
size_t offset = next_out - compressed->data();
|
||||||
|
compressed->resize(compressed->size() * 2);
|
||||||
|
next_out = compressed->data() + offset;
|
||||||
|
avail_out = compressed->size() - offset;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
compressed->resize(next_out - compressed->data());
|
||||||
|
if (JXL_ENC_SUCCESS != process_result) {
|
||||||
|
fprintf(stderr, "JxlEncoderProcessOutput failed\n");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
~JPEGXL() {}
|
||||||
|
|
||||||
|
bool WriteFile(const char* filename) {
|
||||||
|
FILE* file = fopen(filename, "wb");
|
||||||
|
if (!file) {
|
||||||
|
fprintf(stderr, "Could not open %s for writing\n", filename);
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
if (fwrite(compressed_data.data(), sizeof(uint8_t), compressed_data.size(), file) !=
|
||||||
|
compressed_data.size()) {
|
||||||
|
fprintf(stderr, "Could not write bytes to %s\n", filename);
|
||||||
|
fclose(file);
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
if (fclose(file) != 0) {
|
||||||
|
fprintf(stderr, "Could not close %s\n", filename);
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
private:
|
||||||
|
int m_width;
|
||||||
|
int m_height;
|
||||||
|
int m_num_channels;
|
||||||
|
float m_quality;
|
||||||
|
int m_efford;
|
||||||
|
std::vector<uint8_t> compressed_data;
|
||||||
|
};
|
||||||
|
|
@ -0,0 +1,155 @@
|
||||||
|
#include "MQTT.h"
|
||||||
|
|
||||||
|
const int QOS = 1;
|
||||||
|
const int N_RETRY_ATTEMPTS = 5;
|
||||||
|
|
||||||
|
|
||||||
|
void MQTTCallback::on_failure(const mqtt::token& tok)
|
||||||
|
{
|
||||||
|
std::cout << "Connection failed" << std::endl;
|
||||||
|
if (++nretry_ > N_RETRY_ATTEMPTS)
|
||||||
|
std::cout << "Connection attempt failed already a few times" << std::endl;
|
||||||
|
reconnect();
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// (Re)connection success
|
||||||
|
void MQTTCallback::connected(const std::string& cause) {
|
||||||
|
std::cout << "\nConnection success" << std::endl;
|
||||||
|
std::cout << "\nSubscribing to topics.." << std::endl;
|
||||||
|
|
||||||
|
cli_.subscribe(tpoic_target_hdg, QOS, nullptr, subListener_);
|
||||||
|
cli_.subscribe(topic_control_mode, QOS, nullptr, subListener_);
|
||||||
|
}
|
||||||
|
|
||||||
|
void MQTTCallback::message_arrived(mqtt::const_message_ptr msg) {
|
||||||
|
std::cout << "Message arrived" << std::endl;
|
||||||
|
std::cout << "\ttopic: '" << msg->get_topic() << "'" << std::endl;
|
||||||
|
std::cout << "\tpayload: '" << msg->to_string() << "'\n" << std::endl;
|
||||||
|
if (msg->get_topic() == "GGS/FWT/" + fwt_name + "/target_HDG") {
|
||||||
|
try {
|
||||||
|
int value = static_cast<int>(std::stoi(msg->to_string()));
|
||||||
|
std::unique_lock<std::mutex> ul(mqtt_mut);
|
||||||
|
sub_data.set_target_heading(msg->to_string());
|
||||||
|
}
|
||||||
|
catch (const std::invalid_argument& e) {
|
||||||
|
std::cerr << "MQTT type convertion invalid argument: " << e.what() << std::endl;
|
||||||
|
}
|
||||||
|
catch (const std::out_of_range& e) {
|
||||||
|
std::cerr << "MQTT type convertion out of range: " << e.what() << std::endl;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if (msg->get_topic() == "GGS/FWT/" + fwt_name + "/ControlCode") {
|
||||||
|
try {
|
||||||
|
std::unique_lock<std::mutex> ul(mqtt_mut);
|
||||||
|
sub_data.set_control_code(static_cast<int>(std::stoi(msg->to_string())));
|
||||||
|
}
|
||||||
|
catch (const std::invalid_argument& e) {
|
||||||
|
std::cerr << "MQTT type convertion invalid argument: " << e.what() << std::endl;
|
||||||
|
}
|
||||||
|
catch (const std::out_of_range& e) {
|
||||||
|
std::cerr << "MQTT type convertion out of range: " << e.what() << std::endl;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
//////////////////////////////////////////////////////////////////////////////////////////////////////////////7x
|
||||||
|
|
||||||
|
MQTTClient::MQTTClient(const std::string& serverURI, const std::string& clientId, std::string tower_name, std::string login_user, std::string login_pw)
|
||||||
|
: client(serverURI, clientId), callback(client, connOpts, tower_name)
|
||||||
|
{
|
||||||
|
connOpts.set_keep_alive_interval(20);
|
||||||
|
connOpts.set_clean_session(true);
|
||||||
|
connOpts.set_user_name(login_user);
|
||||||
|
connOpts.set_password(login_pw);
|
||||||
|
std::cout << "MQTT object created" << std::endl;
|
||||||
|
client.set_callback(callback);
|
||||||
|
std::cout << "MQTT callback set" << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
MQTTClient::~MQTTClient() {
|
||||||
|
if (client.is_connected())
|
||||||
|
disconnect();
|
||||||
|
}
|
||||||
|
|
||||||
|
void MQTTClient::connect_client() {
|
||||||
|
std::cout << "connecting MQTT..." << std::endl;
|
||||||
|
running = true;
|
||||||
|
mqtt_thread = std::thread(&MQTTClient::run, this);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
void MQTTClient::run() {
|
||||||
|
const auto TIMEOUT = std::chrono::seconds(5);
|
||||||
|
try {
|
||||||
|
conToken = client.connect(connOpts, nullptr, callback);
|
||||||
|
conToken->wait_for(TIMEOUT);
|
||||||
|
connected = true;
|
||||||
|
}
|
||||||
|
catch (const mqtt::exception& exc) {
|
||||||
|
std::cerr << "Error: " << exc.what() << std::endl;
|
||||||
|
running = false;
|
||||||
|
connected = false;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
std::cout << "MQTT connected" << std::endl;
|
||||||
|
|
||||||
|
while (running)
|
||||||
|
;
|
||||||
|
}
|
||||||
|
|
||||||
|
void MQTTClient::disconnect() {
|
||||||
|
// Disconnect
|
||||||
|
try {
|
||||||
|
std::cout << "\nDisconnecting from the MQTT server..." << std::flush;
|
||||||
|
client.disconnect()->wait();
|
||||||
|
std::cout << "OK" << std::endl;
|
||||||
|
}
|
||||||
|
catch (const mqtt::exception& exc) {
|
||||||
|
std::cerr << exc.what() << std::endl;
|
||||||
|
}
|
||||||
|
running = false;
|
||||||
|
mqtt_thread.join();
|
||||||
|
}
|
||||||
|
|
||||||
|
void MQTTClient::publish(const std::string& topic, const std::string& payload) {
|
||||||
|
const auto TIMEOUT = std::chrono::seconds(3);
|
||||||
|
mqtt::delivery_token_ptr pubtok;
|
||||||
|
mqtt::message_ptr msg = mqtt::make_message(topic, payload);
|
||||||
|
msg->set_qos(1);
|
||||||
|
msg->set_retained(true);
|
||||||
|
try {
|
||||||
|
pubtok = client.publish(msg);
|
||||||
|
pubtok->wait_for(TIMEOUT);
|
||||||
|
}
|
||||||
|
catch (const mqtt::exception& exc) {
|
||||||
|
std::cerr << "Error: " << exc.what() << std::endl;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void MQTTClient::subscribe(const std::string& topic) {
|
||||||
|
try {
|
||||||
|
subToken = client.subscribe(topic, 1);
|
||||||
|
subToken->wait();
|
||||||
|
}
|
||||||
|
catch (const mqtt::exception& exc) {
|
||||||
|
std::cerr << "Error: " << exc.what() << std::endl;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void MQTTClient::receiveMessages() {
|
||||||
|
try {
|
||||||
|
client.start_consuming();
|
||||||
|
}
|
||||||
|
catch (const mqtt::exception& exc) {
|
||||||
|
std::cerr << "Error: " << exc.what() << std::endl;
|
||||||
|
}
|
||||||
|
while (true) {
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool MQTTClient::is_connected_to_server()
|
||||||
|
{
|
||||||
|
return client.is_connected();
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,157 @@
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <iostream>
|
||||||
|
#include <mqtt/async_client.h>
|
||||||
|
#include <thread>
|
||||||
|
#include <mutex>
|
||||||
|
struct mqtt_sub_data
|
||||||
|
{
|
||||||
|
bool ctl_avail = false;
|
||||||
|
bool hdg_avail = false;
|
||||||
|
std::string target_heading="";
|
||||||
|
int control_code = 0;
|
||||||
|
void set_control_code(int c) {
|
||||||
|
ctl_avail = true;
|
||||||
|
control_code = c;
|
||||||
|
}
|
||||||
|
void set_target_heading(std::string target) {
|
||||||
|
hdg_avail = true;
|
||||||
|
target_heading = target;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
// Callbacks for the success or failures of requested actions.
|
||||||
|
// This could be used to initiate further action, but here we just log the
|
||||||
|
// results to the console.
|
||||||
|
class action_listener : public virtual mqtt::iaction_listener
|
||||||
|
{
|
||||||
|
std::string name_;
|
||||||
|
|
||||||
|
void on_failure(const mqtt::token& tok) override {
|
||||||
|
std::cout << name_ << " failure";
|
||||||
|
if (tok.get_message_id() != 0)
|
||||||
|
std::cout << " for token: [" << tok.get_message_id() << "]" << std::endl;
|
||||||
|
std::cout << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
void on_success(const mqtt::token& tok) override {
|
||||||
|
std::cout << name_ << " success";
|
||||||
|
if (tok.get_message_id() != 0)
|
||||||
|
std::cout << " for token: [" << tok.get_message_id() << "]" << std::endl;
|
||||||
|
auto top = tok.get_topics();
|
||||||
|
if (top && !top->empty())
|
||||||
|
std::cout << "\ttoken topic: '" << (*top)[0] << "', ..." << std::endl;
|
||||||
|
std::cout << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
public:
|
||||||
|
action_listener(const std::string& name) : name_(name) {}
|
||||||
|
};
|
||||||
|
|
||||||
|
/////////////////////////////////////////////////////////////////////////////
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Local callback & listener class for use with the client connection.
|
||||||
|
* This is primarily intended to receive messages, but it will also monitor
|
||||||
|
* the connection to the broker. If the connection is lost, it will attempt
|
||||||
|
* to restore the connection and re-subscribe to the topic.
|
||||||
|
*/
|
||||||
|
class MQTTCallback : public virtual mqtt::callback,
|
||||||
|
public virtual mqtt::iaction_listener
|
||||||
|
|
||||||
|
{
|
||||||
|
const std::string tpoic_target_hdg;
|
||||||
|
const std::string topic_control_mode;
|
||||||
|
mqtt_sub_data sub_data;
|
||||||
|
std::mutex mqtt_mut;
|
||||||
|
std::string fwt_name;
|
||||||
|
// Counter for the number of connection retries
|
||||||
|
int nretry_;
|
||||||
|
// The MQTT client
|
||||||
|
mqtt::async_client& cli_;
|
||||||
|
// Options to use if we need to reconnect
|
||||||
|
mqtt::connect_options& connOpts_;
|
||||||
|
// An action listener to display the result of actions.
|
||||||
|
action_listener subListener_;
|
||||||
|
|
||||||
|
// This deomonstrates manually reconnecting to the broker by calling
|
||||||
|
// connect() again. This is a possibility for an application that keeps
|
||||||
|
// a copy of it's original connect_options, or if the app wants to
|
||||||
|
// reconnect with different options.
|
||||||
|
// Another way this can be done manually, if using the same options, is
|
||||||
|
// to just call the async_client::reconnect() method.
|
||||||
|
void reconnect() {
|
||||||
|
std::this_thread::sleep_for(std::chrono::milliseconds(2500));
|
||||||
|
try {
|
||||||
|
cli_.connect(connOpts_, nullptr, *this);
|
||||||
|
}
|
||||||
|
catch (const mqtt::exception& exc) {
|
||||||
|
std::cerr << "Error: " << exc.what() << std::endl;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// Re-connection failure
|
||||||
|
void on_failure(const mqtt::token& tok) override;
|
||||||
|
|
||||||
|
// (Re)connection success
|
||||||
|
// Either this or connected() can be used for callbacks.
|
||||||
|
void on_success(const mqtt::token& tok) override {}
|
||||||
|
// Re-connection success
|
||||||
|
void connected(const std::string& cause) override;
|
||||||
|
|
||||||
|
// Callback for when the connection is lost.
|
||||||
|
// This will initiate the attempt to manually reconnect.
|
||||||
|
void connection_lost(const std::string& cause) override {
|
||||||
|
std::cout << "\nConnection lost" << std::endl;
|
||||||
|
if (!cause.empty())
|
||||||
|
std::cout << "\tcause: " << cause << std::endl;
|
||||||
|
|
||||||
|
std::cout << "Reconnecting..." << std::endl;
|
||||||
|
nretry_ = 0;
|
||||||
|
reconnect();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Callback for when a message arrives.
|
||||||
|
void message_arrived(mqtt::const_message_ptr msg) override;
|
||||||
|
|
||||||
|
void delivery_complete(mqtt::delivery_token_ptr token) override {
|
||||||
|
std::cout << "MQTT delivery complete" << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
public:
|
||||||
|
MQTTCallback(mqtt::async_client& cli, mqtt::connect_options& connOpts, std::string tower_name)
|
||||||
|
: nretry_(0), cli_(cli), connOpts_(connOpts), subListener_("Subscription"), fwt_name(tower_name), tpoic_target_hdg("GGS/FWT/" + tower_name + "/target_HDG"), topic_control_mode("GGS/FWT/" + tower_name + "/ControlCode") {}
|
||||||
|
mqtt_sub_data get_sub_data() {
|
||||||
|
std::unique_lock<std::mutex> ul(mqtt_mut);
|
||||||
|
mqtt_sub_data tmp = sub_data;
|
||||||
|
sub_data.hdg_avail = false;
|
||||||
|
sub_data.ctl_avail = false;
|
||||||
|
return tmp;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
class MQTTClient {
|
||||||
|
public:
|
||||||
|
MQTTClient(const std::string& serverURI, const std::string& clientId, std::string tower_name, std::string login_user, std::string login_pw);
|
||||||
|
~MQTTClient();
|
||||||
|
|
||||||
|
void connect_client();
|
||||||
|
void run();
|
||||||
|
void disconnect();
|
||||||
|
void publish(const std::string& topic, const std::string& payload);
|
||||||
|
void subscribe(const std::string& topic);
|
||||||
|
void receiveMessages();
|
||||||
|
bool is_connected_to_server();
|
||||||
|
|
||||||
|
bool running = false;
|
||||||
|
bool connected = false;
|
||||||
|
MQTTCallback callback;
|
||||||
|
private:
|
||||||
|
std::string fwt_name;
|
||||||
|
mqtt::async_client client;
|
||||||
|
mqtt::token_ptr subToken;
|
||||||
|
mqtt::token_ptr conToken;
|
||||||
|
mqtt::connect_options connOpts;
|
||||||
|
std::thread mqtt_thread;
|
||||||
|
};
|
||||||
|
|
@ -0,0 +1,35 @@
|
||||||
|
CXX := g++
|
||||||
|
CC := gcc
|
||||||
|
CXXFLAGS := -std=c++17 -g -Wall -I.
|
||||||
|
CFLAGS := -g -Wall -I.
|
||||||
|
LDFLAGS := -lpaho-mqttpp3 -lpaho-mqtt3a \
|
||||||
|
-lopencv_core -lopencv_highgui \
|
||||||
|
-ljxl -ljxl_threads \
|
||||||
|
-lboost_program_options \
|
||||||
|
-lVmbC -lVmbCPP
|
||||||
|
|
||||||
|
TARGET := bin/Fire_Gimbal_Control.out
|
||||||
|
OBJDIR := obj
|
||||||
|
|
||||||
|
CXX_SRCS := main.cpp MQTT.cpp Camera.cpp
|
||||||
|
C_SRCS := ini.c
|
||||||
|
|
||||||
|
CXX_OBJS := $(CXX_SRCS:%.cpp=$(OBJDIR)/%.o)
|
||||||
|
C_OBJS := $(C_SRCS:%.c=$(OBJDIR)/%.o)
|
||||||
|
OBJS := $(CXX_OBJS) $(C_OBJS)
|
||||||
|
|
||||||
|
.PHONY: all clean
|
||||||
|
|
||||||
|
all: $(TARGET)
|
||||||
|
|
||||||
|
$(TARGET): $(OBJS)
|
||||||
|
$(CXX) $(CXXFLAGS) -o $@ $^ $(LDFLAGS)
|
||||||
|
|
||||||
|
$(OBJDIR)/%.o: %.cpp
|
||||||
|
$(CXX) $(CXXFLAGS) -c -o $@ $<
|
||||||
|
|
||||||
|
$(OBJDIR)/%.o: %.c
|
||||||
|
$(CC) $(CFLAGS) -c -o $@ $<
|
||||||
|
|
||||||
|
clean:
|
||||||
|
rm -f $(OBJS) $(TARGET)
|
||||||
|
|
@ -0,0 +1,102 @@
|
||||||
|
#pragma once
|
||||||
|
#include <mutex>
|
||||||
|
#include <string>
|
||||||
|
#include <boost/spirit/include/qi.hpp>
|
||||||
|
#include <boost/spirit/include/phoenix.hpp>
|
||||||
|
|
||||||
|
namespace qi = boost::spirit::qi;
|
||||||
|
|
||||||
|
enum InputCommands
|
||||||
|
{
|
||||||
|
no_cmd,
|
||||||
|
startcamera,
|
||||||
|
stopcamera,
|
||||||
|
setcamera,
|
||||||
|
setimagerate,
|
||||||
|
setgimbal,
|
||||||
|
setmotorcontrol,
|
||||||
|
setdebug
|
||||||
|
};
|
||||||
|
|
||||||
|
struct parser_data {
|
||||||
|
std::string command="";
|
||||||
|
std::string device = "";
|
||||||
|
std::string option = "";
|
||||||
|
double command_val=0.0;
|
||||||
|
};
|
||||||
|
|
||||||
|
struct Parser
|
||||||
|
{
|
||||||
|
parser_data p_data;
|
||||||
|
std::mutex mut;
|
||||||
|
void set_parser_data(parser_data& data) {
|
||||||
|
std::cout << "parser set data..." << std::endl;
|
||||||
|
std::lock_guard<std::mutex> lg(mut);
|
||||||
|
p_data = data;
|
||||||
|
}
|
||||||
|
|
||||||
|
void clear_parser_data() {
|
||||||
|
//std::cout << "parser clear data..." << std::endl;
|
||||||
|
std::lock_guard<std::mutex> lg(mut);
|
||||||
|
p_data.command = "";
|
||||||
|
}
|
||||||
|
|
||||||
|
parser_data get_parser_data() {
|
||||||
|
//std::cout << "parser get data..." << std::endl;
|
||||||
|
std::lock_guard<std::mutex> lg(mut);
|
||||||
|
parser_data temp= p_data;
|
||||||
|
//clear_parser_data();
|
||||||
|
return temp;
|
||||||
|
}
|
||||||
|
|
||||||
|
void parse_input(std::string input) {
|
||||||
|
parser_data temp_data;
|
||||||
|
qi::parse(
|
||||||
|
input.begin(), input.end(),
|
||||||
|
(*qi::char_("a-zA-Z") >> ' ' >> *qi::char_("a-zA-Z") >> ' ' >> *qi::char_("a-zA-Z") >> ' ' >> qi::double_),
|
||||||
|
temp_data.command, temp_data.device, temp_data.option, temp_data.command_val
|
||||||
|
);
|
||||||
|
std::cout << temp_data.command << " ... " << temp_data.device << " ... " << temp_data.option << " ... " << temp_data.command_val << std::endl;
|
||||||
|
set_parser_data(temp_data);
|
||||||
|
std::cout << "parser data set" << temp_data.command_val << std::endl;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
struct CMD_eval {
|
||||||
|
InputCommands eval(Parser& parse) {
|
||||||
|
if (parse.p_data.command == "start") {
|
||||||
|
std::cout << "starting Camera" << std::endl;
|
||||||
|
parse.clear_parser_data();
|
||||||
|
return startcamera;
|
||||||
|
}
|
||||||
|
else if (parse.p_data.command == "stop") {
|
||||||
|
std::cout << "stopping Camera" << std::endl;
|
||||||
|
parse.clear_parser_data();
|
||||||
|
return stopcamera;
|
||||||
|
}
|
||||||
|
else if (parse.p_data.command == "debug") {
|
||||||
|
parse.clear_parser_data();
|
||||||
|
return setdebug;
|
||||||
|
}
|
||||||
|
else if (parse.p_data.command == "set") {
|
||||||
|
//std::cout << "setting" << std::endl;
|
||||||
|
if (parse.p_data.device == "camera") {
|
||||||
|
parse.clear_parser_data();
|
||||||
|
return setcamera;
|
||||||
|
}
|
||||||
|
else if (parse.p_data.device == "fps") {
|
||||||
|
parse.clear_parser_data();
|
||||||
|
return setimagerate;
|
||||||
|
}
|
||||||
|
else if (parse.p_data.device == "motorctl") {
|
||||||
|
parse.clear_parser_data();
|
||||||
|
return setmotorcontrol;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
return no_cmd;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
@ -0,0 +1,74 @@
|
||||||
|
# Fire Gimbal Control (Staeffelsberg)
|
||||||
|
|
||||||
|
Real-time control software for an automated **fire-watch gimbal**. A single C++17 binary runs on a
|
||||||
|
tower-mounted PC, rotates a pan gimbal carrying up to four industrial cameras, captures a 360° panorama
|
||||||
|
for wildfire detection, compresses each frame to JPEG XL, and reports to a ground station over MQTT.
|
||||||
|
|
||||||
|
This is the deployment for the **Staeffelsberg** fire-watch tower (FWT). The same codebase is used across
|
||||||
|
towers; the tower identity comes from `config.ini`.
|
||||||
|
|
||||||
|
State is held in memory (mutex-guarded structs), configuration is read once from an INI file, images are
|
||||||
|
written to the filesystem as `.jxl`, and telemetry flows over MQTT. See [docs/architecture.md](docs/architecture.md)
|
||||||
|
for the full picture.
|
||||||
|
|
||||||
|
## What it does (at a glance)
|
||||||
|
|
||||||
|
```
|
||||||
|
motor controller (MCU) this program (Fire_Gimbal_Control.out) ground station
|
||||||
|
┌───────────────────┐ serial ┌──────────────────────────────────────┐ MQTT ┌──────────────┐
|
||||||
|
│ gimbal + sensors │ ─────────▶ │ read telemetry → decide when to stop │ ──────▶ │ broker / ZKMS │
|
||||||
|
│ /dev/ttyACM0 │ ◀───────── │ send move/stop commands │ ◀────── │ control UI │
|
||||||
|
└───────────────────┘ │ trigger cameras → encode JXL → save │ └──────────────┘
|
||||||
|
└──────────────┬───────────────────────┘
|
||||||
|
Vimba X (GigE/USB) │ files
|
||||||
|
cameras ───────────────────▶ ▼ RGB/ACR/NIR/<unix_ms>.jxl
|
||||||
|
```
|
||||||
|
|
||||||
|
## Quick start
|
||||||
|
|
||||||
|
```bash
|
||||||
|
# 1. Install dependencies (see docs/build-and-setup.md for details + Vimba X SDK)
|
||||||
|
# 2. Build
|
||||||
|
make # produces bin/Fire_Gimbal_Control.out
|
||||||
|
# 3. Run (requires camera(s), motor MCU on /dev/ttyACM0, and a reachable MQTT broker)
|
||||||
|
./bin/Fire_Gimbal_Control.out --start 1 # auto-start capture
|
||||||
|
./bin/Fire_Gimbal_Control.out --init 1 --start 1 # also find endstops first
|
||||||
|
```
|
||||||
|
|
||||||
|
> **Before it will run on this machine**, several paths are hardcoded to `/home/ggs/projects/Fire_Gimbal_Control/...`
|
||||||
|
> and the program exits if MQTT can't connect. Read **[docs/known-issues.md](docs/known-issues.md)** first — it
|
||||||
|
> lists every reproduction blocker and the deployed layout the binary expects.
|
||||||
|
|
||||||
|
## Documentation
|
||||||
|
|
||||||
|
| Document | Contents |
|
||||||
|
|----------|----------|
|
||||||
|
| [docs/architecture.md](docs/architecture.md) | System overview, threading model, end-to-end data flow, capture state machine |
|
||||||
|
| [docs/build-and-setup.md](docs/build-and-setup.md) | Toolchain, dependencies, build, serial/MQTT setup, directory layout |
|
||||||
|
| [docs/configuration.md](docs/configuration.md) | `config.ini` keys, CLI flags, console command grammar |
|
||||||
|
| [docs/mqtt-api.md](docs/mqtt-api.md) | MQTT topic catalog, payloads, QoS/retain, ControlCode semantics |
|
||||||
|
| [docs/modules-reference.md](docs/modules-reference.md) | Per-file reference and key data structures |
|
||||||
|
| [docs/known-issues.md](docs/known-issues.md) | Reproduction blockers and recommended follow-ups |
|
||||||
|
|
||||||
|
## Repository layout
|
||||||
|
|
||||||
|
```
|
||||||
|
.
|
||||||
|
├── main.cpp Entry point: config, CLI args, threads, main control loop
|
||||||
|
├── Serial.h SerialPort + motor_info telemetry parser (Boost.Asio)
|
||||||
|
├── MQTT.h / MQTT.cpp MQTTClient + callbacks (Eclipse Paho C++)
|
||||||
|
├── Camera.h / Camera.cpp VimbaHandler: acquisition, queue, JXL save (Vimba X + OpenCV)
|
||||||
|
├── JPEG_XL.h JPEG XL encoder wrapper (libjxl)
|
||||||
|
├── Parser.h Console command parser (Boost.Spirit Qi) + command evaluator
|
||||||
|
├── timing.h Timer / timestamp helpers
|
||||||
|
├── ini.c / ini.h inih INI parser (third-party)
|
||||||
|
├── cxxopts.hpp Third-party CLI parser (legacy/unused — Boost is used instead)
|
||||||
|
├── Log.h Empty stub
|
||||||
|
├── config.ini Configuration (also a separate copy under bin/x64/Release/)
|
||||||
|
├── Makefile Build definition
|
||||||
|
└── bin/x64/Release/ Deployed/runtime directory (binary, config, startup scripts, image folders)
|
||||||
|
```
|
||||||
|
|
||||||
|
## License / ownership
|
||||||
|
|
||||||
|
Internal tooling for the GGS fire-watch tower network. No license file is present in the repository.
|
||||||
|
|
@ -0,0 +1,144 @@
|
||||||
|
#include <boost/asio.hpp>
|
||||||
|
#include <iostream>
|
||||||
|
#include <thread>
|
||||||
|
#include <mutex>
|
||||||
|
#include <string>
|
||||||
|
struct motor_info
|
||||||
|
{
|
||||||
|
int Xenc;
|
||||||
|
int Xerr;
|
||||||
|
int sgt_val;
|
||||||
|
int sgt_stat;
|
||||||
|
int is_moving;
|
||||||
|
int control_status;
|
||||||
|
float hdg;
|
||||||
|
int deviation_warn;
|
||||||
|
int temp;
|
||||||
|
int humid;
|
||||||
|
int fan_pwm;
|
||||||
|
};
|
||||||
|
class SerialPort {
|
||||||
|
public:
|
||||||
|
SerialPort(const std::string& port, unsigned int baud_rate)
|
||||||
|
: io_service_(), serial_(io_service_) {
|
||||||
|
boost::system::error_code ec;
|
||||||
|
|
||||||
|
serial_.open(port, ec);
|
||||||
|
if (ec) {
|
||||||
|
throw std::runtime_error("Failed to open port: " + ec.message());
|
||||||
|
}
|
||||||
|
|
||||||
|
setOption(boost::asio::serial_port_base::baud_rate(baud_rate), "baud_rate");
|
||||||
|
setOption(boost::asio::serial_port_base::character_size(8), "character_size");
|
||||||
|
setOption(boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none), "parity");
|
||||||
|
setOption(boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one), "stop_bits");
|
||||||
|
setOption(boost::asio::serial_port_base::flow_control(boost::asio::serial_port_base::flow_control::none), "flow_control");
|
||||||
|
}
|
||||||
|
|
||||||
|
void startReading() {
|
||||||
|
read();
|
||||||
|
}
|
||||||
|
|
||||||
|
void sendCommand(std::string cmd) {
|
||||||
|
write(cmd);
|
||||||
|
}
|
||||||
|
|
||||||
|
void write(const std::string& data) {
|
||||||
|
boost::asio::async_write(serial_, boost::asio::buffer(data),
|
||||||
|
[](const boost::system::error_code& error, std::size_t) {
|
||||||
|
if (error) {
|
||||||
|
std::cerr << "Write failed: " << error.message() << std::endl;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
);
|
||||||
|
}
|
||||||
|
|
||||||
|
void run() {
|
||||||
|
io_service_.run();
|
||||||
|
}
|
||||||
|
|
||||||
|
void stop() {
|
||||||
|
io_service_.stop();
|
||||||
|
}
|
||||||
|
|
||||||
|
void set_controller_info(motor_info& inf) {
|
||||||
|
std::lock_guard<std::mutex> lg(mut);
|
||||||
|
motorcontroller_information = inf;
|
||||||
|
}
|
||||||
|
|
||||||
|
motor_info get_controller_info() {
|
||||||
|
std::lock_guard<std::mutex> lg(mut);
|
||||||
|
return motorcontroller_information;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool parser(std::string& message)
|
||||||
|
{
|
||||||
|
|
||||||
|
if (message[0] == '$') {
|
||||||
|
//std::cout << message;
|
||||||
|
std::vector<std::string> values;
|
||||||
|
std::string delimiter = ";";
|
||||||
|
size_t pos = 0;
|
||||||
|
while ((pos = message.find(delimiter)) != std::string::npos) {
|
||||||
|
values.push_back(message.substr(0, pos));
|
||||||
|
message.erase(0, pos + delimiter.length());
|
||||||
|
}
|
||||||
|
//for (int i = 0; i < values.size(); i++) {
|
||||||
|
// std::cout << values[i] << std::endl;
|
||||||
|
//}
|
||||||
|
if (values.size() == 12) {
|
||||||
|
motor_info tmp_info;
|
||||||
|
tmp_info.Xenc = stoi(values[1]);
|
||||||
|
tmp_info.Xerr = stoi(values[2]);
|
||||||
|
tmp_info.sgt_val = stoi(values[3]);
|
||||||
|
tmp_info.sgt_stat = stoi(values[4]);
|
||||||
|
tmp_info.is_moving = stoi(values[5]);
|
||||||
|
tmp_info.control_status = stoi(values[6]);
|
||||||
|
tmp_info.hdg = stof(values[7]);
|
||||||
|
tmp_info.deviation_warn = stoi(values[8]);
|
||||||
|
tmp_info.humid = stoi(values[9]);
|
||||||
|
tmp_info.temp = stoi(values[10]);
|
||||||
|
tmp_info.fan_pwm = stoi(values[11]);
|
||||||
|
set_controller_info(tmp_info);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
return false;
|
||||||
|
//else
|
||||||
|
//std::cout << values.size() << std::endl;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
std::mutex mut;
|
||||||
|
motor_info motorcontroller_information;
|
||||||
|
|
||||||
|
template <typename Option>
|
||||||
|
void setOption(const Option& option, const std::string& option_name) {
|
||||||
|
boost::system::error_code ec;
|
||||||
|
serial_.set_option(option, ec);
|
||||||
|
if (ec) {
|
||||||
|
throw std::runtime_error("Failed to set " + option_name + ": " + ec.message());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
void read() {
|
||||||
|
boost::asio::async_read_until(serial_, buffer_, '\n',
|
||||||
|
[this](const boost::system::error_code& error, std::size_t bytes_transferred) {
|
||||||
|
if (!error) {
|
||||||
|
std::istream is(&buffer_);
|
||||||
|
std::string line;
|
||||||
|
std::getline(is, line);
|
||||||
|
//std::cout << "Received: " << line << std::endl;
|
||||||
|
parser(line);
|
||||||
|
read(); // Continue reading
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
std::cerr << "Read failed: " << error.message() << std::endl;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
);
|
||||||
|
}
|
||||||
|
|
||||||
|
boost::asio::io_service io_service_;
|
||||||
|
boost::asio::serial_port serial_;
|
||||||
|
boost::asio::streambuf buffer_;
|
||||||
|
};
|
||||||
|
|
@ -0,0 +1 @@
|
||||||
|
~/projects/Fire_Gimbal_Control/bin/x64/Release/Fire_Gimbal_Control.out -s 1
|
||||||
|
|
@ -0,0 +1 @@
|
||||||
|
~/projects/Fire_Gimbal_Control/bin/x64/Release/Fire_Gimbal_Control.out -i 1 -s 1
|
||||||
Binary file not shown.
|
|
@ -0,0 +1,31 @@
|
||||||
|
; Fire Gimbal Control - example configuration.
|
||||||
|
;
|
||||||
|
; Copy this file to "config.ini" and edit for your deployment:
|
||||||
|
; cp config/config.example.ini config.ini
|
||||||
|
;
|
||||||
|
; The real config.ini is gitignored because it contains credentials.
|
||||||
|
; MQTT credentials may instead be supplied via the environment variables
|
||||||
|
; FGC_MQTT_USER and FGC_MQTT_PW, which take precedence over the values here.
|
||||||
|
|
||||||
|
[General]
|
||||||
|
; Tower identity, substituted into all MQTT topics and CamEvent payloads.
|
||||||
|
tower_name = ExampleTower
|
||||||
|
; Seconds between captures.
|
||||||
|
image_interval = 5
|
||||||
|
; 1 = print motor telemetry each loop tick, 0 = quiet.
|
||||||
|
debug = 0
|
||||||
|
|
||||||
|
[Network]
|
||||||
|
; MQTT broker address (the ground-station / ZKMS server).
|
||||||
|
zkms_server_ip = 127.0.0.1
|
||||||
|
; Prefer the FGC_MQTT_USER / FGC_MQTT_PW environment variables over these.
|
||||||
|
mqtt_user = CHANGE_ME
|
||||||
|
mqtt_pw = CHANGE_ME
|
||||||
|
|
||||||
|
[Camera]
|
||||||
|
; Camera IDs: GigE IP (e.g. 192.168.11.101) or USB device ID (e.g. DEV_1AB22C0AADED).
|
||||||
|
; Leave entries blank/absent for cameras you do not have. Order maps to RGB, ACR, NIR.
|
||||||
|
id_Cam1 = DEV_0000000000
|
||||||
|
id_Cam2 =
|
||||||
|
id_Cam3 =
|
||||||
|
id_Cam4 =
|
||||||
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,131 @@
|
||||||
|
# Architecture
|
||||||
|
|
||||||
|
## Purpose
|
||||||
|
|
||||||
|
The program automates a **pan gimbal** carrying up to four cameras. It periodically rotates the gimbal to a
|
||||||
|
series of headings, stops, triggers the cameras, compresses each captured frame to JPEG XL, writes it to disk,
|
||||||
|
and announces the capture over MQTT so a ground station can ingest the images for wildfire detection. Remote
|
||||||
|
operators can override the behaviour over MQTT (e.g. point at a specific heading).
|
||||||
|
|
||||||
|
There is no persistent state beyond image files. Everything is one of:
|
||||||
|
- **Configuration** — read once at startup from `config.ini`.
|
||||||
|
- **Live state** — in-memory C++ structs, guarded by mutexes, overwritten each cycle.
|
||||||
|
- **Artifacts** — `.jxl` image files on disk.
|
||||||
|
- **Messages** — MQTT publish/subscribe (no local persistence).
|
||||||
|
|
||||||
|
## Component overview
|
||||||
|
|
||||||
|
```
|
||||||
|
┌──────────────────────────────────────────────────────┐
|
||||||
|
│ main.cpp │
|
||||||
|
│ - load config.ini (ini.c) │
|
||||||
|
│ - parse CLI args (Boost.Program_options) │
|
||||||
|
│ - own the 10 ms control loop │
|
||||||
|
└───┬───────────────┬───────────────┬──────────────┬─────┘
|
||||||
|
│ │ │ │
|
||||||
|
reads/writes │ reads │ reads │ drives │
|
||||||
|
▼ ▼ ▼ ▼
|
||||||
|
┌────────────────┐ ┌───────────┐ ┌──────────────┐ ┌──────────┐
|
||||||
|
│ SerialPort │ │ MQTTClient│ │ Parser / │ │VimbaHandler│
|
||||||
|
│ (Serial.h) │ │ (MQTT.*) │ │ CMD_eval │ │ (Camera.*) │
|
||||||
|
│ Boost.Asio │ │ Paho C++ │ │ Boost.Spirit │ │ Vimba X │
|
||||||
|
└──────┬─────────┘ └─────┬─────┘ └──────────────┘ └─────┬──────┘
|
||||||
|
│ │ │
|
||||||
|
serial │ /dev/ttyACM0 │ TCP GigE/USB │
|
||||||
|
▼ ▼ ▼
|
||||||
|
motor controller MQTT broker cameras
|
||||||
|
(MCU + sensors) (ground station) (RGB/ACR/NIR)
|
||||||
|
```
|
||||||
|
|
||||||
|
| Component | File(s) | Responsibility |
|
||||||
|
|-----------|---------|----------------|
|
||||||
|
| Entry / control loop | [main.cpp](../main.cpp) | Config, CLI, thread orchestration, capture cycle logic |
|
||||||
|
| Serial telemetry & commands | [Serial.h](../Serial.h) | Open `/dev/ttyACM0`, parse `motor_info` telemetry, send motor commands |
|
||||||
|
| MQTT client | [MQTT.h](../MQTT.h), [MQTT.cpp](../MQTT.cpp) | Connect to broker, subscribe to control topics, publish status & cam events |
|
||||||
|
| Camera acquisition | [Camera.h](../Camera.h), [Camera.cpp](../Camera.cpp) | Vimba X cameras, frame queue, JXL encode + save, cam event publish |
|
||||||
|
| JPEG XL encoder | [JPEG_XL.h](../JPEG_XL.h) | Wrap libjxl to encode an 8-bit image buffer to a `.jxl` file |
|
||||||
|
| Console parser | [Parser.h](../Parser.h) | Parse stdin command lines into actions |
|
||||||
|
| Timing helpers | [timing.h](../timing.h) | Stopwatch + Unix-ms timestamps used for filenames and loop pacing |
|
||||||
|
| INI parser | [ini.c](../ini.c), [ini.h](../ini.h) | Third-party `inih` used to read `config.ini` |
|
||||||
|
|
||||||
|
## Threading model
|
||||||
|
|
||||||
|
The process runs five concurrent contexts:
|
||||||
|
|
||||||
|
| Thread | Created in | Loop / blocking behaviour |
|
||||||
|
|--------|-----------|---------------------------|
|
||||||
|
| **Main / control loop** | `main()` | `while(running)`; 10 ms sleep per tick ([main.cpp](../main.cpp) lines 243-336) |
|
||||||
|
| **Serial I/O** | `main()` → `io_thread` | Runs `boost::asio::io_service::run()`; async read-until `\n` re-arms itself ([Serial.h](../Serial.h) lines 123-139) |
|
||||||
|
| **stdin reader** | `main()` → `inputThread` | `readInput()` blocks on `std::getline`, feeds the `Parser` ([main.cpp](../main.cpp) lines 42-59) |
|
||||||
|
| **MQTT client** | `MQTTClient::connect_client()` → `mqtt_thread` | Connects, then **busy-waits** `while(running);` ([MQTT.cpp](../MQTT.cpp) lines 82-99). Paho's own threads deliver callbacks |
|
||||||
|
| **Image saver** | `VimbaHandler::Start()` → `image_saver_thread` | Waits on a condition variable, drains the per-camera queues, encodes + writes JXL ([Camera.cpp](../Camera.cpp) lines 303-394) |
|
||||||
|
|
||||||
|
Vimba X additionally delivers frames on its own internal acquisition threads via the `FrameObserver` callback.
|
||||||
|
|
||||||
|
### Shared state and synchronization
|
||||||
|
|
||||||
|
| Shared data | Type | Guard | Producer → Consumer |
|
||||||
|
|-------------|------|-------|---------------------|
|
||||||
|
| `motor_info` telemetry | struct | `SerialPort::mut` | Serial I/O thread → main loop (`get_controller_info()`) |
|
||||||
|
| `mqtt_sub_data` (target heading, control code) | struct | `MQTTCallback::mqtt_mut` | Paho callback thread → main loop (`get_sub_data()`, which clears the "available" flags) |
|
||||||
|
| Per-camera frame queues | `std::queue<ImageStore8Ptr>` (max 100) | `VimbaHandler::queue_mut` | FrameObserver/enqueue → saver thread; saver woken by `cv_proc` |
|
||||||
|
| `parser_data` (console command) | struct | `Parser::mut` | stdin thread → main loop |
|
||||||
|
|
||||||
|
## End-to-end data flow
|
||||||
|
|
||||||
|
1. **Startup** — `main()` loads `config.ini`, parses CLI flags, constructs `SerialPort`, `MQTTClient`,
|
||||||
|
and `VimbaHandler`. If `--init` is set, it sends an endstop-finding command sequence to the motor controller
|
||||||
|
(`r`, `e`, `q`, wait 60 s, `y`, `ud56`).
|
||||||
|
2. **Telemetry ingest** — the motor controller streams lines like
|
||||||
|
`$;<Xenc>;<Xerr>;<sgt_val>;<sgt_stat>;<is_moving>;<control_status>;<hdg>;<deviation_warn>;<humid>;<temp>;<fan_pwm>`.
|
||||||
|
`SerialPort::parser()` splits on `;`, validates 12 fields, and stores a fresh `motor_info`.
|
||||||
|
3. **Control decision** (per 10 ms tick, [main.cpp](../main.cpp) lines 293-334):
|
||||||
|
- Read current `motor_info` and current MQTT `ControlCode`/`target_HDG`.
|
||||||
|
- **ControlCode 0 (automatic sweep):** when the image interval has elapsed and the camera is started, send
|
||||||
|
`p` (stop/advance), then once stopped, trigger the cameras.
|
||||||
|
- **ControlCode 1 (directed):** same timing, but send `kd<heading>` to drive to the MQTT-supplied target
|
||||||
|
heading before triggering.
|
||||||
|
4. **Trigger & settle** — `TriggerCamera()` resets each observer's `settle` counter to 3 and fires the
|
||||||
|
`TriggerSoftware` command 4 times (~400 ms apart). The first 3 frames are intentionally dumped (sensor
|
||||||
|
settling); the 4th is the real image.
|
||||||
|
5. **Enqueue** — for each completed frame, `EnqueueToStoreStruct()` deep-copies the pixel buffer into an
|
||||||
|
`image_store_8bit` with a Unix-ms timestamp and pushes it onto that camera's bounded queue (oldest reused if
|
||||||
|
the queue is at 100). Enqueuing camera 0 notifies the saver via `cv_proc`.
|
||||||
|
6. **Save** — `SaveImage()` pops frames, builds an OpenCV `Mat`, rotates 90° CCW, optionally displays it, then
|
||||||
|
(unless in demo mode) encodes with `JPEGXL` and writes to
|
||||||
|
`$HOME/projects/Fire_Gimbal_Control/bin/x64/Release/<RGB|ACR|NIR>/<unix_ms>.jxl`. In demo mode it copies
|
||||||
|
`test_smoke.jxl` instead.
|
||||||
|
7. **Announce** — after each saved frame, publish a `CamEvent` JSON message to
|
||||||
|
`GGS/FWT/<tower>/CamEvent` with the tower, camera name, heading (×10, integer), and timestamp.
|
||||||
|
|
||||||
|
## Capture state machine (per camera trigger cycle)
|
||||||
|
|
||||||
|
```
|
||||||
|
interval elapsed AND cam_started
|
||||||
|
│
|
||||||
|
▼
|
||||||
|
┌──────────────────────┐ ControlCode 0 → send "p"
|
||||||
|
│ request gimbal stop │ ControlCode 1 → send "kd<target_HDG>"
|
||||||
|
└──────────┬───────────┘ set trigger_after_stopping = true, reset loop_timer
|
||||||
|
│
|
||||||
|
▼ (after >100 ms, when is_moving == 1)
|
||||||
|
┌──────────────────────┐
|
||||||
|
│ TriggerCamera() │ reset settle=3; fire TriggerSoftware ×4 (400 ms apart)
|
||||||
|
└──────────┬───────────┘
|
||||||
|
│ frames 1-3 dumped (settling), frame 4 enqueued
|
||||||
|
▼
|
||||||
|
┌──────────────────────┐
|
||||||
|
│ enqueue → save → MQTT │ trigger_after_stopping = false
|
||||||
|
└──────────────────────┘
|
||||||
|
```
|
||||||
|
|
||||||
|
> The settle/trigger timing and the `is_moving == 1` conditions are documented exactly as implemented; see
|
||||||
|
> [docs/known-issues.md](known-issues.md) for behaviour that looks surprising (e.g. triggering is gated on
|
||||||
|
> `is_moving == 1` rather than `== 0`).
|
||||||
|
|
||||||
|
## Why this shape
|
||||||
|
|
||||||
|
The design favours **low-latency real-time control** over durability: a tight polling loop reacts to motor
|
||||||
|
feedback in milliseconds, the camera pipeline is decoupled by an in-memory queue so encoding never blocks
|
||||||
|
acquisition, and all coordination with the outside world is asynchronous (serial async I/O + MQTT). Persistence
|
||||||
|
is deliberately limited to image files plus fire-and-forget MQTT messages.
|
||||||
|
|
@ -0,0 +1,118 @@
|
||||||
|
# Build & Setup
|
||||||
|
|
||||||
|
This guide covers building the binary and the host setup needed to run it. For the things that will stop a
|
||||||
|
clean reproduction on a fresh machine, read [known-issues.md](known-issues.md) alongside this page.
|
||||||
|
|
||||||
|
## Toolchain
|
||||||
|
|
||||||
|
- **g++** with **C++17** (`-std=c++17`), GNU Make. The C file `ini.c` is compiled with **gcc**.
|
||||||
|
- Linux. Developed/deployed on a tower PC (LattePanda Sigma) running Linux; serial device is `/dev/ttyACM0`.
|
||||||
|
|
||||||
|
See [Makefile](../Makefile) — `CXXFLAGS := -std=c++17 -g -Wall -I.`
|
||||||
|
|
||||||
|
## Dependencies
|
||||||
|
|
||||||
|
The link line in the [Makefile](../Makefile) is:
|
||||||
|
|
||||||
|
```
|
||||||
|
-lpaho-mqttpp3 -lpaho-mqtt3a -lopencv_core -lopencv_highgui -ljxl -ljxl_threads -lboost_program_options -lVmbC -lVmbCPP
|
||||||
|
```
|
||||||
|
|
||||||
|
| Dependency | Libraries linked | Headers used | Typical source |
|
||||||
|
|------------|------------------|--------------|----------------|
|
||||||
|
| **Eclipse Paho MQTT C++** | `paho-mqttpp3`, `paho-mqtt3a` | `mqtt/async_client.h` | `libpaho-mqttpp-dev` / build from source (also needs Paho MQTT C) |
|
||||||
|
| **OpenCV** | `opencv_core`, `opencv_highgui` | `opencv2/opencv.hpp` | `libopencv-dev` |
|
||||||
|
| **libjxl (JPEG XL)** | `jxl`, `jxl_threads` | `jxl/encode.h`, `jxl/encode_cxx.h`, `jxl/thread_parallel_runner*.h` | `libjxl-dev` / build from source |
|
||||||
|
| **Boost** | `boost_program_options` | `boost/asio.hpp`, `boost/program_options.hpp`, `boost/spirit/include/qi.hpp`, `phoenix.hpp` | `libboost-all-dev` (or at least program_options, system, and the header-only Asio/Spirit) |
|
||||||
|
| **Allied Vision Vimba X SDK** | `VmbC`, `VmbCPP` | `VmbC/VmbC.h`, `VmbCPP/VmbCPP.h` | **Proprietary** — download from Allied Vision (see below) |
|
||||||
|
|
||||||
|
`cxxopts.hpp` is bundled but **not used** (the active CLI parser is Boost.Program_options); it does not need to
|
||||||
|
be installed.
|
||||||
|
|
||||||
|
### Debian/Ubuntu package hint
|
||||||
|
|
||||||
|
The non-proprietary dependencies are roughly:
|
||||||
|
|
||||||
|
```bash
|
||||||
|
sudo apt install build-essential libboost-all-dev libopencv-dev libjxl-dev \
|
||||||
|
libpaho-mqttpp-dev libpaho-mqtt-dev
|
||||||
|
```
|
||||||
|
|
||||||
|
Package names vary by distro/version; `libjxl-dev` and the Paho C++ packages may not exist on older releases and
|
||||||
|
then have to be built from source. This machine is **Manjaro/Arch** — use `pacman`/AUR equivalents
|
||||||
|
(`boost`, `opencv`, `libjxl`, `paho-mqtt-c`, `paho-mqtt-cpp`).
|
||||||
|
|
||||||
|
### Allied Vision Vimba X SDK (proprietary — required)
|
||||||
|
|
||||||
|
`VmbC`/`VmbCPP` are **not** available through any package manager. Download and install the Vimba X SDK from
|
||||||
|
Allied Vision (<https://www.alliedvision.com/en/products/software/vimba-x-sdk/>). After install you typically
|
||||||
|
need to:
|
||||||
|
|
||||||
|
- Make the SDK headers visible to the compiler (add `-I<vimbax>/api/include`) and the libs to the linker
|
||||||
|
(`-L<vimbax>/api/lib`), and/or add the lib dir to `LD_LIBRARY_PATH` / an `ld.so.conf.d` entry at runtime.
|
||||||
|
- Run the SDK's transport-layer setup so cameras are discoverable (GenICam GenTL producers).
|
||||||
|
|
||||||
|
The current [Makefile](../Makefile) assumes the headers/libs are already on the default include/link paths; on
|
||||||
|
a fresh machine you will likely need to extend `CXXFLAGS`/`LDFLAGS` with the SDK paths.
|
||||||
|
|
||||||
|
## Build
|
||||||
|
|
||||||
|
```bash
|
||||||
|
make # compiles ini.c, main.cpp, MQTT.cpp, Camera.cpp → bin/Fire_Gimbal_Control.out
|
||||||
|
make clean # removes object files and the binary
|
||||||
|
```
|
||||||
|
|
||||||
|
- Object files land in `obj/` (the Makefile uses `OBJDIR := obj`).
|
||||||
|
- The output binary is `bin/Fire_Gimbal_Control.out`.
|
||||||
|
|
||||||
|
> Note: the committed build artifacts live under `bin/x64/Release/` and `obj/x64/Release/`, but the current
|
||||||
|
> Makefile writes to `bin/` and `obj/`. The `x64/Release/` layout is the *deployed* layout the running binary
|
||||||
|
> expects for config and image output (see below and [known-issues.md](known-issues.md)).
|
||||||
|
|
||||||
|
## Directory layout (build vs. deployed)
|
||||||
|
|
||||||
|
```
|
||||||
|
<repo>/
|
||||||
|
├── obj/ ← Makefile object output
|
||||||
|
├── bin/Fire_Gimbal_Control.out ← Makefile binary output
|
||||||
|
└── bin/x64/Release/ ← deployed/runtime directory
|
||||||
|
├── Fire_Gimbal_Control.out ← deployed binary
|
||||||
|
├── config.ini ← runtime config (differs from repo-root config.ini)
|
||||||
|
├── startup_gimbal.sh ← launches with --start 1
|
||||||
|
├── startup_gimbal_with_init.sh ← launches with --init 1 --start 1
|
||||||
|
├── test_smoke.jxl ← demo-mode placeholder image
|
||||||
|
├── RGB/ (ACR/ NIR/) ← image output folders (created on demand)
|
||||||
|
└── crash_genicam.txt ← captured GenICam trigger error log (reference)
|
||||||
|
```
|
||||||
|
|
||||||
|
`compile_commands.json` is present for editor/clangd integration.
|
||||||
|
|
||||||
|
## Host setup to run
|
||||||
|
|
||||||
|
1. **Serial device** — the motor controller must enumerate as `/dev/ttyACM0` at 115200 8N1. Add your user to
|
||||||
|
the `dialout` group (or set a udev rule) for non-root access:
|
||||||
|
```bash
|
||||||
|
sudo usermod -aG dialout "$USER" # log out/in afterward
|
||||||
|
```
|
||||||
|
2. **Cameras** — connect the Allied Vision cameras and confirm Vimba X can see them (vendor `VimbaXViewer` /
|
||||||
|
`ListCameras`). The camera IDs in `config.ini` must match (GigE IPs like `192.168.11.101` or USB
|
||||||
|
`DEV_...` IDs). For GigE, configure the host NIC on the `192.168.11.x` subnet.
|
||||||
|
3. **MQTT broker** — a broker must be reachable at the `zkms_server_ip` from `config.ini` with the configured
|
||||||
|
username/password. **The program exits immediately if it cannot connect** ([main.cpp](../main.cpp) lines
|
||||||
|
162-165). For local testing run e.g. Mosquitto and point `zkms_server_ip` at `127.0.0.1`.
|
||||||
|
4. **Paths** — the binary reads config from a hardcoded absolute path and writes images under
|
||||||
|
`$HOME/projects/Fire_Gimbal_Control/...`. See [known-issues.md](known-issues.md) for the exact paths and how
|
||||||
|
to satisfy them.
|
||||||
|
|
||||||
|
## Run
|
||||||
|
|
||||||
|
```bash
|
||||||
|
# from the deployed directory
|
||||||
|
./Fire_Gimbal_Control.out --help # show options
|
||||||
|
./Fire_Gimbal_Control.out --start 1 # connect, start camera capture immediately
|
||||||
|
./Fire_Gimbal_Control.out --init 1 --start 1 # find endstops, then start
|
||||||
|
./Fire_Gimbal_Control.out --demo 1 # demo mode (copies test_smoke.jxl instead of encoding)
|
||||||
|
```
|
||||||
|
|
||||||
|
Type `exit` on the console (or send EOF) to stop. See [configuration.md](configuration.md) for all CLI flags
|
||||||
|
and the interactive console commands.
|
||||||
|
|
@ -0,0 +1,115 @@
|
||||||
|
# Configuration & Commands
|
||||||
|
|
||||||
|
Three layers control behaviour: the **`config.ini`** file (read once at startup), **command-line flags**, and
|
||||||
|
**interactive console commands** typed on stdin while running.
|
||||||
|
|
||||||
|
## `config.ini`
|
||||||
|
|
||||||
|
Parsed by the `inih` library ([ini.c](../ini.c)). Keys are flattened to `Section.name` and stored in a
|
||||||
|
`std::map<std::string,std::string>` ([main.cpp](../main.cpp) lines 35-40, 64-95).
|
||||||
|
|
||||||
|
> **Important:** the binary reads from a **hardcoded absolute path**, not the file next to the binary:
|
||||||
|
> `/home/ggs/projects/Fire_Gimbal_Control/bin/x64/Release/config.ini` ([main.cpp](../main.cpp) line 66).
|
||||||
|
> See [known-issues.md](known-issues.md).
|
||||||
|
|
||||||
|
### Keys
|
||||||
|
|
||||||
|
| Section | Key | Type | Used as | Notes |
|
||||||
|
|---------|-----|------|---------|-------|
|
||||||
|
| `General` | `tower_name` | string | Tower identity; substituted into all MQTT topics and CamEvent payloads | e.g. `Rietschen`, `Staeffelsberg` |
|
||||||
|
| `General` | `image_interval` | int | Seconds between captures; converted to `imagerate = 1/interval` | Required (parsed with `stoi`) |
|
||||||
|
| `General` | `debug` | int (0/1) | Sets `motorctl_info_out`; when on, prints full telemetry each tick | |
|
||||||
|
| `Network` | `zkms_server_ip` | string | MQTT broker address the client connects to | "ZKMS" = the ground-station server |
|
||||||
|
| `Network` | `mqtt_user` | string | MQTT username | Plaintext |
|
||||||
|
| `Network` | `mqtt_pw` | string | MQTT password | Plaintext |
|
||||||
|
| `Camera` | `id_Cam1`..`id_Cam4` | string | Camera IDs; non-empty ones are added in order | GigE IP (`192.168.11.101`) or USB ID (`DEV_1AB22C0AADED`) |
|
||||||
|
|
||||||
|
Camera index → output folder mapping is by **position** (first configured camera = index 0):
|
||||||
|
`0 → RGB`, `1 → ACR`, `2 → NIR` ([Camera.cpp](../Camera.cpp) lines 342-347).
|
||||||
|
|
||||||
|
### Two config files exist (and differ)
|
||||||
|
|
||||||
|
| File | tower_name | interval | broker | cameras |
|
||||||
|
|------|-----------|----------|--------|---------|
|
||||||
|
| [config.ini](../config.ini) (repo root) | `Rietschen` | 5 | `10.11.12.13` | 3× GigE IPs `192.168.11.101-103` |
|
||||||
|
| [bin/x64/Release/config.ini](../bin/x64/Release/config.ini) | `Staeffelsberg` | 3 | `127.0.0.1` | 1× USB `DEV_1AB22C0AADED` |
|
||||||
|
|
||||||
|
The Release copy is the Staeffelsberg deployment. Neither sits at the hardcoded path the binary actually reads —
|
||||||
|
see [known-issues.md](known-issues.md).
|
||||||
|
|
||||||
|
### Example
|
||||||
|
|
||||||
|
```ini
|
||||||
|
[General]
|
||||||
|
tower_name = Staeffelsberg
|
||||||
|
image_interval = 3
|
||||||
|
debug = 0
|
||||||
|
|
||||||
|
[Network]
|
||||||
|
zkms_server_ip = 127.0.0.1
|
||||||
|
mqtt_user = fwt_gimbal
|
||||||
|
mqtt_pw = aeroaero
|
||||||
|
|
||||||
|
[Camera]
|
||||||
|
id_Cam1 = DEV_1AB22C0AADED
|
||||||
|
```
|
||||||
|
|
||||||
|
## Command-line flags
|
||||||
|
|
||||||
|
Parsed by Boost.Program_options ([main.cpp](../main.cpp) lines 104-153). All take a `bool` value
|
||||||
|
(presence + value); the code treats *presence* of the option as "on" regardless of the value.
|
||||||
|
|
||||||
|
| Flag | Short | Value | Effect |
|
||||||
|
|------|-------|-------|--------|
|
||||||
|
| `--help` | `-h` | — | Print options and exit |
|
||||||
|
| `--init` | `-i` | bool | Run the endstop-finding init sequence before the main loop |
|
||||||
|
| `--start` | `-s` | bool | Start camera capture automatically (no `start` console command needed) |
|
||||||
|
| `--demo` | `-d` | bool | Demo/simulation mode: copy `test_smoke.jxl` instead of encoding real frames |
|
||||||
|
|
||||||
|
Usage: `./Fire_Gimbal_Control.out --init 1 --start 1`
|
||||||
|
|
||||||
|
### Init sequence (`--init`)
|
||||||
|
|
||||||
|
When `--init` is set, before entering the loop the program sends to the motor controller, with delays
|
||||||
|
([main.cpp](../main.cpp) lines 220-241): `r` → `e` → `q` → *(wait 60 s)* → `y` → `ud56` ("set number of
|
||||||
|
intervals per turn" = 56). This finds endstops and configures the sweep resolution.
|
||||||
|
|
||||||
|
## Interactive console commands
|
||||||
|
|
||||||
|
While running, lines typed on stdin are parsed by [Parser.h](../Parser.h) (Boost.Spirit Qi) into up to four
|
||||||
|
tokens: `command device option value`. The grammar expects alphabetic tokens separated by spaces and an
|
||||||
|
optional trailing number. `CMD_eval` then maps them to actions ([main.cpp](../main.cpp) lines 244-273).
|
||||||
|
|
||||||
|
| Type this | Meaning |
|
||||||
|
|-----------|---------|
|
||||||
|
| `start` | Start camera acquisition + saver thread |
|
||||||
|
| `stop` | Stop camera acquisition |
|
||||||
|
| `debug` | Toggle telemetry printout (`motorctl_info_out`) |
|
||||||
|
| `set camera fps <value>` | Change camera acquisition frame rate (`AcquisitionFrameRate`) |
|
||||||
|
| `set camera jxlq <value>` | Set JPEG XL quality/distance (0 = lossless; higher = lossier) |
|
||||||
|
| `set camera jxle <value>` | Set JPEG XL effort (libjxl effort level) |
|
||||||
|
| `set camera display <0\|1>` | Toggle live OpenCV preview window |
|
||||||
|
| `set fps <value>` | Set the **capture interval rate** `imagerate` (images per second logic), not camera fps |
|
||||||
|
| `set motorctl <cmd>` | Forward a raw command string to the motor controller over serial |
|
||||||
|
| `exit` | Stop the program (sets `running = false`) |
|
||||||
|
|
||||||
|
Notes:
|
||||||
|
- `set camera ...` options are handled in `VimbaHandler::evaluateCommand` ([Camera.cpp](../Camera.cpp) lines
|
||||||
|
241-251); only `fps`, `jxlq`, `jxle`, `display` are recognized.
|
||||||
|
- Because the grammar tokenizes on spaces with a trailing number, commands like `set motorctl <cmd>` rely on the
|
||||||
|
`<cmd>` landing in the `option` field. Numeric-only motor commands and multi-token strings may not parse as
|
||||||
|
expected — see [known-issues.md](known-issues.md).
|
||||||
|
|
||||||
|
## Raw motor-controller command reference (observed)
|
||||||
|
|
||||||
|
These short strings are sent over serial (`sendCommand`) by the program or via `set motorctl`. They are
|
||||||
|
interpreted by the **motor controller firmware** (not in this repo); listed here for reference of what the
|
||||||
|
software emits:
|
||||||
|
|
||||||
|
| Command | Sent when | Apparent meaning |
|
||||||
|
|---------|-----------|------------------|
|
||||||
|
| `r`, `e`, `q` | init sequence | endstop/homing steps |
|
||||||
|
| `y` | init sequence | (post-home step) |
|
||||||
|
| `ud56` | init sequence | set intervals-per-turn = 56 |
|
||||||
|
| `p` | ControlCode 0 capture | stop / advance to next interval |
|
||||||
|
| `kd<heading>` | ControlCode 1 capture | drive to target heading `<heading>` |
|
||||||
|
|
@ -0,0 +1,116 @@
|
||||||
|
# Known Issues & Reproduction Blockers
|
||||||
|
|
||||||
|
This page lists everything that stands between a freshly-copied checkout and a running system, plus a few
|
||||||
|
correctness/robustness notes. These are **documentation only** — no code has been changed. Each entry notes a
|
||||||
|
recommended fix if you choose to act on it later.
|
||||||
|
|
||||||
|
## Reproduction blockers
|
||||||
|
|
||||||
|
### 1. Hardcoded config path (will fail on this machine)
|
||||||
|
[main.cpp](../main.cpp) line 66 reads the config from an absolute path owned by user `ggs`:
|
||||||
|
|
||||||
|
```cpp
|
||||||
|
ini_parse("/home/ggs/projects/Fire_Gimbal_Control/bin/x64/Release/config.ini", handler, &config)
|
||||||
|
```
|
||||||
|
|
||||||
|
On this machine (user `pedro`, repo at `~/code/Fire_Gimbal_Control_staeffelsberg`) that file does not exist, so
|
||||||
|
the program prints `Can't load config.ini file!` and exits.
|
||||||
|
|
||||||
|
**To run as-is:** create the expected tree and place a config there:
|
||||||
|
```bash
|
||||||
|
mkdir -p /home/ggs/projects/Fire_Gimbal_Control/bin/x64/Release
|
||||||
|
cp bin/x64/Release/config.ini /home/ggs/projects/Fire_Gimbal_Control/bin/x64/Release/
|
||||||
|
```
|
||||||
|
(Or run under a `ggs` user / adjust the path.)
|
||||||
|
**Recommended fix:** make the config path a CLI argument or resolve it relative to the executable / `$HOME`.
|
||||||
|
|
||||||
|
### 2. Hardcoded image output path
|
||||||
|
[Camera.cpp](../Camera.cpp) line 348 writes images under:
|
||||||
|
|
||||||
|
```
|
||||||
|
$HOME/projects/Fire_Gimbal_Control/bin/x64/Release/<RGB|ACR|NIR>/<unix_ms>.jxl
|
||||||
|
```
|
||||||
|
|
||||||
|
This uses `$HOME` (so it follows the running user) but a **fixed `projects/Fire_Gimbal_Control/...` subtree**,
|
||||||
|
not the repo location. The directories are created on demand, so this mainly matters for knowing where images
|
||||||
|
land and for free-space planning.
|
||||||
|
**Recommended fix:** derive the output root from config.
|
||||||
|
|
||||||
|
### 3. Startup scripts point at the deployed tree
|
||||||
|
[bin/x64/Release/startup_gimbal.sh](../bin/x64/Release/startup_gimbal.sh) and
|
||||||
|
[startup_gimbal_with_init.sh](../bin/x64/Release/startup_gimbal_with_init.sh) invoke:
|
||||||
|
|
||||||
|
```
|
||||||
|
~/projects/Fire_Gimbal_Control/bin/x64/Release/Fire_Gimbal_Control.out ...
|
||||||
|
```
|
||||||
|
|
||||||
|
i.e. the deployed `~/projects/...` layout, **not** `~/code/Fire_Gimbal_Control_staeffelsberg`. To use the
|
||||||
|
scripts unchanged, deploy the built binary + config into that tree; otherwise edit the scripts to point at your
|
||||||
|
build output.
|
||||||
|
|
||||||
|
### 4. Proprietary SDK required — Allied Vision Vimba X
|
||||||
|
`VmbC`/`VmbCPP` are not installable via any package manager. Without the Vimba X SDK the project **will not
|
||||||
|
compile or link** (`-lVmbC -lVmbCPP`, headers `VmbC/VmbC.h`, `VmbCPP/VmbCPP.h`). Download from Allied Vision and
|
||||||
|
add the SDK include/lib paths — see [build-and-setup.md](build-and-setup.md). Camera IDs in `config.ini` must
|
||||||
|
match the transport: GigE IPs (`192.168.11.10x`, host NIC on that subnet) or USB IDs (`DEV_...`).
|
||||||
|
|
||||||
|
### 5. Hardware + broker needed for a full run
|
||||||
|
A complete run needs all of:
|
||||||
|
- the **motor controller MCU** enumerated at `/dev/ttyACM0` (115200 8N1), user in `dialout`;
|
||||||
|
- the **cameras** reachable by Vimba X;
|
||||||
|
- a **reachable MQTT broker** at `zkms_server_ip` with the configured credentials.
|
||||||
|
|
||||||
|
If serial open fails it is caught and logged (the program continues but has no telemetry). If MQTT does not
|
||||||
|
connect, the program **exits** ([main.cpp](../main.cpp) lines 162-165).
|
||||||
|
|
||||||
|
**Demo mode is not a no-hardware path.** `--demo 1` skips JXL encoding (copies `test_smoke.jxl` instead), but
|
||||||
|
`VimbaHandler`'s constructor still starts the Vimba system and calls `Open()` on each configured camera, so
|
||||||
|
cameras must still be present. There is no flag to bypass cameras entirely.
|
||||||
|
|
||||||
|
## Correctness / robustness notes
|
||||||
|
|
||||||
|
### 6. Telemetry field order (humid before temp)
|
||||||
|
The serial parser maps `values[9] → humid` and `values[10] → temp` ([Serial.h](../Serial.h) lines 99-100). The
|
||||||
|
motor-controller firmware must emit fields in that order. If temperature/humidity look swapped, this is why.
|
||||||
|
|
||||||
|
### 7. Trigger gated on `is_moving == 1`
|
||||||
|
In the capture state machine the actual `TriggerCamera()` call happens only while `act_info.is_moving == 1`
|
||||||
|
([main.cpp](../main.cpp) lines 305-306 and 328-329), i.e. it triggers *while the gimbal still reports moving*
|
||||||
|
rather than after it has stopped. Documented as-is; verify against the firmware's `is_moving` semantics if
|
||||||
|
captured frames look motion-blurred.
|
||||||
|
|
||||||
|
### 8. Plaintext MQTT credentials
|
||||||
|
`mqtt_user` / `mqtt_pw` are stored in plaintext in `config.ini` and printed paths/values go to stdout.
|
||||||
|
**Recommended:** restrict file permissions and/or source credentials from the environment or a secrets store.
|
||||||
|
|
||||||
|
### 9. `MQTTClient::run()` busy-waits
|
||||||
|
After connecting, the MQTT thread spins on `while (running) ;` ([MQTT.cpp](../MQTT.cpp) lines 97-98), pegging a
|
||||||
|
CPU core. Functionally harmless but wasteful. **Recommended:** replace with a condition variable / sleep, or
|
||||||
|
simply let the thread exit since Paho's own threads handle delivery.
|
||||||
|
|
||||||
|
### 10. `SerialPort::parser()` missing return on non-`$` lines
|
||||||
|
`parser()` returns nothing when the first character isn't `$` ([Serial.h](../Serial.h) lines 74-109) — undefined
|
||||||
|
behaviour for a non-void function. In practice the result is ignored, but compile with `-Wall -Wreturn-type`
|
||||||
|
(already `-Wall`) and consider adding an explicit `return false;`.
|
||||||
|
|
||||||
|
### 11. Console grammar limits on `set motorctl`
|
||||||
|
The Boost.Spirit grammar expects `command device option <double>` with alphabetic tokens
|
||||||
|
([Parser.h](../Parser.h) lines 52-62). Raw motor commands that are numeric or multi-token may not land in the
|
||||||
|
`option` field as intended. Verify any `set motorctl <cmd>` you rely on actually parses (watch the echoed
|
||||||
|
`command/device/option/value` line).
|
||||||
|
|
||||||
|
### 12. Two divergent `config.ini` files
|
||||||
|
[config.ini](../config.ini) (root: `Rietschen`, 3 GigE cameras) and
|
||||||
|
[bin/x64/Release/config.ini](../bin/x64/Release/config.ini) (`Staeffelsberg`, 1 USB camera) differ, and
|
||||||
|
**neither is at the path the binary reads** (issue #1). Decide which is authoritative for this deployment and
|
||||||
|
place it at the expected path.
|
||||||
|
|
||||||
|
## Minimal local bring-up checklist
|
||||||
|
|
||||||
|
1. Install non-proprietary deps + Vimba X SDK; `make` succeeds. ([build-and-setup.md](build-and-setup.md))
|
||||||
|
2. Put a valid `config.ini` at `/home/ggs/projects/Fire_Gimbal_Control/bin/x64/Release/config.ini` (issue #1),
|
||||||
|
with `zkms_server_ip = 127.0.0.1` and a local broker running (e.g. Mosquitto).
|
||||||
|
3. Ensure at least one configured camera is visible to Vimba X and on the right subnet/USB.
|
||||||
|
4. Connect the motor MCU on `/dev/ttyACM0` (or expect telemetry-less operation).
|
||||||
|
5. Run `./Fire_Gimbal_Control.out --start 1` and watch `GGS/FWT/<tower>/CamEvent` over MQTT and the
|
||||||
|
`<RGB|ACR|NIR>/` image folders.
|
||||||
|
|
@ -0,0 +1,118 @@
|
||||||
|
# Module Reference
|
||||||
|
|
||||||
|
Per-file reference for the source tree, plus the key in-memory data structures (this project's "data model").
|
||||||
|
|
||||||
|
## Source files
|
||||||
|
|
||||||
|
### [main.cpp](../main.cpp) — entry point & control loop
|
||||||
|
- `handler()` — inih callback flattening INI keys to `Section.name`.
|
||||||
|
- `readInput()` — stdin thread; reads lines, forwards to the `Parser`, handles `exit`.
|
||||||
|
- `main()` — loads config, parses CLI flags, constructs `SerialPort`, `MQTTClient`, `VimbaHandler`; starts the
|
||||||
|
serial I/O thread and stdin thread; optionally runs the init sequence; runs the 10 ms control loop
|
||||||
|
(command eval → telemetry poll → MQTT poll → capture state machine); cleans up on exit.
|
||||||
|
- Global state lives at file scope (`running`, `imagerate`, `ctl_code`, `mqtt_hdg`, `mqtt_client`,
|
||||||
|
`camera_id_vec`, …).
|
||||||
|
|
||||||
|
### [Serial.h](../Serial.h) — motor controller link
|
||||||
|
- `struct motor_info` — telemetry snapshot (see below).
|
||||||
|
- `class SerialPort` — opens the port (115200 8N1, no parity/flow), async read-until `\n`, async write.
|
||||||
|
- `parser()` — splits a `$`-prefixed `;`-delimited line into 12 fields and stores a `motor_info`.
|
||||||
|
- `set_controller_info()` / `get_controller_info()` — mutex-guarded accessors.
|
||||||
|
- `sendCommand()` — async-write a command string.
|
||||||
|
- `run()` / `stop()` — drive the Boost.Asio `io_service`.
|
||||||
|
|
||||||
|
### [MQTT.h](../MQTT.h) / [MQTT.cpp](../MQTT.cpp) — MQTT client
|
||||||
|
- `struct mqtt_sub_data` — latest remote control state + "available" flags (see below).
|
||||||
|
- `class MQTTCallback` — Paho callback/listener: subscribes on connect, parses inbound messages, auto-reconnects
|
||||||
|
on loss; `get_sub_data()` returns + clears the available flags.
|
||||||
|
- `class MQTTClient` — owns the `async_client`, connect options (auth, keep-alive, clean session); `publish()`
|
||||||
|
sends QoS 1 retained messages. See [mqtt-api.md](mqtt-api.md) for the topic catalog.
|
||||||
|
|
||||||
|
### [Camera.h](../Camera.h) / [Camera.cpp](../Camera.cpp) — camera pipeline (Vimba X)
|
||||||
|
- `struct image_store_8bit` — owns a deep copy of one frame's pixel buffer + metadata (see below).
|
||||||
|
- `class FrameObserver` (in .cpp) — Vimba `IFrameObserver`; dumps the first 3 frames after each trigger
|
||||||
|
(`settle`), enqueues complete frames, re-queues buffers to the camera.
|
||||||
|
- `class VimbaHandler` — starts up the Vimba system, opens cameras by ID, manages the per-camera bounded
|
||||||
|
queues and the saver thread.
|
||||||
|
- `Start()`/`Stop()` — begin/end continuous acquisition + saver thread.
|
||||||
|
- `EnqueueToStoreStruct()` — copy a received frame into the queue (reuse oldest if at 100).
|
||||||
|
- `SaveImage()` — saver loop: rotate 90° CCW, optionally display, encode JXL (or copy `test_smoke.jxl` in
|
||||||
|
demo), write file, publish CamEvent.
|
||||||
|
- `TriggerCamera()` / `TriggerSettle()` — fire `TriggerSoftware` ×4 with settle reset.
|
||||||
|
- `ChangeFramerate()`, `evaluateCommand()`, `SetTowerName()`.
|
||||||
|
|
||||||
|
### [JPEG_XL.h](../JPEG_XL.h) — `class JPEGXL`
|
||||||
|
Wraps libjxl. Constructor encodes an 8-bit interleaved buffer (1 or 3 channels) into an in-memory JXL
|
||||||
|
codestream using a thread-parallel runner; `q == 0` → lossless, otherwise `q` is the frame distance; `e` is the
|
||||||
|
effort level. `WriteFile()` flushes the codestream to disk.
|
||||||
|
|
||||||
|
### [Parser.h](../Parser.h) — console command parsing
|
||||||
|
- `enum InputCommands` — command kinds (`startcamera`, `stopcamera`, `setcamera`, `setimagerate`,
|
||||||
|
`setmotorcontrol`, `setdebug`, `no_cmd`, `setgimbal`).
|
||||||
|
- `struct parser_data` — parsed tokens (see below).
|
||||||
|
- `struct Parser` — Boost.Spirit Qi grammar splitting input into `command device option value`; mutex-guarded.
|
||||||
|
- `struct CMD_eval` — maps `parser_data` to an `InputCommands` value. See [configuration.md](configuration.md).
|
||||||
|
|
||||||
|
### [timing.h](../timing.h) — time helpers
|
||||||
|
- `class Timer` — high-resolution stopwatch (`Reset`, `Elapsed`, `ElapsedMillis`) plus string/file timestamp
|
||||||
|
helpers; used for loop pacing and profiling.
|
||||||
|
- `class NanoUnixTimer` — Unix-epoch **millisecond** timestamps (`Stamp_longlong`, `Stamp_string`) used for
|
||||||
|
image filenames and CamEvent `time`.
|
||||||
|
- `class ScopedTimer` — RAII timer that logs elapsed time on destruction.
|
||||||
|
|
||||||
|
### [ini.c](../ini.c) / [ini.h](../ini.h) — third-party `inih`
|
||||||
|
Minimal INI parser (`ini_parse`). Unmodified vendored library.
|
||||||
|
|
||||||
|
### Other files
|
||||||
|
- [cxxopts.hpp](../cxxopts.hpp) — third-party CLI parser, **included but unused** (the live CLI parsing uses
|
||||||
|
Boost.Program_options; the cxxopts block in `main.cpp` is commented out).
|
||||||
|
- [Log.h](../Log.h) — empty stub (`#pragma once` only).
|
||||||
|
|
||||||
|
## Data structures (the in-memory "data model")
|
||||||
|
|
||||||
|
### `motor_info` ([Serial.h](../Serial.h) lines 6-19)
|
||||||
|
Telemetry from the motor controller, parsed from a 12-field `$`-line.
|
||||||
|
|
||||||
|
| Field | Type | Meaning | Source field index |
|
||||||
|
|-------|------|---------|---------------------|
|
||||||
|
| `Xenc` | int | Encoder position | 1 |
|
||||||
|
| `Xerr` | int | Encoder error | 2 |
|
||||||
|
| `sgt_val` | int | StallGuard value | 3 |
|
||||||
|
| `sgt_stat` | int | StallGuard status | 4 |
|
||||||
|
| `is_moving` | int | Movement flag | 5 |
|
||||||
|
| `control_status` | int | Driver/controller status | 6 |
|
||||||
|
| `hdg` | float | Heading (degrees) | 7 |
|
||||||
|
| `deviation_warn` | int | Deviation warning | 8 |
|
||||||
|
| `humid` | int | Humidity | **9** |
|
||||||
|
| `temp` | int | Temperature | **10** |
|
||||||
|
| `fan_pwm` | int | Fan PWM (0-255) | 11 |
|
||||||
|
|
||||||
|
> Field 0 is the literal `$` marker. Note the order: index 9 → `humid`, index 10 → `temp` (see
|
||||||
|
> [known-issues.md](known-issues.md) so the firmware emits them in this order).
|
||||||
|
|
||||||
|
### `mqtt_sub_data` ([MQTT.h](../MQTT.h) lines 7-21)
|
||||||
|
Latest remote-control state.
|
||||||
|
|
||||||
|
| Field | Type | Meaning |
|
||||||
|
|-------|------|---------|
|
||||||
|
| `ctl_avail` | bool | A new ControlCode arrived since last read |
|
||||||
|
| `hdg_avail` | bool | A new target heading arrived since last read |
|
||||||
|
| `target_heading` | string | Target heading (kept as string, forwarded as `kd<heading>`) |
|
||||||
|
| `control_code` | int | 0 = auto sweep, 1 = directed |
|
||||||
|
|
||||||
|
### `parser_data` ([Parser.h](../Parser.h) lines 21-26)
|
||||||
|
One parsed console command: `command`, `device`, `option` (strings) and `command_val` (double).
|
||||||
|
|
||||||
|
### `image_store_8bit` ([Camera.h](../Camera.h) lines 14-63)
|
||||||
|
One captured frame held in the queue: a `std::vector<VmbUchar_t>` pixel buffer plus `width`, `height`,
|
||||||
|
`pixelFormat`, a Unix-ms `timestamp`, and `cam_id`. Provides `equal()`/`setData()` so the queue can reuse the
|
||||||
|
oldest buffer in place when full.
|
||||||
|
|
||||||
|
## On-disk artifacts
|
||||||
|
|
||||||
|
| Artifact | Path | Format |
|
||||||
|
|----------|------|--------|
|
||||||
|
| Captured images | `$HOME/projects/Fire_Gimbal_Control/bin/x64/Release/<RGB\|ACR\|NIR>/<unix_ms>.jxl` | JPEG XL, rotated 90° CCW |
|
||||||
|
| Demo placeholder | `bin/x64/Release/test_smoke.jxl` | JPEG XL (copied verbatim in demo mode) |
|
||||||
|
|
||||||
|
There is no other persistence: state lives in memory and diagnostics go to stdout/stderr only (no log files).
|
||||||
|
|
@ -0,0 +1,88 @@
|
||||||
|
# MQTT API
|
||||||
|
|
||||||
|
The program is both an MQTT **subscriber** (remote control) and **publisher** (status + capture events). It
|
||||||
|
uses the Eclipse Paho C++ async client ([MQTT.cpp](../MQTT.cpp)). All topics are namespaced by tower name:
|
||||||
|
|
||||||
|
```
|
||||||
|
GGS/FWT/<tower_name>/...
|
||||||
|
```
|
||||||
|
|
||||||
|
`<tower_name>` comes from `config.ini` (`General.tower_name`). All messages use **QoS 1**. Published messages
|
||||||
|
are **retained**.
|
||||||
|
|
||||||
|
## Connection
|
||||||
|
|
||||||
|
- Broker URI = `Network.zkms_server_ip` from `config.ini`; client ID = the tower name.
|
||||||
|
- Auth: `mqtt_user` / `mqtt_pw` from config; `clean_session = true`, keep-alive 20 s.
|
||||||
|
- Connect timeout 5 s; on connection loss the client auto-reconnects (`reconnect()` with a 2.5 s backoff,
|
||||||
|
re-subscribing on success).
|
||||||
|
- **The program exits at startup if the initial connect fails** ([main.cpp](../main.cpp) lines 162-165).
|
||||||
|
|
||||||
|
## Subscribed topics (inbound — remote control)
|
||||||
|
|
||||||
|
Subscribed in `MQTTCallback::connected()` ([MQTT.cpp](../MQTT.cpp) lines 17-23). Handled in
|
||||||
|
`message_arrived()` ([MQTT.cpp](../MQTT.cpp) lines 25-54).
|
||||||
|
|
||||||
|
| Topic | Payload | Parsed as | Effect |
|
||||||
|
|-------|---------|-----------|--------|
|
||||||
|
| `GGS/FWT/<tower>/target_HDG` | integer heading as string, e.g. `"137"` | validated with `stoi`, stored as **string** | Sets the target heading used in ControlCode 1 (`kd<heading>`) |
|
||||||
|
| `GGS/FWT/<tower>/ControlCode` | integer as string, `"0"` or `"1"` | `stoi` → int | Selects the capture mode (see below) |
|
||||||
|
|
||||||
|
Invalid (non-integer) payloads are caught and logged; the previous value is kept.
|
||||||
|
|
||||||
|
The main loop consumes these via `get_sub_data()`, which returns a snapshot and **clears the "available"
|
||||||
|
flags** so each update is acted on once ([MQTT.h](../MQTT.h) lines 125-131).
|
||||||
|
|
||||||
|
### ControlCode semantics
|
||||||
|
|
||||||
|
| ControlCode | Behaviour ([main.cpp](../main.cpp) lines 293-334) |
|
||||||
|
|-------------|---------------------------------------------------|
|
||||||
|
| `0` | **Automatic sweep.** On each interval, send `p` to advance/stop the gimbal, then trigger cameras. |
|
||||||
|
| `1` | **Directed.** On each interval, send `kd<target_HDG>` to drive to the heading from `target_HDG`, then trigger. |
|
||||||
|
|
||||||
|
When a ControlCode message arrives, the program echoes the current code back on the StatusCode topic.
|
||||||
|
|
||||||
|
## Published topics (outbound)
|
||||||
|
|
||||||
|
| Topic | When | Payload | QoS / Retain |
|
||||||
|
|-------|------|---------|--------------|
|
||||||
|
| `GGS/FWT/<tower>/StatusCode` | At startup (`"0"`); whenever a ControlCode message is received (echoes the code) | integer as string | 1 / retained |
|
||||||
|
| `GGS/FWT/<tower>/CamEvent` | After each image is saved | JSON object (below) | 1 / retained |
|
||||||
|
|
||||||
|
### CamEvent payload
|
||||||
|
|
||||||
|
Built in [Camera.cpp](../Camera.cpp) line 389:
|
||||||
|
|
||||||
|
```json
|
||||||
|
{ "fwt":"Staeffelsberg", "cam":"RGB", "hdg":1373, "time":1719312345678 }
|
||||||
|
```
|
||||||
|
|
||||||
|
| Field | Type | Meaning |
|
||||||
|
|-------|------|---------|
|
||||||
|
| `fwt` | string | Tower name (`config.ini` `tower_name`) |
|
||||||
|
| `cam` | string | Camera label: `RGB`, `ACR`, or `NIR` (by camera index) |
|
||||||
|
| `hdg` | int | Gimbal heading **× 10** (one decimal place encoded as integer) at capture time |
|
||||||
|
| `time` | int | Capture timestamp, Unix epoch **milliseconds** (matches the `.jxl` filename) |
|
||||||
|
|
||||||
|
> The `time` value is the same Unix-ms timestamp used as the image filename, so a consumer can locate the file
|
||||||
|
> for a given event: `<RGB|ACR|NIR>/<time>.jxl`.
|
||||||
|
|
||||||
|
## Topic summary
|
||||||
|
|
||||||
|
```
|
||||||
|
subscribe: GGS/FWT/<tower>/target_HDG (int heading)
|
||||||
|
GGS/FWT/<tower>/ControlCode (0 = auto sweep, 1 = directed)
|
||||||
|
|
||||||
|
publish: GGS/FWT/<tower>/StatusCode (echoed control code)
|
||||||
|
GGS/FWT/<tower>/CamEvent (JSON: fwt, cam, hdg×10, time-ms)
|
||||||
|
```
|
||||||
|
|
||||||
|
## Local testing
|
||||||
|
|
||||||
|
Point `zkms_server_ip` at a local broker and exercise the topics with Mosquitto:
|
||||||
|
|
||||||
|
```bash
|
||||||
|
mosquitto_sub -t 'GGS/FWT/Staeffelsberg/#' -v # watch everything for the tower
|
||||||
|
mosquitto_pub -t 'GGS/FWT/Staeffelsberg/ControlCode' -m '0'
|
||||||
|
mosquitto_pub -t 'GGS/FWT/Staeffelsberg/target_HDG' -m '180'
|
||||||
|
```
|
||||||
|
|
@ -0,0 +1,310 @@
|
||||||
|
/* inih -- simple .INI file parser
|
||||||
|
|
||||||
|
SPDX-License-Identifier: BSD-3-Clause
|
||||||
|
|
||||||
|
Copyright (C) 2009-2020, Ben Hoyt
|
||||||
|
|
||||||
|
inih is released under the New BSD license (see LICENSE.txt). Go to the project
|
||||||
|
home page for more info:
|
||||||
|
|
||||||
|
https://github.com/benhoyt/inih
|
||||||
|
|
||||||
|
*/
|
||||||
|
|
||||||
|
#if defined(_MSC_VER) && !defined(_CRT_SECURE_NO_WARNINGS)
|
||||||
|
#define _CRT_SECURE_NO_WARNINGS
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <ctype.h>
|
||||||
|
#include <string.h>
|
||||||
|
|
||||||
|
#include "ini.h"
|
||||||
|
|
||||||
|
#if !INI_USE_STACK
|
||||||
|
#if INI_CUSTOM_ALLOCATOR
|
||||||
|
#include <stddef.h>
|
||||||
|
void* ini_malloc(size_t size);
|
||||||
|
void ini_free(void* ptr);
|
||||||
|
void* ini_realloc(void* ptr, size_t size);
|
||||||
|
#else
|
||||||
|
#include <stdlib.h>
|
||||||
|
#define ini_malloc malloc
|
||||||
|
#define ini_free free
|
||||||
|
#define ini_realloc realloc
|
||||||
|
#endif
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#define MAX_SECTION 50
|
||||||
|
#define MAX_NAME 50
|
||||||
|
|
||||||
|
/* Used by ini_parse_string() to keep track of string parsing state. */
|
||||||
|
typedef struct {
|
||||||
|
const char* ptr;
|
||||||
|
size_t num_left;
|
||||||
|
} ini_parse_string_ctx;
|
||||||
|
|
||||||
|
/* Strip whitespace chars off end of given string, in place. Return s. */
|
||||||
|
static char* ini_rstrip(char* s)
|
||||||
|
{
|
||||||
|
char* p = s + strlen(s);
|
||||||
|
while (p > s && isspace((unsigned char)(*--p)))
|
||||||
|
*p = '\0';
|
||||||
|
return s;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Return pointer to first non-whitespace char in given string. */
|
||||||
|
static char* ini_lskip(const char* s)
|
||||||
|
{
|
||||||
|
while (*s && isspace((unsigned char)(*s)))
|
||||||
|
s++;
|
||||||
|
return (char*)s;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Return pointer to first char (of chars) or inline comment in given string,
|
||||||
|
or pointer to NUL at end of string if neither found. Inline comment must
|
||||||
|
be prefixed by a whitespace character to register as a comment. */
|
||||||
|
static char* ini_find_chars_or_comment(const char* s, const char* chars)
|
||||||
|
{
|
||||||
|
#if INI_ALLOW_INLINE_COMMENTS
|
||||||
|
int was_space = 0;
|
||||||
|
while (*s && (!chars || !strchr(chars, *s)) &&
|
||||||
|
!(was_space && strchr(INI_INLINE_COMMENT_PREFIXES, *s))) {
|
||||||
|
was_space = isspace((unsigned char)(*s));
|
||||||
|
s++;
|
||||||
|
}
|
||||||
|
#else
|
||||||
|
while (*s && (!chars || !strchr(chars, *s))) {
|
||||||
|
s++;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
return (char*)s;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Similar to strncpy, but ensures dest (size bytes) is
|
||||||
|
NUL-terminated, and doesn't pad with NULs. */
|
||||||
|
static char* ini_strncpy0(char* dest, const char* src, size_t size)
|
||||||
|
{
|
||||||
|
/* Could use strncpy internally, but it causes gcc warnings (see issue #91) */
|
||||||
|
size_t i;
|
||||||
|
for (i = 0; i < size - 1 && src[i]; i++)
|
||||||
|
dest[i] = src[i];
|
||||||
|
dest[i] = '\0';
|
||||||
|
return dest;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* See documentation in header file. */
|
||||||
|
int ini_parse_stream(ini_reader reader, void* stream, ini_handler handler,
|
||||||
|
void* user)
|
||||||
|
{
|
||||||
|
/* Uses a fair bit of stack (use heap instead if you need to) */
|
||||||
|
#if INI_USE_STACK
|
||||||
|
char line[INI_MAX_LINE];
|
||||||
|
size_t max_line = INI_MAX_LINE;
|
||||||
|
#else
|
||||||
|
char* line;
|
||||||
|
size_t max_line = INI_INITIAL_ALLOC;
|
||||||
|
#endif
|
||||||
|
#if INI_ALLOW_REALLOC && !INI_USE_STACK
|
||||||
|
char* new_line;
|
||||||
|
size_t offset;
|
||||||
|
#endif
|
||||||
|
char section[MAX_SECTION] = "";
|
||||||
|
#if INI_ALLOW_MULTILINE
|
||||||
|
char prev_name[MAX_NAME] = "";
|
||||||
|
#endif
|
||||||
|
|
||||||
|
char* start;
|
||||||
|
char* end;
|
||||||
|
char* name;
|
||||||
|
char* value;
|
||||||
|
int lineno = 0;
|
||||||
|
int error = 0;
|
||||||
|
|
||||||
|
#if !INI_USE_STACK
|
||||||
|
line = (char*)ini_malloc(INI_INITIAL_ALLOC);
|
||||||
|
if (!line) {
|
||||||
|
return -2;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#if INI_HANDLER_LINENO
|
||||||
|
#define HANDLER(u, s, n, v) handler(u, s, n, v, lineno)
|
||||||
|
#else
|
||||||
|
#define HANDLER(u, s, n, v) handler(u, s, n, v)
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/* Scan through stream line by line */
|
||||||
|
while (reader(line, (int)max_line, stream) != NULL) {
|
||||||
|
#if INI_ALLOW_REALLOC && !INI_USE_STACK
|
||||||
|
offset = strlen(line);
|
||||||
|
while (offset == max_line - 1 && line[offset - 1] != '\n') {
|
||||||
|
max_line *= 2;
|
||||||
|
if (max_line > INI_MAX_LINE)
|
||||||
|
max_line = INI_MAX_LINE;
|
||||||
|
new_line = ini_realloc(line, max_line);
|
||||||
|
if (!new_line) {
|
||||||
|
ini_free(line);
|
||||||
|
return -2;
|
||||||
|
}
|
||||||
|
line = new_line;
|
||||||
|
if (reader(line + offset, (int)(max_line - offset), stream) == NULL)
|
||||||
|
break;
|
||||||
|
if (max_line >= INI_MAX_LINE)
|
||||||
|
break;
|
||||||
|
offset += strlen(line + offset);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
lineno++;
|
||||||
|
|
||||||
|
start = line;
|
||||||
|
#if INI_ALLOW_BOM
|
||||||
|
if (lineno == 1 && (unsigned char)start[0] == 0xEF &&
|
||||||
|
(unsigned char)start[1] == 0xBB &&
|
||||||
|
(unsigned char)start[2] == 0xBF) {
|
||||||
|
start += 3;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
start = ini_rstrip(ini_lskip(start));
|
||||||
|
|
||||||
|
if (strchr(INI_START_COMMENT_PREFIXES, *start)) {
|
||||||
|
/* Start-of-line comment */
|
||||||
|
}
|
||||||
|
#if INI_ALLOW_MULTILINE
|
||||||
|
else if (*prev_name && *start && start > line) {
|
||||||
|
#if INI_ALLOW_INLINE_COMMENTS
|
||||||
|
end = ini_find_chars_or_comment(start, NULL);
|
||||||
|
if (*end)
|
||||||
|
*end = '\0';
|
||||||
|
ini_rstrip(start);
|
||||||
|
#endif
|
||||||
|
/* Non-blank line with leading whitespace, treat as continuation
|
||||||
|
of previous name's value (as per Python configparser). */
|
||||||
|
if (!HANDLER(user, section, prev_name, start) && !error)
|
||||||
|
error = lineno;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
else if (*start == '[') {
|
||||||
|
/* A "[section]" line */
|
||||||
|
end = ini_find_chars_or_comment(start + 1, "]");
|
||||||
|
if (*end == ']') {
|
||||||
|
*end = '\0';
|
||||||
|
ini_strncpy0(section, start + 1, sizeof(section));
|
||||||
|
#if INI_ALLOW_MULTILINE
|
||||||
|
*prev_name = '\0';
|
||||||
|
#endif
|
||||||
|
#if INI_CALL_HANDLER_ON_NEW_SECTION
|
||||||
|
if (!HANDLER(user, section, NULL, NULL) && !error)
|
||||||
|
error = lineno;
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
else if (!error) {
|
||||||
|
/* No ']' found on section line */
|
||||||
|
error = lineno;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if (*start) {
|
||||||
|
/* Not a comment, must be a name[=:]value pair */
|
||||||
|
end = ini_find_chars_or_comment(start, "=:");
|
||||||
|
if (*end == '=' || *end == ':') {
|
||||||
|
*end = '\0';
|
||||||
|
name = ini_rstrip(start);
|
||||||
|
value = end + 1;
|
||||||
|
#if INI_ALLOW_INLINE_COMMENTS
|
||||||
|
end = ini_find_chars_or_comment(value, NULL);
|
||||||
|
if (*end)
|
||||||
|
*end = '\0';
|
||||||
|
#endif
|
||||||
|
value = ini_lskip(value);
|
||||||
|
ini_rstrip(value);
|
||||||
|
|
||||||
|
#if INI_ALLOW_MULTILINE
|
||||||
|
ini_strncpy0(prev_name, name, sizeof(prev_name));
|
||||||
|
#endif
|
||||||
|
/* Valid name[=:]value pair found, call handler */
|
||||||
|
if (!HANDLER(user, section, name, value) && !error)
|
||||||
|
error = lineno;
|
||||||
|
}
|
||||||
|
else if (!error) {
|
||||||
|
/* No '=' or ':' found on name[=:]value line */
|
||||||
|
#if INI_ALLOW_NO_VALUE
|
||||||
|
*end = '\0';
|
||||||
|
name = ini_rstrip(start);
|
||||||
|
if (!HANDLER(user, section, name, NULL) && !error)
|
||||||
|
error = lineno;
|
||||||
|
#else
|
||||||
|
error = lineno;
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#if INI_STOP_ON_FIRST_ERROR
|
||||||
|
if (error)
|
||||||
|
break;
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
#if !INI_USE_STACK
|
||||||
|
ini_free(line);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
return error;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* See documentation in header file. */
|
||||||
|
int ini_parse_file(FILE* file, ini_handler handler, void* user)
|
||||||
|
{
|
||||||
|
return ini_parse_stream((ini_reader)fgets, file, handler, user);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* See documentation in header file. */
|
||||||
|
int ini_parse(const char* filename, ini_handler handler, void* user)
|
||||||
|
{
|
||||||
|
FILE* file;
|
||||||
|
int error;
|
||||||
|
|
||||||
|
file = fopen(filename, "r");
|
||||||
|
if (!file)
|
||||||
|
return -1;
|
||||||
|
error = ini_parse_file(file, handler, user);
|
||||||
|
fclose(file);
|
||||||
|
return error;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* An ini_reader function to read the next line from a string buffer. This
|
||||||
|
is the fgets() equivalent used by ini_parse_string(). */
|
||||||
|
static char* ini_reader_string(char* str, int num, void* stream) {
|
||||||
|
ini_parse_string_ctx* ctx = (ini_parse_string_ctx*)stream;
|
||||||
|
const char* ctx_ptr = ctx->ptr;
|
||||||
|
size_t ctx_num_left = ctx->num_left;
|
||||||
|
char* strp = str;
|
||||||
|
char c;
|
||||||
|
|
||||||
|
if (ctx_num_left == 0 || num < 2)
|
||||||
|
return NULL;
|
||||||
|
|
||||||
|
while (num > 1 && ctx_num_left != 0) {
|
||||||
|
c = *ctx_ptr++;
|
||||||
|
ctx_num_left--;
|
||||||
|
*strp++ = c;
|
||||||
|
if (c == '\n')
|
||||||
|
break;
|
||||||
|
num--;
|
||||||
|
}
|
||||||
|
|
||||||
|
*strp = '\0';
|
||||||
|
ctx->ptr = ctx_ptr;
|
||||||
|
ctx->num_left = ctx_num_left;
|
||||||
|
return str;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* See documentation in header file. */
|
||||||
|
int ini_parse_string(const char* string, ini_handler handler, void* user) {
|
||||||
|
ini_parse_string_ctx ctx;
|
||||||
|
|
||||||
|
ctx.ptr = string;
|
||||||
|
ctx.num_left = strlen(string);
|
||||||
|
return ini_parse_stream((ini_reader)ini_reader_string, &ctx, handler,
|
||||||
|
user);
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,178 @@
|
||||||
|
/* inih -- simple .INI file parser
|
||||||
|
|
||||||
|
SPDX-License-Identifier: BSD-3-Clause
|
||||||
|
|
||||||
|
Copyright (C) 2009-2020, Ben Hoyt
|
||||||
|
|
||||||
|
inih is released under the New BSD license (see LICENSE.txt). Go to the project
|
||||||
|
home page for more info:
|
||||||
|
|
||||||
|
https://github.com/benhoyt/inih
|
||||||
|
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef INI_H
|
||||||
|
#define INI_H
|
||||||
|
|
||||||
|
/* Make this header file easier to include in C++ code */
|
||||||
|
#ifdef __cplusplus
|
||||||
|
extern "C" {
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#include <stdio.h>
|
||||||
|
|
||||||
|
/* Nonzero if ini_handler callback should accept lineno parameter. */
|
||||||
|
#ifndef INI_HANDLER_LINENO
|
||||||
|
#define INI_HANDLER_LINENO 0
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/* Visibility symbols, required for Windows DLLs */
|
||||||
|
#ifndef INI_API
|
||||||
|
#if defined _WIN32 || defined __CYGWIN__
|
||||||
|
# ifdef INI_SHARED_LIB
|
||||||
|
# ifdef INI_SHARED_LIB_BUILDING
|
||||||
|
# define INI_API __declspec(dllexport)
|
||||||
|
# else
|
||||||
|
# define INI_API __declspec(dllimport)
|
||||||
|
# endif
|
||||||
|
# else
|
||||||
|
# define INI_API
|
||||||
|
# endif
|
||||||
|
#else
|
||||||
|
# if defined(__GNUC__) && __GNUC__ >= 4
|
||||||
|
# define INI_API __attribute__ ((visibility ("default")))
|
||||||
|
# else
|
||||||
|
# define INI_API
|
||||||
|
# endif
|
||||||
|
#endif
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/* Typedef for prototype of handler function. */
|
||||||
|
#if INI_HANDLER_LINENO
|
||||||
|
typedef int (*ini_handler)(void* user, const char* section,
|
||||||
|
const char* name, const char* value,
|
||||||
|
int lineno);
|
||||||
|
#else
|
||||||
|
typedef int (*ini_handler)(void* user, const char* section,
|
||||||
|
const char* name, const char* value);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/* Typedef for prototype of fgets-style reader function. */
|
||||||
|
typedef char* (*ini_reader)(char* str, int num, void* stream);
|
||||||
|
|
||||||
|
/* Parse given INI-style file. May have [section]s, name=value pairs
|
||||||
|
(whitespace stripped), and comments starting with ';' (semicolon). Section
|
||||||
|
is "" if name=value pair parsed before any section heading. name:value
|
||||||
|
pairs are also supported as a concession to Python's configparser.
|
||||||
|
|
||||||
|
For each name=value pair parsed, call handler function with given user
|
||||||
|
pointer as well as section, name, and value (data only valid for duration
|
||||||
|
of handler call). Handler should return nonzero on success, zero on error.
|
||||||
|
|
||||||
|
Returns 0 on success, line number of first error on parse error (doesn't
|
||||||
|
stop on first error), -1 on file open error, or -2 on memory allocation
|
||||||
|
error (only when INI_USE_STACK is zero).
|
||||||
|
*/
|
||||||
|
INI_API int ini_parse(const char* filename, ini_handler handler, void* user);
|
||||||
|
|
||||||
|
/* Same as ini_parse(), but takes a FILE* instead of filename. This doesn't
|
||||||
|
close the file when it's finished -- the caller must do that. */
|
||||||
|
INI_API int ini_parse_file(FILE* file, ini_handler handler, void* user);
|
||||||
|
|
||||||
|
/* Same as ini_parse(), but takes an ini_reader function pointer instead of
|
||||||
|
filename. Used for implementing custom or string-based I/O (see also
|
||||||
|
ini_parse_string). */
|
||||||
|
INI_API int ini_parse_stream(ini_reader reader, void* stream, ini_handler handler,
|
||||||
|
void* user);
|
||||||
|
|
||||||
|
/* Same as ini_parse(), but takes a zero-terminated string with the INI data
|
||||||
|
instead of a file. Useful for parsing INI data from a network socket or
|
||||||
|
already in memory. */
|
||||||
|
INI_API int ini_parse_string(const char* string, ini_handler handler, void* user);
|
||||||
|
|
||||||
|
/* Nonzero to allow multi-line value parsing, in the style of Python's
|
||||||
|
configparser. If allowed, ini_parse() will call the handler with the same
|
||||||
|
name for each subsequent line parsed. */
|
||||||
|
#ifndef INI_ALLOW_MULTILINE
|
||||||
|
#define INI_ALLOW_MULTILINE 1
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/* Nonzero to allow a UTF-8 BOM sequence (0xEF 0xBB 0xBF) at the start of
|
||||||
|
the file. See https://github.com/benhoyt/inih/issues/21 */
|
||||||
|
#ifndef INI_ALLOW_BOM
|
||||||
|
#define INI_ALLOW_BOM 1
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/* Chars that begin a start-of-line comment. Per Python configparser, allow
|
||||||
|
both ; and # comments at the start of a line by default. */
|
||||||
|
#ifndef INI_START_COMMENT_PREFIXES
|
||||||
|
#define INI_START_COMMENT_PREFIXES ";#"
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/* Nonzero to allow inline comments (with valid inline comment characters
|
||||||
|
specified by INI_INLINE_COMMENT_PREFIXES). Set to 0 to turn off and match
|
||||||
|
Python 3.2+ configparser behaviour. */
|
||||||
|
#ifndef INI_ALLOW_INLINE_COMMENTS
|
||||||
|
#define INI_ALLOW_INLINE_COMMENTS 1
|
||||||
|
#endif
|
||||||
|
#ifndef INI_INLINE_COMMENT_PREFIXES
|
||||||
|
#define INI_INLINE_COMMENT_PREFIXES ";"
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/* Nonzero to use stack for line buffer, zero to use heap (malloc/free). */
|
||||||
|
#ifndef INI_USE_STACK
|
||||||
|
#define INI_USE_STACK 1
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/* Maximum line length for any line in INI file (stack or heap). Note that
|
||||||
|
this must be 3 more than the longest line (due to '\r', '\n', and '\0'). */
|
||||||
|
#ifndef INI_MAX_LINE
|
||||||
|
#define INI_MAX_LINE 200
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/* Nonzero to allow heap line buffer to grow via realloc(), zero for a
|
||||||
|
fixed-size buffer of INI_MAX_LINE bytes. Only applies if INI_USE_STACK is
|
||||||
|
zero. */
|
||||||
|
#ifndef INI_ALLOW_REALLOC
|
||||||
|
#define INI_ALLOW_REALLOC 0
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/* Initial size in bytes for heap line buffer. Only applies if INI_USE_STACK
|
||||||
|
is zero. */
|
||||||
|
#ifndef INI_INITIAL_ALLOC
|
||||||
|
#define INI_INITIAL_ALLOC 200
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/* Stop parsing on first error (default is to keep parsing). */
|
||||||
|
#ifndef INI_STOP_ON_FIRST_ERROR
|
||||||
|
#define INI_STOP_ON_FIRST_ERROR 0
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/* Nonzero to call the handler at the start of each new section (with
|
||||||
|
name and value NULL). Default is to only call the handler on
|
||||||
|
each name=value pair. */
|
||||||
|
#ifndef INI_CALL_HANDLER_ON_NEW_SECTION
|
||||||
|
#define INI_CALL_HANDLER_ON_NEW_SECTION 0
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/* Nonzero to allow a name without a value (no '=' or ':' on the line) and
|
||||||
|
call the handler with value NULL in this case. Default is to treat
|
||||||
|
no-value lines as an error. */
|
||||||
|
#ifndef INI_ALLOW_NO_VALUE
|
||||||
|
#define INI_ALLOW_NO_VALUE 0
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/* Nonzero to use custom ini_malloc, ini_free, and ini_realloc memory
|
||||||
|
allocation functions (INI_USE_STACK must also be 0). These functions must
|
||||||
|
have the same signatures as malloc/free/realloc and behave in a similar
|
||||||
|
way. ini_realloc is only needed if INI_ALLOW_REALLOC is set. */
|
||||||
|
#ifndef INI_CUSTOM_ALLOCATOR
|
||||||
|
#define INI_CUSTOM_ALLOCATOR 0
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#endif /* INI_H */
|
||||||
|
|
@ -0,0 +1,346 @@
|
||||||
|
#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 "ini.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" };
|
||||||
|
|
||||||
|
// Callback function for parsing
|
||||||
|
static int handler(void* user, const char* section, const char* name, const char* value) {
|
||||||
|
auto* config = static_cast<std::map<std::string, std::string>*>(user);
|
||||||
|
std::string key = std::string(section) + "." + std::string(name);
|
||||||
|
(*config)[key] = value;
|
||||||
|
return 1; // Return 0 to stop parsing on error
|
||||||
|
}
|
||||||
|
|
||||||
|
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[])
|
||||||
|
{
|
||||||
|
//ini handling with ini.h
|
||||||
|
std::map<std::string, std::string> config;
|
||||||
|
// Parse the INI file
|
||||||
|
if (ini_parse("/home/ggs/projects/Fire_Gimbal_Control/bin/x64/Release/config.ini", handler, &config) < 0) {
|
||||||
|
std::cerr << "Can't load config.ini file!" << std::endl;
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
// Access configuration values
|
||||||
|
std::string tower_name = config["General.tower_name"];
|
||||||
|
int rate = std::stoi(config["General.image_interval"]);
|
||||||
|
int debug = std::stoi(config["General.debug"]);
|
||||||
|
std::string zkms_server_ip = config["Network.zkms_server_ip"];
|
||||||
|
std::string mqtt_user = config["Network.mqtt_user"];
|
||||||
|
std::string mqtt_pw = config["Network.mqtt_pw"];
|
||||||
|
std::string cam_id1 = config["Camera.id_Cam1"];
|
||||||
|
std::string cam_id2 = config["Camera.id_Cam2"];
|
||||||
|
std::string cam_id3 = config["Camera.id_Cam3"];
|
||||||
|
std::string cam_id4 = config["Camera.id_Cam4"];
|
||||||
|
if (cam_id1 != "")
|
||||||
|
camera_id_vec.push_back(cam_id1);
|
||||||
|
if (cam_id2 != "")
|
||||||
|
camera_id_vec.push_back(cam_id2);
|
||||||
|
if (cam_id3 != "")
|
||||||
|
camera_id_vec.push_back(cam_id3);
|
||||||
|
if (cam_id4 != "")
|
||||||
|
camera_id_vec.push_back(cam_id4);
|
||||||
|
std::cout << camera_id_vec.size() << " camera(s) should be connected according the config file." << "\n";
|
||||||
|
|
||||||
|
//set globals
|
||||||
|
fwt_name = tower_name;
|
||||||
|
imagerate = 1/(double)rate;
|
||||||
|
motorctl_info_out = (bool)debug;
|
||||||
|
server_ip = zkms_server_ip;
|
||||||
|
// Print the parsed values
|
||||||
|
std::cout << "tower_name: " << tower_name << std::endl;
|
||||||
|
std::cout << "rate: " << rate << std::endl;
|
||||||
|
std::cout << "debug: " << debug << std::endl;
|
||||||
|
std::cout << "zkms_server_ip: " << zkms_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("/dev/ttyACM0", 115200);
|
||||||
|
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);
|
||||||
|
//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;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,116 @@
|
||||||
|
#pragma once
|
||||||
|
#include <iostream>
|
||||||
|
#include <string>
|
||||||
|
#include <chrono>
|
||||||
|
#include <ctime>
|
||||||
|
class Timer
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
Timer()
|
||||||
|
{
|
||||||
|
Reset();
|
||||||
|
}
|
||||||
|
|
||||||
|
void Reset()
|
||||||
|
{
|
||||||
|
m_Start = std::chrono::high_resolution_clock::now();
|
||||||
|
}
|
||||||
|
|
||||||
|
float Elapsed()
|
||||||
|
{
|
||||||
|
return std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::high_resolution_clock::now() - m_Start).count() * 0.001f * 0.001f * 0.001f;
|
||||||
|
}
|
||||||
|
|
||||||
|
float ElapsedMillis()
|
||||||
|
{
|
||||||
|
return Elapsed() * 1000.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
long long Stamp_millis()
|
||||||
|
{
|
||||||
|
// Get the current time point with nanosecond precision
|
||||||
|
auto now = std::chrono::system_clock::now();
|
||||||
|
|
||||||
|
// Calculate the number of nanoseconds since the epoch
|
||||||
|
auto since_epoch = now.time_since_epoch();
|
||||||
|
long long milliseconds = std::chrono::duration_cast<std::chrono::milliseconds>(since_epoch).count();
|
||||||
|
|
||||||
|
return milliseconds;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::string GetFileStamp()
|
||||||
|
{
|
||||||
|
time_t now = time(nullptr);
|
||||||
|
struct tm* ltm = localtime(&now);
|
||||||
|
std::string stamp = std::to_string(1900 + ltm->tm_year) + "_" + std::to_string(1 + ltm->tm_mon) + "_" + std::to_string(ltm->tm_mday) + "_" + std::to_string(ltm->tm_hour) + "_" + std::to_string(ltm->tm_min);
|
||||||
|
//printf(stamp.c_str());
|
||||||
|
return stamp;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::string GetStringStamp()
|
||||||
|
{
|
||||||
|
time_t now = time(nullptr);
|
||||||
|
struct tm* ltm = localtime(&now);
|
||||||
|
std::string stamp = std::to_string(1900 + ltm->tm_year) + "_" + std::to_string(1 + ltm->tm_mon) + "_" + std::to_string(ltm->tm_mday) + "_" + std::to_string(ltm->tm_hour) + "_" + std::to_string(ltm->tm_min) + "_" + std::to_string(ltm->tm_sec);
|
||||||
|
//printf(stamp.c_str());
|
||||||
|
return stamp;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::string GetDirStamp()
|
||||||
|
{
|
||||||
|
time_t now = time(nullptr);
|
||||||
|
struct tm* ltm = localtime(&now);
|
||||||
|
std::string stamp = std::to_string(1900 + ltm->tm_year) + "_" + std::to_string(1 + ltm->tm_mon) + "_" + std::to_string(ltm->tm_mday) + "_" + std::to_string(ltm->tm_hour);
|
||||||
|
//printf(stamp.c_str());
|
||||||
|
return stamp;
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
std::chrono::time_point<std::chrono::high_resolution_clock> m_Start;
|
||||||
|
};
|
||||||
|
|
||||||
|
class ScopedTimer
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
ScopedTimer(const std::string& name)
|
||||||
|
: m_Name(name) {}
|
||||||
|
~ScopedTimer()
|
||||||
|
{
|
||||||
|
float time = m_Timer.ElapsedMillis();
|
||||||
|
std::cout << "[TIMER] " << m_Name << " - " << time << "ms\n";
|
||||||
|
}
|
||||||
|
private:
|
||||||
|
std::string m_Name;
|
||||||
|
Timer m_Timer;
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
class NanoUnixTimer
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
|
||||||
|
std::string Stamp_string()
|
||||||
|
{
|
||||||
|
// Get the current time point with nanosecond precision
|
||||||
|
auto now = std::chrono::system_clock::now();
|
||||||
|
|
||||||
|
// Calculate the number of nanoseconds since the epoch
|
||||||
|
auto since_epoch = now.time_since_epoch();
|
||||||
|
auto milliseconds = std::chrono::duration_cast<std::chrono::milliseconds>(since_epoch).count();
|
||||||
|
|
||||||
|
// Convert the nanoseconds to a string
|
||||||
|
return std::to_string(milliseconds);
|
||||||
|
}
|
||||||
|
|
||||||
|
long long Stamp_longlong()
|
||||||
|
{
|
||||||
|
// Get the current time point with nanosecond precision
|
||||||
|
auto now = std::chrono::system_clock::now();
|
||||||
|
|
||||||
|
// Calculate the number of nanoseconds since the epoch
|
||||||
|
auto since_epoch = now.time_since_epoch();
|
||||||
|
long long milliseconds = std::chrono::duration_cast<std::chrono::milliseconds>(since_epoch).count();
|
||||||
|
|
||||||
|
return milliseconds;
|
||||||
|
}
|
||||||
|
};
|
||||||
Loading…
Reference in New Issue