From dd2edd9453f255b6edf57dde3b795586c1f27476 Mon Sep 17 00:00:00 2001 From: DanielePettenuzzo Date: Thu, 15 Nov 2018 18:18:05 +0100 Subject: [PATCH] camera trigger: add support for fmu pins 7 and 8 --- ROMFS/px4fmu_common/init.d/rcS | 18 ++++++++++++------ .../interfaces/src/camera_interface.h | 3 ++- .../camera_trigger/interfaces/src/gpio.cpp | 10 +++++++++- .../camera_trigger/interfaces/src/gpio.h | 8 ++++++-- .../camera_trigger/interfaces/src/pwm.cpp | 6 ++++++ .../interfaces/src/seagull_map2.cpp | 5 +++++ 6 files changed, 40 insertions(+), 10 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index cd59fe865ee0..e895088cfcf6 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -462,15 +462,21 @@ else if param greater TRIG_MODE 0 then - # We ONLY support trigger on pins 5 and 6 when simultanously using AUX for actuator output. - if param compare TRIG_PINS 56 - then - # clear pins 5 and 6 + # We ONLY support trigger on pins 5, 6, 7 and 8 when simultanously using AUX for actuator output. + if param compare TRIG_PINS 78 + # clear pins 7 and 8 set FMU_MODE pwm4 set AUX_MODE pwm4 else - set FMU_MODE none - set AUX_MODE none + if param compare TRIG_PINS 56 + then + # clear pins 5, 6 + set FMU_MODE pwm4 + set AUX_MODE pwm4 + else + set FMU_MODE none + set AUX_MODE none + fi fi camera_trigger start diff --git a/src/drivers/camera_trigger/interfaces/src/camera_interface.h b/src/drivers/camera_trigger/interfaces/src/camera_interface.h index d95f2a195941..aa6f0a714c04 100644 --- a/src/drivers/camera_trigger/interfaces/src/camera_interface.h +++ b/src/drivers/camera_trigger/interfaces/src/camera_interface.h @@ -6,6 +6,7 @@ #include #include +#include #define arraySize(a) (sizeof((a))/sizeof(((a)[0]))) @@ -74,6 +75,6 @@ class CameraInterface param_t _p_pin; - int _pins[6]; + int _pins[DIRECT_PWM_OUTPUT_CHANNELS]; }; diff --git a/src/drivers/camera_trigger/interfaces/src/gpio.cpp b/src/drivers/camera_trigger/interfaces/src/gpio.cpp index 8104fc189b9e..98dbb3e3641f 100644 --- a/src/drivers/camera_trigger/interfaces/src/gpio.cpp +++ b/src/drivers/camera_trigger/interfaces/src/gpio.cpp @@ -3,7 +3,7 @@ #include "gpio.h" #include -constexpr uint32_t CameraInterfaceGPIO::_gpios[6]; +constexpr uint32_t CameraInterfaceGPIO::_gpios[DIRECT_PWM_OUTPUT_CHANNELS]; CameraInterfaceGPIO::CameraInterfaceGPIO(): CameraInterface(), @@ -50,9 +50,17 @@ void CameraInterfaceGPIO::trigger(bool trigger_on_true) void CameraInterfaceGPIO::info() { + +#if defined(DIRECT_PWM_OUTPUT_CHANNELS) && DIRECT_PWM_OUTPUT_CHANNELS <= 6 PX4_INFO("GPIO trigger mode, pins enabled : [%d][%d][%d][%d][%d][%d], polarity : %s", _pins[5], _pins[4], _pins[3], _pins[2], _pins[1], _pins[0], _trigger_invert ? "ACTIVE_LOW" : "ACTIVE_HIGH"); +#else + PX4_INFO("GPIO trigger mode, pins enabled : [%d][%d][%d][%d][%d][%d][%d][%d], polarity : %s", + _pins[7], _pins[6], _pins[5], _pins[4], _pins[3], _pins[2], _pins[1], _pins[0], + _trigger_invert ? "ACTIVE_LOW" : "ACTIVE_HIGH"); +#endif + } #endif /* ifdef __PX4_NUTTX */ diff --git a/src/drivers/camera_trigger/interfaces/src/gpio.h b/src/drivers/camera_trigger/interfaces/src/gpio.h index 38150d1409ff..8c6f576b20d4 100644 --- a/src/drivers/camera_trigger/interfaces/src/gpio.h +++ b/src/drivers/camera_trigger/interfaces/src/gpio.h @@ -30,13 +30,17 @@ class CameraInterfaceGPIO : public CameraInterface bool _trigger_invert; - static constexpr uint32_t _gpios[6] = { + static constexpr uint32_t _gpios[DIRECT_PWM_OUTPUT_CHANNELS] = { GPIO_GPIO0_OUTPUT, GPIO_GPIO1_OUTPUT, GPIO_GPIO2_OUTPUT, GPIO_GPIO3_OUTPUT, GPIO_GPIO4_OUTPUT, - GPIO_GPIO5_OUTPUT + GPIO_GPIO5_OUTPUT, +#if defined(DIRECT_PWM_OUTPUT_CHANNELS) && DIRECT_PWM_OUTPUT_CHANNELS > 6 + GPIO_GPIO6_OUTPUT, + GPIO_GPIO7_OUTPUT, +#endif }; uint32_t _triggers[arraySize(_gpios)]; diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.cpp b/src/drivers/camera_trigger/interfaces/src/pwm.cpp index 04ca8f72518e..69dad1cbf180 100644 --- a/src/drivers/camera_trigger/interfaces/src/pwm.cpp +++ b/src/drivers/camera_trigger/interfaces/src/pwm.cpp @@ -58,8 +58,14 @@ void CameraInterfacePWM::trigger(bool trigger_on_true) void CameraInterfacePWM::info() { +#if defined(DIRECT_PWM_OUTPUT_CHANNELS) && DIRECT_PWM_OUTPUT_CHANNELS <= 6 PX4_INFO("PWM trigger mode (generic), pins enabled : [%d][%d][%d][%d][%d][%d]", _pins[5], _pins[4], _pins[3], _pins[2], _pins[1], _pins[0]); +#else + PX4_INFO("PWM trigger mode (generic), pins enabled : [%d][%d][%d][%d][%d][%d][%d][%d]", + _pins[7], _pins[6], _pins[5], _pins[4], _pins[3], _pins[2], _pins[1], _pins[0]); +#endif + } #endif /* ifdef __PX4_NUTTX */ diff --git a/src/drivers/camera_trigger/interfaces/src/seagull_map2.cpp b/src/drivers/camera_trigger/interfaces/src/seagull_map2.cpp index 3d2dd54177e2..c5c39ea46f80 100644 --- a/src/drivers/camera_trigger/interfaces/src/seagull_map2.cpp +++ b/src/drivers/camera_trigger/interfaces/src/seagull_map2.cpp @@ -102,8 +102,13 @@ void CameraInterfaceSeagull::send_toggle_power(bool enable) void CameraInterfaceSeagull::info() { +#if defined(DIRECT_PWM_OUTPUT_CHANNELS) && DIRECT_PWM_OUTPUT_CHANNELS <= 6 PX4_INFO("PWM trigger mode (Seagull MAP2) , pins enabled : [%d][%d][%d][%d][%d][%d]", _pins[5], _pins[4], _pins[3], _pins[2], _pins[1], _pins[0]); +#else + PX4_INFO("PWM trigger mode (Seagull MAP2) , pins enabled : [%d][%d][%d][%d][%d][%d][%d][%d]", + _pins[7], _pins[6], _pins[5], _pins[4], _pins[3], _pins[2], _pins[1], _pins[0]); +#endif } #endif /* ifdef __PX4_NUTTX */