forked from Archive/PX4-Autopilot
bst (Black Sheep Telemetery) module added
This commit is contained in:
Anton Babushkin
Lorenz Meier
parent
4e824e55d7
commit
5f18182acc
|
|
@ -0,0 +1,362 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file bst.cpp
|
||||
*
|
||||
* Black Sheep Telemetry driver
|
||||
*
|
||||
* @author Anton Babushkin <anton.babushkin@me.com>
|
||||
*/
|
||||
|
||||
#include <px4_config.h>
|
||||
#include <px4_getopt.h>
|
||||
#include <px4_workqueue.h>
|
||||
#include <drivers/device/i2c.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
|
||||
#define DEFAULT_DEVICE 1 // TODO use PX4 define for external I2C
|
||||
#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 {
|
||||
namespace bst {
|
||||
|
||||
#pragma pack(push, 1)
|
||||
|
||||
template <typename T>
|
||||
struct BSTPacket {
|
||||
uint8_t length;
|
||||
uint8_t type;
|
||||
T payload;
|
||||
uint8_t crc;
|
||||
};
|
||||
|
||||
struct BSTDeviceInfoRequest {
|
||||
uint8_t cmd;
|
||||
};
|
||||
|
||||
struct BSTDeviceInfoReply {
|
||||
uint32_t hw_id;
|
||||
uint16_t fw_id;
|
||||
uint8_t dev_name_len;
|
||||
char dev_name[32];
|
||||
};
|
||||
|
||||
struct BSTGPSPosition {
|
||||
int32_t lat;
|
||||
int32_t lon;
|
||||
uint16_t gs;
|
||||
uint16_t heading;
|
||||
uint16_t alt;
|
||||
uint8_t sats;
|
||||
};
|
||||
|
||||
struct BSTAttitude {
|
||||
int16_t pitch;
|
||||
int16_t roll;
|
||||
int16_t yaw;
|
||||
};
|
||||
|
||||
#pragma pack(pop)
|
||||
|
||||
class BST : public device::I2C {
|
||||
public:
|
||||
BST(int bus);
|
||||
|
||||
virtual ~BST();
|
||||
|
||||
virtual int init();
|
||||
|
||||
virtual int probe();
|
||||
|
||||
virtual int info() { return 0; }
|
||||
|
||||
virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg) { return 0; }
|
||||
|
||||
work_s *work_ptr() { return &_work; }
|
||||
|
||||
void stop();
|
||||
|
||||
static void start_trampoline(void *arg);
|
||||
|
||||
private:
|
||||
work_s _work = {};
|
||||
bool _should_run = false;
|
||||
unsigned _interval = 100;
|
||||
int _gps_sub;
|
||||
int _attitude_sub;
|
||||
int _battery_sub;
|
||||
|
||||
void start();
|
||||
|
||||
static void cycle_trampoline(void *arg);
|
||||
|
||||
void cycle();
|
||||
|
||||
template <typename T>
|
||||
void send_packet(BSTPacket<T> &packet) {
|
||||
packet.length = sizeof(packet) - 1; // Length
|
||||
packet.crc = crc8(reinterpret_cast<uint8_t *>(&packet.type), sizeof(packet) - 2);
|
||||
|
||||
transfer(reinterpret_cast<uint8_t *>(&packet), sizeof(packet), nullptr, 0);
|
||||
}
|
||||
|
||||
template <typename T_SEND, typename T_RECV>
|
||||
void send_packet(BSTPacket<T_SEND> &packet_send, BSTPacket<T_RECV> &packet_recv) {
|
||||
packet_send.length = sizeof(packet_send) - 1; // Length
|
||||
packet_send.crc = crc8(reinterpret_cast<uint8_t *>(&packet_send.type), sizeof(packet_send) - 2);
|
||||
transfer(reinterpret_cast<uint8_t *>(&packet_send), sizeof(packet_send), reinterpret_cast<uint8_t *>(&packet_recv), sizeof(packet_recv));
|
||||
}
|
||||
|
||||
static uint8_t crc8(uint8_t *data, size_t len);
|
||||
|
||||
//! Byte swap unsigned short
|
||||
uint16_t swap_uint16( uint16_t val )
|
||||
{
|
||||
return (val << 8) | (val >> 8 );
|
||||
}
|
||||
|
||||
//! Byte swap short
|
||||
int16_t swap_int16( int16_t val )
|
||||
{
|
||||
return (val << 8) | ((val >> 8) & 0xFF);
|
||||
}
|
||||
|
||||
//! Byte swap unsigned int
|
||||
uint32_t swap_uint32( uint32_t val )
|
||||
{
|
||||
val = ((val << 8) & 0xFF00FF00 ) | ((val >> 8) & 0xFF00FF );
|
||||
return (val << 16) | (val >> 16);
|
||||
}
|
||||
|
||||
//! Byte swap int
|
||||
int32_t swap_int32( int32_t val )
|
||||
{
|
||||
val = ((val << 8) & 0xFF00FF00) | ((val >> 8) & 0xFF00FF );
|
||||
return (val << 16) | ((val >> 16) & 0xFFFF);
|
||||
}
|
||||
};
|
||||
|
||||
static BST *g_bst = nullptr;
|
||||
|
||||
BST::BST(int bus) :
|
||||
I2C("bst", BST_DEVICE_PATH, bus, BST_ADDR
|
||||
#ifdef __PX4_NUTTX
|
||||
,100000 /* maximum speed supported */
|
||||
#endif
|
||||
) {
|
||||
}
|
||||
|
||||
BST::~BST() {
|
||||
_should_run = false;
|
||||
|
||||
work_cancel(LPWORK, &_work);
|
||||
}
|
||||
|
||||
int BST::probe() {
|
||||
int retries_prev = _retries;
|
||||
_retries = 3;
|
||||
|
||||
BSTPacket<BSTDeviceInfoRequest> dev_info_req = {};
|
||||
dev_info_req.type = 0x0A;
|
||||
dev_info_req.payload.cmd = 0x04;
|
||||
BSTPacket<BSTDeviceInfoReply> dev_info_reply = {};
|
||||
send_packet(dev_info_req, dev_info_reply);
|
||||
if (dev_info_reply.type != 0x05) {
|
||||
warnx("no devices found");
|
||||
return -EIO;
|
||||
}
|
||||
uint8_t *reply_raw = reinterpret_cast<uint8_t *>(&dev_info_reply);
|
||||
uint8_t crc_calc = crc8(reinterpret_cast<uint8_t *>(&dev_info_reply.type), dev_info_reply.length - 1);
|
||||
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);
|
||||
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", (int)swap_uint32(dev_info_reply.payload.hw_id), (int)swap_uint16(dev_info_reply.payload.fw_id), dev_info_reply.payload.dev_name);
|
||||
|
||||
_retries = retries_prev;
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
int BST::init() {
|
||||
int ret;
|
||||
ret = I2C::init();
|
||||
|
||||
if (ret != OK) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
work_queue(LPWORK, &_work, BST::start_trampoline, g_bst, 0);
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
void BST::start_trampoline(void *arg) {
|
||||
reinterpret_cast<BST *>(arg)->start();
|
||||
}
|
||||
|
||||
void BST::start() {
|
||||
_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_address(0x00); // General call address
|
||||
|
||||
work_queue(LPWORK, &_work, BST::cycle_trampoline, this, 0);
|
||||
}
|
||||
|
||||
void BST::cycle_trampoline(void *arg) {
|
||||
reinterpret_cast<BST *>(arg)->cycle();
|
||||
}
|
||||
|
||||
void BST::cycle() {
|
||||
if (_should_run) {
|
||||
bool updated = false;
|
||||
|
||||
orb_check(_attitude_sub, &updated);
|
||||
if (updated) {
|
||||
vehicle_attitude_s att;
|
||||
orb_copy(ORB_ID(vehicle_attitude), _attitude_sub, &att);
|
||||
BSTPacket<BSTAttitude> bst_att = {};
|
||||
bst_att.type = 0x1E;
|
||||
bst_att.payload.roll = swap_int32(att.roll * 10000);
|
||||
bst_att.payload.pitch = swap_int32(att.pitch * 10000);
|
||||
bst_att.payload.yaw = swap_int32(att.yaw * 10000);
|
||||
send_packet(bst_att);
|
||||
}
|
||||
|
||||
updated = false;
|
||||
orb_check(_gps_sub, &updated);
|
||||
if (updated) {
|
||||
vehicle_gps_position_s gps;
|
||||
orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &gps);
|
||||
if (gps.fix_type >= 3 && gps.eph < 50.0f) {
|
||||
BSTPacket<BSTGPSPosition> gps_pos = {};
|
||||
gps_pos.type = 0x02;
|
||||
gps_pos.payload.lat = swap_int32(gps.lat);
|
||||
gps_pos.payload.lon = swap_int32(gps.lon);
|
||||
gps_pos.payload.alt = swap_int16(gps.alt / 1000 + 1000);
|
||||
gps_pos.payload.gs = swap_int16(gps.vel_m_s * 360.0f);
|
||||
gps_pos.payload.heading = swap_int16(gps.cog_rad * 18000.0f / M_PI_F);
|
||||
gps_pos.payload.sats = gps.satellites_used;
|
||||
send_packet(gps_pos);
|
||||
}
|
||||
}
|
||||
|
||||
work_queue(LPWORK, &_work, BST::cycle_trampoline, this, _interval);
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t BST::crc8(uint8_t *data, size_t len) {
|
||||
uint8_t crc = 0x00;
|
||||
|
||||
while (len--) {
|
||||
crc ^= *data++;
|
||||
|
||||
for (int i = 0; i < 8; i++) {
|
||||
crc = crc & 0x80 ? (crc << 1) ^ 0xD5 : crc << 1;
|
||||
}
|
||||
}
|
||||
|
||||
return crc;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
using namespace px4::bst;
|
||||
|
||||
int bst_main(int argc, char *argv[]) {
|
||||
if (argc < 2) {
|
||||
errx(1, "missing command\n%s", commandline_usage);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
|
||||
if (g_bst) {
|
||||
warnx("already running");
|
||||
exit(0);
|
||||
}
|
||||
|
||||
g_bst = new BST(DEFAULT_DEVICE);
|
||||
|
||||
if (g_bst != nullptr && OK != g_bst->init()) {
|
||||
delete g_bst;
|
||||
g_bst = nullptr;
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "stop")) {
|
||||
if (!g_bst) {
|
||||
warnx("not running");
|
||||
exit(0);
|
||||
}
|
||||
|
||||
delete g_bst;
|
||||
g_bst = nullptr;
|
||||
warnx("stopped");
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "status")) {
|
||||
if (g_bst) {
|
||||
warnx("is running");
|
||||
|
||||
} else {
|
||||
warnx("is not running");
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
errx(1, "unrecognized command\n%s", commandline_usage);
|
||||
}
|
||||
|
|
@ -0,0 +1,9 @@
|
|||
#
|
||||
# Black Sheep Telemetry driver
|
||||
#
|
||||
|
||||
MODULE_COMMAND = bst
|
||||
|
||||
SRCS = bst.cpp
|
||||
|
||||
MAXOPTIMIZATION = -Os
|
||||
Loading…
Reference in New Issue