diff --git a/src/drivers/camera_trigger/interfaces/src/camera_interface.cpp b/src/drivers/camera_trigger/interfaces/src/camera_interface.cpp index d464296dda..6bed3d8dd0 100644 --- a/src/drivers/camera_trigger/interfaces/src/camera_interface.cpp +++ b/src/drivers/camera_trigger/interfaces/src/camera_interface.cpp @@ -5,10 +5,43 @@ * */ -CameraInterface::CameraInterface() +CameraInterface::CameraInterface(): + _pins{} { } CameraInterface::~CameraInterface() { } + +void CameraInterface::get_pins() +{ + + // Get parameter handle + _p_pin = param_find("TRIG_PINS"); + + int pin_list; + param_get(_p_pin, &pin_list); + + // Set all pins as invalid + for (unsigned i = 0; i < arraySize(_pins); 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] = -1; + } + + pin_list /= 10; + i++; + } + +} \ No newline at end of file diff --git a/src/drivers/camera_trigger/interfaces/src/camera_interface.h b/src/drivers/camera_trigger/interfaces/src/camera_interface.h index 8879c353ee..957a343812 100644 --- a/src/drivers/camera_trigger/interfaces/src/camera_interface.h +++ b/src/drivers/camera_trigger/interfaces/src/camera_interface.h @@ -4,6 +4,9 @@ #pragma once +#include +#include + #define arraySize(a) (sizeof((a))/sizeof(((a)[0]))) class CameraInterface @@ -55,7 +58,6 @@ public: */ virtual int powerOff() { return -1; } - protected: /** @@ -63,4 +65,13 @@ protected: */ virtual void setup() {}; + /** + * get the hardware configuration + */ + void get_pins(); + + param_t _p_pin; + + int _pins[6]; + }; diff --git a/src/drivers/camera_trigger/interfaces/src/gpio.cpp b/src/drivers/camera_trigger/interfaces/src/gpio.cpp index 642dafdfbc..060f10f4a6 100644 --- a/src/drivers/camera_trigger/interfaces/src/gpio.cpp +++ b/src/drivers/camera_trigger/interfaces/src/gpio.cpp @@ -1,41 +1,17 @@ #ifdef __PX4_NUTTX + #include "gpio.h" constexpr uint32_t CameraInterfaceGPIO::_gpios[6]; CameraInterfaceGPIO::CameraInterfaceGPIO(): 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 < arraySize(_pins); 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(arraySize(_gpios))) { - _pins[i] = -1; - } - - pin_list /= 10; - i++; - } - + get_pins(); setup(); } diff --git a/src/drivers/camera_trigger/interfaces/src/gpio.h b/src/drivers/camera_trigger/interfaces/src/gpio.h index ccafcef681..eb2cc3a32e 100644 --- a/src/drivers/camera_trigger/interfaces/src/gpio.h +++ b/src/drivers/camera_trigger/interfaces/src/gpio.h @@ -8,10 +8,7 @@ #ifdef __PX4_NUTTX -#include -#include #include -#include #include "camera_interface.h" @@ -25,16 +22,14 @@ public: void info(); - int _pins[6]; - int _polarity; - private: void setup(); - param_t _p_pin; param_t _p_polarity; + int _polarity; + static constexpr uint32_t _gpios[6] = { GPIO_GPIO0_OUTPUT, GPIO_GPIO1_OUTPUT, diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.cpp b/src/drivers/camera_trigger/interfaces/src/pwm.cpp index 30a629eeba..7d3a2ba2ff 100644 --- a/src/drivers/camera_trigger/interfaces/src/pwm.cpp +++ b/src/drivers/camera_trigger/interfaces/src/pwm.cpp @@ -13,31 +13,7 @@ CameraInterfacePWM::CameraInterfacePWM(): CameraInterface() { - _p_pin = param_find("TRIG_PINS"); - int pin_list; - param_get(_p_pin, &pin_list); - - // Set all pins as invalid - for (unsigned i = 0; i < arraySize(_pins); 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] = -1; - } - - pin_list /= 10; - i++; - } - + get_pins(); setup(); } diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.h b/src/drivers/camera_trigger/interfaces/src/pwm.h index 3ad24d701f..b54657d9aa 100644 --- a/src/drivers/camera_trigger/interfaces/src/pwm.h +++ b/src/drivers/camera_trigger/interfaces/src/pwm.h @@ -24,11 +24,9 @@ public: void info(); - int _pins[6]; private: - void setup(); - param_t _p_pin; + void setup(); }; diff --git a/src/drivers/camera_trigger/interfaces/src/seagull_map2.cpp b/src/drivers/camera_trigger/interfaces/src/seagull_map2.cpp index 0341e23dff..35c03deb6e 100644 --- a/src/drivers/camera_trigger/interfaces/src/seagull_map2.cpp +++ b/src/drivers/camera_trigger/interfaces/src/seagull_map2.cpp @@ -21,31 +21,7 @@ CameraInterfaceSeagull::CameraInterfaceSeagull(): CameraInterface(), _camera_is_on(false) { - _p_pin = param_find("TRIG_PINS"); - int pin_list; - param_get(_p_pin, &pin_list); - - // Set all pins as invalid - for (unsigned i = 0; i < arraySize(_pins); 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] = -1; - } - - pin_list /= 10; - i++; - } - + get_pins(); setup(); } @@ -65,9 +41,11 @@ void CameraInterfaceSeagull::setup() up_pwm_trigger_set(_pins[i], math::constrain(PWM_CAMERA_DISARMED, PWM_CAMERA_DISARMED, 2000)); // We only support 2 consecutive pins while using the Seagull MAP2 - break; + return; } } + + PX4_ERROR("Bad pin configuration - Seagull MAP2 requires 2 consecutive pins for control."); } void CameraInterfaceSeagull::trigger(bool enable) diff --git a/src/drivers/camera_trigger/interfaces/src/seagull_map2.h b/src/drivers/camera_trigger/interfaces/src/seagull_map2.h index e409998a7e..49b809aa18 100644 --- a/src/drivers/camera_trigger/interfaces/src/seagull_map2.h +++ b/src/drivers/camera_trigger/interfaces/src/seagull_map2.h @@ -9,10 +9,7 @@ #ifdef __PX4_NUTTX #include -#include -#include -#include #include "camera_interface.h" class CameraInterfaceSeagull : public CameraInterface @@ -28,11 +25,9 @@ public: void info(); - int _pins[6]; private: void setup(); - param_t _p_pin; bool _camera_is_on; };