update spi_led driver.

ROMFS/cannode/init.d/rcS

@@ -65,8 +65,11 @@ unset BOARD_RC_DEFAULTS
 #
 # Start system state indicator.
 #
-rgbled start -X -q
-rgbled_ncp5623c start -X -q
+if [ $UCAN146 == none ]
+then
+	rgbled start -X -q
+	rgbled_ncp5623c start -X -q
+fi
 
 #
 # board sensors: rc.sensors
@@ -86,10 +89,16 @@ unset BOARD_RC_SENSORS
 . ${R}etc/init.d/rc.serial
 
 # Check for flow sensor
-if param compare SENS_EN_PX4FLOW 1
+if [ $UCAN146 == none ]
 then
-	px4flow start -X &
+	if param compare SENS_EN_PX4FLOW 1
+	then
+		px4flow start -X &
+	fi
 fi
 
-uavcannode start
+if [ $UCAN146 == none ]
+then
+	uavcannode start
+fi
 unset R

boards/nxp/ucans32k146/init/rc.board_defaults

@@ -3,7 +3,13 @@
 # board specific defaults
 #------------------------------------------------------------------------------
 
-pwm_out mode_pwm1 start
+set UCAN146 1
+param set-default UCAN1_PCA_PWM_S 500
+param set-default UCAN1_SPI_LED_S 501
+
+pca9685_ucan start -X
+spi_led start -X
 
 ifup can0
 cyphal start
+

boards/nxp/ucans32k146/nuttx-config/nsh/defconfig

@@ -25,6 +25,10 @@ CONFIG_BCH=y
 CONFIG_BOARDCTL_RESET=y
 CONFIG_BOARD_LOOPSPERMSEC=3997
 CONFIG_BUILTIN=y
+CONFIG_CAN=y
+CONFIG_CANUTILS_CANDUMP=y
+CONFIG_CANUTILS_CANSEND=y
+CONFIG_NET_CAN_CANFD=y
 CONFIG_CLOCK_MONOTONIC=y
 CONFIG_DEBUG_FULLOPT=y
 CONFIG_DEBUG_HARDFAULT_ALERT=y

boards/nxp/ucans32k146/pca9685.px4board

@@ -0,0 +1,7 @@
+CONFIG_DRIVERS_GPS=n
+CONFIG_CYPHAL_CANFD_SUPPORT=y
+CONFIG_CYPHAL_UORB_PCA_PWM_SUBSCRIBER=y
+CONFIG_CYPHAL_UORB_SPI_LED_SUBSCRIBER=y
+CONFIG_DRIVERS_PCA9685_UCAN=y
+CONFIG_DRIVERS_SPI_LED=y
+CONFIG_SYSTEMCMDS_UORB=y

msg/CMakeLists.txt

@@ -124,6 +124,7 @@ set(msg_files
 	optical_flow.msg
 	orbit_status.msg
 	parameter_update.msg
+	pca_pwm.msg
 	ping.msg
 	pps_capture.msg
 	position_controller_landing_status.msg
@@ -157,6 +158,7 @@ set(msg_files
 	sensor_selection.msg
 	sensors_status_imu.msg
 	sensors_status.msg
+	spi_led.msg
 	system_power.msg
 	takeoff_status.msg
 	task_stack_info.msg

msg/pca_pwm.msg

@@ -0,0 +1,5 @@
+uint64 timestamp				# time since system start (microseconds)
+uint16 pwm_period				# PWM period in microseconds (us) [roof((1s/1526hz)*10^7us/s)=656us:floor((1s/24hz)*10^7us/s)=41666us]
+uint16[16] pulse_width				# pulse width in microseconds (us) [656:41666]
+
+# TOPICS pca_pwm

msg/spi_led.msg

@@ -0,0 +1,10 @@
+# Control up to 1001 LEDs on APA102
+
+uint64 timestamp                # time since system start (microseconds)
+
+uint16 number_leds              #total number of leds [0,1000]
+uint8 offset_group              #offset group (multiple of 10) of leds to control [0,100]
+uint32[10] led_values           #follows spi 32 bit LED schema for apa102
+
+# [  PAD 1's  ][ BRIGHT 0:31 ][ BLU 0:255 ][ GRN 0:255 ][ RED 0:255 ]
+# [ 31 ... 29 ][ 28  ...  24 ][ 23 ... 16 ][ 15 ...  8 ][ 7  ...  0 ]

src/drivers/cyphal/CanardSocketCAN.cpp

@@ -56,8 +56,11 @@ int CanardSocketCAN::init()
 	struct ifreq ifr;
 
 	//FIXME HOTFIX to make this code compile
+#if CONFIG_CYPHAL_CANFD_SUPPORT
+	bool can_fd = 1;
+#else
 	bool can_fd = 0;
-
+#endif
 	_can_fd = can_fd;
 
 	/* open socket */

src/drivers/cyphal/Cyphal.cpp

@@ -117,7 +117,11 @@ int CyphalNode::start(uint32_t node_id, uint32_t bitrate)
 		return -1;
 	}
 
+#if CONFIG_CYPHAL_CANFD_SUPPORT
+	bool can_fd = true;
+#else
 	bool can_fd = false;
+#endif
 
 	if (can_fd) {
 		_instance = new CyphalNode(node_id, 8, CANARD_MTU_CAN_FD);

src/drivers/cyphal/Kconfig

@@ -41,6 +41,9 @@ if DRIVERS_CYPHAL
         help
             When the board uses the UAVCANv0 bootloader functionality you need a AppImageDescriptor defined
 
+    config CYPHAL_CANFD_SUPPORT
+        bool "Enable CANFD Support in Cyphal"
+        default n
 
     menu "Publisher support"
 
@@ -87,6 +90,14 @@ if DRIVERS_CYPHAL
         config CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER
             bool "uORB sensor_gps Subscriber"
             default n
+
+        config CYPHAL_UORB_PCA_PWM_SUBSCRIBER
+            bool "uORB pca_pwm Subscriber"
+            default n
+
+	config CYPHAL_UORB_SPI_LED_SUBSCRIBER
+            bool "uORB spi_led Subscriber"
+            default n
     endmenu
 
     menu "Advertised Services"

src/drivers/cyphal/ParamManager.hpp

@@ -116,20 +116,22 @@ public:
 private:
 
 
-	const UavcanParamBinder _uavcan_params[13] {
-		{"uavcan.pub.esc.0.id",                "UCAN1_ESC_PUB",		    px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.pub.servo.0.id",              "UCAN1_SERVO_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.pub.gps.0.id",                "UCAN1_GPS_PUB",		    px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.pub.actuator_outputs.0.id",   "UCAN1_ACTR_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.sub.esc.0.id",                "UCAN1_ESC0_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.sub.gps.0.id",                "UCAN1_GPS0_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.sub.gps.1.id",                "UCAN1_GPS1_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.sub.energy_source.0.id",      "UCAN1_BMS_ES_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.sub.battery_status.0.id",     "UCAN1_BMS_BS_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.sub.battery_parameters.0.id", "UCAN1_BMS_BP_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.sub.legacy_bms.0.id",         "UCAN1_LG_BMS_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.sub.uorb.sensor_gps.0.id",    "UCAN1_UORB_GPS",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.pub.uorb.sensor_gps.0.id",    "UCAN1_UORB_GPS_P",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+	const UavcanParamBinder _uavcan_params[15] {
+		{"uavcan.pub.esc.0.id",			"UCAN1_ESC_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.pub.servo.0.id",		"UCAN1_SERVO_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.pub.gps.0.id",			"UCAN1_GPS_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.pub.actuator_outputs.0.id",	"UCAN1_ACTR_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.esc.0.id",			"UCAN1_ESC0_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.gps.0.id",			"UCAN1_GPS0_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.gps.1.id",			"UCAN1_GPS1_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.energy_source.0.id",	"UCAN1_BMS_ES_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.battery_status.0.id",	"UCAN1_BMS_BS_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.battery_parameters.0.id",	"UCAN1_BMS_BP_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.legacy_bms.0.id",		"UCAN1_LG_BMS_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.uorb.sensor_gps.0.id",	"UCAN1_UORB_GPS",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.pub.uorb.sensor_gps.0.id",	"UCAN1_UORB_GPS_P",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.uorb.pca_pwm.0.id",	"UCAN1_PCA_PWM_S",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.uorb.spi_led.0.id",	"UCAN1_SPI_LED_S",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		//{"uavcan.sub.bms.0.id",   "UCAN1_BMS0_SUB"}, //FIXME instancing
 		//{"uavcan.sub.bms.1.id",   "UCAN1_BMS1_SUB"},
 	};

src/drivers/cyphal/Subscribers/uORB/uorb_subscriber.hpp

@@ -47,6 +47,8 @@
 #include <uORB/topics/actuator_outputs.h>
 
 #include <uORB/topics/sensor_gps.h>
+#include <uORB/topics/pca_pwm.h>
+#include <uORB/topics/spi_led.h>
 
 template <class T>
 class uORB_over_UAVCAN_Subscriber : public UavcanDynamicPortSubscriber

src/drivers/cyphal/SubscriptionManager.cpp

@@ -71,6 +71,8 @@ void SubscriptionManager::subscribe()
 
 void SubscriptionManager::updateDynamicSubscriptions()
 {
+	PX4_INFO("Cyphal sub count: %d", UAVCAN_SUB_COUNT);
+
 	for (auto &sub : _uavcan_subs) {
 
 		bool found_subscriber = false;

src/drivers/cyphal/SubscriptionManager.hpp

@@ -61,13 +61,23 @@
 #define CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER 0
 #endif
 
+#ifndef CONFIG_CYPHAL_UORB_PCA_PWM_SUBSCRIBER
+#define CONFIG_CYPHAL_UORB_PCA_PWM_SUBSCRIBER 0
+#endif
+
+#ifndef CONFIG_CYPHAL_UORB_SPI_LED_SUBSCRIBER
+#define CONFIG_CYPHAL_UORB_SPI_LED_SUBSCRIBER 0
+#endif
+
 /* Preprocessor calculation of Subscribers count */
 
 #define UAVCAN_SUB_COUNT CONFIG_CYPHAL_ESC_SUBSCRIBER + \
 	CONFIG_CYPHAL_GNSS_SUBSCRIBER_0 + \
 	CONFIG_CYPHAL_GNSS_SUBSCRIBER_1 + \
 	CONFIG_CYPHAL_BMS_SUBSCRIBER + \
-	CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER
+	CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER + \
+	CONFIG_CYPHAL_UORB_PCA_PWM_SUBSCRIBER + \
+	CONFIG_CYPHAL_UORB_SPI_LED_SUBSCRIBER
 
 #include <px4_platform_common/defines.h>
 #include <drivers/drv_hrt.h>
@@ -179,6 +189,26 @@ private:
 			"uorb.sensor_gps",
 			0
 		},
+#endif
+#if CONFIG_CYPHAL_UORB_PCA_PWM_SUBSCRIBER
+		{
+			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
+			{
+				return new uORB_over_UAVCAN_Subscriber<pca_pwm_s>(handle, pmgr, ORB_ID(pca_pwm));
+			},
+			"uorb.pca_pwm",
+			0
+		},
+#endif
+#if CONFIG_CYPHAL_UORB_SPI_LED_SUBSCRIBER
+		{
+			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
+			{
+				return new uORB_over_UAVCAN_Subscriber<spi_led_s>(handle, pmgr, ORB_ID(spi_led));
+			},
+			"uorb.spi_led",
+			0
+		},
 #endif
 	};
 };

src/drivers/cyphal/parameters.c

@@ -151,6 +151,24 @@ PARAM_DEFINE_INT32(UCAN1_UORB_GPS, -1);
  */
 PARAM_DEFINE_INT32(UCAN1_UORB_GPS_P, -1);
 
+/**
+ * pca_pwm uORB over Cyphal subscription port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(UCAN1_PCA_PWM_S, -1);
+
+/**
+ * spi_led uORB over Cyphal subscription port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(UCAN1_SPI_LED_S, -1);
+
 // Publication Port IDs
 
 /**

src/drivers/pca9685_ucan/CMakeLists.txt

@@ -0,0 +1,39 @@
+############################################################################
+#
+#   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__pca9685_ucan
+	MAIN pca9685_ucan
+	SRCS
+		pca9685_ucan.cpp
+	DEPENDS
+	)

src/drivers/pca9685_ucan/Kconfig

@@ -0,0 +1,5 @@
+menuconfig DRIVERS_PCA9685_UCAN
+	bool "pca9685_ucan"
+	default n
+	---help---
+		Enable support for pca9685_ucan

src/drivers/pca9685_ucan/pca9685_ucan.cpp

@@ -0,0 +1,466 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file pca9685_ucan.cpp
+ *
+ * Driver for the PCA9685 I2C PWM module
+ * The chip is used on the Adafruit I2C/PWM converter https://www.adafruit.com/product/815
+ *
+ * Parts of the code are adapted from the arduino library for the board
+ * https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library
+ * for the license of these parts see the
+ * arduino_Adafruit_PWM_Servo_Driver_Library_license.txt file
+ * see https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library for contributors
+ *
+ * @author Benjamin Perseghetti <bperseghetti@rudislabs.com>
+ * @author Landon Haugh <landon.haugh@nxp.com>
+ */
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+
+#include <drivers/device/i2c.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <ctype.h>
+#include <math.h>
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/i2c_spi_buses.h>
+#include <nuttx/clock.h>
+
+#include <perf/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <uORB/uORB.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/pca_pwm.h>
+
+#include <board_config.h>
+
+#define PCA9685_MODE1 0x0
+#define PCA9685_PRESCALE 0xFE
+
+#define LED0_ON_L 0x6
+
+#define ADDR 0x40	// I2C adress
+
+#define ORB_SUB_UPDATE_PERIOD 10 // uORB subscription update period in milliseconds
+
+#define NUMBER_PWM_CHANNELS 16 // Number of PWM channels.
+
+#define PWM_PERIOD_MIN_US 656
+
+#define PWM_PERIOD_MAX_US 41666
+
+
+enum IOX_MODE {
+	IOX_MODE_ON,
+	IOX_MODE_TEST_OUT
+};
+
+using namespace time_literals;
+
+class PCA9685 : public device::I2C, public I2CSPIDriver<PCA9685>
+{
+public:
+	PCA9685(const I2CSPIDriverConfig &config);
+	~PCA9685() override = default;
+
+	static void print_usage();
+
+	void print_status();
+
+	int		init() override;
+	int		reset();
+
+	void RunImpl();
+
+protected:
+	void custom_method(const BusCLIArguments &cli) override;
+private:
+
+	enum IOX_MODE		_mode;
+	uint64_t		_i2cpwm_interval;
+	perf_counter_t		_comms_errors;
+
+	uint8_t			_msg[6];
+
+
+	int			_pca_pwm_sub;
+	struct pca_pwm_s	_pca_pwm;
+	uint16_t		_pwm_period_us = 20000;
+	float			_pwm_freq = 50.0f;
+	uint16_t		_test_pwm = 0;
+	uint16_t		_current_values[NUMBER_PWM_CHANNELS]; /**< stores the current pwm output values as sent to the setPin() */
+
+	bool _mode_on_initialized;  /** Set to true after the first call of i2cpwm in mode IOX_MODE_ON */
+	bool _updated = false;
+
+	/**
+	 * Helper function to set the pwm frequency
+	 */
+	int setPWMFreq(float freq);
+
+	/**
+	 * Helper function to set the demanded pwm value
+	 * @param num pwm output number
+	 */
+	int setPWM(uint8_t num, uint16_t on, uint16_t off);
+
+	/**
+	 * Sets pin without having to deal with on/off tick placement and properly handles
+	 * a zero value as completely off.
+	 * @param num pwm output number
+	 * @param val should be a value from 0 to 4095 inclusive.
+	 */
+	int setPin(uint8_t num, uint16_t val);
+
+
+	/* Wrapper to read a byte from addr */
+	int read8(uint8_t addr, uint8_t &value);
+
+	/* Wrapper to wite a byte to addr */
+	int write8(uint8_t addr, uint8_t value);
+
+};
+
+PCA9685::PCA9685(const I2CSPIDriverConfig &config) :
+	I2C(config),
+	I2CSPIDriver(config),
+	_mode(IOX_MODE_ON),
+	_i2cpwm_interval((float)_pwm_period_us),
+	_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err")),
+	_pca_pwm_sub(-1),
+	_pca_pwm(),
+	_mode_on_initialized(false)
+{
+	memset(_msg, 0, sizeof(_msg));
+	memset(_current_values, 0, sizeof(_current_values));
+}
+
+int
+PCA9685::init()
+{
+	int ret;
+	ret = I2C::init();
+
+	if (ret != OK) {
+		return ret;
+	}
+
+	ret = reset();
+
+	if (ret != OK) {
+		return ret;
+	}
+
+	ret = setPWMFreq(_pwm_freq);
+
+	if (ret == 0) {
+		ScheduleNow();
+	}
+
+	return ret;
+}
+
+void
+PCA9685::print_status()
+{
+	I2CSPIDriverBase::print_status();
+	PX4_INFO("Mode: %u", _mode);
+}
+
+void
+PCA9685::RunImpl()
+{
+	if (_mode == IOX_MODE_TEST_OUT) {
+		if (_test_pwm > 4096) {
+			_test_pwm = 0;
+		}
+
+		for (int i = 0; i < NUMBER_PWM_CHANNELS; i++) {
+				setPin(i, _test_pwm);
+		}
+
+		_test_pwm += 4096/10;
+
+	} else {
+		if (!_mode_on_initialized) {
+			_pca_pwm_sub = orb_subscribe(ORB_ID(pca_pwm));
+			orb_set_interval(_pca_pwm_sub, ORB_SUB_UPDATE_PERIOD);
+
+			_mode_on_initialized = true;
+		}
+
+		orb_check(_pca_pwm_sub, &_updated);
+
+		if (_updated) {
+			orb_copy(ORB_ID(pca_pwm), _pca_pwm_sub, &_pca_pwm);
+			if (_pwm_period_us != _pca_pwm.pwm_period &&
+			    _pca_pwm.pwm_period >= PWM_PERIOD_MIN_US &&
+			    _pca_pwm.pwm_period <= PWM_PERIOD_MAX_US) {
+				_pwm_period_us = _pca_pwm.pwm_period;
+				_pwm_freq = 1000000.0f / (float)_pwm_period_us;
+			}
+			for (int i = 0; i < NUMBER_PWM_CHANNELS; i++) {
+				uint16_t new_value = (uint16_t)(4096.0f*(float)_pca_pwm.pulse_width[i]/
+							(float)_pwm_period_us);
+				/*PX4_INFO("PWM_%d: pulse_width: %u, new_value: %u, pwm_period: %u",
+						i, _pca_pwm.pulse_width[i], new_value, _pwm_period_us);*/
+				if (new_value != _current_values[i] && new_value <= 4096) {
+					setPin(i, new_value);
+					_current_values[i] = new_value;
+				}
+			}
+		}
+	}
+
+	ScheduleDelayed(_i2cpwm_interval);
+}
+
+int
+PCA9685::setPWM(uint8_t num, uint16_t on, uint16_t off)
+{
+	int ret;
+	/* convert to correct message */
+	_msg[0] = LED0_ON_L + 4 * num;
+	_msg[1] = on;
+	_msg[2] = on >> 8;
+	_msg[3] = off;
+	_msg[4] = off >> 8;
+
+	/* try i2c transfer */
+	ret = transfer(_msg, 5, nullptr, 0);
+
+	if (OK != ret) {
+		perf_count(_comms_errors);
+		DEVICE_LOG("i2c::transfer returned %d", ret);
+	}
+
+	return ret;
+}
+
+int
+PCA9685::setPin(uint8_t num, uint16_t val)
+{
+	// Clamp value between 0 and 4095 inclusive.
+	if (val > 4095) {
+		val = 4095;
+	}
+
+	if (val == 4095) {
+		// Special value for signal fully on.
+		return setPWM(num, 4096, 0);
+
+	} else if (val == 0) {
+		// Special value for signal fully off.
+		return setPWM(num, 0, 4096);
+
+	} else {
+		return setPWM(num, 0, val);
+	}
+
+	return PX4_ERROR;
+}
+
+int
+PCA9685::setPWMFreq(float freq)
+{
+	int ret  = OK;
+	float prescaleval = 25000000;
+	prescaleval /= 4096;
+	prescaleval /= freq;
+	prescaleval -= 1;
+	uint8_t prescale = uint8_t(prescaleval + 0.5f); //implicit floor()
+	uint8_t oldmode;
+	ret = read8(PCA9685_MODE1, oldmode);
+
+	if (ret != OK) {
+		return ret;
+	}
+
+	uint8_t newmode = (oldmode & 0x7F) | 0x10; // sleep
+
+	ret = write8(PCA9685_MODE1, newmode); // go to sleep
+
+	if (ret != OK) {
+		return ret;
+	}
+
+	ret = write8(PCA9685_PRESCALE, prescale); // set the prescaler
+
+	if (ret != OK) {
+		return ret;
+	}
+
+	ret = write8(PCA9685_MODE1, oldmode);
+
+	if (ret != OK) {
+		return ret;
+	}
+
+	usleep(5000); //5ms delay (from arduino driver)
+
+	ret = write8(PCA9685_MODE1, oldmode | 0xa1);  //  This sets the MODE1 register to turn on auto increment.
+
+	if (ret != OK) {
+		return ret;
+	}
+
+	return ret;
+}
+
+/* Wrapper to read a byte from addr */
+int
+PCA9685::read8(uint8_t addr, uint8_t &value)
+{
+	int ret = OK;
+
+	/* send addr */
+	ret = transfer(&addr, sizeof(addr), nullptr, 0);
+
+	if (ret != OK) {
+		goto fail_read;
+	}
+
+	/* get value */
+	ret = transfer(nullptr, 0, &value, 1);
+
+	if (ret != OK) {
+		goto fail_read;
+	}
+
+	return ret;
+
+fail_read:
+	perf_count(_comms_errors);
+	DEVICE_LOG("i2c::transfer returned %d", ret);
+
+	return ret;
+}
+
+int PCA9685::reset(void)
+{
+	warnx("resetting");
+	return write8(PCA9685_MODE1, 0x0);
+}
+
+/* Wrapper to wite a byte to addr */
+int
+PCA9685::write8(uint8_t addr, uint8_t value)
+{
+	int ret = OK;
+	_msg[0] = addr;
+	_msg[1] = value;
+	/* send addr and value */
+	ret = transfer(_msg, 2, nullptr, 0);
+
+	if (ret != OK) {
+		perf_count(_comms_errors);
+		DEVICE_LOG("i2c::transfer returned %d", ret);
+	}
+
+	return ret;
+}
+
+void
+PCA9685::print_usage()
+{
+	PRINT_MODULE_USAGE_NAME("pca9685_ucan", "driver");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
+	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x40);
+	PRINT_MODULE_USAGE_COMMAND("reset");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("test", "enter test mode");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+}
+
+void PCA9685::custom_method(const BusCLIArguments &cli)
+{
+	switch (cli.custom1) {
+	case 0: reset(); break;
+
+	case 1: _mode = IOX_MODE_TEST_OUT; break;
+	}
+}
+
+extern "C" int pca9685_ucan_main(int argc, char *argv[])
+{
+	using ThisDriver = PCA9685;
+	BusCLIArguments cli{true, false};
+	cli.default_i2c_frequency = 100000;
+	cli.i2c_address = ADDR;
+
+	const char *verb = cli.parseDefaultArguments(argc, argv);
+
+	if (!verb) {
+		ThisDriver::print_usage();
+		return -1;
+	}
+
+	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_PWM_DEVTYPE_PCA9685);
+
+	if (!strcmp(verb, "start")) {
+		return ThisDriver::module_start(cli, iterator);
+	}
+
+	if (!strcmp(verb, "stop")) {
+		return ThisDriver::module_stop(iterator);
+	}
+
+	if (!strcmp(verb, "status")) {
+		return ThisDriver::module_status(iterator);
+	}
+
+	if (!strcmp(verb, "reset")) {
+		cli.custom1 = 0;
+		return ThisDriver::module_custom_method(cli, iterator);
+	}
+
+	if (!strcmp(verb, "test")) {
+		cli.custom1 = 1;
+		return ThisDriver::module_custom_method(cli, iterator);
+	}
+
+	ThisDriver::print_usage();
+	return -1;
+}

src/drivers/spi_led/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_module(
+	MODULE drivers__spi_led
+	MAIN spi_led
+	COMPILE_FLAGS
+		#-DDEBUG_BUILD   # uncomment for PX4_DEBUG output
+		#-O0             # uncomment when debugging
+	SRCS
+		spi_led.cpp
+		spi_led.hpp
+	DEPENDS
+		px4_work_queue
+	)

src/drivers/spi_led/Kconfig

@@ -0,0 +1,5 @@
+menuconfig DRIVERS_SPI_LED
+	bool "spi_led"
+	default n
+	---help---
+		Enable support for spi_led

src/drivers/spi_led/spi_led.cpp

@@ -0,0 +1,177 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "spi_led.hpp"
+
+spi_led::spi_led(int bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode) :
+	SPI(DRV_DEVTYPE_UNUSED, MODULE_NAME, bus, device, spi_mode, bus_frequency),
+	ModuleParams(nullptr),
+	ScheduledWorkItem("spi_led", px4::wq_configurations::test1)
+{
+}
+
+spi_led::~spi_led()
+{
+	perf_free(_loop_perf);
+	perf_free(_loop_interval_perf);
+	delete _buf;
+	delete _rbuf;
+}
+
+int spi_led::init()
+{
+	if(SPI::init() != OK) {
+		printf("SPI::init() failed\n");
+		DEVICE_DEBUG("SPI init failed");
+		return false;
+	}
+
+	// execute Run() on every spi_led publication
+
+	if (!_spi_led_sub.registerCallback()) {
+		PX4_ERR("callback registration failed");
+		return false;
+	}
+
+	// alternatively, Run on fixed interval
+	//ScheduleOnInterval(100_ms); // 2000 us interval, 200 Hz rate
+
+	return true;
+}
+
+void spi_led::Run()
+{
+	//printf("made it into run()\n");
+
+	if (should_exit()) {
+		ScheduleClear();
+		exit_and_cleanup();
+		return;
+	}
+
+	perf_begin(_loop_perf);
+	perf_count(_loop_interval_perf);
+
+	if (_spi_led_sub.updated()) {
+		struct spi_led_s led_struct;
+
+		if (_spi_led_sub.copy(&led_struct)) {
+
+			uint32_t size = led_struct.number_leds + 11;
+			if(size != _size) {
+				_size = size;
+				delete _buf;
+				delete _rbuf;
+				_buf = new uint32_t[size];
+				_rbuf = new uint32_t[size];
+				for(uint32_t i = 0; i < size; i++) {
+					_buf[i] = 0x00000000;
+				}
+				_buf[size-1] = 0xFFFFFFFF;
+			}
+
+			//printf("offset: %d\n", (led_struct.offset_group*10)+1);
+			for(uint32_t i = 0; i < 10; i++) {
+				if ((i + (led_struct.offset_group*10)) < led_struct.number_leds) {
+					_buf[i + (1+led_struct.offset_group)*10] = led_struct.led_values[i];
+				}
+			}
+
+
+			transfer((uint8_t *)_buf, (uint8_t *)_rbuf, _size*4);
+
+		}
+	}
+
+	perf_end(_loop_perf);
+}
+
+int spi_led::task_spawn(int argc, char *argv[])
+{
+	spi_led *instance = new spi_led(1, 0, 4000000, SPIDEV_MODE0);
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init()) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+int spi_led::print_status()
+{
+	perf_print_counter(_loop_perf);
+	perf_print_counter(_loop_interval_perf);
+	return 0;
+}
+
+int spi_led::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int spi_led::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+spi_led driver for leds like apa102c and sk9822.
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("spi_led", "driver");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int spi_led_main(int argc, char *argv[])
+{
+	return spi_led::main(argc, argv);
+}

src/drivers/spi_led/spi_led.hpp

@@ -0,0 +1,94 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/* Authors: Landon Haugh (landon.haugh@nxp.com)
+ *	    Benjamin Perseghetti (benjamin.perseghetti@nxp.com)
+ */
+
+#pragma once
+
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+
+#include <drivers/drv_hrt.h>
+#include <lib/perf/perf_counter.h>
+
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/spi_led.h>
+#include <drivers/device/spi.h>
+
+using namespace time_literals;
+
+class spi_led : public device::SPI, public ModuleBase<spi_led>, public ModuleParams, public px4::ScheduledWorkItem
+{
+public:
+	spi_led(int bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode);
+	~spi_led() override;
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	int init();
+
+	int print_status() override;
+
+private:
+	void Run() override;
+
+	// Subscriptions
+	uORB::SubscriptionCallbackWorkItem _spi_led_sub{this, ORB_ID(spi_led)};        // subscription that schedules spi_led when updated
+
+	// Performance (perf) counters
+	perf_counter_t	_loop_perf{perf_alloc(PC_ELAPSED, "spi_led: cycle")};
+	perf_counter_t	_loop_interval_perf{perf_alloc(PC_INTERVAL, "spi_led: interval")};
+
+	// Parameters
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::SYS_AUTOSTART>) _param_sys_autostart,   /**< example parameter */
+		(ParamInt<px4::params::SYS_AUTOCONFIG>) _param_sys_autoconfig  /**< another parameter */
+	)
+
+	uint32_t *_buf = new uint32_t[10];
+	uint32_t *_rbuf = new uint32_t[10];
+	uint32_t _size = 0;
+};

src/examples/pca_pwm_test/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_module(
+	MODULE examples__pca_pwm_test
+	MAIN pca_pwm_test
+	COMPILE_FLAGS
+		#-DDEBUG_BUILD   # uncomment for PX4_DEBUG output
+		#-O0             # uncomment when debugging
+	SRCS
+		PCAPWMTest.cpp
+		PCAPWMTest.hpp
+	DEPENDS
+		px4_work_queue
+	)

src/examples/pca_pwm_test/Kconfig

@@ -0,0 +1,5 @@
+menuconfig EXAMPLES_PCA_PWM_TEST
+	bool "pca_pwm_test"
+	default n
+	---help---
+		Enable support for pca_pwm_test

src/examples/pca_pwm_test/PCAPWMTest.cpp

@@ -0,0 +1,141 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "PCAPWMTest.hpp"
+
+PCAPWMTest::PCAPWMTest() :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::test1)
+{
+}
+
+PCAPWMTest::~PCAPWMTest()
+{
+	perf_free(_loop_perf);
+	perf_free(_loop_interval_perf);
+}
+
+bool PCAPWMTest::init()
+{
+	ScheduleOnInterval(100_ms); // 2000 us interval, 200 Hz rate
+
+	return true;
+}
+
+void PCAPWMTest::Run()
+{
+	if (should_exit()) {
+		ScheduleClear();
+		exit_and_cleanup();
+		return;
+	}
+
+	perf_begin(_loop_perf);
+	perf_count(_loop_interval_perf);
+
+	pca_pwm.pwm_period = period_us;
+
+	if(val > period_us) val = 0;
+
+
+	for(int i = 0; i < 16; i++) {
+		pca_pwm.pulse_width[i] = val;
+	}
+
+	pca_pwm.timestamp = hrt_absolute_time();
+	_pca_pwm_pub.publish(pca_pwm);
+
+	val += period_us/20;
+
+	perf_end(_loop_perf);
+}
+
+int PCAPWMTest::task_spawn(int argc, char *argv[])
+{
+	PCAPWMTest *instance = new PCAPWMTest();
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init()) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+int PCAPWMTest::print_status()
+{
+	perf_print_counter(_loop_perf);
+	perf_print_counter(_loop_interval_perf);
+	return 0;
+}
+
+int PCAPWMTest::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int PCAPWMTest::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+Example of a simple module running out of a work queue.
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("pca_pwm_test", "template");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int pca_pwm_test_main(int argc, char *argv[])
+{
+	return PCAPWMTest::main(argc, argv);
+}

src/examples/pca_pwm_test/PCAPWMTest.hpp

@@ -0,0 +1,85 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+
+#include <drivers/drv_hrt.h>
+#include <lib/perf/perf_counter.h>
+
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/pca_pwm.h>
+
+using namespace time_literals;
+
+class PCAPWMTest : public ModuleBase<PCAPWMTest>, public ModuleParams, public px4::ScheduledWorkItem
+{
+public:
+	PCAPWMTest();
+	~PCAPWMTest() override;
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	bool init();
+
+	int print_status() override;
+
+private:
+	void Run() override;
+
+	// Publications
+	uORB::Publication<pca_pwm_s> _pca_pwm_pub{ORB_ID(pca_pwm)};
+
+	// Performance (perf) counters
+	perf_counter_t	_loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
+	perf_counter_t	_loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
+
+
+	pca_pwm_s pca_pwm;
+	uint16_t val = 0;
+	uint16_t period_us =  10000;
+};