From 06a4e9c58593ecc311c70d623d7e6d9877f5caaf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 7 Aug 2015 14:35:56 +0200 Subject: [PATCH] Rework trigger to operate on work queue and timers without jitter --- src/drivers/camera_trigger/camera_trigger.cpp | 225 +++++++++--------- .../camera_trigger/camera_trigger_params.c | 24 +- 2 files changed, 132 insertions(+), 117 deletions(-) diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp index 1c7a8f556c..30d9f88ba6 100644 --- a/src/drivers/camera_trigger/camera_trigger.cpp +++ b/src/drivers/camera_trigger/camera_trigger.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * * Copyright (c) 2015 PX4 Development Team. All rights reserved. - * Author: Mohammed Kabir * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -47,6 +46,7 @@ #include #include #include +#include #include #include #include @@ -60,6 +60,8 @@ #include #include +#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() @@ -191,39 +184,53 @@ CameraTrigger::~CameraTrigger() } void -CameraTrigger::start() +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); - _sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - _vcommand_sub = orb_subscribe(ORB_ID(vehicle_command)); + // 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() +{ 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(arg); + if (trig->_vcommand_sub < 0) { + trig->_vcommand_sub = orb_subscribe(ORB_ID(vehicle_command)); + } + bool updated; orb_check(trig->_vcommand_sub, &updated); - if (updated) { + // while the trigger is inactive it has to be ready + // to become active instantaneously + int poll_interval_usec = 5000; - orb_copy(ORB_ID(vehicle_command), trig->_vcommand_sub, &trig->_command); + if (updated) { - 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 ; - } + struct vehicle_command_s cmd; - } 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 (!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 (cmd.param1 < 1.0f) { + trig->control(false); - 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); + } 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; + } } - - 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(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(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" - "\t-p \tUse specified AUX OUT pin number (default: 1)" + "\t-p \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(); } diff --git a/src/drivers/camera_trigger/camera_trigger_params.c b/src/drivers/camera_trigger/camera_trigger_params.c index a9f4b96061..177844ff59 100644 --- a/src/drivers/camera_trigger/camera_trigger_params.c +++ b/src/drivers/camera_trigger/camera_trigger_params.c @@ -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);