fmu handle sensor_reset, peripheral_reset, i2c in board

ROMFS/px4fmu_common/init.d/rc.sensors

@@ -7,14 +7,6 @@
 
 if ! ver hwcmp AEROFC_V1 OMNIBUS_F4SD
 then
-	if ! ver hwcmp BITCRAZE_CRAZYFLIE
-	then
-		# Configure all I2C buses to 100 KHz as they
-		# are all external or slow
-		fmu i2c 1 100000
-		fmu i2c 2 100000
-	fi
-
 	# External SPI
 	ms5611 -S start
 

boards/px4/fmu-v4/init/rc.board_sensors

@@ -3,15 +3,6 @@
 # PX4 FMUv4 specific board sensors init
 #------------------------------------------------------------------------------
 
-# We know there are sketchy boards out there
-# as chinese companies produce Pixracers without
-# fully understanding the critical parts of the
-# schematic and BOM, leading to sensor brownouts
-# on boot. Original Pixracers following the
-# open hardware design do not require this.
-fmu sensor_reset 50
-
-
 # External I2C bus
 hmc5883 -C -T -X start
 lis3mdl -X start

boards/px4/fmu-v4/src/init.c

@@ -228,7 +228,7 @@ stm32_boardinitialize(void)
 
 #endif /* CONFIG_STM32_SPI4 */
 
-// Configure SPI all interfaces GPIO.
+	// Configure SPI all interfaces GPIO.
 	stm32_spiinitialize(spi_init_mask);
 
 	// Configure heater GPIO.
@@ -399,5 +399,15 @@ __EXPORT int board_app_initialize(uintptr_t arg)
 
 #endif
 
+	/*
+	 * We know there are sketchy boards out there
+	 * as chinese companies produce Pixracers without
+	 * fully understanding the critical parts of the
+	 * schematic and BOM, leading to sensor brownouts
+	 * on boot. Original Pixracers following the
+	 * open hardware design do not require this.
+	 */
+	board_spi_reset(50);
+
 	return OK;
 }

boards/px4/fmu-v5/default.cmake

@@ -87,6 +87,7 @@ px4_add_board(
 
 	SYSTEMCMDS
 		bl_update
+		board_control
 		config
 		dumpfile
 		esc_calib

src/drivers/px4fmu/fmu.cpp

@@ -41,7 +41,6 @@
 
 #include <board_config.h>
 #include <drivers/device/device.h>
-#include <drivers/device/i2c.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_input_capture.h>
 #include <drivers/drv_mixer.h>
@@ -50,6 +49,7 @@
 #include <px4_getopt.h>
 #include <px4_log.h>
 #include <px4_module.h>
+#include <px4_workqueue.h>
 #include <circuit_breaker/circuit_breaker.h>
 #include <lib/cdev/CDev.hpp>
 #include <lib/mixer/mixer.h>
@@ -168,8 +168,6 @@ class PX4FMU : public cdev::CDev, public ModuleBase<PX4FMU>
 	int		set_pwm_alt_rate(unsigned rate);
 	int		set_pwm_alt_channels(uint32_t channels);
 
-	static int	set_i2c_bus_clock(unsigned bus, unsigned clock_hz);
-
 	static void	capture_trampoline(void *context, uint32_t chan_index,
 					   hrt_abstime edge_time, uint32_t edge_state,
 					   uint32_t overflow);
@@ -265,9 +263,6 @@ class PX4FMU : public cdev::CDev, public ModuleBase<PX4FMU>
 
 	void		update_params();
 
-	static void		sensor_reset(int ms);
-	static void		peripheral_reset(int ms);
-
 	int		capture_ioctl(file *filp, int cmd, unsigned long arg);
 
 	PX4FMU(const PX4FMU &) = delete;
@@ -809,12 +804,6 @@ PX4FMU::set_pwm_alt_channels(uint32_t channels)
 	return set_pwm_rate(channels, _pwm_default_rate, _pwm_alt_rate);
 }
 
-int
-PX4FMU::set_i2c_bus_clock(unsigned bus, unsigned clock_hz)
-{
-	return device::I2C::set_bus_clock(bus, clock_hz);
-}
-
 void
 PX4FMU::subscribe()
 {
@@ -2263,26 +2252,6 @@ PX4FMU::reorder_outputs(uint16_t values[MAX_ACTUATORS])
 	 */
 }
 
-void
-PX4FMU::sensor_reset(int ms)
-{
-	if (ms < 1) {
-		ms = 1;
-	}
-
-	board_spi_reset(ms);
-}
-
-void
-PX4FMU::peripheral_reset(int ms)
-{
-	if (ms < 1) {
-		ms = 10;
-	}
-
-	board_peripheral_reset(ms);
-}
-
 int
 PX4FMU::capture_ioctl(struct file *filp, int cmd, unsigned long arg)
 {
@@ -2548,17 +2517,6 @@ PX4FMU::fmu_new_mode(PortMode new_mode)
 	return OK;
 }
 
-
-namespace
-{
-
-int fmu_new_i2c_speed(unsigned bus, unsigned clock_hz)
-{
-	return PX4FMU::set_i2c_bus_clock(bus, clock_hz);
-}
-
-} // namespace
-
 int
 PX4FMU::test()
 {
@@ -2817,50 +2775,6 @@ int PX4FMU::custom_command(int argc, char *argv[])
 	PortMode new_mode = PORT_MODE_UNSET;
 	const char *verb = argv[0];
 
-	/* does not operate on a FMU instance */
-	if (!strcmp(verb, "i2c")) {
-		if (argc > 2) {
-			int bus = strtol(argv[1], 0, 0);
-			int clock_hz = strtol(argv[2], 0, 0);
-			int ret = fmu_new_i2c_speed(bus, clock_hz);
-
-			if (ret) {
-				PX4_ERR("setting I2C clock failed");
-			}
-
-			return ret;
-		}
-
-		return print_usage("not enough arguments");
-	}
-
-	if (!strcmp(verb, "sensor_reset")) {
-		if (argc > 1) {
-			int reset_time = strtol(argv[1], nullptr, 0);
-			sensor_reset(reset_time);
-
-		} else {
-			sensor_reset(0);
-			PX4_INFO("reset default time");
-		}
-
-		return 0;
-	}
-
-	if (!strcmp(verb, "peripheral_reset")) {
-		if (argc > 2) {
-			int reset_time = strtol(argv[2], 0, 0);
-			peripheral_reset(reset_time);
-
-		} else {
-			peripheral_reset(0);
-			PX4_INFO("reset default time");
-		}
-
-		return 0;
-	}
-
-
 	/* start the FMU if not running */
 	if (!is_running()) {
 		int ret = PX4FMU::task_spawn(argc, argv);
@@ -3019,14 +2933,6 @@ mixer files.
   PRINT_MODULE_USAGE_COMMAND("mode_pwm1");
 #endif
 
-	PRINT_MODULE_USAGE_COMMAND_DESCR("sensor_reset", "Do a sensor reset (SPI bus)");
-	PRINT_MODULE_USAGE_ARG("<ms>", "Delay time in ms between reset and re-enabling", true);
-	PRINT_MODULE_USAGE_COMMAND_DESCR("peripheral_reset", "Reset board peripherals");
-	PRINT_MODULE_USAGE_ARG("<ms>", "Delay time in ms between reset and re-enabling", true);
-
-	PRINT_MODULE_USAGE_COMMAND_DESCR("i2c", "Configure I2C clock rate");
-	PRINT_MODULE_USAGE_ARG("<bus_id> <rate>", "Specify the bus id (>=0) and rate in Hz", false);
-
 	PRINT_MODULE_USAGE_COMMAND_DESCR("test", "Test inputs and outputs");
 	PRINT_MODULE_USAGE_COMMAND_DESCR("fake", "Arm and send an actuator controls command");
 	PRINT_MODULE_USAGE_ARG("<roll> <pitch> <yaw> <thrust>", "Control values in range [-100, 100]", false);

src/systemcmds/board_control/CMakeLists.txt

@@ -0,0 +1,43 @@
+############################################################################
+#
+#   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 systemcmds__board_control
+	MAIN board_control
+	STACK_MAIN 1200
+	COMPILE_FLAGS
+	SRCS
+		board_control.cpp
+	DEPENDS
+
+	)

src/systemcmds/board_control/board_control.cpp

@@ -0,0 +1,116 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include <px4_config.h>
+#include <px4_log.h>
+#include <px4_module.h>
+
+#include <drivers/device/device.h>
+#include <drivers/device/i2c.h>
+
+extern "C" __EXPORT int board_control_main(int argc, char *argv[]);
+
+static int print_usage(const char *reason = nullptr);
+
+int
+board_control_main(int argc, char *argv[])
+{
+	const char *verb = argv[0];
+
+	/* does not operate on a FMU instance */
+	if (!strcmp(verb, "i2c")) {
+		if (argc > 2) {
+			int bus = strtol(argv[1], 0, 0);
+			int clock_hz = strtol(argv[2], 0, 0);
+			int ret = device::I2C::set_bus_clock(bus, clock_hz);
+
+			if (ret) {
+				PX4_ERR("setting I2C clock failed");
+			}
+
+			return ret;
+		}
+
+		return print_usage("not enough arguments");
+	}
+
+	if (!strcmp(verb, "sensor_reset")) {
+		if (argc > 1) {
+			int reset_time = strtol(argv[1], nullptr, 0);
+			board_spi_reset(reset_time);
+
+		} else {
+			board_spi_reset(10);
+			PX4_INFO("reset default time");
+		}
+
+		return 0;
+	}
+
+	if (!strcmp(verb, "peripheral_reset")) {
+		if (argc > 2) {
+			int reset_time = strtol(argv[2], 0, 0);
+			board_peripheral_reset(reset_time);
+
+		} else {
+			board_peripheral_reset(10);
+			PX4_INFO("reset default time");
+		}
+
+		return 0;
+	}
+
+	return print_usage();
+}
+
+static int print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION("board control (sensor bus power, etc)");
+
+	PRINT_MODULE_USAGE_NAME("board", "command");
+
+	PRINT_MODULE_USAGE_COMMAND_DESCR("sensor_reset", "Do a sensor reset (SPI bus)");
+	PRINT_MODULE_USAGE_ARG("<ms>", "Delay time in ms between reset and re-enabling", true);
+
+	PRINT_MODULE_USAGE_COMMAND_DESCR("peripheral_reset", "Reset board peripherals");
+	PRINT_MODULE_USAGE_ARG("<ms>", "Delay time in ms between reset and re-enabling", true);
+
+	PRINT_MODULE_USAGE_COMMAND_DESCR("i2c", "Configure I2C clock rate");
+	PRINT_MODULE_USAGE_ARG("<bus_id> <rate>", "Specify the bus id (>=0) and rate in Hz", false);
+
+	return 0;
+}