diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 219ad26e83..e48308f162 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -476,7 +476,7 @@ else # Blacksheep telemetry if param greater TEL_BST_EN 0 then - bst start + bst start -X fi # diff --git a/ROMFS/px4fmu_test/init.d/rc.sensors b/ROMFS/px4fmu_test/init.d/rc.sensors index c9bc1a3bd1..e23d322ef2 100644 --- a/ROMFS/px4fmu_test/init.d/rc.sensors +++ b/ROMFS/px4fmu_test/init.d/rc.sensors @@ -13,7 +13,7 @@ then fi # Blacksheep telemetry -if bst start +if bst start -X then fi diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 9755d0fb70..97c0f40c4a 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -146,6 +146,7 @@ #define DRV_BAT_DEVTYPE_SMBUS 0x7c #define DRV_SENS_DEVTYPE_IRLOCK 0x7d #define DRV_SENS_DEVTYPE_PCF8583 0x7e +#define DRV_TEL_DEVTYPE_BST 0x7f #define DRV_DEVTYPE_UNUSED 0xff diff --git a/src/drivers/telemetry/bst/bst.cpp b/src/drivers/telemetry/bst/bst.cpp index 958cbe1e73..479b8129cf 100644 --- a/src/drivers/telemetry/bst/bst.cpp +++ b/src/drivers/telemetry/bst/bst.cpp @@ -41,7 +41,8 @@ #include #include -#include +#include +#include #include #include #include @@ -54,8 +55,7 @@ #include using namespace matrix; - -static const char commandline_usage[] = "usage: bst start|status|stop"; +using namespace time_literals; #define BST_ADDR 0x76 @@ -108,37 +108,29 @@ struct BSTBattery { #pragma pack(pop) -class BST : public device::I2C, public px4::ScheduledWorkItem +class BST : public device::I2C, public I2CSPIDriver { public: - BST(int bus); + BST(I2CSPIBusOption bus_option, const int bus, int address, int bus_frequency); + ~BST() override = default; - virtual ~BST(); + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); - virtual int init(); + int init() override; - 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); + int probe() override; + void RunImpl(); private: - bool _should_run = false; - unsigned _interval = 100; + static constexpr unsigned _interval{100_ms}; 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 void send_packet(BSTPacket &packet) @@ -187,21 +179,12 @@ private: } }; -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(I2CSPIBusOption bus_option, const int bus, int address, int bus_frequency) : + I2C("bst", nullptr, bus, address, bus_frequency), + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address) { } -BST::~BST() -{ - ScheduleClear(); - - _should_run = false; -} - int BST::probe() { int retries_prev = _retries; @@ -214,7 +197,7 @@ int BST::probe() send_packet(dev_info_req, dev_info_reply); if (dev_info_reply.type != 0x05) { - warnx("no devices found"); + PX4_ERR("no devices found"); return -EIO; } @@ -223,14 +206,14 @@ int BST::probe() 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); + PX4_ERR("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); + PX4_DEBUG("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; @@ -250,65 +233,58 @@ int BST::init() return OK; } -void BST::Run() +void BST::RunImpl() { - if (!_should_run) { - _should_run = true; - set_device_address(0x00); // General call address + if (_attitude_sub.updated()) { + vehicle_attitude_s att; + _attitude_sub.copy(&att); + Quatf q(att.q); + Eulerf euler(q); + + BSTPacket 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 (_should_run) { - if (_attitude_sub.updated()) { - vehicle_attitude_s att; - _attitude_sub.copy(&att); - Quatf q(att.q); - Eulerf euler(q); + if (_battery_sub.updated()) { + battery_status_s batt; + _battery_sub.copy(&batt); - BSTPacket 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); + BSTPacket 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(discharged >> 16); + bst_batt.payload.capacity[1] = static_cast(discharged >> 8); + bst_batt.payload.capacity[2] = static_cast(discharged); - send_packet(bst_att); - } - - if (_battery_sub.updated()) { - battery_status_s batt; - _battery_sub.copy(&batt); - - BSTPacket 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(discharged >> 16); - bst_batt.payload.capacity[1] = static_cast(discharged >> 8); - bst_batt.payload.capacity[2] = static_cast(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 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); + 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 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) @@ -331,53 +307,62 @@ uint8_t BST::crc8(uint8_t *data, size_t len) using namespace px4::bst; +void +BST::print_usage() +{ + PRINT_MODULE_USAGE_NAME("bst", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x76); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +I2CSPIDriverBase *BST::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) +{ + BST *instance = new BST(iterator.configuredBusOption(), iterator.bus(), cli.i2c_address, cli.bus_frequency); + + if (instance == nullptr) { + PX4_ERR("alloc failed"); + return nullptr; + } + + if (instance->init() != PX4_OK) { + delete instance; + return nullptr; + } + + return instance; +} + extern "C" __EXPORT int bst_main(int argc, char *argv[]) { - if (argc < 2) { - errx(1, "missing command\n%s", commandline_usage); + using ThisDriver = BST; + BusCLIArguments cli{true, false}; + cli.i2c_address = BST_ADDR; + cli.default_i2c_frequency = 100000; + + const char *verb = cli.parseDefaultArguments(argc, argv); + + if (!verb) { + ThisDriver::print_usage(); + return -1; } - if (!strcmp(argv[1], "start")) { + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_TEL_DEVTYPE_BST); - 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(verb, "start")) { + return ThisDriver::module_start(cli, iterator); } - 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(verb, "stop")) { + return ThisDriver::module_stop(iterator); } - if (!strcmp(argv[1], "status")) { - if (g_bst) { - warnx("is running"); - - } else { - warnx("is not running"); - } - - exit(0); + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); } - PX4_WARN("unrecognized command\n%s", commandline_usage); - return 1; + ThisDriver::print_usage(); + return -1; }