Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 02:16:53 +08:00
Code Issues Actions 4 Packages Projects Releases Wiki Activity

bst move to px4 work queue

Browse Source
This commit is contained in:
Daniel Agar
2019-02-22 10:52:39 -05:00
parent 525fdc87c7
commit 8c3821c806
1 changed files with 20 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/telemetry/bst/bst.cpp
+20 -34
View File
@@ -41,7 +41,7 @@
#include <px4_config.h> #include <px4_config.h>
#include <px4_getopt.h> #include <px4_getopt.h>
#include <px4_workqueue.h> #include <px4_work_queue/ScheduledWorkItem.hpp>
#include <drivers/device/i2c.h> #include <drivers/device/i2c.h>
#include <systemlib/err.h> #include <systemlib/err.h>
#include <string.h> #include <string.h>
@@ -111,7 +111,7 @@ struct BSTBattery {
#pragma pack(pop) #pragma pack(pop)
class BST : public device::I2C class BST : public device::I2C, public px4::ScheduledWorkItem
{ {
public: public:
BST(int bus); BST(int bus);
@@ -126,14 +126,12 @@ public:
virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg) { return 0; } virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg) { return 0; }
work_s *work_ptr() { return &_work; }
void stop(); void stop();
static void start_trampoline(void *arg); static void start_trampoline(void *arg);
private: private:
work_s _work = {};
bool _should_run = false; bool _should_run = false;
unsigned _interval = 100; unsigned _interval = 100;
int _gps_sub; int _gps_sub;
@@ -142,7 +140,7 @@ private:
void start(); void start();
static void cycle_trampoline(void *arg); void Run() override;
void cycle(); void cycle();
@@ -196,19 +194,16 @@ private:
static BST *g_bst = nullptr; static BST *g_bst = nullptr;
BST::BST(int bus) : BST::BST(int bus) :
I2C("bst", BST_DEVICE_PATH, bus, BST_ADDR I2C("bst", BST_DEVICE_PATH, bus, BST_ADDR, 100000),
#ifdef __PX4_NUTTX ScheduledWorkItem(px4::device_bus_to_wq(get_device_id()))
, 100000 /* maximum speed supported */
#endif
)
{ {
} }
BST::~BST() BST::~BST()
{ {
_should_run = false; ScheduleClear();
work_cancel(LPWORK, &_work); _should_run = false;
} }
int BST::probe() int BST::probe()
@@ -248,39 +243,30 @@ int BST::probe()
int BST::init() int BST::init()
{ {
int ret; int ret = I2C::init();
ret = I2C::init();
if (ret != OK) { if (ret != OK) {
return ret; return ret;
} }
work_queue(LPWORK, &_work, BST::start_trampoline, g_bst, 0); ScheduleNow();
return OK; return OK;
} }
void BST::start_trampoline(void *arg) void BST::Run()
{ {
reinterpret_cast<BST *>(arg)->start(); if (!_should_run) {
} _should_run = true;
void BST::start() _attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
{ _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
_should_run = true; _battery_sub = orb_subscribe(ORB_ID(battery_status));
_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); set_device_address(0x00); // General call address
_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();
work_queue(LPWORK, &_work, BST::cycle_trampoline, this, 0);
}
void BST::cycle_trampoline(void *arg)
{
reinterpret_cast<BST *>(arg)->cycle();
} }
void BST::cycle() void BST::cycle()
@@ -340,7 +326,7 @@ void BST::cycle()
} }
} }
work_queue(LPWORK, &_work, BST::cycle_trampoline, this, _interval); ScheduleDelayed(_interval);
} }
} }