diff --git a/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/io_timer.h index 40052ed9b89..c0f0377d1e7 100644 --- a/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/io_timer.h @@ -137,5 +137,17 @@ __EXPORT int io_timer_free_channel(unsigned channel); __EXPORT int io_timer_get_channel_mode(unsigned channel); __EXPORT int io_timer_get_mode_channels(io_timer_channel_mode_t mode); __EXPORT extern void io_timer_trigger(void); +/** + * Returns the pin configuration for a specific channel, to be used as GPIO output. + * 0 is returned if the channel is not valid. + */ +__EXPORT uint32_t io_timer_channel_get_gpio_output(unsigned channel); +/** + * Returns the pin configuration for a specific channel, to be used as PWM input. + * 0 is returned if the channel is not valid. + */ +__EXPORT uint32_t io_timer_channel_get_as_pwm_input(unsigned channel); + __END_DECLS + diff --git a/platforms/nuttx/src/px4/nxp/kinetis/io_pins/io_timer.c b/platforms/nuttx/src/px4/nxp/kinetis/io_pins/io_timer.c index 2485cc0532a..393920d39d1 100644 --- a/platforms/nuttx/src/px4/nxp/kinetis/io_pins/io_timer.c +++ b/platforms/nuttx/src/px4/nxp/kinetis/io_pins/io_timer.c @@ -31,8 +31,8 @@ * ****************************************************************************/ -/* - * @file drv_io_timer.c +/** + * @file io_timer.c * * Servo driver supporting PWM servos connected to Kinetis FTM timer blocks. */ @@ -362,6 +362,25 @@ int io_timer_validate_channel_index(unsigned channel) return rv; } +uint32_t io_timer_channel_get_gpio_output(unsigned channel) +{ + if (io_timer_validate_channel_index(channel) != 0) { + return 0; + } + + return (timer_io_channels[channel].gpio_out & ~(_PIN_MODE_MASK | _PIN_OPTIONS_MASK)) | GPIO_HIGHDRIVE; +} + +uint32_t io_timer_channel_get_as_pwm_input(unsigned channel) +{ + if (io_timer_validate_channel_index(channel) != 0) { + return 0; + } + + return timer_io_channels[channel].gpio_in; +} + + int io_timer_get_mode_channels(io_timer_channel_mode_t mode) { if (mode < IOTimerChanModeSize) { diff --git a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h index 2e10fc93453..a2751dc53ff 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h @@ -149,4 +149,15 @@ __EXPORT void io_timer_update_dma_req(uint8_t timer, bool enable); __EXPORT extern int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_rate, uint8_t dma_burst_length); +/** + * Returns the pin configuration for a specific channel, to be used as GPIO output. + * 0 is returned if the channel is not valid. + */ +__EXPORT uint32_t io_timer_channel_get_gpio_output(unsigned channel); +/** + * Returns the pin configuration for a specific channel, to be used as PWM input. + * 0 is returned if the channel is not valid. + */ +__EXPORT uint32_t io_timer_channel_get_as_pwm_input(unsigned channel); + __END_DECLS diff --git a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c index ebdb66561b8..65ec0380c11 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c @@ -31,8 +31,10 @@ * ****************************************************************************/ -/* - * @file drv_io_timer.c +/** + * @file io_timer.c + * + * Servo driver supporting PWM servos connected to STM32 timer blocks. */ #include @@ -370,6 +372,30 @@ int io_timer_validate_channel_index(unsigned channel) return rv; } +uint32_t io_timer_channel_get_gpio_output(unsigned channel) +{ + if (io_timer_validate_channel_index(channel) != 0) { + return 0; + } + +#ifdef CONFIG_STM32_STM32F10XX + return (timer_io_channels[channel].gpio_out & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | + (GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_CLEAR); +#else + return (timer_io_channels[channel].gpio_out & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | + (GPIO_OUTPUT | GPIO_PUSHPULL | GPIO_SPEED_2MHz | GPIO_OUTPUT_CLEAR); +#endif +} + +uint32_t io_timer_channel_get_as_pwm_input(unsigned channel) +{ + if (io_timer_validate_channel_index(channel) != 0) { + return 0; + } + + return timer_io_channels[channel].gpio_in; +} + int io_timer_get_mode_channels(io_timer_channel_mode_t mode) { if (mode < IOTimerChanModeSize) { diff --git a/src/drivers/camera_trigger/interfaces/src/gpio.cpp b/src/drivers/camera_trigger/interfaces/src/gpio.cpp index 8cb13d5c9c5..1362fda92f3 100644 --- a/src/drivers/camera_trigger/interfaces/src/gpio.cpp +++ b/src/drivers/camera_trigger/interfaces/src/gpio.cpp @@ -2,13 +2,12 @@ #include "gpio.h" #include - -constexpr uint32_t CameraInterfaceGPIO::_gpios[ngpios]; +#include CameraInterfaceGPIO::CameraInterfaceGPIO(): CameraInterface(), _trigger_invert(false), - _triggers{0} + _triggers{} { _p_polarity = param_find("TRIG_POLARITY"); @@ -25,10 +24,9 @@ void CameraInterfaceGPIO::setup() { for (unsigned i = 0, t = 0; i < arraySize(_pins); i++) { - // Pin range is from 1 to 5 or 6, indexes are 0 to 4 or 5 - - if (_pins[i] >= 0 && _pins[i] < (int)arraySize(_gpios)) { - uint32_t gpio = _gpios[_pins[i]]; + // Pin range is from 0 to num_gpios - 1 + if (_pins[i] >= 0 && t < (int)arraySize(_triggers) && _pins[i] < num_gpios) { + uint32_t gpio = io_timer_channel_get_gpio_output(_pins[i]); _triggers[t++] = gpio; px4_arch_configgpio(gpio); px4_arch_gpiowrite(gpio, false ^ _trigger_invert); @@ -50,17 +48,13 @@ void CameraInterfaceGPIO::trigger(bool trigger_on_true) void CameraInterfaceGPIO::info() { - if (ngpios == 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"); + PX4_INFO_RAW("GPIO trigger mode, pins enabled: "); + + for (unsigned i = 0; i < arraySize(_pins); ++i) { + PX4_INFO_RAW("[%d]", _pins[i]); } - if (ngpios == 5) { - PX4_INFO("GPIO trigger mode, pins enabled : [%d][%d][%d][%d][%d], polarity : %s", - _pins[4], _pins[3], _pins[2], _pins[1], _pins[0], - _trigger_invert ? "ACTIVE_LOW" : "ACTIVE_HIGH"); - } + PX4_INFO_RAW(", polarity : %s\n", _trigger_invert ? "ACTIVE_LOW" : "ACTIVE_HIGH"); } #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 9cb227efeb3..d623e31f4d6 100644 --- a/src/drivers/camera_trigger/interfaces/src/gpio.h +++ b/src/drivers/camera_trigger/interfaces/src/gpio.h @@ -21,13 +21,9 @@ public: void trigger(bool trigger_on_true); void info(); -#if defined(GPIO_GPIO5_OUTPUT) - static const int ngpios = 6; -#else - static const int ngpios = 5; -#endif private: + static const int num_gpios = DIRECT_PWM_OUTPUT_CHANNELS > 6 ? 6 : DIRECT_PWM_OUTPUT_CHANNELS; void setup(); @@ -35,18 +31,7 @@ private: bool _trigger_invert; - static constexpr uint32_t _gpios[ngpios] = { - GPIO_GPIO0_OUTPUT, - GPIO_GPIO1_OUTPUT, - GPIO_GPIO2_OUTPUT, - GPIO_GPIO3_OUTPUT, - GPIO_GPIO4_OUTPUT, -#if defined(GPIO_GPIO5_OUTPUT) - GPIO_GPIO5_OUTPUT -#endif - }; - - uint32_t _triggers[arraySize(_gpios)]; + uint32_t _triggers[num_gpios]; }; #endif /* ifdef __PX4_NUTTX */ diff --git a/src/drivers/distance_sensor/ll40ls/LidarLitePWM.cpp b/src/drivers/distance_sensor/ll40ls/LidarLitePWM.cpp index 5d5e0222865..756d99a2c7a 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLitePWM.cpp +++ b/src/drivers/distance_sensor/ll40ls/LidarLitePWM.cpp @@ -46,11 +46,13 @@ #ifdef LIDAR_LITE_PWM_SUPPORTED +#include + LidarLitePWM::LidarLitePWM(const uint8_t rotation) : LidarLite(rotation), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) { - px4_arch_configgpio(GPIO_VDD_RANGEFINDER_EN); + px4_arch_configgpio(io_timer_channel_get_gpio_output(GPIO_VDD_RANGEFINDER_EN_CHAN)); } LidarLitePWM::~LidarLitePWM() @@ -128,4 +130,4 @@ LidarLitePWM::collect() return EAGAIN; } -#endif \ No newline at end of file +#endif diff --git a/src/drivers/distance_sensor/ll40ls/LidarLitePWM.h b/src/drivers/distance_sensor/ll40ls/LidarLitePWM.h index 2e4dfc4b672..8bf4fa3d8e7 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLitePWM.h +++ b/src/drivers/distance_sensor/ll40ls/LidarLitePWM.h @@ -49,11 +49,11 @@ #include #include +#include -#ifdef GPIO_GPIO5_OUTPUT - +#if DIRECT_PWM_OUTPUT_CHANNELS >= 6 +#define GPIO_VDD_RANGEFINDER_EN_CHAN 5 // use pin 6 #define LIDAR_LITE_PWM_SUPPORTED -#define GPIO_VDD_RANGEFINDER_EN GPIO_GPIO5_OUTPUT class LidarLitePWM : public LidarLite, public px4::ScheduledWorkItem {