diff --git a/src/drivers/osd/atxxxx/atxxxx.cpp b/src/drivers/osd/atxxxx/atxxxx.cpp
index d6c7e0296b9c4949171799c2eee33c009c59ecf4..2d5c7a34c742cdda87a553d1049e38ab748849f2 100644
--- a/src/drivers/osd/atxxxx/atxxxx.cpp
+++ b/src/drivers/osd/atxxxx/atxxxx.cpp
@@ -34,44 +34,62 @@
/**
* @file atxxxx.cpp
* @author Daniele Pettenuzzo
+ * @author Beat Küng <beat-kueng@gmx.net>
*
* Driver for the ATXXXX chip on the omnibus fcu connected via SPI.
*/
#include "atxxxx.h"
+struct work_s OSDatxxxx::_work = {};
+
OSDatxxxx::OSDatxxxx(int bus) :
SPI("OSD", OSD_DEVICE_PATH, bus, OSD_SPIDEV, SPIDEV_MODE0, OSD_SPI_BUS_SPEED),
- _measure_ticks(0),
- _sample_perf(perf_alloc(PC_ELAPSED, "osd_read")),
- _comms_errors(perf_alloc(PC_COUNT, "osd_com_err")),
- _battery_sub(-1),
- _local_position_sub(-1),
- _vehicle_status_sub(-1),
- _battery_voltage_filtered_v(0),
- _battery_discharge_mah(0),
- _battery_valid(false),
- _local_position_z(0),
- _local_position_valid(false),
- _arming_state(1),
- _arming_timestamp(0)
+ ModuleParams(nullptr)
{
- _p_tx_mode = param_find("OSD_ATXXXX_CFG");
- param_get(_p_tx_mode, &_tx_mode);
-
- // work_cancel in the dtor will explode if we don't do this...
- memset(&_work, 0, sizeof(_work));
+ _battery_sub = orb_subscribe(ORB_ID(battery_status));
+ _local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
+ _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
}
OSDatxxxx::~OSDatxxxx()
{
- /* make sure we are truly inactive */
- stop();
+ orb_unsubscribe(_battery_sub);
+ orb_unsubscribe(_local_position_sub);
+ orb_unsubscribe(_vehicle_status_sub);
+}
+
+int OSDatxxxx::task_spawn(int argc, char *argv[])
+{
+ int ch;
+ int myoptind = 1;
+ const char *myoptarg = nullptr;
+ int spi_bus = OSD_BUS;
+
+ while ((ch = px4_getopt(argc, argv, "b:", &myoptind, &myoptarg)) != EOF) {
+ switch (ch) {
+ case 'b':
+ spi_bus = (uint8_t)atoi(myoptarg);
+ break;
+ }
+ }
+
+ int ret = work_queue(LPWORK, &_work, (worker_t)&OSDatxxxx::initialize_trampoline, (void *)(long)spi_bus, 0);
- // free perf counters
- perf_free(_sample_perf);
- perf_free(_comms_errors);
+ if (ret < 0) {
+ return ret;
+ }
+
+ ret = wait_until_running();
+
+ if (ret < 0) {
+ return ret;
+ }
+
+ _task_id = task_id_is_work_queue;
+
+ return 0;
}
@@ -79,16 +97,18 @@ int
OSDatxxxx::init()
{
/* do SPI init (and probe) first */
- if (SPI::init() != PX4_OK) {
- goto fail;
+ int ret = SPI::init();
+
+ if (ret != PX4_OK) {
+ return ret;
}
- if (reset() != PX4_OK) {
- goto fail;
+ if ((ret = reset()) != PX4_OK) {
+ return ret;
}
- if (init_osd() != PX4_OK) {
- goto fail;
+ if ((ret = init_osd()) != PX4_OK) {
+ return ret;
}
for (int i = 0; i < OSD_CHARS_PER_ROW; i++) {
@@ -97,15 +117,41 @@ OSDatxxxx::init()
}
}
- _battery_sub = orb_subscribe(ORB_ID(battery_status));
- // _local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
- _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+ return ret;
+}
- return PX4_OK;
+int OSDatxxxx::start()
+{
+ if (is_running()) {
+ return 0;
+ }
-fail:
- return PX4_ERROR;
+ init();
+ // Kick off the cycling. We can call it directly because we're already in the work queue context.
+ cycle();
+
+ return 0;
+}
+
+void OSDatxxxx::initialize_trampoline(void *arg)
+{
+ OSDatxxxx *osd = new OSDatxxxx((long)arg);
+
+ if (!osd) {
+ PX4_ERR("alloc failed");
+ return;
+ }
+
+ osd->start();
+ _object.store(osd);
+}
+
+void OSDatxxxx::cycle_trampoline(void *arg)
+{
+ OSDatxxxx *obj = reinterpret_cast<OSDatxxxx *>(arg);
+
+ obj->cycle();
}
@@ -131,7 +177,7 @@ OSDatxxxx::init_osd()
int ret = PX4_OK;
uint8_t data = OSD_ZERO_BYTE;
- if (_tx_mode == 2) {
+ if (_param_atxxxx_cfg.get() == 2) {
data |= OSD_PAL_TX_MODE;
}
@@ -148,20 +194,6 @@ OSDatxxxx::init_osd()
}
-
-int
-OSDatxxxx::ioctl(device::file_t *filp, int cmd, unsigned long arg)
-{
- return -1;
-}
-
-ssize_t
-OSDatxxxx::read(device::file_t *filp, char *buffer, size_t buflen)
-{
- return -1;
-}
-
-
int
OSDatxxxx::readRegister(unsigned reg, uint8_t *data, unsigned count)
{
@@ -173,7 +205,6 @@ OSDatxxxx::readRegister(unsigned reg, uint8_t *data, unsigned count)
ret = transfer(&cmd[0], &cmd[0], count + 1);
if (OK != ret) {
- perf_count(_comms_errors);
DEVICE_LOG("spi::transfer returned %d", ret);
return ret;
}
@@ -197,7 +228,6 @@ OSDatxxxx::writeRegister(unsigned reg, uint8_t data)
ret = transfer(&cmd[0], nullptr, 2);
if (OK != ret) {
- perf_count(_comms_errors);
DEVICE_LOG("spi::transfer returned %d", ret);
return ret;
}
@@ -440,20 +470,6 @@ OSDatxxxx::update_screen()
}
-
-void
-OSDatxxxx::start()
-{
- /* schedule a cycle to start things */
- work_queue(LPWORK, &_work, (worker_t)&OSDatxxxx::cycle_trampoline, this, USEC2TICK(OSD_US));
-}
-
-void
-OSDatxxxx::stop()
-{
- work_cancel(LPWORK, &_work);
-}
-
int
OSDatxxxx::reset()
{
@@ -463,17 +479,14 @@ OSDatxxxx::reset()
return ret;
}
-void
-OSDatxxxx::cycle_trampoline(void *arg)
-{
- OSDatxxxx *dev = (OSDatxxxx *)arg;
-
- dev->cycle();
-}
-
void
OSDatxxxx::cycle()
{
+ if (should_exit()) {
+ exit_and_cleanup();
+ return;
+ }
+
update_topics();
if (_battery_valid || _local_position_valid || _arming_state > 1) {
@@ -489,179 +502,32 @@ OSDatxxxx::cycle()
}
-void
-OSDatxxxx::print_info()
+int OSDatxxxx::print_usage(const char *reason)
{
- perf_print_counter(_sample_perf);
- perf_print_counter(_comms_errors);
- printf("poll interval: %u ticks\n", _measure_ticks);
- printf("battery_status: %.3f\n", (double)_battery_voltage_filtered_v);
- printf("arming_state: %d\n", _arming_state);
- printf("arming_timestamp: %5.1f\n", (double)_arming_timestamp);
-
-}
-
-
-/**
- * Local functions in support of the shell command.
- */
-namespace osd
-{
-
-OSDatxxxx *g_dev;
-
-int start(int spi_bus);
-int stop();
-int info();
-void usage();
-
-
-/**
- * Start the driver.
- */
-int
-start(int spi_bus)
-{
- int fd;
-
- if (g_dev != nullptr) {
- PX4_ERR("already started");
- goto fail;
- }
-
- /* create the driver */
- g_dev = new OSDatxxxx(spi_bus);
-
- if (g_dev == nullptr) {
- goto fail;
+ if (reason) {
+ printf("%s\n\n", reason);
}
- if (OK != g_dev->init()) {
- goto fail;
- }
+ PRINT_MODULE_DESCRIPTION(
+ R"DESCR_STR(
+### Description
+OSD driver for the ATXXXX chip that is mounted on the OmnibusF4SD board for example.
+)DESCR_STR");
- fd = open(OSD_DEVICE_PATH, O_RDONLY);
-
- if (fd < 0) {
- goto fail;
- }
+ PRINT_MODULE_USAGE_NAME("atxxxx", "driver");
+ PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the driver");
+ PRINT_MODULE_USAGE_PARAM_INT('b', -1, 0, 100, "SPI bus (default: use board-specific bus)", true);
+ PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
- g_dev->start();
-
- return PX4_OK;
-
-fail:
-
- if (g_dev != nullptr) {
- delete g_dev;
- g_dev = nullptr;
- }
-
- PX4_ERR("driver start failed");
- return PX4_ERROR;
+ return 0;
}
-/**
- * Stop the driver
- */
-int
-stop()
+int OSDatxxxx::custom_command(int argc, char *argv[])
{
- if (g_dev != nullptr) {
- delete g_dev;
- g_dev = nullptr;
-
- } else {
- PX4_ERR("driver not running");
- return PX4_ERROR;
- }
-
- return PX4_OK;
+ return print_usage("unrecognized command");
}
-/**
- * Print a little info about the driver.
- */
-int
-info()
+int atxxxx_main(int argc, char *argv[])
{
- if (g_dev == nullptr) {
- PX4_ERR("driver not running");
- return PX4_ERROR;
- }
-
- printf("state @ %p\n", g_dev);
- g_dev->print_info();
-
- return PX4_OK;
-}
-
-/**
- * Print a little info about how to start/stop/use the driver
- */
-void usage()
-{
- PX4_INFO("usage: atxxxx {start|stop|status'}");
- PX4_INFO(" [-b SPI_BUS]");
-}
-
-} // namespace osd
-
-
-int
-atxxxx_main(int argc, char *argv[])
-{
- if (argc < 2) {
- osd::usage();
- return PX4_ERROR;
- }
-
- // don't exit from getopt loop to leave getopt global variables in consistent state,
- // set error flag instead
- bool err_flag = false;
- int ch;
- int myoptind = 1;
- const char *myoptarg = nullptr;
- int spi_bus = OSD_BUS;
-
- while ((ch = px4_getopt(argc, argv, "b:", &myoptind, &myoptarg)) != EOF) {
- switch (ch) {
- case 'b':
- spi_bus = (uint8_t)atoi(myoptarg);
- break;
-
- default:
- err_flag = true;
- break;
- }
- }
-
- if (err_flag) {
- osd::usage();
- return PX4_ERROR;
- }
-
- /*
- * Start/load the driver.
- */
- if (!strcmp(argv[myoptind], "start")) {
- return osd::start(spi_bus);
- }
-
- /*
- * Stop the driver
- */
- if (!strcmp(argv[myoptind], "stop")) {
- return osd::stop();
- }
-
- /*
- * Print driver information.
- */
- if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) {
- return osd::info();
- }
-
- osd::usage();
- return PX4_ERROR;
+ return OSDatxxxx::main(argc, argv);
}
diff --git a/src/drivers/osd/atxxxx/atxxxx.h b/src/drivers/osd/atxxxx/atxxxx.h
index 90c9e2c49cef42e55d89d4e1fdff6e22d26ad701..ca15b85f6120c2b5848a299a9be58d106e23fed2 100644
--- a/src/drivers/osd/atxxxx/atxxxx.h
+++ b/src/drivers/osd/atxxxx/atxxxx.h
@@ -45,9 +45,10 @@
#include <string.h>
#include <stdio.h>
-#include <perf/perf_counter.h>
#include <parameters/param.h>
#include <px4_getopt.h>
+#include <px4_module.h>
+#include <px4_module_params.h>
#include <px4_workqueue.h>
#include <board_config.h>
@@ -92,75 +93,45 @@
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
-class OSDatxxxx : public device::SPI
+
+extern "C" __EXPORT int atxxxx_main(int argc, char *argv[]);
+
+
+class OSDatxxxx : public device::SPI, public ModuleBase<OSDatxxxx>, public ModuleParams
{
public:
OSDatxxxx(int bus = OSD_BUS);
- virtual ~OSDatxxxx();
+ ~OSDatxxxx();
virtual int init();
- virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen);
-
- virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg);
+ /**
+ * @see ModuleBase::custom_command
+ */
+ static int custom_command(int argc, char *argv[]);
/**
- * Diagnostics - print some basic information about the driver.
- */
- void print_info();
+ * @see ModuleBase::print_usage
+ */
+ static int print_usage(const char *reason = nullptr);
/**
- * Initialise the automatic measurement state machine and start it.
- *
- * @note This function is called at open and error time. It might make sense
- * to make it more aggressive about resetting the bus in case of errors.
- */
- void start();
+ * @see ModuleBase::task_spawn
+ */
+ static int task_spawn(int argc, char *argv[]);
protected:
virtual int probe();
private:
- work_s _work;
-
- int _measure_ticks;
-
- perf_counter_t _sample_perf;
- perf_counter_t _comms_errors;
-
- param_t _p_tx_mode;
- int32_t _tx_mode;
-
- int _battery_sub;
- int _local_position_sub;
- int _vehicle_status_sub;
-
- bool _on;
-
- // battery
- float _battery_voltage_filtered_v;
- float _battery_discharge_mah;
- bool _battery_valid;
-
- // altitude
- float _local_position_z;
- bool _local_position_valid;
+ static void cycle_trampoline(void *arg);
- // flight time
- uint8_t _arming_state;
- uint64_t _arming_timestamp;
+ void cycle();
- /**
- * Stop the automatic measurement state machine.
- */
- void stop();
+ static void initialize_trampoline(void *arg);
- /**
- * Perform a poll cycle; collect from the previous measurement
- * and start a new one.
- */
- void cycle();
+ int start();
int reset();
@@ -183,17 +154,26 @@ private:
int update_topics();
int update_screen();
- /**
- * Static trampoline from the workq context; because we don't have a
- * generic workq wrapper yet.
- *
- * @param arg Instance pointer for the driver that is polling.
- */
- static void cycle_trampoline(void *arg);
+ static work_s _work;
+ int _battery_sub{-1};
+ int _local_position_sub{-1};
+ int _vehicle_status_sub{-1};
+ // battery
+ float _battery_voltage_filtered_v{0.f};
+ float _battery_discharge_mah{0.f};
+ bool _battery_valid{false};
+
+ // altitude
+ float _local_position_z{0.f};
+ bool _local_position_valid{false};
+
+ // flight time
+ uint8_t _arming_state{0};
+ uint64_t _arming_timestamp{0};
+
+ DEFINE_PARAMETERS(
+ (ParamInt<px4::params::OSD_ATXXXX_CFG>) _param_atxxxx_cfg
+ )
};
-/*
- * Driver 'main' command.
- */
-extern "C" __EXPORT int atxxxx_main(int argc, char *argv[]);