make demo work with pca.

Tools/flash_nxp/flash_ucan.jlink

@@ -0,0 +1,7 @@
+if SWD
+speed 1000
+r
+loadbin /Users/landon/git/px4/build/nxp_ucans32k146_nxp_demo/deploy/34.bin,0x6000
+r
+g
+q

Tools/flash_nxp/flash_ucan.sh

@@ -0,0 +1,3 @@
+#!/bin/zsh
+
+JLinkExe -device S32K146 -CommandFile /Users/landon/git/px4/Tools/flash_nxp/flash_ucan.jlink

boards/nxp/ucans32k146/init/rc.board_defaults

@@ -4,12 +4,14 @@
 #------------------------------------------------------------------------------
 
 # pre-arm stuff
-param set-default SYS_CTRL_ALLOC 1
+#param set-default SYS_CTRL_ALLOC 1
 
-pwm_out start
+#pwm_out start
 
-control_allocator start
+#control_allocator start
 
 ifup can0
 cyphal start
 lighting_state_converter start
+actuator_controls_driver start
+pca9685 start -X

boards/nxp/ucans32k146/nxp_demo.px4board

@@ -1,9 +1,9 @@
-CONFIG_DRIVERS_LIGHTS_APA102=y
-CONFIG_DRIVERS_CYPHAL=y
-CONFIG_DRIVERS_UAVCANNODE_GPS_DEMO=n
-CONFIG_MODULES_LIGHTING_STATE_CONVERTER=y
-CONFIG_CYPHAL_NODE_CLIENT=y
-CONFIG_CYPHAL_UORB_LIGHTING_STATES_SUBSCRIBER=y
 CONFIG_DRIVERS_GPS=n
+CONFIG_DRIVERS_LIGHTS_APA102=y
+CONFIG_DRIVERS_PCA9685=y
+CONFIG_DRIVERS_PCA9685_PWM_OUT=y
+CONFIG_EXAMPLES_ACTUATOR_CONTROLS_DRIVER=y
+CONFIG_MODULES_LIGHTING_STATE_CONVERTER=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_MOTOR_TEST=y
+CONFIG_SYSTEMCMDS_UORB=y

msg/actuator_controls.msg

@@ -1,5 +1,5 @@
 uint64 timestamp			# time since system start (microseconds)
-uint8 NUM_ACTUATOR_CONTROLS = 8
+uint8 NUM_ACTUATOR_CONTROLS = 16
 uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
 uint8 INDEX_ROLL = 0
 uint8 INDEX_PITCH = 1
@@ -20,7 +20,7 @@ uint8 GROUP_INDEX_MANUAL_PASSTHROUGH = 3
 uint8 GROUP_INDEX_PAYLOAD = 6
 
 uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
-float32[9] control
+float32[16] control
 
 # TOPICS actuator_controls actuator_controls_0 actuator_controls_1 actuator_controls_2 actuator_controls_3
 # TOPICS actuator_controls_virtual_fw actuator_controls_virtual_mc

platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/io_timer.h

@@ -45,7 +45,7 @@
 #pragma once
 __BEGIN_DECLS
 /* configuration limits */
-#define MAX_IO_TIMERS			1
+#define MAX_IO_TIMERS			2
 #define MAX_TIMER_IO_CHANNELS		2
 
 #define MAX_LED_TIMERS			1

src/drivers/pca9685/pca9685.cpp

@@ -93,18 +93,21 @@
 
 #define ADDR 0x40	// I2C adress
 
-#define PCA9685_PWMFREQ 60.0f
+#define PCA9685_PWMFREQ 400
 #define PCA9685_NCHANS 16 // total amount of pwm outputs
 
-#define PCA9685_PWMMIN 150 // this is the 'minimum' pulse length count (out of 4096)
-#define PCA9685_PWMMAX 600 // this is the 'maximum' pulse length count (out of 4096)_PWMFREQ 60.0f
+//#define PCA9685_PWMINT (1.0f / PCA9685_PWMFREQ) * 1000000
 
-#define PCA9685_PWMCENTER ((PCA9685_PWMMAX + PCA9685_PWMMIN)/2)
+#define PCA9685_PWMMIN (1000 / PCA9685_PWMINT) * 4096.0f // this is the 'minimum' pulse length count (out of 4096)
+#define PCA9685_PWMMAX (2000 / PCA9685_PWMINT) * 4096.0f // this is the 'maximum' pulse length count (out of 4096)
+
+#define PCA9685_PWMCENTER 308 //(PCA9685_PWMMAX + PCA9685_PWMMIN)/2
 #define PCA9685_MAXSERVODEG 90.0f /* maximal servo deflection in degrees
 				     PCA9685_PWMMIN <--> -PCA9685_MAXSERVODEG
 				     PCA9685_PWMMAX <--> PCA9685_MAXSERVODEG
 				     */
-#define PCA9685_SCALE ((PCA9685_PWMMAX - PCA9685_PWMCENTER)/(M_DEG_TO_RAD_F * PCA9685_MAXSERVODEG)) // scales from rad to PWM
+#define PCA9685_SCALE (PCA9685_PWMMAX - PCA9685_PWMMIN) / 2 // scales from rad to PWM
+
 
 enum IOX_MODE {
 	IOX_MODE_ON,
@@ -232,7 +235,7 @@ PCA9685::RunImpl()
 			/* Subscribe to actuator control 2 (payload group for gimbal) */
 			_actuator_controls_sub = orb_subscribe(ORB_ID(actuator_controls_2));
 			/* set the uorb update interval lower than the driver pwm interval */
-			orb_set_interval(_actuator_controls_sub, 1000.0f / PCA9685_PWMFREQ - 5);
+			orb_set_interval(_actuator_controls_sub, 1.05f);
 
 			_mode_on_initialized = true;
 		}
@@ -243,18 +246,40 @@ PCA9685::RunImpl()
 
 		if (updated) {
 			orb_copy(ORB_ID(actuator_controls_2), _actuator_controls_sub, &_actuator_controls);
+			//PX4_INFO("freq: %.2f, int: %.2f, max: %d, min: %d, pwmcenter: %d, control: %.2f, scale: %d", (double)PCA9685_PWMFREQ, (double)PCA9685_PWMINT, PCA9685_PWMMAX, PCA9685_PWMMIN,
+			// 	PCA9685_PWMCENTER, (double)_actuator_controls.control[0], PCA9685_SCALE);
+
+			/*
+			#define PCA9685_PWMFREQ 50.0f
+			#define PCA9685_NCHANS 16 // total amount of pwm outputs
+
+			//#define PCA9685_PWMINT (1.0f / PCA9685_PWMFREQ) * 1000000
+
+			#define PCA9685_PWMMIN (1000 / PCA9685_PWMINT) * 4096.0f // this is the 'minimum' pulse length count (out of 4096)
+			#define PCA9685_PWMMAX (2000 / PCA9685_PWMINT) * 4096.0f // this is the 'maximum' pulse length count (out of 4096)
+
+			#define PCA9685_PWMCENTER 308 //(PCA9685_PWMMAX + PCA9685_PWMMIN)/2
+
+			#define PCA9685_SCALE (PCA9685_PWMMAX - PCA9685_PWMMIN) / 2 // scales from rad to PWM
+			*/
+
+			float pwm_int = (1.0f / PCA9685_PWMFREQ) * 1000000;
+			float pwm_min = (1000 / pwm_int) * 4096;
+			float pwm_max = (2000 / pwm_int) * 4096;
+			float pwm_center = (pwm_max + pwm_min) / 2;
+			float pwm_scale = (pwm_max - pwm_min) / 2;
 
 			for (int i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROLS; i++) {
 				/* Scale the controls to PWM, first multiply by pi to get rad,
 				 * the control[i] values are on the range -1 ... 1 */
-				uint16_t new_value = PCA9685_PWMCENTER +
-						     (_actuator_controls.control[i] * M_PI_F * PCA9685_SCALE);
-				DEVICE_DEBUG("%d: current: %u, new %u, control %.2f", i, _current_values[i], new_value,
-					     (double)_actuator_controls.control[i]);
+				uint16_t new_value = pwm_center +
+						     (_actuator_controls.control[i] * pwm_scale);
+				//PX4_INFO("%d: current: %u, new %u, control %.2f", i, _current_values[i], new_value,
+				//	     (double)_actuator_controls.control[i]);
 
 				if (new_value != _current_values[i] &&
-				    new_value >= PCA9685_PWMMIN &&
-				    new_value <= PCA9685_PWMMAX) {
+				    new_value >= pwm_min &&
+				    new_value <= pwm_max) {
 					/* This value was updated, send the command to adjust the PWM value */
 					setPin(i, new_value);
 					_current_values[i] = new_value;
@@ -330,8 +355,8 @@ int
 PCA9685::setPWMFreq(float freq)
 {
 	int ret  = OK;
-	freq *= 0.9f;  /* Correct for overshoot in the frequency setting (see issue
-		https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library/issues/11). */
+	//freq *= 0.9f;  /* Correct for overshoot in the frequency setting (see issue
+		//https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library/issues/11). */
 	float prescaleval = 25000000;
 	prescaleval /= 4096;
 	prescaleval /= freq;

src/examples/actuator_controls_driver/ActuatorControlsDriver.cpp

@@ -0,0 +1,196 @@
+/****************************************************************************
+ *
+ *   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 "ActuatorControlsDriver.hpp"
+
+ActuatorControlsDriver::ActuatorControlsDriver() :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::test1)
+{
+}
+
+ActuatorControlsDriver::~ActuatorControlsDriver()
+{
+	perf_free(_loop_perf);
+	perf_free(_loop_interval_perf);
+}
+
+bool ActuatorControlsDriver::init()
+{
+	// execute Run() on every sensor_accel publication
+	/*
+	if (!_sensor_accel_sub.registerCallback()) {
+		PX4_ERR("callback registration failed");
+		return false;
+	}
+	*/
+	// alternatively, Run on fixed interval
+	ScheduleOnInterval(500_ms); // 2000 us interval, 200 Hz rate
+
+	return true;
+}
+
+void ActuatorControlsDriver::Run()
+{
+	if (should_exit()) {
+		ScheduleClear();
+		exit_and_cleanup();
+		return;
+	}
+
+	perf_begin(_loop_perf);
+	perf_count(_loop_interval_perf);
+
+	// Check if parameters have changed
+	if (_parameter_update_sub.updated()) {
+		// clear update
+		parameter_update_s param_update;
+		_parameter_update_sub.copy(&param_update);
+		updateParams(); // update module parameters (in DEFINE_PARAMETERS)
+	}
+
+
+	// Example
+	//  update vehicle_status to check arming state
+	/*
+	if (_vehicle_status_sub.updated()) {
+		vehicle_status_s vehicle_status;
+
+		if (_vehicle_status_sub.copy(&vehicle_status)) {
+
+			const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+
+			if (armed && !_armed) {
+				PX4_WARN("vehicle armed due to %d", vehicle_status.latest_arming_reason);
+
+			} else if (!armed && _armed) {
+				PX4_INFO("vehicle disarmed due to %d", vehicle_status.latest_disarming_reason);
+			}
+
+			_armed = armed;
+		}
+	}
+	*/
+
+	// Example
+	//  grab latest accelerometer data
+	/*
+	if (_sensor_accel_sub.updated()) {
+		sensor_accel_s accel;
+
+		if (_sensor_accel_sub.copy(&accel)) {
+			// DO WORK
+
+			// access parameter value (SYS_AUTOSTART)
+			if (_param_sys_autostart.get() == 1234) {
+				// do something if SYS_AUTOSTART is 1234
+			}
+		}
+	}
+	*/
+
+	// Example
+	//  publish some data
+	if(loop > 10) loop = -10;
+
+	for(int i = 0; i < 16; i++) {
+		actuator_controls.control[i] = 0 + (loop * 0.1);
+	}
+
+	actuator_controls.timestamp = hrt_absolute_time();
+	_actuator_controls_pub.publish(actuator_controls);
+
+	loop = loop + 1;
+
+	perf_end(_loop_perf);
+}
+
+int ActuatorControlsDriver::task_spawn(int argc, char *argv[])
+{
+	ActuatorControlsDriver *instance = new ActuatorControlsDriver();
+
+	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 ActuatorControlsDriver::print_status()
+{
+	perf_print_counter(_loop_perf);
+	perf_print_counter(_loop_interval_perf);
+	return 0;
+}
+
+int ActuatorControlsDriver::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int ActuatorControlsDriver::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("actuator_controls_driver", "template");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int actuator_controls_driver_main(int argc, char *argv[])
+{
+	return ActuatorControlsDriver::main(argc, argv);
+}

src/examples/actuator_controls_driver/ActuatorControlsDriver.hpp

@@ -0,0 +1,100 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#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/actuator_controls.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/sensor_accel.h>
+#include <uORB/topics/vehicle_status.h>
+
+using namespace time_literals;
+
+class ActuatorControlsDriver : public ModuleBase<ActuatorControlsDriver>, public ModuleParams, public px4::ScheduledWorkItem
+{
+public:
+	ActuatorControlsDriver();
+	~ActuatorControlsDriver() 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<actuator_controls_s> _actuator_controls_pub{ORB_ID(actuator_controls_2)};
+
+	// Subscriptions
+
+	//uORB::SubscriptionCallbackWorkItem _sensor_accel_sub{this, ORB_ID(sensor_accel)};        // subscription that schedules ActuatorControlsDriver when updated
+	uORB::SubscriptionInterval         _parameter_update_sub{ORB_ID(parameter_update), 1_s}; // subscription limited to 1 Hz updates
+	//uORB::Subscription                 _vehicle_status_sub{ORB_ID(vehicle_status)};          // regular subscription for additional data
+
+
+	// 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")};
+
+	// Parameters
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::SYS_AUTOSTART>) _param_sys_autostart,   /**< example parameter */
+		(ParamInt<px4::params::SYS_AUTOCONFIG>) _param_sys_autoconfig  /**< another parameter */
+	)
+
+	actuator_controls_s actuator_controls;
+	int loop = 0;
+	//bool _armed{false};
+};

src/examples/actuator_controls_driver/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 examples__actuator_controls_driver
+	MAIN actuator_controls_driver
+	COMPILE_FLAGS
+		#-DDEBUG_BUILD   # uncomment for PX4_DEBUG output
+		#-O0             # uncomment when debugging
+	SRCS
+		ActuatorControlsDriver.cpp
+		ActuatorControlsDriver.hpp
+	DEPENDS
+		px4_work_queue
+	)

src/examples/actuator_controls_driver/Kconfig

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