/*
* Get absolute time.
*/
hrt_abstime hrt_absolute_time(void)
{
struct timespec ts;
if (!px4_timestart) {
px4_clock_gettime(CLOCK_MONOTONIC, &ts);
px4_timestart = ts_to_abstime(&ts);
}
px4_clock_gettime(CLOCK_MONOTONIC, &ts);
return ts_to_abstime(&ts) - px4_timestart;
}
void
CameraTrigger::engage(void *arg)
{
CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);
// Trigger the camera
trig->_camera_interface->trigger(true);
if (trig->_test_shot) {
// do not send messages or increment frame count for test shots
return;
}
// Send camera trigger message. This messages indicates that we sent
// the camera trigger request. Does not guarantee capture.
struct camera_trigger_s trigger = {};
// Set timestamp the instant after the trigger goes off
trigger.timestamp = hrt_absolute_time();
timespec tv = {};
px4_clock_gettime(CLOCK_REALTIME, &tv);
trigger.timestamp_utc = (uint64_t) tv.tv_sec * 1000000 + tv.tv_nsec / 1000;
trigger.seq = trig->_trigger_seq;
orb_publish(ORB_ID(camera_trigger), trig->_trigger_pub, &trigger);
// increment frame count
trig->_trigger_seq++;
}
int px4_sem_timedwait(px4_sem_t *sem, const struct timespec *ts)
{
work_s _hpwork = {};
// Get the current time.
struct timespec ts_now;
px4_clock_gettime(CLOCK_MONOTONIC, &ts_now);
// We get an absolute time but want to calculate a timeout in us.
hrt_abstime timeout_us = ts_to_abstime((struct timespec *)ts) - ts_to_abstime(&ts_now);
// Create a timer to unblock.
hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)sem, timeout_us);
sem_wait(sem);
hrt_work_cancel(&_hpwork);
return 0;
}
static int micrortps_start(int argc, char *argv[])
{
if (0 > parse_options(argc, argv)) {
printf("EXITING...\n");
_rtps_task = -1;
return -1;
}
switch (_options.transport) {
case options::eTransports::UART: {
transport_node = new UART_node(_options.device, _options.baudrate, _options.poll_ms);
printf("\nUART transport: device: %s; baudrate: %d; sleep: %dms; poll: %dms\n\n",
_options.device, _options.baudrate, _options.sleep_ms, _options.poll_ms);
}
break;
case options::eTransports::UDP: {
transport_node = new UDP_node(_options.recv_port, _options.send_port);
printf("\nUDP transport: recv port: %u; send port: %u; sleep: %dms\n\n",
_options.recv_port, _options.send_port, _options.sleep_ms);
}
break;
default:
_rtps_task = -1;
printf("EXITING...\n");
return -1;
}
if (0 > transport_node->init()) {
printf("EXITING...\n");
_rtps_task = -1;
return -1;
}
struct timespec begin;
int total_read = 0, loop = 0;
uint32_t received = 0;
micrortps_start_topics(begin, total_read, received, loop);
struct timespec end;
px4_clock_gettime(CLOCK_REALTIME, &end);
double elapsed_secs = double(end.tv_sec - begin.tv_sec) + double(end.tv_nsec - begin.tv_nsec) / double(1000000000);
printf("RECEIVED: %lu messages in %d LOOPS, %d bytes in %.03f seconds - %.02fKB/s\n\n",
(unsigned long)received, loop, total_read, elapsed_secs, (double)total_read / (1000 * elapsed_secs));
delete transport_node;
transport_node = nullptr;
PX4_INFO("exiting");
fflush(stdout);
_rtps_task = -1;
return 0;
}
