1095 lines
43 KiB
C++
1095 lines
43 KiB
C++
#include "fgc/TarisCameraSource.h"
|
|
|
|
#include "fgc/Logger.h"
|
|
#include "fgc/RawImage.h"
|
|
|
|
#include <algorithm>
|
|
#include <atomic>
|
|
#include <cctype>
|
|
#include <chrono>
|
|
#include <cmath>
|
|
#include <condition_variable>
|
|
#include <cstring>
|
|
#include <deque>
|
|
#include <clocale>
|
|
#include <fstream>
|
|
#include <map>
|
|
#include <mutex>
|
|
#include <sstream>
|
|
#include <thread>
|
|
#include <vector>
|
|
|
|
namespace fgc {
|
|
|
|
using namespace irbgrab;
|
|
|
|
namespace {
|
|
|
|
long long unixMs() {
|
|
using namespace std::chrono;
|
|
return duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count();
|
|
}
|
|
|
|
long long steadyMs() {
|
|
using namespace std::chrono;
|
|
return duration_cast<milliseconds>(steady_clock::now().time_since_epoch()).count();
|
|
}
|
|
|
|
std::string lower(std::string s) {
|
|
for (auto& c : s) c = static_cast<char>(std::tolower(static_cast<unsigned char>(c)));
|
|
return s;
|
|
}
|
|
|
|
// One line of a sysfs file, or "" if it cannot be read.
|
|
std::string sysfs(const std::string& path) {
|
|
std::ifstream f(path);
|
|
std::string s;
|
|
std::getline(f, s);
|
|
return s;
|
|
}
|
|
|
|
// The SDK changes the process's C locale (seen on tower 111: LC_NUMERIC turned
|
|
// to a comma one under LANG=en_US.UTF-8). The program parses and prints numbers
|
|
// with the C library, so every SDK call that may do this runs inside one of
|
|
// these, which puts the locale back as it was.
|
|
class LocaleGuard {
|
|
public:
|
|
LocaleGuard() {
|
|
const char* now = std::setlocale(LC_ALL, nullptr);
|
|
saved_ = now ? now : "C";
|
|
}
|
|
~LocaleGuard() {
|
|
const char* now = std::setlocale(LC_ALL, nullptr);
|
|
if (!now || saved_ != now) std::setlocale(LC_ALL, saved_.c_str());
|
|
}
|
|
LocaleGuard(const LocaleGuard&) = delete;
|
|
LocaleGuard& operator=(const LocaleGuard&) = delete;
|
|
|
|
private:
|
|
std::string saved_;
|
|
};
|
|
|
|
constexpr long long kLogEveryMs = 60000; // repeated trouble: one line a minute
|
|
constexpr long long kTelemetryEveryMs = 5000;
|
|
// Diagnostics for pictures that get no thermal frame: a pause in the frame
|
|
// stream this long is logged with what the camera said meanwhile, and a summary
|
|
// of the stream goes out this often.
|
|
constexpr long long kGapLogMs = 500;
|
|
constexpr long long kSummaryEveryMs = 600000;
|
|
|
|
} // namespace
|
|
|
|
TarisCameraSource::Options TarisCameraSource::fromSpec(const CameraSpec& spec) {
|
|
Options o;
|
|
o.serial = spec.id;
|
|
o.calib_profile = spec.calib_profile;
|
|
o.frame_rate_hz = spec.frame_rate_hz;
|
|
o.max_frame_age_ms = spec.max_frame_age_ms;
|
|
o.library = spec.library;
|
|
o.interface = spec.interface;
|
|
o.nuc_interval_ms = spec.nuc_interval_s * 1000;
|
|
return o;
|
|
}
|
|
|
|
double TarisCameraSource::expectedKelvinPerCount(int calib_profile) {
|
|
switch (calib_profile) {
|
|
case 0: return 0.01; // FF (-40-120C)
|
|
case 1: return 0.02; // FF (0-600C)
|
|
default: return 0.0;
|
|
}
|
|
}
|
|
|
|
bool TarisCameraSource::processHasNetRaw() {
|
|
std::ifstream f("/proc/self/status");
|
|
std::string line;
|
|
while (std::getline(f, line)) {
|
|
if (line.rfind("CapEff:", 0) != 0) continue;
|
|
try {
|
|
const unsigned long long caps = std::stoull(line.substr(7), nullptr, 16);
|
|
return (caps >> 13) & 1; // CAP_NET_RAW
|
|
} catch (const std::exception&) {
|
|
return false;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
struct TarisCameraSource::Impl {
|
|
Options o;
|
|
std::shared_ptr<const Api> api;
|
|
std::function<bool()> has_net_raw;
|
|
|
|
// ---- the worker's own (only it calls the SDK) ---------------------------
|
|
Handle dev = 0;
|
|
bool sdk_init = false;
|
|
bool connected = false;
|
|
int cal = -1; // active profile as the camera reports it
|
|
double kpc = 0.0; // measured from the LUT; 0 = not yet
|
|
long long last_frame_steady = 0;
|
|
double last_sdk_ts = -1.0; // MemInfo time of the last frame taken
|
|
int scale_tries = 0; // frames whose LUT was still empty
|
|
long long last_telemetry = 0;
|
|
long long nuc_or_licence = 0; // getdata 0x80000017 since the last log line
|
|
long long nuc_logged_ms = 0;
|
|
bool zeros_logged = false;
|
|
std::string last_offline_logged;
|
|
|
|
// Stream diagnostics (see kGapLogMs). Since the last frame taken:
|
|
double last_cam_ts = -1.0; // its MemInfo time, ms
|
|
long long polls_since_frame = 0;
|
|
std::map<Ret, int> codes_since_frame; // getdata answers other than a new frame
|
|
// The frame rate (see setFrameRate()): set by restarting the grab once the
|
|
// first frame is in, then checked on the wire.
|
|
int rate_attempts = 0;
|
|
bool rate_pending = false; // restart the grab with it at the next frame
|
|
long long wire_check_ms = 0; // steady: when to count packets; 0 = none
|
|
long long wire_packets0 = -1;
|
|
long long wire_start_ms = 0;
|
|
size_t frame_bytes = 0;
|
|
// NUC run by the program (Options::nuc_interval_ms).
|
|
int nuc_interval_found = -1; // the camera's own timer, put back on close
|
|
bool nuc_ours = false; // the camera's timer is off and we run it
|
|
long long last_nuc_ms = 0; // steady
|
|
// For the summary:
|
|
long long sum_frames = 0, sum_gaps = 0, sum_nucs = 0, sum_max_gap = 0;
|
|
double sum_cam_span = 0.0; // camera clock covered by sum_frames
|
|
long long sum_since_ms = 0;
|
|
|
|
// ---- shared, under m -------------------------------------------------
|
|
std::mutex m;
|
|
std::condition_variable cv;
|
|
Frame latest;
|
|
uint64_t latest_seq = 0;
|
|
uint64_t delivered_seq = 0;
|
|
std::string why_offline = "not opened";
|
|
CameraDeviceInfo info;
|
|
std::deque<long long> arrivals; // steady ms of recent frames, for fps
|
|
long long missed_shots = 0;
|
|
long long packets_lost = 0;
|
|
int cal_wanted = -1; // a pending setFeature("CalibProfile")
|
|
bool cal_done = false;
|
|
bool cal_ok = false;
|
|
bool reset_wanted = false;
|
|
uint64_t connects = 0;
|
|
bool nuc_now = false; // a NUC of ours is running
|
|
long long nuc_now_since = 0; // steady ms it began
|
|
bool move_started = false; // moving() since the worker last looked
|
|
long long missed_window = 0; // misses since the last summary
|
|
|
|
std::atomic<bool> running{false};
|
|
std::atomic<bool> gate{false};
|
|
FrameCallback callback;
|
|
std::thread worker;
|
|
|
|
explicit Impl(Options opts, std::shared_ptr<const Api> a, std::function<bool()> net_raw)
|
|
: o(std::move(opts)), api(std::move(a)), has_net_raw(std::move(net_raw)) {
|
|
if (!has_net_raw) has_net_raw = &TarisCameraSource::processHasNetRaw;
|
|
info.id = o.serial;
|
|
info.serial = o.serial;
|
|
info.model = "TarisIR";
|
|
}
|
|
|
|
void setOffline(const std::string& why) {
|
|
{
|
|
std::lock_guard<std::mutex> l(m);
|
|
why_offline = why;
|
|
info.streaming = false;
|
|
}
|
|
if (why != last_offline_logged) {
|
|
LOG_WARN << "thermal camera " << o.serial << " offline: " << why;
|
|
last_offline_logged = why;
|
|
}
|
|
}
|
|
|
|
// Sleep up to ms, or less if close() wants the worker to stop or another
|
|
// thread has asked for a profile switch or a reset.
|
|
void pause(long long ms) {
|
|
std::unique_lock<std::mutex> l(m);
|
|
cv.wait_for(l, std::chrono::milliseconds(ms),
|
|
[this] {
|
|
return !running.load() || cal_wanted >= 0 || reset_wanted || move_started;
|
|
});
|
|
}
|
|
|
|
// ---- parameter helpers -------------------------------------------------
|
|
bool getInt32(uint32_t what, int32_t& v) {
|
|
return ok(api->dev_getparam(dev, what, &v, kTypeInt32));
|
|
}
|
|
bool getIdxInt32(uint32_t what, int32_t index, int32_t& v) {
|
|
IdxInt32 p{index, 0};
|
|
if (!ok(api->dev_getparam(dev, what, &p, kTypeIdxInt32))) return false;
|
|
v = p.value;
|
|
return true;
|
|
}
|
|
bool getSingle(uint32_t what, float& v) {
|
|
return ok(api->dev_getparam(dev, what, &v, kTypeSingle));
|
|
}
|
|
bool getString(uint32_t what, std::string& out) {
|
|
std::vector<char> buf(kCharBuffer, 0);
|
|
String s{buf.data(), kCharBuffer};
|
|
if (!ok(api->dev_getparam(dev, what, &s, kTypeString))) return false;
|
|
out.assign(buf.data(), strnlen(buf.data(), buf.size()));
|
|
return true;
|
|
}
|
|
bool getIdxString(uint32_t what, int32_t index, std::string& out) {
|
|
std::vector<char> buf(kCharBuffer, 0);
|
|
IdxString s{index, {buf.data(), kCharBuffer}};
|
|
if (!ok(api->dev_getparam(dev, what, &s, kTypeIdxString))) return false;
|
|
out.assign(buf.data(), strnlen(buf.data(), buf.size()));
|
|
return true;
|
|
}
|
|
Ret setInt32(uint32_t what, int32_t v) { return api->dev_setparam(dev, what, &v, kTypeInt32); }
|
|
|
|
// Calib_Active reads as a plain or an indexed value (both work on 4.3.44).
|
|
// Right after a reconnect it can fail for a moment, so it is asked a few times.
|
|
int readCal() {
|
|
for (int attempt = 0; attempt < 10; ++attempt) {
|
|
int32_t v = -1;
|
|
if (getInt32(param::kCalibActive, v) && v >= 0) return v;
|
|
if (getIdxInt32(param::kCalibActive, 0, v) && v >= 0) return v;
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(300));
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
// Switch the calibration profile and wait for the camera to finish.
|
|
// Calib_Active reports the new profile at once, but the frames keep the old
|
|
// scale until Calib_ChangeInProgress has run 2, 4, 2 and reached 0 (about
|
|
// 3 s on tower 111; at rest, before any switch, it reads 6). Nothing is
|
|
// polled meanwhile, so no frame of the wrong scale is taken.
|
|
bool switchCal(int profile, std::string& why) {
|
|
int32_t count = 0;
|
|
if (getInt32(param::kCalibCount, count) && (profile < 0 || profile >= count)) {
|
|
why = "calibration profile " + std::to_string(profile) + " does not exist (camera has " +
|
|
std::to_string(count) + ")";
|
|
return false;
|
|
}
|
|
IdxInt32 v{0, profile};
|
|
const Ret r = api->dev_setparam(dev, param::kCalibActive, &v, kTypeIdxInt32);
|
|
if (!ok(r)) {
|
|
why = "cannot set calibration profile " + std::to_string(profile) + ": " + describe(r);
|
|
return false;
|
|
}
|
|
const long long until = steadyMs() + o.calib_timeout_ms;
|
|
int32_t busy = 1;
|
|
while (steadyMs() < until) {
|
|
if (getInt32(param::kCalibChangeInProgress, busy) && busy == 0) break;
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
}
|
|
if (busy != 0) {
|
|
why = "calibration profile change did not finish within " +
|
|
std::to_string(o.calib_timeout_ms) + " ms";
|
|
return false;
|
|
}
|
|
cal = readCal();
|
|
kpc = 0.0; // measure the new scale from the next frames' LUT
|
|
scale_tries = 0;
|
|
if (cal != profile) {
|
|
why = "camera reports profile " + std::to_string(cal) + " after switching to " +
|
|
std::to_string(profile);
|
|
return false;
|
|
}
|
|
std::string name;
|
|
getIdxString(param::kCalibName, cal, name);
|
|
LOG_INFO << "thermal camera " << o.serial << ": calibration profile " << cal
|
|
<< (name.empty() ? "" : " (" + name + ")");
|
|
return true;
|
|
}
|
|
|
|
// ---- connecting --------------------------------------------------------
|
|
std::string linkHint() const {
|
|
if (o.interface.empty()) return {};
|
|
const std::string base = "/sys/class/net/" + o.interface;
|
|
const std::string state = sysfs(base + "/operstate");
|
|
if (state.empty()) return "; interface " + o.interface + " does not exist";
|
|
std::string hint = "; " + o.interface + " is " + state;
|
|
const std::string mtu = sysfs(base + "/mtu");
|
|
if (!mtu.empty() && mtu != "1500") hint += ", MTU " + mtu + " (the camera needs 1500)";
|
|
return hint;
|
|
}
|
|
|
|
void freeDevice() {
|
|
if (dev) api->dev_free(&dev);
|
|
dev = 0;
|
|
}
|
|
|
|
void disconnect() {
|
|
if (connected) {
|
|
api->dev_stopgrab(dev, 0);
|
|
api->dev_disconnect(dev);
|
|
}
|
|
connected = false;
|
|
freeDevice();
|
|
}
|
|
|
|
bool connect(std::string& why) {
|
|
// The device type the camera package registered.
|
|
std::vector<char> names(kCharBuffer, 0);
|
|
Ret r = api->dll_devicetypenames(names.data(), kCharBuffer);
|
|
if (!ok(r)) {
|
|
why = "SDK lists no device types: " + describe(r);
|
|
return false;
|
|
}
|
|
int type = -1, i = 0;
|
|
std::stringstream ss(std::string(names.data(), strnlen(names.data(), names.size())));
|
|
for (std::string n; std::getline(ss, n, ';'); ++i)
|
|
if (lower(n).find("taris") != std::string::npos) { type = i; break; }
|
|
if (type < 0) {
|
|
why = "SDK has no TarisIR device type (camera package infratec-itcamtarisirmini "
|
|
"missing?)";
|
|
return false;
|
|
}
|
|
|
|
std::vector<char> ini(kCharBuffer, 0);
|
|
r = api->dev_create(&dev, type, ini.data());
|
|
if (!ok(r)) {
|
|
dev = 0;
|
|
why = "cannot create the device: " + describe(r);
|
|
return false;
|
|
}
|
|
|
|
// Search first: it fills the SDK's adapter table that connect() needs.
|
|
int32_t count = 0;
|
|
r = api->dev_search(dev, &count);
|
|
if (!ok(r) || count <= 0) {
|
|
freeDevice();
|
|
why = "no thermal camera found on the network" + linkHint();
|
|
return false;
|
|
}
|
|
std::string chosen;
|
|
std::vector<std::string> seen;
|
|
for (int32_t k = 0; k < count && k < 32; ++k) {
|
|
std::string s;
|
|
if (!getIdxString(param::kSearchString, k, s)) break;
|
|
seen.push_back(s);
|
|
if (chosen.empty() && s.find(o.serial) != std::string::npos) chosen = s;
|
|
}
|
|
if (chosen.empty()) {
|
|
std::string list;
|
|
for (const auto& s : seen) list += (list.empty() ? "" : ", ") + s;
|
|
freeDevice();
|
|
why = "camera " + o.serial + " not found (found: " + (list.empty() ? "none" : list) + ")";
|
|
return false;
|
|
}
|
|
|
|
// SN:<serial>|<MAC>|<IP>#<adapter>. The adapter index moves when Docker or
|
|
// VPN interfaces come and go, so if the one found does not connect, try
|
|
// them all.
|
|
std::vector<std::string> candidates{chosen};
|
|
const std::string base = chosen.substr(0, chosen.rfind('#'));
|
|
for (int a = 0; a < 16; ++a) {
|
|
std::string c = base + "#" + std::to_string(a);
|
|
if (c != chosen) candidates.push_back(c);
|
|
}
|
|
Ret last = 0;
|
|
for (const auto& c : candidates) {
|
|
std::vector<char> buf(kCharBuffer, 0);
|
|
std::memcpy(buf.data(), c.data(), std::min<size_t>(c.size(), kCharBuffer - 1));
|
|
last = api->dev_connect(dev, buf.data());
|
|
if (ok(last)) {
|
|
connected = true;
|
|
LOG_INFO << "thermal camera connected via " << c;
|
|
break;
|
|
}
|
|
}
|
|
if (!connected) {
|
|
freeDevice();
|
|
why = "cannot connect to " + chosen + ": " + describe(last) + linkHint();
|
|
return false;
|
|
}
|
|
|
|
// Profile: it sets the counts' scale, so it is checked before any frame.
|
|
cal = readCal();
|
|
if (cal < 0) {
|
|
disconnect();
|
|
why = "cannot read the active calibration profile";
|
|
return false;
|
|
}
|
|
if (cal != o.calib_profile) {
|
|
LOG_INFO << "thermal camera on calibration profile " << cal << ", switching to "
|
|
<< o.calib_profile;
|
|
std::string err;
|
|
if (!switchCal(o.calib_profile, err)) {
|
|
disconnect();
|
|
why = err;
|
|
return false;
|
|
}
|
|
}
|
|
|
|
// Polling, before and again after starting: the SDK resets it on startgrab.
|
|
setInt32(param::kPollingEnable, 1);
|
|
r = api->dev_startgrab(dev, 0);
|
|
if (!ok(r)) {
|
|
disconnect();
|
|
why = "cannot start grabbing: " + describe(r);
|
|
return false;
|
|
}
|
|
setInt32(param::kPollingEnable, 1);
|
|
|
|
rate_attempts = 0;
|
|
rate_pending = true;
|
|
wire_check_ms = 0;
|
|
|
|
std::string model;
|
|
getString(param::kCameraName, model);
|
|
{
|
|
std::lock_guard<std::mutex> l(m);
|
|
if (!model.empty()) info.model = model;
|
|
++connects;
|
|
}
|
|
takeOverNuc();
|
|
|
|
kpc = 0.0;
|
|
scale_tries = 0;
|
|
last_sdk_ts = -1.0;
|
|
last_cam_ts = -1.0;
|
|
polls_since_frame = 0;
|
|
codes_since_frame.clear();
|
|
zeros_logged = false;
|
|
last_frame_steady = steadyMs(); // grace for the first frame
|
|
return true;
|
|
}
|
|
|
|
// ---- frames ------------------------------------------------------------
|
|
// Take the newest frame from the SDK, if there is one.
|
|
void poll() {
|
|
++polls_since_frame;
|
|
// As before every getdata in the vendor's examples: without it the SDK
|
|
// now and then answers 0x80000001 instead of a frame.
|
|
setInt32(param::kPollingEnable, 1);
|
|
Handle mem = 0;
|
|
const Ret r = api->dev_getdata(dev, kMemIrDigitFrame, &mem);
|
|
if (!ok(r)) {
|
|
++codes_since_frame[r];
|
|
if (r == kErrNucOrLicence) {
|
|
++nuc_or_licence;
|
|
const long long now = steadyMs();
|
|
if (now - nuc_logged_ms >= kLogEveryMs) {
|
|
LOG_WARN << "thermal camera: " << describe(r) << " x" << nuc_or_licence
|
|
<< " in the last minute; if this persists, check the licence in "
|
|
"/var/lib/infratec/.license before the NUC";
|
|
nuc_logged_ms = now;
|
|
nuc_or_licence = 0;
|
|
}
|
|
}
|
|
return;
|
|
}
|
|
struct MemGuard {
|
|
const Api* api;
|
|
Handle* h;
|
|
~MemGuard() { if (*h) api->mem_free(h); }
|
|
} guard{api.get(), &mem};
|
|
|
|
uint32_t w = 0, h = 0, type = 0;
|
|
Handle ptr = 0;
|
|
int32_t bytes = 0;
|
|
if (!ok(api->mem_getdimension(mem, &w, &h, &type)) ||
|
|
!ok(api->mem_getdataptr(mem, &ptr, &bytes)) || !ptr || w == 0 || h == 0)
|
|
return;
|
|
const size_t need = static_cast<size_t>(w) * h * 2;
|
|
if (bytes < 0 || static_cast<size_t>(bytes) < need) return;
|
|
frame_bytes = need;
|
|
|
|
// Polling hands out the newest frame again until a newer one arrives. The
|
|
// camera's frame counter stays 0 (TarisIR mini, SDK 4.3.44), so a new
|
|
// frame is told by its time.
|
|
MemInfo mi{};
|
|
mi.struct_size = sizeof(MemInfo);
|
|
if (!ok(api->mem_getinfo(mem, &mi))) {
|
|
mi.image_num = -1;
|
|
mi.timestamp_ms = -1.0;
|
|
}
|
|
if (mi.timestamp_ms >= 0.0 && mi.timestamp_ms == last_sdk_ts) { // seen it
|
|
++codes_since_frame[kSuccess];
|
|
return;
|
|
}
|
|
last_sdk_ts = mi.timestamp_ms;
|
|
|
|
const auto* counts = reinterpret_cast<const uint16_t*>(ptr);
|
|
// The first frames after starting are all zero.
|
|
if (std::all_of(counts, counts + static_cast<size_t>(w) * h,
|
|
[](uint16_t c) { return c == 0; })) {
|
|
if (!zeros_logged) LOG_INFO << "thermal camera: discarding zero frames after start";
|
|
zeros_logged = true;
|
|
return;
|
|
}
|
|
|
|
if (kpc == 0.0) measureScale(mem);
|
|
|
|
Frame f;
|
|
f.width = w;
|
|
f.height = h;
|
|
f.channels = 1;
|
|
f.layout = PixelLayout::Mono16;
|
|
f.timestamp_ms = unixMs();
|
|
f.cam_id = 0;
|
|
f.data.assign(reinterpret_cast<const uint8_t*>(ptr),
|
|
reinterpret_cast<const uint8_t*>(ptr) + need);
|
|
f.brightness = sampleBrightness(f.data.data(), f.data.size(), f.layout, w, h);
|
|
ThermalFrameInfo t;
|
|
t.cal = cal;
|
|
t.kelvin_per_count = kpc > 0.0 ? kpc : expectedKelvinPerCount(cal);
|
|
t.missed_packets = mi.missed_packets;
|
|
t.image_num = mi.image_num;
|
|
f.thermal = t;
|
|
|
|
const long long now = steadyMs();
|
|
noteFrame(now, mi.timestamp_ms);
|
|
last_frame_steady = now;
|
|
{
|
|
std::lock_guard<std::mutex> l(m);
|
|
latest = std::move(f);
|
|
++latest_seq;
|
|
++info.frames_delivered;
|
|
packets_lost += std::max(0, mi.missed_packets);
|
|
info.frames_dropped = missed_shots + packets_lost;
|
|
info.width = w;
|
|
info.height = h;
|
|
info.payload_bytes = static_cast<long long>(need);
|
|
info.streaming = true;
|
|
why_offline.clear();
|
|
arrivals.push_back(now);
|
|
while (!arrivals.empty() && now - arrivals.front() > 10000) arrivals.pop_front();
|
|
if (arrivals.size() > 1)
|
|
info.actual_fps = (arrivals.size() - 1) * 1000.0 /
|
|
std::max<long long>(1, arrivals.back() - arrivals.front());
|
|
}
|
|
cv.notify_all();
|
|
last_offline_logged.clear();
|
|
}
|
|
|
|
// The counts' scale, from the LUT at emissivity and transmission 1, where it
|
|
// is a straight line through zero. The first frames after connecting come
|
|
// with an empty table, so it is asked again on later ones; frames meanwhile
|
|
// use the documented scale, and if the table stays empty, so does the run.
|
|
static constexpr int kScaleTries = 50;
|
|
void measureScale(Handle mem) {
|
|
std::vector<float> lut(65536, 0.f);
|
|
const Ret r = api->mem_getdvlut(mem, lut.data(), 1.f, 1.f, 293.15f, 293.15f);
|
|
const double expected = expectedKelvinPerCount(cal);
|
|
double measured = 0.0;
|
|
if (ok(r)) measured = (static_cast<double>(lut[40000]) - lut[30000]) / 10000.0;
|
|
if (!(measured > 0.0) && ++scale_tries < kScaleTries) return; // try the next frame
|
|
LOG_DEBUG << "thermal camera LUT after " << scale_tries << " empty: " << describe(r)
|
|
<< ", [0] " << lut[0] << " [29315] " << lut[29315] << " [65280] "
|
|
<< lut[65280] << " [65535] " << lut[65535] << " K";
|
|
if (!(measured > 0.0)) {
|
|
kpc = expected > 0.0 ? expected : -1.0; // -1: unknown, and not asked again
|
|
LOG_WARN << "thermal camera: no usable LUT (" << describe(r) << "); taking the "
|
|
<< "documented " << expected << " K per count for profile " << cal;
|
|
} else {
|
|
kpc = measured;
|
|
if (expected > 0.0 && std::abs(kpc - expected) > expected * 0.01)
|
|
LOG_WARN << "thermal camera: profile " << cal << " has " << kpc
|
|
<< " K per count, not the documented " << expected;
|
|
else
|
|
LOG_INFO << "thermal camera: profile " << cal << ", " << kpc << " K per count";
|
|
}
|
|
}
|
|
|
|
// ---- frame rate ----------------------------------------------------------
|
|
// The camera sends 50 fps from power-up: 31 MB/s and 22 000 packets a second
|
|
// for the SDK and the CPU to take in, for one picture every few seconds. Set
|
|
// right after connecting, Framerate_Hz answers ok and changes nothing; set
|
|
// while grabbing, it thins out what the SDK hands over but the camera keeps
|
|
// sending. Set while the grab is stopped, after the stream has run, it holds
|
|
// on the wire (tower 111: 5 Hz, 2 100 packets a second). So that is what is
|
|
// done once the first frame is in, and checked on the port's packet count.
|
|
void setFrameRate() {
|
|
float rate = static_cast<float>(o.frame_rate_hz);
|
|
const Ret r = api->dev_setparam(dev, param::kFramerateHz, &rate, kTypeSingle);
|
|
++rate_attempts;
|
|
if (!ok(r))
|
|
LOG_WARN << "thermal camera: cannot set " << o.frame_rate_hz << " Hz: " << describe(r)
|
|
<< "; it runs at its own rate";
|
|
}
|
|
|
|
static long long rxPackets(const std::string& interface) {
|
|
if (interface.empty()) return -1;
|
|
std::ifstream f("/sys/class/net/" + interface + "/statistics/rx_packets");
|
|
long long v = -1;
|
|
f >> v;
|
|
return f ? v : -1;
|
|
}
|
|
|
|
void restartForRate() {
|
|
rate_pending = false;
|
|
api->dev_stopgrab(dev, 0);
|
|
setFrameRate();
|
|
setInt32(param::kPollingEnable, 1);
|
|
const Ret r = api->dev_startgrab(dev, 0);
|
|
setInt32(param::kPollingEnable, 1);
|
|
last_frame_steady = steadyMs(); // grace for the restart
|
|
if (!ok(r)) {
|
|
setOffline("cannot restart grabbing: " + describe(r));
|
|
disconnect();
|
|
return;
|
|
}
|
|
wire_check_ms = steadyMs() + 1000; // count packets once the stream is up again
|
|
wire_packets0 = -1;
|
|
}
|
|
|
|
// Packets per second on the camera's port against what the rate should send.
|
|
void checkWire() {
|
|
if (wire_check_ms == 0 || steadyMs() < wire_check_ms) return;
|
|
if (o.interface.empty()) {
|
|
wire_check_ms = 0;
|
|
LOG_INFO << "thermal camera: " << o.frame_rate_hz
|
|
<< " fps set; not checked on the wire (no [Camera.tir] interface)";
|
|
return;
|
|
}
|
|
const long long now = steadyMs(), pk = rxPackets(o.interface);
|
|
if (pk < 0) { wire_check_ms = 0; return; }
|
|
if (wire_packets0 < 0) { // start counting
|
|
wire_packets0 = pk;
|
|
wire_start_ms = now;
|
|
wire_check_ms = now + 3000;
|
|
return;
|
|
}
|
|
wire_check_ms = 0;
|
|
const double pps = (pk - wire_packets0) * 1000.0 / std::max<long long>(1, now - wire_start_ms);
|
|
const double per_fr = frame_bytes > 0 ? frame_bytes / 1464.0 : 420.0; // GVSP payload
|
|
const double fps_wire = pps / per_fr;
|
|
if (fps_wire <= o.frame_rate_hz * 1.5) {
|
|
LOG_INFO << "thermal camera: " << std::lround(fps_wire * 10) / 10.0 << " fps on "
|
|
<< o.interface << " (" << std::lround(pps) << " packets/s), set on attempt "
|
|
<< rate_attempts;
|
|
} else if (rate_attempts >= 3) {
|
|
LOG_WARN << "thermal camera: still " << std::lround(fps_wire) << " fps on " << o.interface
|
|
<< " (" << std::lround(pps) << " packets/s), not " << o.frame_rate_hz
|
|
<< "; frame rate not taken";
|
|
} else {
|
|
LOG_INFO << "thermal camera: " << std::lround(fps_wire) << " fps on " << o.interface
|
|
<< ", not " << o.frame_rate_hz << "; restarting the grab again";
|
|
rate_pending = true;
|
|
}
|
|
}
|
|
|
|
// ---- NUC ---------------------------------------------------------------
|
|
// Switch the camera's own NUC timer off: from now on the program runs the
|
|
// NUC, while the gimbal moves. The timer's value goes back on close. (Read
|
|
// back right after a set, the camera still reports the old value.)
|
|
void takeOverNuc() {
|
|
nuc_ours = false;
|
|
if (o.nuc_interval_ms <= 0) {
|
|
LOG_INFO << "thermal camera: NUC left to the camera's own timer";
|
|
return;
|
|
}
|
|
int32_t found = -1;
|
|
if (!getInt32(param::kNucInterval, found)) {
|
|
LOG_WARN << "thermal camera: cannot read NUC_Interval; NUC left to the camera";
|
|
return;
|
|
}
|
|
if (nuc_interval_found < 0) nuc_interval_found = found;
|
|
const Ret r = setInt32(param::kNucInterval, 0);
|
|
if (!ok(r)) {
|
|
LOG_WARN << "thermal camera: cannot switch the camera's NUC timer off (" << describe(r)
|
|
<< "); a picture may now and then fall into a NUC";
|
|
return;
|
|
}
|
|
nuc_ours = true;
|
|
last_nuc_ms = steadyMs() - o.nuc_interval_ms; // the first move runs one
|
|
LOG_INFO << "thermal camera: NUC every " << o.nuc_interval_ms / 1000
|
|
<< " s at the start of a move (camera's own timer: " << found << " s, now off)";
|
|
}
|
|
|
|
// Put the camera's timer back, so it corrects itself without us.
|
|
void handBackNuc() {
|
|
if (!nuc_ours || nuc_interval_found <= 0) return;
|
|
const Ret r = setInt32(param::kNucInterval, nuc_interval_found);
|
|
LOG_INFO << "thermal camera: NUC timer back to " << nuc_interval_found << " s ("
|
|
<< describe(r) << ")";
|
|
nuc_ours = false;
|
|
}
|
|
|
|
// A NUC when one is due and the gimbal has just started moving, or when it
|
|
// is overdue (twice the interval) with no move to wait for.
|
|
void serviceNuc() {
|
|
bool moved;
|
|
{
|
|
std::lock_guard<std::mutex> l(m);
|
|
moved = move_started;
|
|
move_started = false; // always: pause() wakes on it
|
|
}
|
|
if (!nuc_ours) return;
|
|
const long long since = steadyMs() - last_nuc_ms;
|
|
if (moved ? since < o.nuc_interval_ms : since < 2LL * o.nuc_interval_ms) return;
|
|
runNuc(moved ? "move" : "no move for a while");
|
|
}
|
|
|
|
void runNuc(const char* why) {
|
|
const long long start = steadyMs();
|
|
{
|
|
std::lock_guard<std::mutex> l(m);
|
|
nuc_now = true;
|
|
nuc_now_since = start;
|
|
}
|
|
Int2 nuc{kNucMode1Point, 0};
|
|
// Returns once the NUC is done, about 0.7 s.
|
|
const Ret r = api->dev_setparam(dev, param::kNucPerform, &nuc, kType2Int32);
|
|
const long long done = steadyMs();
|
|
{
|
|
std::lock_guard<std::mutex> l(m);
|
|
nuc_now = false;
|
|
// A frame from before the NUC is no longer fresh for a picture.
|
|
delivered_seq = latest_seq;
|
|
}
|
|
last_nuc_ms = done;
|
|
++sum_nucs;
|
|
// A frame is due as soon as the NUC is over; do not count it as a stall.
|
|
last_frame_steady = std::max(last_frame_steady, done);
|
|
if (!ok(r)) {
|
|
LOG_WARN << "thermal camera: NUC failed (" << describe(r)
|
|
<< "); handing NUC back to the camera's own timer";
|
|
handBackNuc();
|
|
return;
|
|
}
|
|
LOG_DEBUG << "thermal camera: NUC (" << why << ") took " << done - start << " ms";
|
|
}
|
|
|
|
// A new frame: log the pause before it if it was long, and count it.
|
|
void noteFrame(long long now, double cam_ts) {
|
|
const long long host_gap = now - last_frame_steady;
|
|
const double cam_gap = last_cam_ts >= 0.0 && cam_ts >= 0.0 ? cam_ts - last_cam_ts : -1.0;
|
|
if (last_cam_ts >= 0.0) {
|
|
++sum_frames;
|
|
if (cam_gap > 0.0) sum_cam_span += cam_gap;
|
|
sum_max_gap = std::max(sum_max_gap, host_gap);
|
|
// A pause of our own making is no news.
|
|
const bool ours = now - last_nuc_ms < 2000;
|
|
if ((host_gap > kGapLogMs || cam_gap > kGapLogMs) && !ours) {
|
|
++sum_gaps;
|
|
std::string codes;
|
|
for (const auto& [code, n] : codes_since_frame)
|
|
codes += (codes.empty() ? "" : ", ") +
|
|
(code == kSuccess ? std::string("same frame") : describe(code)) +
|
|
" x" + std::to_string(n);
|
|
LOG_INFO << "thermal camera: frame gap " << host_gap << " ms (camera clock "
|
|
<< static_cast<long long>(cam_gap) << " ms), " << polls_since_frame
|
|
<< " polls: " << (codes.empty() ? "-" : codes);
|
|
}
|
|
}
|
|
last_cam_ts = cam_ts;
|
|
polls_since_frame = 0;
|
|
codes_since_frame.clear();
|
|
if (now - sum_since_ms >= kSummaryEveryMs) summary(now);
|
|
}
|
|
|
|
void summary(long long now) {
|
|
if (sum_since_ms != 0 && sum_frames > 0) {
|
|
long long missed;
|
|
{
|
|
std::lock_guard<std::mutex> l(m);
|
|
missed = missed_window;
|
|
missed_window = 0;
|
|
}
|
|
const double fps = sum_cam_span > 0.0 ? sum_frames * 1000.0 / sum_cam_span : 0.0;
|
|
LOG_INFO << "thermal camera, last " << (now - sum_since_ms) / 60000 << " min: "
|
|
<< sum_frames << " frames (" << std::lround(fps * 10) / 10.0
|
|
<< " fps by the camera's clock), longest gap " << sum_max_gap << " ms, "
|
|
<< sum_gaps << " unexplained over " << kGapLogMs << " ms, " << sum_nucs
|
|
<< " NUC" << (nuc_ours ? " (ours)" : "") << ", "
|
|
<< missed << " picture(s) without a thermal frame";
|
|
}
|
|
sum_frames = sum_gaps = sum_nucs = sum_max_gap = 0;
|
|
sum_cam_span = 0.0;
|
|
sum_since_ms = now;
|
|
}
|
|
|
|
void telemetry() {
|
|
float k = 0.f;
|
|
if (getSingle(param::kCameraTempHousing, k) && k > 0.f) {
|
|
std::lock_guard<std::mutex> l(m);
|
|
info.temperature_c = k - 273.15;
|
|
}
|
|
}
|
|
|
|
// ---- the worker ------------------------------------------------------
|
|
void run() {
|
|
if (!api) {
|
|
std::string err;
|
|
LocaleGuard keep;
|
|
api = irbgrab::load(o.library, err);
|
|
if (!api) {
|
|
setOffline(err + " (InfraTec SDK not installed?)");
|
|
return;
|
|
}
|
|
}
|
|
if (!has_net_raw()) {
|
|
setOffline("this binary may not open raw sockets (no cap_net_raw): run deploy.sh, "
|
|
"which sets it, or sudo setcap cap_net_raw+ep build/fire_gimbal_control");
|
|
return;
|
|
}
|
|
Ret r;
|
|
{
|
|
LocaleGuard keep;
|
|
r = api->dll_init();
|
|
}
|
|
if (!ok(r)) {
|
|
setOffline("SDK init failed: " + describe(r));
|
|
return;
|
|
}
|
|
sdk_init = true;
|
|
std::vector<char> ver(kCharBuffer, 0);
|
|
if (ok(api->dll_version(ver.data(), kCharBuffer)))
|
|
LOG_INFO << "IRBGRAB SDK " << ver.data();
|
|
|
|
long long retry = o.retry_min_ms;
|
|
const long long poll_ms =
|
|
std::max<long long>(5, static_cast<long long>(500.0 / o.frame_rate_hz));
|
|
while (running) {
|
|
LocaleGuard keep_locale; // every SDK call in this pass, polling included
|
|
if (!connected) {
|
|
{
|
|
// Connecting is what a reset asks for; it must not keep the
|
|
// retry pause from pausing.
|
|
std::lock_guard<std::mutex> l(m);
|
|
reset_wanted = false;
|
|
move_started = false; // nor may a move cut the retry pause short
|
|
}
|
|
std::string why;
|
|
bool up;
|
|
{
|
|
LocaleGuard keep;
|
|
up = connect(why);
|
|
}
|
|
if (up) {
|
|
retry = o.retry_min_ms;
|
|
std::lock_guard<std::mutex> l(m);
|
|
why_offline = "waiting for the first frame";
|
|
} else {
|
|
setOffline(why);
|
|
pause(retry);
|
|
retry = std::min<long long>(retry * 2, o.retry_max_ms);
|
|
continue;
|
|
}
|
|
}
|
|
serviceRequests();
|
|
if (!connected) continue;
|
|
serviceNuc();
|
|
poll();
|
|
if (rate_pending && last_cam_ts >= 0.0) restartForRate(); // once a frame is in
|
|
if (!connected) continue;
|
|
checkWire();
|
|
|
|
const long long now = steadyMs();
|
|
if (now - last_frame_steady > o.stall_ms) {
|
|
setOffline("no frame for " + std::to_string(o.stall_ms) + " ms; reconnecting");
|
|
disconnect();
|
|
std::lock_guard<std::mutex> l(m);
|
|
++info.restarts;
|
|
continue;
|
|
}
|
|
if (now - last_telemetry >= kTelemetryEveryMs) {
|
|
telemetry();
|
|
last_telemetry = now;
|
|
}
|
|
pause(poll_ms);
|
|
}
|
|
LocaleGuard keep;
|
|
if (connected) handBackNuc();
|
|
disconnect();
|
|
if (sdk_init) api->dll_uninit();
|
|
sdk_init = false;
|
|
}
|
|
|
|
// A profile switch or a reset asked for from another thread.
|
|
void serviceRequests() {
|
|
int want = -1;
|
|
bool reset = false;
|
|
{
|
|
std::lock_guard<std::mutex> l(m);
|
|
want = cal_wanted;
|
|
reset = reset_wanted;
|
|
reset_wanted = false;
|
|
}
|
|
if (reset) {
|
|
LOG_INFO << "thermal camera: reset requested, reconnecting";
|
|
disconnect();
|
|
std::lock_guard<std::mutex> l(m);
|
|
++info.restarts;
|
|
return;
|
|
}
|
|
if (want < 0) return;
|
|
std::string why;
|
|
bool done = switchCal(want, why);
|
|
if (done) o.calib_profile = want; // a reconnect keeps it
|
|
else LOG_WARN << "thermal camera: " << why;
|
|
{
|
|
std::lock_guard<std::mutex> l(m);
|
|
cal_wanted = -1;
|
|
cal_done = true;
|
|
cal_ok = done;
|
|
// Frames from before the switch are on the old scale: never hand one out.
|
|
delivered_seq = latest_seq;
|
|
}
|
|
cv.notify_all();
|
|
}
|
|
};
|
|
|
|
TarisCameraSource::TarisCameraSource(Options opts, std::shared_ptr<const irbgrab::Api> api,
|
|
std::function<bool()> has_net_raw)
|
|
: impl_(std::make_unique<Impl>(std::move(opts), std::move(api), std::move(has_net_raw))) {}
|
|
|
|
TarisCameraSource::~TarisCameraSource() { close(); }
|
|
|
|
void TarisCameraSource::open() {
|
|
if (impl_->running.exchange(true)) return;
|
|
{
|
|
std::lock_guard<std::mutex> l(impl_->m);
|
|
impl_->why_offline = "connecting";
|
|
}
|
|
impl_->worker = std::thread([this] { impl_->run(); });
|
|
LOG_INFO << "thermal camera " << impl_->o.serial << ": connecting in the background";
|
|
}
|
|
|
|
void TarisCameraSource::close() {
|
|
if (!impl_->running.exchange(false)) return;
|
|
impl_->cv.notify_all();
|
|
if (impl_->worker.joinable()) impl_->worker.join();
|
|
std::lock_guard<std::mutex> l(impl_->m);
|
|
impl_->why_offline = "closed";
|
|
impl_->info.streaming = false;
|
|
}
|
|
|
|
void TarisCameraSource::start() { impl_->gate = true; }
|
|
void TarisCameraSource::stop() { impl_->gate = false; }
|
|
|
|
void TarisCameraSource::setFrameCallback(FrameCallback cb) { impl_->callback = std::move(cb); }
|
|
|
|
void TarisCameraSource::moving() {
|
|
{
|
|
std::lock_guard<std::mutex> l(impl_->m);
|
|
impl_->move_started = true;
|
|
}
|
|
impl_->cv.notify_all(); // the worker may be pausing between polls
|
|
}
|
|
|
|
bool TarisCameraSource::frameReady() {
|
|
Impl& d = *impl_;
|
|
std::lock_guard<std::mutex> l(d.m);
|
|
if (!d.running || !d.why_offline.empty()) return true; // nothing to wait for
|
|
if (d.nuc_now) return false;
|
|
return d.latest_seq > d.delivered_seq &&
|
|
unixMs() - d.latest.timestamp_ms <= d.o.max_frame_age_ms;
|
|
}
|
|
|
|
bool TarisCameraSource::trigger() {
|
|
Impl& d = *impl_;
|
|
if (!d.gate || !d.callback) return false;
|
|
Frame f;
|
|
{
|
|
std::lock_guard<std::mutex> l(d.m);
|
|
const bool new_frame = d.latest_seq > d.delivered_seq;
|
|
const long long age = d.latest_seq ? unixMs() - d.latest.timestamp_ms : -1;
|
|
const bool fresh = new_frame && age <= d.o.max_frame_age_ms;
|
|
if (!fresh) {
|
|
// Every miss, with what tells its cause apart: the stream paused (and
|
|
// whether a NUC was running), or nothing came since the last picture.
|
|
++d.missed_shots;
|
|
++d.missed_window;
|
|
d.info.frames_dropped = d.missed_shots + d.packets_lost;
|
|
const long long now = steadyMs();
|
|
std::string why = !d.why_offline.empty()
|
|
? d.why_offline
|
|
: "newest frame " + std::to_string(age) + " ms old (limit " +
|
|
std::to_string(d.o.max_frame_age_ms) + " ms)" +
|
|
(new_frame ? "" : ", none since the last picture") +
|
|
(d.nuc_now ? ", NUC in progress for " +
|
|
std::to_string(now - d.nuc_now_since) + " ms"
|
|
: "");
|
|
LOG_WARN << "thermal camera: no thermal frame for this picture: " << why;
|
|
return false;
|
|
}
|
|
f = d.latest;
|
|
d.delivered_seq = d.latest_seq;
|
|
}
|
|
d.callback(f);
|
|
return true;
|
|
}
|
|
|
|
std::vector<CameraDeviceInfo> TarisCameraSource::deviceInfo() {
|
|
std::lock_guard<std::mutex> l(impl_->m);
|
|
return {impl_->info};
|
|
}
|
|
|
|
std::string TarisCameraSource::offline(int camera_index) {
|
|
if (camera_index != 0) return "no such camera";
|
|
std::lock_guard<std::mutex> l(impl_->m);
|
|
return impl_->why_offline;
|
|
}
|
|
|
|
bool TarisCameraSource::grabFrame(int camera_index, Frame& out, int timeout_ms) {
|
|
if (camera_index != 0) return false;
|
|
Impl& d = *impl_;
|
|
std::unique_lock<std::mutex> l(d.m);
|
|
const uint64_t after = d.latest_seq;
|
|
if (!d.cv.wait_for(l, std::chrono::milliseconds(timeout_ms),
|
|
[&] { return d.latest_seq > after || !d.running; }) ||
|
|
d.latest_seq <= after)
|
|
return false;
|
|
out = d.latest;
|
|
return true;
|
|
}
|
|
|
|
bool TarisCameraSource::readFeature(int camera_index, const std::string& name, double& value) {
|
|
if (camera_index != 0) return false;
|
|
std::lock_guard<std::mutex> l(impl_->m);
|
|
if (name == "CalibProfile") {
|
|
if (!impl_->latest.thermal) return false;
|
|
value = impl_->latest.thermal->cal;
|
|
return true;
|
|
}
|
|
if (name == "KelvinPerCount") {
|
|
if (!impl_->latest.thermal) return false;
|
|
value = impl_->latest.thermal->kelvin_per_count;
|
|
return true;
|
|
}
|
|
if (name == "HousingTemperature") {
|
|
if (impl_->info.temperature_c == 0.0) return false;
|
|
value = impl_->info.temperature_c;
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
bool TarisCameraSource::setFeature(int camera_index, const std::string& name,
|
|
const std::string& value) {
|
|
if (camera_index != 0 || name != "CalibProfile") return false;
|
|
int profile = -1;
|
|
try {
|
|
size_t used = 0;
|
|
profile = std::stoi(value, &used);
|
|
if (used != value.size()) return false;
|
|
} catch (const std::exception&) {
|
|
return false;
|
|
}
|
|
Impl& d = *impl_;
|
|
std::unique_lock<std::mutex> l(d.m);
|
|
if (!d.running || !d.why_offline.empty()) return false;
|
|
d.cal_wanted = profile;
|
|
d.cal_done = false;
|
|
d.cv.notify_all();
|
|
if (!d.cv.wait_for(l, std::chrono::milliseconds(d.o.calib_timeout_ms + 5000),
|
|
[&] { return d.cal_done || !d.running; }))
|
|
return false;
|
|
return d.cal_done && d.cal_ok;
|
|
}
|
|
|
|
bool TarisCameraSource::resetCamera(int camera_index) {
|
|
if (camera_index != 0) return false;
|
|
Impl& d = *impl_;
|
|
std::unique_lock<std::mutex> l(d.m);
|
|
if (!d.running) return false;
|
|
const uint64_t before = d.connects;
|
|
const uint64_t seq = d.latest_seq;
|
|
d.reset_wanted = true;
|
|
d.cv.notify_all();
|
|
// Back once it has connected again and delivered a frame.
|
|
return d.cv.wait_for(l, std::chrono::seconds(45), [&] {
|
|
return (d.connects > before && d.latest_seq > seq) || !d.running;
|
|
}) && d.running;
|
|
}
|
|
|
|
} // namespace fgc
|