diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index c21f80edfc..e6d95e59f7 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -537,7 +537,7 @@ then # Get FMU driver out of the way set MIXER_AUX none set AUX_MODE none - camera_trigger start --pwm + camera_trigger start fi fi @@ -606,7 +606,7 @@ then # Try to get an USB console nshterm /dev/ttyACM0 & else - mavlink start -r 80000 -d /dev/ttyACM0 -m config -x + mavlink start -r 800000 -d /dev/ttyACM0 -m config -x fi # diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp index 5b1a739620..8bb18e3466 100644 --- a/src/drivers/camera_trigger/camera_trigger.cpp +++ b/src/drivers/camera_trigger/camera_trigger.cpp @@ -36,7 +36,11 @@ * * External camera-IMU synchronisation and triggering via FMU auxiliary pins. * + * Support for camera manipulation via PWM signal over servo pins. + * * @author Mohammed Kabir + * @author Kelly Steich + * @author Andreas Bircher */ #include @@ -63,10 +67,19 @@ #include #include +#include "interfaces/src/pwm.h" +#include "interfaces/src/relay.h" + #define TRIGGER_PIN_DEFAULT 1 extern "C" __EXPORT int camera_trigger_main(int argc, char *argv[]); +typedef enum { + CAMERA_INTERFACE_MODE_NONE = 0, + CAMERA_INTERFACE_MODE_RELAY, + CAMERA_INTERFACE_MODE_PWM +} camera_interface_mode_t; + class CameraTrigger { public: @@ -105,8 +118,6 @@ public: */ void info(); - int _pins[6]; - private: struct hrt_call _engagecall; @@ -115,7 +126,6 @@ private: int _gpio_fd; - int _polarity; int _mode; float _activation_time; float _interval; @@ -130,7 +140,6 @@ private: orb_advert_t _trigger_pub; - param_t _p_polarity; param_t _p_mode; param_t _p_activation_time; param_t _p_interval; @@ -153,12 +162,9 @@ private: */ static void disengage(void *arg); - static void trigger(CameraTrigger *trig, bool trigger); - }; struct work_s CameraTrigger::_work; -constexpr uint32_t CameraTrigger::_gpios[6]; namespace camera_trigger { @@ -170,7 +176,6 @@ CameraTrigger::CameraTrigger(camera_interface_mode_t camera_interface_mode) : _engagecall {}, _disengagecall {}, _gpio_fd(-1), - _polarity(0), _mode(0), _activation_time(0.5f /* ms */), _interval(100.0f /* ms */), @@ -185,44 +190,38 @@ CameraTrigger::CameraTrigger(camera_interface_mode_t camera_interface_mode) : _camera_interface_mode(camera_interface_mode), _camera_interface(nullptr) { + //Initiate Camera interface basedon camera_interface_mode + if (_camera_interface != nullptr) { + delete(_camera_interface); + /* set to zero to ensure parser is not used while not instantiated */ + _camera_interface = nullptr; + } + + switch (_camera_interface_mode) { + case CAMERA_INTERFACE_MODE_RELAY: + _camera_interface = new CameraInterfaceRelay; + break; + + case CAMERA_INTERFACE_MODE_PWM: + _camera_interface = new CameraInterfacePWM; + break; + + default: + break; + } + memset(&_work, 0, sizeof(_work)); // Parameters - _p_polarity = param_find("TRIG_POLARITY"); _p_interval = param_find("TRIG_INTERVAL"); _p_distance = param_find("TRIG_DISTANCE"); _p_activation_time = param_find("TRIG_ACT_TIME"); _p_mode = param_find("TRIG_MODE"); - _p_pin = param_find("TRIG_PINS"); - param_get(_p_polarity, &_polarity); param_get(_p_activation_time, &_activation_time); param_get(_p_interval, &_interval); param_get(_p_distance, &_distance); param_get(_p_mode, &_mode); - int pin_list; - param_get(_p_pin, &pin_list); - - // 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++; - } struct camera_trigger_s camera_trigger = {}; @@ -278,12 +277,6 @@ CameraTrigger::shootOnce() void CameraTrigger::start() { - - for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) { - px4_arch_configgpio(_gpios[_pins[i]]); - px4_arch_gpiowrite(_gpios[_pins[i]], !_polarity); - } - // enable immediate if configured that way if (_mode == 2) { control(true); @@ -438,23 +431,10 @@ CameraTrigger::disengage(void *arg) trig->_camera_interface->trigger(false); } -void -CameraTrigger::trigger(CameraTrigger *trig, bool trigger) -{ - for (unsigned i = 0; i < sizeof(trig->_pins) / sizeof(trig->_pins[0]); i++) { - if (trig->_pins[i] >= 0) { - // ACTIVE_LOW == 1 - px4_arch_gpiowrite(trig->_gpios[trig->_pins[i]], trigger); - } - } -} - void CameraTrigger::info() { warnx("state : %s", _trigger_enabled ? "enabled" : "disabled"); - warnx("pins 1-3 : %d,%d,%d polarity : %s", _pins[0], _pins[1], _pins[2], - _polarity ? "ACTIVE_HIGH" : "ACTIVE_LOW"); warnx("mode : %i", _mode); warnx("interval : %.2f [ms]", (double)_interval); warnx("distance : %.2f [m]", (double)_distance); diff --git a/src/drivers/camera_trigger/interfaces/src/camera_interface.cpp b/src/drivers/camera_trigger/interfaces/src/camera_interface.cpp index e69de29bb2..d464296dda 100644 --- a/src/drivers/camera_trigger/interfaces/src/camera_interface.cpp +++ b/src/drivers/camera_trigger/interfaces/src/camera_interface.cpp @@ -0,0 +1,14 @@ +#include "camera_interface.h" + +/** + * @file camera_interface.cpp + * + */ + +CameraInterface::CameraInterface() +{ +} + +CameraInterface::~CameraInterface() +{ +} diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.cpp b/src/drivers/camera_trigger/interfaces/src/pwm.cpp index c42faf79da..502f57713c 100644 --- a/src/drivers/camera_trigger/interfaces/src/pwm.cpp +++ b/src/drivers/camera_trigger/interfaces/src/pwm.cpp @@ -1,7 +1,8 @@ #include #include -#include +#include +#include "drivers/drv_pwm_output.h" #include "pwm.h" // PWM levels of the interface to seagull MAP converter to @@ -41,6 +42,7 @@ CameraInterfacePWM::CameraInterfacePWM(): pin_list /= 10; i++; } + setup(); } @@ -118,7 +120,7 @@ int CameraInterfacePWM::powerOff() return 0; } -void CameraInterfaceRelay::info() +void CameraInterfacePWM::info() { warnx("PWM - camera triggering, pins 1-3 : %d,%d,%d", _pins[0], _pins[1], _pins[2]); } diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.h b/src/drivers/camera_trigger/interfaces/src/pwm.h index 43182e2a9f..7310be8b9e 100644 --- a/src/drivers/camera_trigger/interfaces/src/pwm.h +++ b/src/drivers/camera_trigger/interfaces/src/pwm.h @@ -11,20 +11,6 @@ #include #include "camera_interface.h" -// TODO(birchera): check if this is the right device and addresses -#define PWM_DEVICE_PATH "/dev/pwm_output0" -#define PWM_CAMERA_BASE 0x2a00 -#define PWM_CAMERA_SET(_pin) _PX4_IOC(PWM_CAMERA_BASE, 0x30 + _pin) -// PWM levels of the interface to seagull MAP converter to -// Multiport (http://www.seagulluav.com/manuals/Seagull_MAP2-Manual.pdf) -#define PWM_CAMERA_DISARMED 90 // TODO(birchera): check here value -#define PWM_CAMERA_ON 1100 -#define PWM_CAMERA_AUTOFOCUS_SHOOT 1300 -#define PWM_CAMERA_NEUTRAL 1500 -#define PWM_CAMERA_INSTANT_SHOOT 1700 -#define PWM_CAMERA_OFF 1900 - - class CameraInterfacePWM : public CameraInterface { public: diff --git a/src/drivers/camera_trigger/interfaces/src/relay.cpp b/src/drivers/camera_trigger/interfaces/src/relay.cpp index 0922777f64..d67903bd29 100644 --- a/src/drivers/camera_trigger/interfaces/src/relay.cpp +++ b/src/drivers/camera_trigger/interfaces/src/relay.cpp @@ -45,8 +45,8 @@ 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); + px4_arch_configgpio(_gpios[_pins[i]]); + px4_arch_gpiowrite(_gpios[_pins[i]], !_polarity); } } @@ -56,7 +56,7 @@ void CameraInterfaceRelay::trigger(bool 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); + px4_arch_gpiowrite(_gpios[_pins[i]], _polarity); } } @@ -64,7 +64,7 @@ void CameraInterfaceRelay::trigger(bool 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); + px4_arch_gpiowrite(_gpios[_pins[i]], !_polarity); } } }