refactor bst telemetry: use driver base class

ROMFS/px4fmu_common/init.d/rcS

@@ -476,7 +476,7 @@ else
 	# Blacksheep telemetry
 	if param greater TEL_BST_EN 0
 	then
-		bst start
+		bst start -X
 	fi
 
 	#

ROMFS/px4fmu_test/init.d/rc.sensors

@@ -13,7 +13,7 @@ then
 fi
 
 # Blacksheep telemetry
-if bst start
+if bst start -X
 then
 fi
 

src/drivers/drv_sensor.h

@@ -146,6 +146,7 @@
 #define DRV_BAT_DEVTYPE_SMBUS         0x7c
 #define DRV_SENS_DEVTYPE_IRLOCK       0x7d
 #define DRV_SENS_DEVTYPE_PCF8583      0x7e
+#define DRV_TEL_DEVTYPE_BST           0x7f
 
 #define DRV_DEVTYPE_UNUSED		0xff
 

src/drivers/telemetry/bst/bst.cpp

@@ -41,7 +41,8 @@
 
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/getopt.h>
-#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/i2c_spi_buses.h>
 #include <drivers/device/i2c.h>
 #include <systemlib/err.h>
 #include <string.h>
@@ -54,8 +55,7 @@
 #include <matrix/math.hpp>
 
 using namespace matrix;
-
+using namespace time_literals;
-static const char commandline_usage[] = "usage: bst start|status|stop";
 
 #define BST_ADDR		0x76
 
@@ -108,37 +108,29 @@ struct BSTBattery {
 
 #pragma pack(pop)
 
-class BST : public device::I2C, public px4::ScheduledWorkItem
+class BST : public device::I2C, public I2CSPIDriver<BST>
 {
 public:
-	BST(int bus);
+	BST(I2CSPIBusOption bus_option, const int bus, int address, int bus_frequency);
+	~BST() override = default;
 
-	virtual ~BST();
+	static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
+					     int runtime_instance);
+	static void print_usage();
 
-	virtual int		init();
+	int		init() override;
 
-	virtual int		probe();
+	int		probe() override;
-
-	virtual int		info() { return 0; }
-
-	virtual int		ioctl(device::file_t *filp, int cmd, unsigned long arg) { return 0; }
-
-	void stop();
-
-	static void		start_trampoline(void *arg);
 
+	void			RunImpl();
 private:
 
-	bool			_should_run = false;
+	static constexpr unsigned		_interval{100_ms};
-	unsigned		_interval = 100;
 
 	uORB::Subscription	_gps_sub{ORB_ID(vehicle_gps_position)};
 	uORB::Subscription	_attitude_sub{ORB_ID(vehicle_attitude)};
 	uORB::Subscription	_battery_sub{ORB_ID(battery_status)};
 
-	void			start();
-
-	void			Run() override;
 
 	template <typename T>
 	void			send_packet(BSTPacket<T> &packet)
@@ -187,21 +179,12 @@ private:
 	}
 };
 
-static BST *g_bst = nullptr;
+BST::BST(I2CSPIBusOption bus_option, const int bus, int address, int bus_frequency) :
-
+	I2C("bst", nullptr, bus, address, bus_frequency),
-BST::BST(int bus) :
+	I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address)
-	I2C("bst", nullptr, bus, BST_ADDR, 100000),
-	ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id()))
 {
 }
 
-BST::~BST()
-{
-	ScheduleClear();
-
-	_should_run = false;
-}
-
 int BST::probe()
 {
 	int retries_prev = _retries;
@@ -214,7 +197,7 @@ int BST::probe()
 	send_packet(dev_info_req, dev_info_reply);
 
 	if (dev_info_reply.type != 0x05) {
-		warnx("no devices found");
+		PX4_ERR("no devices found");
 		return -EIO;
 	}
 
@@ -223,14 +206,14 @@ int BST::probe()
 	uint8_t crc_recv = reply_raw[dev_info_reply.length];
 
 	if (crc_recv != crc_calc) {
-		warnx("CRC error: got %02x, should be %02x", (int)crc_recv, (int)crc_calc);
+		PX4_ERR("CRC error: got %02x, should be %02x", (int)crc_recv, (int)crc_calc);
 		return -EIO;
 	}
 
 	dev_info_reply.payload.dev_name[dev_info_reply.payload.dev_name_len] = '\0';
-	warnx("device info: hardware ID: 0x%08X, firmware ID: 0x%04X, device name: %s",
+	PX4_DEBUG("device info: hardware ID: 0x%08X, firmware ID: 0x%04X, device name: %s",
-	      (int)swap_uint32(dev_info_reply.payload.hw_id), (int)swap_uint16(dev_info_reply.payload.fw_id),
+		  (int)swap_uint32(dev_info_reply.payload.hw_id), (int)swap_uint16(dev_info_reply.payload.fw_id),
-	      dev_info_reply.payload.dev_name);
+		  dev_info_reply.payload.dev_name);
 
 	_retries = retries_prev;
 
@@ -250,65 +233,58 @@ int BST::init()
 	return OK;
 }
 
-void BST::Run()
+void BST::RunImpl()
 {
-	if (!_should_run) {
+	if (_attitude_sub.updated()) {
-		_should_run = true;
+		vehicle_attitude_s att;
-		set_device_address(0x00); // General call address
+		_attitude_sub.copy(&att);
+		Quatf q(att.q);
+		Eulerf euler(q);
+
+		BSTPacket<BSTAttitude> bst_att = {};
+		bst_att.type = 0x1E;
+		bst_att.payload.roll = swap_int32(euler.phi() * 10000);
+		bst_att.payload.pitch = swap_int32(euler.theta() * 10000);
+		bst_att.payload.yaw = swap_int32(euler.psi() * 10000);
+
+		send_packet(bst_att);
 	}
 
-	if (_should_run) {
+	if (_battery_sub.updated()) {
-		if (_attitude_sub.updated()) {
+		battery_status_s batt;
-			vehicle_attitude_s att;
+		_battery_sub.copy(&batt);
-			_attitude_sub.copy(&att);
-			Quatf q(att.q);
-			Eulerf euler(q);
 
-			BSTPacket<BSTAttitude> bst_att = {};
+		BSTPacket<BSTBattery> bst_batt = {};
-			bst_att.type = 0x1E;
+		bst_batt.type = 0x08;
-			bst_att.payload.roll = swap_int32(euler.phi() * 10000);
+		bst_batt.payload.voltage = swap_uint16(batt.voltage_v * 10.0f);
-			bst_att.payload.pitch = swap_int32(euler.theta() * 10000);
+		bst_batt.payload.current = swap_uint16(batt.current_a * 10.0f);
-			bst_att.payload.yaw = swap_int32(euler.psi() * 10000);
+		uint32_t discharged = batt.discharged_mah;
+		bst_batt.payload.capacity[0] = static_cast<uint8_t>(discharged >> 16);
+		bst_batt.payload.capacity[1] = static_cast<uint8_t>(discharged >> 8);
+		bst_batt.payload.capacity[2] = static_cast<uint8_t>(discharged);
 
-			send_packet(bst_att);
+		send_packet(bst_batt);
-		}
-
-		if (_battery_sub.updated()) {
-			battery_status_s batt;
-			_battery_sub.copy(&batt);
-
-			BSTPacket<BSTBattery> bst_batt = {};
-			bst_batt.type = 0x08;
-			bst_batt.payload.voltage = swap_uint16(batt.voltage_v * 10.0f);
-			bst_batt.payload.current = swap_uint16(batt.current_a * 10.0f);
-			uint32_t discharged = batt.discharged_mah;
-			bst_batt.payload.capacity[0] = static_cast<uint8_t>(discharged >> 16);
-			bst_batt.payload.capacity[1] = static_cast<uint8_t>(discharged >> 8);
-			bst_batt.payload.capacity[2] = static_cast<uint8_t>(discharged);
-
-			send_packet(bst_batt);
-		}
-
-		if (_gps_sub.updated()) {
-			vehicle_gps_position_s gps;
-			_gps_sub.copy(&gps);
-
-			if (gps.fix_type >= 3 && gps.eph < 50.0f) {
-				BSTPacket<BSTGPSPosition> bst_gps = {};
-				bst_gps.type = 0x02;
-				bst_gps.payload.lat = swap_int32(gps.lat);
-				bst_gps.payload.lon = swap_int32(gps.lon);
-				bst_gps.payload.alt = swap_int16(gps.alt / 1000 + 1000);
-				bst_gps.payload.gs = swap_int16(gps.vel_m_s * 360.0f);
-				bst_gps.payload.heading = swap_int16(gps.cog_rad * 18000.0f / M_PI_F);
-				bst_gps.payload.sats = gps.satellites_used;
-
-				send_packet(bst_gps);
-			}
-		}
-
-		ScheduleDelayed(_interval);
 	}
+
+	if (_gps_sub.updated()) {
+		vehicle_gps_position_s gps;
+		_gps_sub.copy(&gps);
+
+		if (gps.fix_type >= 3 && gps.eph < 50.0f) {
+			BSTPacket<BSTGPSPosition> bst_gps = {};
+			bst_gps.type = 0x02;
+			bst_gps.payload.lat = swap_int32(gps.lat);
+			bst_gps.payload.lon = swap_int32(gps.lon);
+			bst_gps.payload.alt = swap_int16(gps.alt / 1000 + 1000);
+			bst_gps.payload.gs = swap_int16(gps.vel_m_s * 360.0f);
+			bst_gps.payload.heading = swap_int16(gps.cog_rad * 18000.0f / M_PI_F);
+			bst_gps.payload.sats = gps.satellites_used;
+
+			send_packet(bst_gps);
+		}
+	}
+
+	ScheduleDelayed(_interval);
 }
 
 uint8_t BST::crc8(uint8_t *data, size_t len)
@@ -331,53 +307,62 @@ uint8_t BST::crc8(uint8_t *data, size_t len)
 
 using namespace px4::bst;
 
+void
+BST::print_usage()
+{
+	PRINT_MODULE_USAGE_NAME("bst", "driver");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
+	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x76);
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+}
+
+I2CSPIDriverBase *BST::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
+				   int runtime_instance)
+{
+	BST *instance = new BST(iterator.configuredBusOption(), iterator.bus(), cli.i2c_address, cli.bus_frequency);
+
+	if (instance == nullptr) {
+		PX4_ERR("alloc failed");
+		return nullptr;
+	}
+
+	if (instance->init() != PX4_OK) {
+		delete instance;
+		return nullptr;
+	}
+
+	return instance;
+}
+
 extern "C" __EXPORT int bst_main(int argc, char *argv[])
 {
-	if (argc < 2) {
+	using ThisDriver = BST;
-		errx(1, "missing command\n%s", commandline_usage);
+	BusCLIArguments cli{true, false};
+	cli.i2c_address = BST_ADDR;
+	cli.default_i2c_frequency = 100000;
+
+	const char *verb = cli.parseDefaultArguments(argc, argv);
+
+	if (!verb) {
+		ThisDriver::print_usage();
+		return -1;
 	}
 
-	if (!strcmp(argv[1], "start")) {
+	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_TEL_DEVTYPE_BST);
 
-		if (g_bst) {
+	if (!strcmp(verb, "start")) {
-			warnx("already running");
+		return ThisDriver::module_start(cli, iterator);
-			exit(0);
-		}
-
-		g_bst = new BST(PX4_I2C_BUS_EXPANSION);
-
-		if (g_bst != nullptr && OK != g_bst->init()) {
-			delete g_bst;
-			g_bst = nullptr;
-		}
-
-		exit(0);
 	}
 
-	if (!strcmp(argv[1], "stop")) {
+	if (!strcmp(verb, "stop")) {
-		if (!g_bst) {
+		return ThisDriver::module_stop(iterator);
-			warnx("not running");
-			exit(0);
-		}
-
-		delete g_bst;
-		g_bst = nullptr;
-		warnx("stopped");
-
-		exit(0);
 	}
 
-	if (!strcmp(argv[1], "status")) {
+	if (!strcmp(verb, "status")) {
-		if (g_bst) {
+		return ThisDriver::module_status(iterator);
-			warnx("is running");
-
-		} else {
-			warnx("is not running");
-		}
-
-		exit(0);
 	}
 
-	PX4_WARN("unrecognized command\n%s", commandline_usage);
+	ThisDriver::print_usage();
-	return 1;
+	return -1;
 }