From 5f18182accf83acad2fab87f92226fa2571f3ef7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 23 Aug 2015 23:01:53 +0200 Subject: [PATCH] bst (Black Sheep Telemetery) module added --- src/drivers/bst/bst.cpp | 362 ++++++++++++++++++++++++++++++++++++++ src/drivers/bst/module.mk | 9 + 2 files changed, 371 insertions(+) create mode 100644 src/drivers/bst/bst.cpp create mode 100644 src/drivers/bst/module.mk diff --git a/src/drivers/bst/bst.cpp b/src/drivers/bst/bst.cpp new file mode 100644 index 0000000000..e21e268e04 --- /dev/null +++ b/src/drivers/bst/bst.cpp @@ -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 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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 +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 + void send_packet(BSTPacket &packet) { + packet.length = sizeof(packet) - 1; // Length + packet.crc = crc8(reinterpret_cast(&packet.type), sizeof(packet) - 2); + + transfer(reinterpret_cast(&packet), sizeof(packet), nullptr, 0); + } + + template + void send_packet(BSTPacket &packet_send, BSTPacket &packet_recv) { + packet_send.length = sizeof(packet_send) - 1; // Length + packet_send.crc = crc8(reinterpret_cast(&packet_send.type), sizeof(packet_send) - 2); + transfer(reinterpret_cast(&packet_send), sizeof(packet_send), reinterpret_cast(&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 dev_info_req = {}; + dev_info_req.type = 0x0A; + dev_info_req.payload.cmd = 0x04; + BSTPacket 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(&dev_info_reply); + uint8_t crc_calc = crc8(reinterpret_cast(&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(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(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 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 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); +} diff --git a/src/drivers/bst/module.mk b/src/drivers/bst/module.mk new file mode 100644 index 0000000000..2e1f81720a --- /dev/null +++ b/src/drivers/bst/module.mk @@ -0,0 +1,9 @@ +# +# Black Sheep Telemetry driver +# + +MODULE_COMMAND = bst + +SRCS = bst.cpp + +MAXOPTIMIZATION = -Os