From 29f31ae6ac8b625422d93873e11e490d0041b5b5 Mon Sep 17 00:00:00 2001 From: Kelly Steich Date: Fri, 10 Jun 2016 11:23:09 +0200 Subject: [PATCH] fixed the triggering function logic Conflicts: PX4/src/drivers/camera_trigger/camera_trigger.cpp PX4/src/drivers/camera_trigger/interfaces/src/camera_interface.h PX4/src/drivers/camera_trigger/interfaces/src/pwm.cpp PX4/src/drivers/camera_trigger/interfaces/src/pwm.h PX4/src/drivers/camera_trigger/interfaces/src/relay.cpp PX4/src/drivers/camera_trigger/interfaces/src/relay.h --- src/drivers/camera_trigger/camera_trigger.cpp | 10 +++ .../interfaces/src/camera_interface.h | 54 +++++++++++++ .../camera_trigger/interfaces/src/pwm.cpp | 30 ++++++++ .../camera_trigger/interfaces/src/pwm.h | 27 +++++++ .../camera_trigger/interfaces/src/relay.cpp | 75 +++++++++++++++++++ .../camera_trigger/interfaces/src/relay.h | 44 +++++++++++ 6 files changed, 240 insertions(+) diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp index fff4acd3da..195d431c90 100644 --- a/src/drivers/camera_trigger/camera_trigger.cpp +++ b/src/drivers/camera_trigger/camera_trigger.cpp @@ -426,7 +426,11 @@ CameraTrigger::engage(void *arg) /* set timestamp the instant before the trigger goes off */ report.timestamp = hrt_absolute_time(); +<<<<<<< HEAD CameraTrigger::trigger(trig, trig->_polarity); +======= + trig->_camera_interface->trigger(true); +>>>>>>> fb669fc... fixed the triggering function logic report.seq = trig->_trigger_seq++; @@ -436,10 +440,16 @@ CameraTrigger::engage(void *arg) void CameraTrigger::disengage(void *arg) { +<<<<<<< HEAD CameraTrigger *trig = reinterpret_cast(arg); CameraTrigger::trigger(trig, !(trig->_polarity)); +======= + CameraTrigger *trig = reinterpret_cast(arg); + + trig->_camera_interface->trigger(false); +>>>>>>> fb669fc... fixed the triggering function logic } void diff --git a/src/drivers/camera_trigger/interfaces/src/camera_interface.h b/src/drivers/camera_trigger/interfaces/src/camera_interface.h index e69de29bb2..5554fc3d6c 100644 --- a/src/drivers/camera_trigger/interfaces/src/camera_interface.h +++ b/src/drivers/camera_trigger/interfaces/src/camera_interface.h @@ -0,0 +1,54 @@ +/** + * @file camera_interface.h + */ + +#pragma once + +class CameraInterface +{ +public: + + /** + * Constructor + */ + CameraInterface(); + + /** + * Destructor. + */ + virtual ~CameraInterface(); + + /** + * setup the interface + */ + virtual void setup() {}; + + /** + * trigger the camera + * @param trigger: + */ + virtual void trigger(bool enable) {}; + + + /** + * Display info. + */ + virtual void info() {}; + + /** + * Power on the camera + * @return 0 on success, <0 on error + */ + virtual int powerOn() { return -1; } + + /** + * Power off the camera + * @return 0 on success, <0 on error + */ + virtual int powerOff() { return -1; } + + +protected: + + +}; diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.cpp b/src/drivers/camera_trigger/interfaces/src/pwm.cpp index e69de29bb2..67eedaf471 100644 --- a/src/drivers/camera_trigger/interfaces/src/pwm.cpp +++ b/src/drivers/camera_trigger/interfaces/src/pwm.cpp @@ -0,0 +1,30 @@ +#include "pwm.h" + +CameraInterfacePWM::CameraInterfacePWM(): + CameraInterface() +{ +} + +CameraInterfacePWM::~CameraInterfacePWM() +{ +} + +void CameraInterfacePWM::setup() +{ + +} + +void CameraInterfacePWM::trigger(bool enable) +{ + +} + +int CameraInterfacePWM::powerOn() +{ + return 0; +} + +int CameraInterfacePWM::powerOff() +{ + return 0; +} diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.h b/src/drivers/camera_trigger/interfaces/src/pwm.h index e69de29bb2..7632feb119 100644 --- a/src/drivers/camera_trigger/interfaces/src/pwm.h +++ b/src/drivers/camera_trigger/interfaces/src/pwm.h @@ -0,0 +1,27 @@ +/** + * @file pwm.h + * + * Interface with cameras via pwm. + * + */ +#pragma once + +#include "camera_interface.h" + + +class CameraInterfacePWM : public CameraInterface +{ +public: + CameraInterfacePWM(); + virtual ~CameraInterfacePWM(); + + void setup(); + + void trigger(bool enable); + + int powerOn(); + int powerOff(); + +private: + +}; diff --git a/src/drivers/camera_trigger/interfaces/src/relay.cpp b/src/drivers/camera_trigger/interfaces/src/relay.cpp index e69de29bb2..c07215bba5 100644 --- a/src/drivers/camera_trigger/interfaces/src/relay.cpp +++ b/src/drivers/camera_trigger/interfaces/src/relay.cpp @@ -0,0 +1,75 @@ +#include "relay.h" + +constexpr uint32_t CameraInterfaceRelay::_gpios[6]; + +CameraInterfaceRelay::CameraInterfaceRelay(): + CameraInterface(), + _pins{}, + _polarity(0) +{ + _p_pin = param_find("TRIG_PINS"); + _p_polarity = param_find("TRIG_POLARITY"); + + int pin_list; + param_get(_p_pin, &pin_list); + param_get(_p_polarity, &_polarity); + + // Set all pins as invalid + for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) { + _pins[i] = -1; + } + + // Convert number to individual channels + unsigned i = 0; + int single_pin; + + while ((single_pin = pin_list % 10)) { + + _pins[i] = single_pin - 1; + + if (_pins[i] < 0 || _pins[i] >= static_cast(sizeof(_gpios) / sizeof(_gpios[0]))) { + _pins[i] = -1; + } + + pin_list /= 10; + i++; + } +} + +CameraInterfaceRelay::~CameraInterfaceRelay() +{ +} + +void CameraInterfaceRelay::setup() +{ + for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) { + stm32_configgpio(_gpios[_pins[i]]); + stm32_gpiowrite(_gpios[_pins[i]], !_polarity); + } +} + +void CameraInterfaceRelay::trigger(bool enable) +{ + if (enable) { + for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) { + if (_pins[i] >= 0) { + // ACTIVE_LOW == 1 + stm32_gpiowrite(_gpios[_pins[i]], _polarity); + } + } + + } else { + for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) { + if (_pins[i] >= 0) { + // ACTIVE_LOW == 1 + stm32_gpiowrite(_gpios[_pins[i]], !_polarity); + } + } + } +} + +void CameraInterfaceRelay::info() +{ + warnx("pins 1-3 : %d,%d,%d polarity : %s", _pins[0], _pins[1], _pins[2], + _polarity ? "ACTIVE_HIGH" : "ACTIVE_LOW"); +} diff --git a/src/drivers/camera_trigger/interfaces/src/relay.h b/src/drivers/camera_trigger/interfaces/src/relay.h index e69de29bb2..371ae9511f 100644 --- a/src/drivers/camera_trigger/interfaces/src/relay.h +++ b/src/drivers/camera_trigger/interfaces/src/relay.h @@ -0,0 +1,44 @@ +/** + * @file relay.h + * + * Interface with cameras via FMU auxiliary pins. + * + */ +#pragma once + +#include +#include +#include + +#include "camera_interface.h" + + +class CameraInterfaceRelay : public CameraInterface +{ +public: + CameraInterfaceRelay(); + virtual ~CameraInterfaceRelay(); + + void setup(); + + void trigger(bool enable); + + void info(); + + int _pins[6]; + int _polarity; + +private: + param_t _p_pin; + param_t _p_polarity; + + static constexpr uint32_t _gpios[6] = { + GPIO_GPIO0_OUTPUT, + GPIO_GPIO1_OUTPUT, + GPIO_GPIO2_OUTPUT, + GPIO_GPIO3_OUTPUT, + GPIO_GPIO4_OUTPUT, + GPIO_GPIO5_OUTPUT + }; + +};