From ec061a7cfd5cf7432ba0400d635a8f7e98b3314a Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Sat, 28 Sep 2019 18:19:38 -0400
Subject: [PATCH] telemetry/bst: move to uORB::Subscription

---
 src/drivers/telemetry/bst/bst.cpp | 55 ++++++++++---------------------
 1 file changed, 18 insertions(+), 37 deletions(-)

diff --git a/src/drivers/telemetry/bst/bst.cpp b/src/drivers/telemetry/bst/bst.cpp
index 9f327ddbbf..645592afd9 100644
--- a/src/drivers/telemetry/bst/bst.cpp
+++ b/src/drivers/telemetry/bst/bst.cpp
@@ -47,6 +47,7 @@
 #include <string.h>
 #include <stdlib.h>
 #include <math.h>
+#include <uORB/Subscription.hpp>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/vehicle_attitude.h>
@@ -54,12 +55,8 @@
 
 using namespace matrix;
 
-#define BST_DEVICE_PATH "/dev/bst0"
-
 static const char commandline_usage[] = "usage: bst start|status|stop";
 
-extern "C" __EXPORT int bst_main(int argc, char *argv[]);
-
 #define BST_ADDR		0x76
 
 namespace px4
@@ -134,16 +131,15 @@ private:
 
 	bool			_should_run = false;
 	unsigned		_interval = 100;
-	int				_gps_sub;
-	int				_attitude_sub;
-	int				_battery_sub;
+
+	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;
 
-	void			cycle();
-
 	template <typename T>
 	void			send_packet(BSTPacket<T> &packet)
 	{
@@ -194,7 +190,7 @@ private:
 static BST *g_bst = nullptr;
 
 BST::BST(int bus) :
-	I2C("bst", BST_DEVICE_PATH, bus, BST_ADDR, 100000),
+	I2C("bst", nullptr, bus, BST_ADDR, 100000),
 	ScheduledWorkItem(px4::device_bus_to_wq(get_device_id()))
 {
 }
@@ -258,43 +254,29 @@ void BST::Run()
 {
 	if (!_should_run) {
 		_should_run = true;
-
-		_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-		_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
-		_battery_sub = orb_subscribe(ORB_ID(battery_status));
-
 		set_device_address(0x00); // General call address
 	}
 
-	cycle();
-}
-
-void BST::cycle()
-{
 	if (_should_run) {
-		bool updated = false;
-
-		orb_check(_attitude_sub, &updated);
-
-		if (updated) {
+		if (_attitude_sub.updated()) {
 			vehicle_attitude_s att;
-			orb_copy(ORB_ID(vehicle_attitude), _attitude_sub, &att);
+			_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);
 		}
 
-		updated = false;
-		orb_check(_battery_sub, &updated);
-
-		if (updated) {
+		if (_battery_sub.updated()) {
 			battery_status_s batt;
-			orb_copy(ORB_ID(battery_status), _battery_sub, &batt);
+			_battery_sub.copy(&batt);
+
 			BSTPacket<BSTBattery> bst_batt = {};
 			bst_batt.type = 0x08;
 			bst_batt.payload.voltage = swap_uint16(batt.voltage_v * 10.0f);
@@ -303,15 +285,13 @@ void BST::cycle()
 			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);
 		}
 
-		updated = false;
-		orb_check(_gps_sub, &updated);
-
-		if (updated) {
+		if (_gps_sub.updated()) {
 			vehicle_gps_position_s gps;
-			orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &gps);
+			_gps_sub.copy(&gps);
 
 			if (gps.fix_type >= 3 && gps.eph < 50.0f) {
 				BSTPacket<BSTGPSPosition> bst_gps = {};
@@ -322,6 +302,7 @@ void BST::cycle()
 				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);
 			}
 		}
@@ -350,7 +331,7 @@ uint8_t BST::crc8(uint8_t *data, size_t len)
 
 using namespace px4::bst;
 
-int bst_main(int argc, char *argv[])
+extern "C" __EXPORT int bst_main(int argc, char *argv[])
 {
 	if (argc < 2) {
 		errx(1, "missing command\n%s", commandline_usage);