mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
Rework trigger to operate on work queue and timers without jitter
This commit is contained in:
Lorenz Meier
parent
e0f203e3da
commit
06a4e9c585
@ -1,7 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
|
||||
* Author: Mohammed Kabir <mhkabir98@gmail.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@ -47,6 +46,7 @@
|
||||
#include <stdbool.h>
|
||||
#include <nuttx/clock.h>
|
||||
#include <nuttx/arch.h>
|
||||
#include <nuttx/wqueue.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <systemlib/param/param.h>
|
||||
@ -60,6 +60,8 @@
|
||||
#include <mavlink/mavlink_log.h>
|
||||
#include <board_config.h>
|
||||
|
||||
#define TRIGGER_PIN_DEFAULT 1
|
||||
|
||||
extern "C" __EXPORT int camera_trigger_main(int argc, char *argv[]);
|
||||
|
||||
class CameraTrigger
|
||||
@ -75,6 +77,11 @@ public:
|
||||
*/
|
||||
~CameraTrigger();
|
||||
|
||||
/**
|
||||
* Set the trigger on / off
|
||||
*/
|
||||
void control(bool on);
|
||||
|
||||
/**
|
||||
* Start the task.
|
||||
*/
|
||||
@ -90,37 +97,32 @@ public:
|
||||
*/
|
||||
void info();
|
||||
|
||||
int pin;
|
||||
int _pin;
|
||||
|
||||
private:
|
||||
|
||||
struct hrt_call _pollcall;
|
||||
struct hrt_call _firecall;
|
||||
struct hrt_call _engagecall;
|
||||
struct hrt_call _disengagecall;
|
||||
static struct work_s _work;
|
||||
|
||||
int _gpio_fd;
|
||||
|
||||
int _polarity;
|
||||
float _activation_time;
|
||||
float _integration_time;
|
||||
float _transfer_time;
|
||||
float _interval;
|
||||
uint32_t _trigger_seq;
|
||||
bool _trigger_enabled;
|
||||
|
||||
int _sensor_sub;
|
||||
int _vcommand_sub;
|
||||
|
||||
orb_advert_t _trigger_pub;
|
||||
|
||||
struct camera_trigger_s _trigger;
|
||||
struct sensor_combined_s _sensor;
|
||||
struct vehicle_command_s _command;
|
||||
|
||||
param_t polarity ;
|
||||
param_t activation_time ;
|
||||
param_t integration_time ;
|
||||
param_t polarity;
|
||||
param_t activation_time;
|
||||
param_t interval;
|
||||
param_t transfer_time ;
|
||||
|
||||
int32_t _gpios[6] = {
|
||||
static constexpr uint32_t _gpios[6] = {
|
||||
GPIO_GPIO0_OUTPUT,
|
||||
GPIO_GPIO1_OUTPUT,
|
||||
GPIO_GPIO2_OUTPUT,
|
||||
@ -130,9 +132,9 @@ private:
|
||||
};
|
||||
|
||||
/**
|
||||
* Topic poller to check for fire info.
|
||||
* Vehicle command handler
|
||||
*/
|
||||
static void poll(void *arg);
|
||||
static void cycle_trampoline(void *arg);
|
||||
/**
|
||||
* Fires trigger
|
||||
*/
|
||||
@ -144,6 +146,9 @@ private:
|
||||
|
||||
};
|
||||
|
||||
struct work_s CameraTrigger::_work;
|
||||
constexpr uint32_t CameraTrigger::_gpios[6];
|
||||
|
||||
namespace camera_trigger
|
||||
{
|
||||
|
||||
@ -151,38 +156,26 @@ CameraTrigger *g_camera_trigger;
|
||||
}
|
||||
|
||||
CameraTrigger::CameraTrigger() :
|
||||
pin(1),
|
||||
_pollcall {},
|
||||
_firecall {},
|
||||
_gpio_fd(-1),
|
||||
_polarity(0),
|
||||
_activation_time(0.0f),
|
||||
_integration_time(0.0f),
|
||||
_transfer_time(0.0f),
|
||||
_trigger_seq(0),
|
||||
_trigger_enabled(false),
|
||||
_sensor_sub(-1),
|
||||
_vcommand_sub(-1),
|
||||
_trigger_pub(nullptr),
|
||||
_trigger {},
|
||||
_sensor {},
|
||||
_command {},
|
||||
_gpios {} {
|
||||
|
||||
memset(&_pollcall, 0, sizeof(_pollcall));
|
||||
memset(&_firecall, 0, sizeof(_firecall));
|
||||
|
||||
memset(&_trigger, 0, sizeof(_trigger));
|
||||
memset(&_sensor, 0, sizeof(_sensor));
|
||||
memset(&_command, 0, sizeof(_command));
|
||||
|
||||
memset(&_gpios, 0, sizeof(_gpios));
|
||||
_pin(TRIGGER_PIN_DEFAULT),
|
||||
_engagecall {},
|
||||
_disengagecall {},
|
||||
_gpio_fd(-1),
|
||||
_polarity(0),
|
||||
_activation_time(0.5f /* ms */),
|
||||
_interval(100.0f /* ms */),
|
||||
_trigger_seq(0),
|
||||
_trigger_enabled(false),
|
||||
_vcommand_sub(-1),
|
||||
_trigger_pub(nullptr)
|
||||
{
|
||||
memset(&_work, 0, sizeof(_work));
|
||||
memset(&_engagecall, 0, sizeof(_engagecall));
|
||||
memset(&_disengagecall, 0, sizeof(_disengagecall));
|
||||
|
||||
// Parameters
|
||||
polarity = param_find("TRIG_POLARITY");
|
||||
interval = param_find("TRIG_INTERVAL");
|
||||
activation_time = param_find("TRIG_ACT_TIME");
|
||||
integration_time = param_find("TRIG_INT_TIME");
|
||||
transfer_time = param_find("TRIG_TRANS_TIME");
|
||||
}
|
||||
|
||||
CameraTrigger::~CameraTrigger()
|
||||
@ -190,40 +183,54 @@ CameraTrigger::~CameraTrigger()
|
||||
camera_trigger::g_camera_trigger = nullptr;
|
||||
}
|
||||
|
||||
void
|
||||
CameraTrigger::control(bool on)
|
||||
{
|
||||
// always execute, even if already on
|
||||
// to reset timings if necessary
|
||||
|
||||
// schedule trigger on and off calls
|
||||
hrt_call_every(&_engagecall, 500, (_interval * 1000),
|
||||
(hrt_callout)&CameraTrigger::engage, this);
|
||||
|
||||
// schedule trigger on and off calls
|
||||
hrt_call_every(&_disengagecall, 500 + (_activation_time * 1000), (_interval * 1000),
|
||||
(hrt_callout)&CameraTrigger::disengage, this);
|
||||
|
||||
_trigger_enabled = on;
|
||||
}
|
||||
|
||||
void
|
||||
CameraTrigger::start()
|
||||
{
|
||||
|
||||
_sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
|
||||
_vcommand_sub = orb_subscribe(ORB_ID(vehicle_command));
|
||||
|
||||
param_get(polarity, &_polarity);
|
||||
param_get(activation_time, &_activation_time);
|
||||
param_get(integration_time, &_integration_time);
|
||||
param_get(transfer_time, &_transfer_time);
|
||||
param_get(interval, &_interval);
|
||||
|
||||
stm32_configgpio(_gpios[pin - 1]);
|
||||
stm32_configgpio(_gpios[_pin - 1]);
|
||||
|
||||
if (_polarity == 0) {
|
||||
stm32_gpiowrite(_gpios[pin - 1], 1); // GPIO pin pull high
|
||||
stm32_gpiowrite(_gpios[_pin - 1], 1); // GPIO pin pull high
|
||||
|
||||
} else if (_polarity == 1) {
|
||||
stm32_gpiowrite(_gpios[pin - 1], 0); // GPIO pin pull low
|
||||
stm32_gpiowrite(_gpios[_pin - 1], 0); // GPIO pin pull low
|
||||
|
||||
} else {
|
||||
warnx(" invalid trigger polarity setting. stopping.");
|
||||
stop();
|
||||
}
|
||||
|
||||
poll(this); // Trampoline call
|
||||
|
||||
// start to monitor at high rate for trigger enable command
|
||||
work_queue(LPWORK, &_work, (worker_t)&CameraTrigger::cycle_trampoline, this, USEC2TICK(1));
|
||||
}
|
||||
|
||||
void
|
||||
CameraTrigger::stop()
|
||||
{
|
||||
hrt_cancel(&_firecall);
|
||||
hrt_cancel(&_pollcall);
|
||||
work_cancel(LPWORK, &_work);
|
||||
hrt_cancel(&_engagecall);
|
||||
hrt_cancel(&_disengagecall);
|
||||
|
||||
if (camera_trigger::g_camera_trigger != nullptr) {
|
||||
delete(camera_trigger::g_camera_trigger);
|
||||
@ -231,62 +238,49 @@ CameraTrigger::stop()
|
||||
}
|
||||
|
||||
void
|
||||
CameraTrigger::poll(void *arg)
|
||||
CameraTrigger::cycle_trampoline(void *arg)
|
||||
{
|
||||
|
||||
CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);
|
||||
|
||||
if (trig->_vcommand_sub < 0) {
|
||||
trig->_vcommand_sub = orb_subscribe(ORB_ID(vehicle_command));
|
||||
}
|
||||
|
||||
bool updated;
|
||||
orb_check(trig->_vcommand_sub, &updated);
|
||||
|
||||
// while the trigger is inactive it has to be ready
|
||||
// to become active instantaneously
|
||||
int poll_interval_usec = 5000;
|
||||
|
||||
if (updated) {
|
||||
|
||||
orb_copy(ORB_ID(vehicle_command), trig->_vcommand_sub, &trig->_command);
|
||||
struct vehicle_command_s cmd;
|
||||
|
||||
if (trig->_command.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) {
|
||||
if (trig->_command.param1 < 1) {
|
||||
if (trig->_trigger_enabled) {
|
||||
trig->_trigger_enabled = false ;
|
||||
}
|
||||
|
||||
} else if (trig->_command.param1 >= 1) {
|
||||
if (!trig->_trigger_enabled) {
|
||||
trig->_trigger_enabled = true ;
|
||||
}
|
||||
}
|
||||
orb_copy(ORB_ID(vehicle_command), trig->_vcommand_sub, &cmd);
|
||||
|
||||
if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) {
|
||||
// Set trigger rate from command
|
||||
if (trig->_command.param2 > 0) {
|
||||
trig->_integration_time = trig->_command.param2;
|
||||
param_set(trig->integration_time, &(trig->_integration_time));
|
||||
if (cmd.param2 > 0) {
|
||||
trig->_interval = cmd.param2;
|
||||
param_set(trig->interval, &(trig->_interval));
|
||||
}
|
||||
|
||||
if (cmd.param1 < 1.0f) {
|
||||
trig->control(false);
|
||||
|
||||
} else if (cmd.param1 >= 1.0f) {
|
||||
trig->control(true);
|
||||
// while the trigger is active there is no
|
||||
// need to poll at a very high rate
|
||||
poll_interval_usec = 100000;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (!trig->_trigger_enabled) {
|
||||
hrt_call_after(&trig->_pollcall, 1e6, (hrt_callout)&CameraTrigger::poll, trig);
|
||||
return;
|
||||
|
||||
} else {
|
||||
engage(trig);
|
||||
hrt_call_after(&trig->_firecall, trig->_activation_time * 1000, (hrt_callout)&CameraTrigger::disengage, trig);
|
||||
|
||||
orb_copy(ORB_ID(sensor_combined), trig->_sensor_sub, &trig->_sensor);
|
||||
|
||||
trig->_trigger.timestamp = trig->_sensor.timestamp; // get IMU timestamp
|
||||
trig->_trigger.seq = trig->_trigger_seq++;
|
||||
|
||||
if (trig->_trigger_pub != nullptr) {
|
||||
orb_publish(ORB_ID(camera_trigger), trig->_trigger_pub, &trig->_trigger);
|
||||
|
||||
} else {
|
||||
trig->_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trig->_trigger);
|
||||
}
|
||||
|
||||
hrt_call_after(&trig->_pollcall, (trig->_transfer_time + trig->_integration_time) * 1000,
|
||||
(hrt_callout)&CameraTrigger::poll, trig);
|
||||
}
|
||||
|
||||
work_queue(LPWORK, &_work, (worker_t)&CameraTrigger::cycle_trampoline,
|
||||
camera_trigger::g_camera_trigger, USEC2TICK(poll_interval_usec));
|
||||
}
|
||||
|
||||
void
|
||||
@ -295,16 +289,23 @@ CameraTrigger::engage(void *arg)
|
||||
|
||||
CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);
|
||||
|
||||
stm32_configgpio(trig->_gpios[trig->pin - 1]);
|
||||
stm32_configgpio(trig->_gpios[trig->_pin - 1]);
|
||||
|
||||
struct camera_trigger_s trigger;
|
||||
|
||||
/* set timestamp the instant before the trigger goes off */
|
||||
trigger.timestamp = hrt_absolute_time();
|
||||
|
||||
if (trig->_polarity == 0) { // ACTIVE_LOW
|
||||
stm32_gpiowrite(trig->_gpios[trig->pin - 1], 0);
|
||||
stm32_gpiowrite(trig->_gpios[trig->_pin - 1], 0);
|
||||
|
||||
} else if (trig->_polarity == 1) { // ACTIVE_HIGH
|
||||
stm32_gpiowrite(trig->_gpios[trig->pin - 1], 1);
|
||||
stm32_gpiowrite(trig->_gpios[trig->_pin - 1], 1);
|
||||
}
|
||||
|
||||
trigger.seq = trig->_trigger_seq++;
|
||||
|
||||
orb_publish(ORB_ID(camera_trigger), trig->_trigger_pub, &trigger);
|
||||
}
|
||||
|
||||
void
|
||||
@ -313,13 +314,13 @@ CameraTrigger::disengage(void *arg)
|
||||
|
||||
CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);
|
||||
|
||||
stm32_configgpio(trig->_gpios[trig->pin - 1]);
|
||||
stm32_configgpio(trig->_gpios[trig->_pin - 1]);
|
||||
|
||||
if (trig->_polarity == 0) { // ACTIVE_LOW
|
||||
stm32_gpiowrite(trig->_gpios[trig->pin - 1], 1);
|
||||
stm32_gpiowrite(trig->_gpios[trig->_pin - 1], 1);
|
||||
|
||||
} else if (trig->_polarity == 1) { // ACTIVE_HIGH
|
||||
stm32_gpiowrite(trig->_gpios[trig->pin - 1], 0);
|
||||
stm32_gpiowrite(trig->_gpios[trig->_pin - 1], 0);
|
||||
}
|
||||
|
||||
}
|
||||
@ -327,16 +328,15 @@ CameraTrigger::disengage(void *arg)
|
||||
void
|
||||
CameraTrigger::info()
|
||||
{
|
||||
warnx("Trigger state : %s", _trigger_enabled ? "enabled" : "disabled");
|
||||
warnx("Trigger pin : %i", pin);
|
||||
warnx("Trigger polarity : %s", _polarity ? "ACTIVE_HIGH" : "ACTIVE_LOW");
|
||||
warnx("Shutter integration time : %.2f", (double)_integration_time);
|
||||
warnx("state : %s", _trigger_enabled ? "enabled" : "disabled");
|
||||
warnx("pin : %i, polarity : %s", _pin, _polarity ? "ACTIVE_HIGH" : "ACTIVE_LOW");
|
||||
warnx("interval : %.2f", (double)_interval);
|
||||
}
|
||||
|
||||
static void usage()
|
||||
{
|
||||
errx(1, "usage: camera_trigger {start|stop|info} [-p <n>]\n"
|
||||
"\t-p <n>\tUse specified AUX OUT pin number (default: 1)"
|
||||
"\t-p <n>\tUse specified AUX OUT pin number (default: %d)", TRIGGER_PIN_DEFAULT
|
||||
);
|
||||
}
|
||||
|
||||
@ -360,11 +360,11 @@ int camera_trigger_main(int argc, char *argv[])
|
||||
|
||||
if (argc > 3) {
|
||||
|
||||
camera_trigger::g_camera_trigger->pin = (int)argv[3];
|
||||
camera_trigger::g_camera_trigger->_pin = (int)argv[3];
|
||||
|
||||
if (atoi(argv[3]) > 0 && atoi(argv[3]) < 6) {
|
||||
warnx("starting trigger on pin : %li ", atoi(argv[3]));
|
||||
camera_trigger::g_camera_trigger->pin = atoi(argv[3]);
|
||||
camera_trigger::g_camera_trigger->_pin = atoi(argv[3]);
|
||||
|
||||
} else {
|
||||
usage();
|
||||
@ -378,14 +378,19 @@ int camera_trigger_main(int argc, char *argv[])
|
||||
|
||||
if (camera_trigger::g_camera_trigger == nullptr) {
|
||||
errx(1, "not running");
|
||||
}
|
||||
|
||||
else if (!strcmp(argv[1], "stop")) {
|
||||
} else if (!strcmp(argv[1], "stop")) {
|
||||
camera_trigger::g_camera_trigger->stop();
|
||||
|
||||
} else if (!strcmp(argv[1], "info")) {
|
||||
camera_trigger::g_camera_trigger->info();
|
||||
|
||||
} else if (!strcmp(argv[1], "enable")) {
|
||||
camera_trigger::g_camera_trigger->control(true);
|
||||
|
||||
} else if (!strcmp(argv[1], "disable")) {
|
||||
camera_trigger::g_camera_trigger->control(false);
|
||||
|
||||
} else {
|
||||
usage();
|
||||
}
|
||||
|
||||
@ -51,19 +51,19 @@
|
||||
* @max 500.0
|
||||
* @group Camera trigger
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TRIG_INT_TIME, 300.0f);
|
||||
PARAM_DEFINE_FLOAT(TRIG_INT_TIME, 1.0f);
|
||||
|
||||
/**
|
||||
* Camera trigger transfer time
|
||||
* Camera trigger interval
|
||||
*
|
||||
* This parameter sets the time the image transfer takes (PointGrey mode_0)
|
||||
* This parameter sets the time between two consecutive trigger events
|
||||
*
|
||||
* @unit milliseconds
|
||||
* @min 15.0
|
||||
* @max 33.0
|
||||
* @group Camera trigger
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TRIG_TRANS_TIME, 15.0f);
|
||||
PARAM_DEFINE_FLOAT(TRIG_INTERVAL, 15.0f);
|
||||
|
||||
/**
|
||||
* Camera trigger polarity
|
||||
@ -79,10 +79,20 @@ PARAM_DEFINE_INT32(TRIG_POLARITY, 0);
|
||||
/**
|
||||
* Camera trigger activation time
|
||||
*
|
||||
* This parameter sets the time the trigger needs to pulled high or low to start light
|
||||
* integration.
|
||||
* This parameter sets the time the trigger needs to pulled high or low.
|
||||
*
|
||||
* @unit milliseconds
|
||||
* @group Camera trigger
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TRIG_ACT_TIME, 5.0f);
|
||||
PARAM_DEFINE_FLOAT(TRIG_ACT_TIME, 0.5f);
|
||||
|
||||
/**
|
||||
* Camera trigger mode
|
||||
*
|
||||
* 0 disables the trigger, 1 sets it to enabled on command, 2 always on
|
||||
*
|
||||
* @min 0
|
||||
* @max 2
|
||||
* @group Camera trigger
|
||||
*/
|
||||
PARAM_DEFINE_INT32(TRIG_MODE, 0);
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user