diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 09bf7f45cb..0832c4f6d8 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -377,21 +377,30 @@ else if param greater -s TRIG_MODE 0 then - # We ONLY support trigger on pins 5+6 or 7+8 when simultanously using AUX for actuator output. - if param compare TRIG_PINS 56 + if param compare TRIG_PINS_EX 0 then - # clear pins 5 and 6 - set FMU_MODE pwm4 - set AUX_MODE pwm4 - else - if param compare TRIG_PINS 78 + # We ONLY support trigger on pins 5+6 or 7+8 when simultanously using AUX for actuator output. + if param compare TRIG_PINS 56 then - # clear pins 7 and 8 - set FMU_MODE pwm6 - set AUX_MODE pwm6 + # clear pins 5 and 6 + set FMU_MODE pwm4 + set AUX_MODE pwm4 else - set FMU_MODE none - set AUX_MODE none + if param compare TRIG_PINS 78 + then + # clear pins 7 and 8 + set FMU_MODE pwm6 + set AUX_MODE pwm6 + else + set FMU_MODE none + set AUX_MODE none + fi + fi + else + if param compare TRIG_PINS_EX 12288 + then + set FMU_MODE pwm12 + set AUX_MODE pwm12 fi fi diff --git a/src/drivers/camera_trigger/camera_trigger_params.c b/src/drivers/camera_trigger/camera_trigger_params.c index 1ed5c7cd6b..eaa3088b5d 100644 --- a/src/drivers/camera_trigger/camera_trigger_params.c +++ b/src/drivers/camera_trigger/camera_trigger_params.c @@ -148,6 +148,24 @@ PARAM_DEFINE_INT32(TRIG_MODE, 0); */ PARAM_DEFINE_INT32(TRIG_PINS, 56); +/** + * Camera trigger pin extended + * + * This Bit mask selects which FMU pin is used (range: AUX9-AUX32) + * If the value is not 0 it takes precedence over TRIG_PINS. + * + * If bits above 8 are set that value is used as the selector for trigger pins. + * greater then 8. 0x00000300 Would be Pins 9,10. If the value is + * + * + * @min 0 + * @max 4294967040 + * @decimal 0 + * @reboot_required true + * @group Camera trigger + */ +PARAM_DEFINE_INT32(TRIG_PINS_EX, 0); + /** * Camera trigger distance * diff --git a/src/drivers/camera_trigger/interfaces/src/camera_interface.cpp b/src/drivers/camera_trigger/interfaces/src/camera_interface.cpp index bc93a8cb3b..2224e926c9 100644 --- a/src/drivers/camera_trigger/interfaces/src/camera_interface.cpp +++ b/src/drivers/camera_trigger/interfaces/src/camera_interface.cpp @@ -36,36 +36,61 @@ void CameraInterface::get_pins() { + // Get parameter handle _p_pin = param_find("TRIG_PINS"); - if (_p_pin == PARAM_INVALID) { + _p_pin_ex = param_find("TRIG_PINS_EX"); + + if (_p_pin == PARAM_INVALID && _p_pin_ex == PARAM_INVALID) { PX4_ERR("param TRIG_PINS not found"); return; } - 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; + int pin_list = 0; + int pin_list_ex = 0; - while ((single_pin = pin_list % 10)) { - - _pins[i] = single_pin - 1; - - if (_pins[i] < 0) { - _pins[i] = -1; - } - - pin_list /= 10; - i++; + if (_p_pin_ex != PARAM_INVALID) { + param_get(_p_pin_ex, &pin_list_ex); } + if (_p_pin != PARAM_INVALID) { + param_get(_p_pin, &pin_list); + } + + if (pin_list_ex == 0) { + + // 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++; + } + + } else { + unsigned int p = 0; + + for (unsigned int i = 0; i < arraySize(_pins); i++) { + int32_t v = (pin_list_ex & (1 << i)) ? i : -1; + + if (v > 0) { + _pins[p++] = v; + } + } + } } diff --git a/src/drivers/camera_trigger/interfaces/src/camera_interface.h b/src/drivers/camera_trigger/interfaces/src/camera_interface.h index 7ffe55cf0e..cb6da5ac0c 100644 --- a/src/drivers/camera_trigger/interfaces/src/camera_interface.h +++ b/src/drivers/camera_trigger/interfaces/src/camera_interface.h @@ -98,7 +98,8 @@ protected: void get_pins(); param_t _p_pin{PARAM_INVALID}; + param_t _p_pin_ex{PARAM_INVALID}; - int _pins[6] {}; + int _pins[32] {}; }; diff --git a/src/drivers/camera_trigger/interfaces/src/gpio.cpp b/src/drivers/camera_trigger/interfaces/src/gpio.cpp index e936d39292..df0ab47104 100644 --- a/src/drivers/camera_trigger/interfaces/src/gpio.cpp +++ b/src/drivers/camera_trigger/interfaces/src/gpio.cpp @@ -80,7 +80,11 @@ void CameraInterfaceGPIO::info() PX4_INFO_RAW("GPIO trigger mode, pins enabled: "); for (unsigned i = 0; i < arraySize(_pins); ++i) { - PX4_INFO_RAW("[%d]", _pins[i]); + if (_pins[i] < 0) { + continue; + } + + PX4_INFO_RAW("[%d]", _pins[i] + 1); } PX4_INFO_RAW(", polarity : %s\n", _trigger_invert ? "ACTIVE_LOW" : "ACTIVE_HIGH"); diff --git a/src/drivers/camera_trigger/interfaces/src/gpio.h b/src/drivers/camera_trigger/interfaces/src/gpio.h index 133a8d316a..9ef78eb471 100644 --- a/src/drivers/camera_trigger/interfaces/src/gpio.h +++ b/src/drivers/camera_trigger/interfaces/src/gpio.h @@ -56,7 +56,7 @@ public: void info(); private: - static const int num_gpios = DIRECT_PWM_OUTPUT_CHANNELS > 6 ? 6 : DIRECT_PWM_OUTPUT_CHANNELS; + static const int num_gpios = 32; void setup(); diff --git a/src/drivers/camera_trigger/interfaces/src/pwm.cpp b/src/drivers/camera_trigger/interfaces/src/pwm.cpp index ced85a0611..136506b582 100644 --- a/src/drivers/camera_trigger/interfaces/src/pwm.cpp +++ b/src/drivers/camera_trigger/interfaces/src/pwm.cpp @@ -91,8 +91,17 @@ void CameraInterfacePWM::trigger(bool trigger_on_true) void CameraInterfacePWM::info() { - 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]); + PX4_INFO_RAW("PWM trigger mode, pins enabled: "); + + for (unsigned i = 0; i < arraySize(_pins); ++i) { + if (_pins[i] < 0) { + continue; + } + + PX4_INFO_RAW("[%d]", _pins[i] + 1); + } + + PX4_INFO_RAW("\n"); } #endif /* ifdef __PX4_NUTTX */