yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

telemetry/bst: move to uORB::Subscription

Browse Source
This commit is contained in:
Daniel Agar
2019-09-28 18:19:38 -04:00
committed by GitHub
parent 105bbef3bf
commit ec061a7cfd
1 changed files with 18 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files

55
src/drivers/telemetry/bst/bst.cpp
View File

@@ -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);