bst.cpp

Go to the documentation of this file.

/****************************************************************************
 *
 *   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_platform_common/px4_config.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <drivers/device/i2c.h>
#include <systemlib/err.h>
#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>
#include <matrix/math.hpp>

using namespace matrix;

static const char commandline_usage[] = "usage: bst start|status|stop";

#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;
};

struct BSTBattery {
    uint16_t voltage;
    uint16_t current;
    uint8_t capacity[3];
};

#pragma pack(pop)

class BST : public device::I2C, public px4::ScheduledWorkItem
{
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; }

    void stop();

    static void     start_trampoline(void *arg);

private:

    bool            _should_run = false;
    unsigned        _interval = 100;

    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;

    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", nullptr, bus, BST_ADDR, 100000),
    ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id()))
{
}

BST::~BST()
{
    ScheduleClear();

    _should_run = false;
}

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 = I2C::init();

    if (ret != OK) {
        return ret;
    }

    ScheduleNow();

    return OK;
}

void BST::Run()
{
    if (!_should_run) {
        _should_run = true;
        set_device_address(0x00); // General call address
    }

    if (_should_run) {
        if (_attitude_sub.updated()) {
            vehicle_attitude_s 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);
        }

        if (_battery_sub.updated()) {
            battery_status_s batt;
            _battery_sub.copy(&batt);

            BSTPacket<BSTBattery> bst_batt = {};
            bst_batt.type = 0x08;
            bst_batt.payload.voltage = swap_uint16(batt.voltage_v * 10.0f);
            bst_batt.payload.current = swap_uint16(batt.current_a * 10.0f);
            uint32_t discharged = batt.discharged_mah;
            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);
        }

        if (_gps_sub.updated()) {
            vehicle_gps_position_s gps;
            _gps_sub.copy(&gps);

            if (gps.fix_type >= 3 && gps.eph < 50.0f) {
                BSTPacket<BSTGPSPosition> bst_gps = {};
                bst_gps.type = 0x02;
                bst_gps.payload.lat = swap_int32(gps.lat);
                bst_gps.payload.lon = swap_int32(gps.lon);
                bst_gps.payload.alt = swap_int16(gps.alt / 1000 + 1000);
                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);
            }
        }

        ScheduleDelayed(_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;

extern "C" __EXPORT 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(PX4_I2C_BUS_EXPANSION);

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

    PX4_WARN("unrecognized command\n%s", commandline_usage);
    return 1;
}