fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 00:53:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Rework trigger to operate on work queue and timers without jitter

Browse Source
This commit is contained in:
Lorenz Meier
2015-08-08 11:34:00 +02:00
parent e0f203e3da
commit 06a4e9c585
2 changed files with 134 additions and 119 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/camera_trigger/camera_trigger.cpp
+117 -112
View File
@@ -1,7 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2015 PX4 Development Team. All rights reserved. * 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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -47,6 +46,7 @@
#include <stdbool.h> #include <stdbool.h>
#include <nuttx/clock.h> #include <nuttx/clock.h>
#include <nuttx/arch.h> #include <nuttx/arch.h>
#include <nuttx/wqueue.h>
#include <systemlib/systemlib.h> #include <systemlib/systemlib.h>
#include <systemlib/err.h> #include <systemlib/err.h>
#include <systemlib/param/param.h> #include <systemlib/param/param.h>
@@ -60,6 +60,8 @@
#include <mavlink/mavlink_log.h> #include <mavlink/mavlink_log.h>
#include <board_config.h> #include <board_config.h>
#define TRIGGER_PIN_DEFAULT 1
extern "C" __EXPORT int camera_trigger_main(int argc, char *argv[]); extern "C" __EXPORT int camera_trigger_main(int argc, char *argv[]);
class CameraTrigger class CameraTrigger
@@ -75,6 +77,11 @@ public:
*/ */
~CameraTrigger(); ~CameraTrigger();
/**
* Set the trigger on / off
*/
void control(bool on);
/** /**
* Start the task. * Start the task.
*/ */
@@ -90,37 +97,32 @@ public:
*/ */
void info(); void info();
int pin; int _pin;
private: private:
struct hrt_call _pollcall; struct hrt_call _engagecall;
struct hrt_call _firecall; struct hrt_call _disengagecall;
static struct work_s _work;
int _gpio_fd; int _gpio_fd;
int _polarity; int _polarity;
float _activation_time; float _activation_time;
float _integration_time; float _interval;
float _transfer_time;
uint32_t _trigger_seq; uint32_t _trigger_seq;
bool _trigger_enabled; bool _trigger_enabled;
int _sensor_sub;
int _vcommand_sub; int _vcommand_sub;
orb_advert_t _trigger_pub; orb_advert_t _trigger_pub;
struct camera_trigger_s _trigger; param_t polarity;
struct sensor_combined_s _sensor; param_t activation_time;
struct vehicle_command_s _command; param_t interval;
param_t polarity ;
param_t activation_time ;
param_t integration_time ;
param_t transfer_time ; param_t transfer_time ;
int32_t _gpios[6] = { static constexpr uint32_t _gpios[6] = {
GPIO_GPIO0_OUTPUT, GPIO_GPIO0_OUTPUT,
GPIO_GPIO1_OUTPUT, GPIO_GPIO1_OUTPUT,
GPIO_GPIO2_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 * Fires trigger
*/ */
@@ -144,6 +146,9 @@ private:
}; };
struct work_s CameraTrigger::_work;
constexpr uint32_t CameraTrigger::_gpios[6];
namespace camera_trigger namespace camera_trigger
{ {
@@ -151,38 +156,26 @@ CameraTrigger *g_camera_trigger;
} }
CameraTrigger::CameraTrigger() : CameraTrigger::CameraTrigger() :
pin(1), _pin(TRIGGER_PIN_DEFAULT),
_pollcall {}, _engagecall {},
_firecall {}, _disengagecall {},
_gpio_fd(-1), _gpio_fd(-1),
_polarity(0), _polarity(0),
_activation_time(0.0f), _activation_time(0.5f /* ms */),
_integration_time(0.0f), _interval(100.0f /* ms */),
_transfer_time(0.0f), _trigger_seq(0),
_trigger_seq(0), _trigger_enabled(false),
_trigger_enabled(false), _vcommand_sub(-1),
_sensor_sub(-1), _trigger_pub(nullptr)
_vcommand_sub(-1), {
_trigger_pub(nullptr), memset(&_work, 0, sizeof(_work));
_trigger {}, memset(&_engagecall, 0, sizeof(_engagecall));
_sensor {}, memset(&_disengagecall, 0, sizeof(_disengagecall));
_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));
// Parameters // Parameters
polarity = param_find("TRIG_POLARITY"); polarity = param_find("TRIG_POLARITY");
interval = param_find("TRIG_INTERVAL");
activation_time = param_find("TRIG_ACT_TIME"); activation_time = param_find("TRIG_ACT_TIME");
integration_time = param_find("TRIG_INT_TIME");
transfer_time = param_find("TRIG_TRANS_TIME");
} }
CameraTrigger::~CameraTrigger() CameraTrigger::~CameraTrigger()
@@ -190,40 +183,54 @@ CameraTrigger::~CameraTrigger()
camera_trigger::g_camera_trigger = nullptr; 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 void
CameraTrigger::start() CameraTrigger::start()
{ {
_sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
_vcommand_sub = orb_subscribe(ORB_ID(vehicle_command));
param_get(polarity, &_polarity); param_get(polarity, &_polarity);
param_get(activation_time, &_activation_time); param_get(activation_time, &_activation_time);
param_get(integration_time, &_integration_time); param_get(interval, &_interval);
param_get(transfer_time, &_transfer_time);
stm32_configgpio(_gpios[pin - 1]); stm32_configgpio(_gpios[_pin - 1]);
if (_polarity == 0) { 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) { } 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 { } else {
warnx(" invalid trigger polarity setting. stopping."); warnx(" invalid trigger polarity setting. stopping.");
stop(); 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 void
CameraTrigger::stop() CameraTrigger::stop()
{ {
hrt_cancel(&_firecall); work_cancel(LPWORK, &_work);
hrt_cancel(&_pollcall); hrt_cancel(&_engagecall);
hrt_cancel(&_disengagecall);
if (camera_trigger::g_camera_trigger != nullptr) { if (camera_trigger::g_camera_trigger != nullptr) {
delete(camera_trigger::g_camera_trigger); delete(camera_trigger::g_camera_trigger);
@@ -231,62 +238,49 @@ CameraTrigger::stop()
} }
void void
CameraTrigger::poll(void *arg) CameraTrigger::cycle_trampoline(void *arg)
{ {
CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg); CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);
if (trig->_vcommand_sub < 0) {
trig->_vcommand_sub = orb_subscribe(ORB_ID(vehicle_command));
}
bool updated; bool updated;
orb_check(trig->_vcommand_sub, &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) { 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) { orb_copy(ORB_ID(vehicle_command), trig->_vcommand_sub, &cmd);
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 ;
}
}
if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) {
// Set trigger rate from command // Set trigger rate from command
if (trig->_command.param2 > 0) { if (cmd.param2 > 0) {
trig->_integration_time = trig->_command.param2; trig->_interval = cmd.param2;
param_set(trig->integration_time, &(trig->_integration_time)); 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) { work_queue(LPWORK, &_work, (worker_t)&CameraTrigger::cycle_trampoline,
hrt_call_after(&trig->_pollcall, 1e6, (hrt_callout)&CameraTrigger::poll, trig); camera_trigger::g_camera_trigger, USEC2TICK(poll_interval_usec));
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);
}
} }
void void
@@ -295,16 +289,23 @@ CameraTrigger::engage(void *arg)
CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(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 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 } 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 void
@@ -313,13 +314,13 @@ CameraTrigger::disengage(void *arg)
CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(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 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 } 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 void
CameraTrigger::info() CameraTrigger::info()
{ {
warnx("Trigger state : %s", _trigger_enabled ? "enabled" : "disabled"); warnx("state : %s", _trigger_enabled ? "enabled" : "disabled");
warnx("Trigger pin : %i", pin); warnx("pin : %i, polarity : %s", _pin, _polarity ? "ACTIVE_HIGH" : "ACTIVE_LOW");
warnx("Trigger polarity : %s", _polarity ? "ACTIVE_HIGH" : "ACTIVE_LOW"); warnx("interval : %.2f", (double)_interval);
warnx("Shutter integration time : %.2f", (double)_integration_time);
} }
static void usage() static void usage()
{ {
errx(1, "usage: camera_trigger {start|stop|info} [-p <n>]\n" 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) { 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) { if (atoi(argv[3]) > 0 && atoi(argv[3]) < 6) {
warnx("starting trigger on pin : %li ", atoi(argv[3])); 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 { } else {
usage(); usage();
@@ -378,14 +378,19 @@ int camera_trigger_main(int argc, char *argv[])
if (camera_trigger::g_camera_trigger == nullptr) { if (camera_trigger::g_camera_trigger == nullptr) {
errx(1, "not running"); errx(1, "not running");
}
else if (!strcmp(argv[1], "stop")) { } else if (!strcmp(argv[1], "stop")) {
camera_trigger::g_camera_trigger->stop(); camera_trigger::g_camera_trigger->stop();
} else if (!strcmp(argv[1], "info")) { } else if (!strcmp(argv[1], "info")) {
camera_trigger::g_camera_trigger->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 { } else {
usage(); usage();
} }
src/drivers/camera_trigger/camera_trigger_params.c
+17 -7
View File
@@ -51,19 +51,19 @@
* @max 500.0 * @max 500.0
* @group Camera trigger * @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 * @unit milliseconds
* @min 15.0 * @min 15.0
* @max 33.0 * @max 33.0
* @group Camera trigger * @group Camera trigger
*/ */
PARAM_DEFINE_FLOAT(TRIG_TRANS_TIME, 15.0f); PARAM_DEFINE_FLOAT(TRIG_INTERVAL, 15.0f);
/** /**
* Camera trigger polarity * Camera trigger polarity
@@ -79,10 +79,20 @@ PARAM_DEFINE_INT32(TRIG_POLARITY, 0);
/** /**
* Camera trigger activation time * Camera trigger activation time
* *
* This parameter sets the time the trigger needs to pulled high or low to start light * This parameter sets the time the trigger needs to pulled high or low.
* integration.
* *
* @unit milliseconds * @unit milliseconds
* @group Camera trigger * @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);