From 55a3128c7d003a7e6356474c78fa82dc17527b13 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 30 Nov 2019 12:38:02 -0500 Subject: [PATCH] qshell update orb usage --- src/drivers/qshell/posix/qshell.cpp | 30 ++++--------------- src/drivers/qshell/posix/qshell.h | 18 +++++++---- .../qshell/posix/qshell_start_posix.cpp | 1 + src/drivers/qshell/qurt/qshell.cpp | 8 ++--- src/drivers/qshell/qurt/qshell.h | 11 +++++-- src/drivers/qshell/qurt/qshell_start_qurt.cpp | 1 + 6 files changed, 31 insertions(+), 38 deletions(-) diff --git a/src/drivers/qshell/posix/qshell.cpp b/src/drivers/qshell/posix/qshell.cpp index 6e683478f6..6ac9b04266 100644 --- a/src/drivers/qshell/posix/qshell.cpp +++ b/src/drivers/qshell/posix/qshell.cpp @@ -51,10 +51,6 @@ px4::AppState QShell::appState; -orb_advert_t QShell::_pub_qshell_req = nullptr; -int QShell::_sub_qshell_retval = -1; -uint32_t QShell::_current_sequence = 0; - int QShell::main(std::vector argList) { int ret = _send_cmd(argList); @@ -81,7 +77,7 @@ int QShell::_send_cmd(std::vector &argList) // it had timed out. ++_current_sequence; - struct qshell_req_s qshell_req; + qshell_req_s qshell_req{}; std::string cmd; for (size_t i = 0; i < argList.size(); i++) { @@ -103,34 +99,20 @@ int QShell::_send_cmd(std::vector &argList) strcpy((char *)qshell_req.cmd, cmd.c_str()); qshell_req.request_sequence = _current_sequence; - int instance; - orb_publish_auto(ORB_ID(qshell_req), &_pub_qshell_req, &qshell_req, &instance, ORB_PRIO_DEFAULT); + qshell_req.timestamp = hrt_absolute_time(); + _qshell_req_pub.publish(qshell_req); return 0; } int QShell::_wait_for_retval() { - if (_sub_qshell_retval < 0) { - _sub_qshell_retval = orb_subscribe(ORB_ID(qshell_retval)); - - if (_sub_qshell_retval < 0) { - PX4_ERR("could not subscribe to retval"); - return -1; - } - } - const hrt_abstime time_started_us = hrt_absolute_time(); while (hrt_elapsed_time(&time_started_us) < 3000000) { - bool updated; - orb_check(_sub_qshell_retval, &updated); - - if (updated) { - - struct qshell_retval_s retval; - orb_copy(ORB_ID(qshell_retval), _sub_qshell_retval, &retval); + qshell_retval_s retval; + if (_qshell_retval_sub.update(&retval)) { if (retval.return_sequence != _current_sequence) { PX4_WARN("Ignoring return value with wrong sequence"); @@ -143,7 +125,7 @@ int QShell::_wait_for_retval() } } - usleep(1000); + px4_usleep(1000); } PX4_ERR("command timed out"); diff --git a/src/drivers/qshell/posix/qshell.h b/src/drivers/qshell/posix/qshell.h index 6295938e17..8aa8cd9e54 100644 --- a/src/drivers/qshell/posix/qshell.h +++ b/src/drivers/qshell/posix/qshell.h @@ -41,15 +41,19 @@ #pragma once #include -#include "uORB/topics/qshell_req.h" +#include +#include +#include +#include + #include #include class QShell { public: - QShell() {} - ~QShell() {} + QShell() = default; + ~QShell() = default; int main(std::vector argList); @@ -59,7 +63,9 @@ private: int _send_cmd(std::vector &argList); int _wait_for_retval(); - static orb_advert_t _pub_qshell_req; - static int _sub_qshell_retval; - static uint32_t _current_sequence; + uORB::Publication _qshell_req_pub{ORB_ID(qshell_req)}; + + uORB::Subscription _qshell_retval_sub{ORB_ID(qshell_retval)}; + + uint32_t _current_sequence{0}; }; diff --git a/src/drivers/qshell/posix/qshell_start_posix.cpp b/src/drivers/qshell/posix/qshell_start_posix.cpp index 688c8dc9bf..e8b681c0b6 100644 --- a/src/drivers/qshell/posix/qshell_start_posix.cpp +++ b/src/drivers/qshell/posix/qshell_start_posix.cpp @@ -57,6 +57,7 @@ static void usage() { PX4_DEBUG("usage: qshell cmd [args]"); } + int qshell_main(int argc, char *argv[]) { if (argc < 2) { diff --git a/src/drivers/qshell/qurt/qshell.cpp b/src/drivers/qshell/qurt/qshell.cpp index bebe99dc5b..5f770842a6 100644 --- a/src/drivers/qshell/qurt/qshell.cpp +++ b/src/drivers/qshell/qurt/qshell.cpp @@ -83,8 +83,6 @@ int QShell::main() fds[0].fd = sub_qshell_req; fds[0].events = POLLIN; - orb_advert_t qshell_pub = nullptr; - while (!appState.exitRequested()) { int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000); @@ -114,7 +112,7 @@ int QShell::main() appargs.push_back(arg); // push last argument - struct qshell_retval_s retval; + qshell_retval_s retval{}; retval.return_value = run_cmd(appargs); retval.return_sequence = m_qshell_req.request_sequence; @@ -125,8 +123,8 @@ int QShell::main() PX4_INFO("Ok executing command: %s", m_qshell_req.cmd); } - int instance; - orb_publish_auto(ORB_ID(qshell_retval), &qshell_pub, &retval, &instance, ORB_PRIO_DEFAULT); + retval.timestamp = hrt_absolute_time(); + _qshell_retval_pub.publish(retval); } else if (pret == 0) { // Timing out is fine. diff --git a/src/drivers/qshell/qurt/qshell.h b/src/drivers/qshell/qurt/qshell.h index 37fb3de518..aa5ee41d6f 100644 --- a/src/drivers/qshell/qurt/qshell.h +++ b/src/drivers/qshell/qurt/qshell.h @@ -43,7 +43,10 @@ #include #include #include -#include "uORB/topics/qshell_req.h" +#include +#include +#include + #include "apps.h" class QShell @@ -59,7 +62,9 @@ public: private: - struct qshell_req_s m_qshell_req; - apps_map_type m_apps; + uORB::Publication _qshell_retval_pub{ORB_ID(qshell_retval)}; + qshell_req_s m_qshell_req{}; + + apps_map_type m_apps; }; diff --git a/src/drivers/qshell/qurt/qshell_start_qurt.cpp b/src/drivers/qshell/qurt/qshell_start_qurt.cpp index 78de3d6f78..18abd360c6 100644 --- a/src/drivers/qshell/qurt/qshell_start_qurt.cpp +++ b/src/drivers/qshell/qurt/qshell_start_qurt.cpp @@ -68,6 +68,7 @@ static void usage() { PX4_INFO("usage: qshell {start|stop|status}"); } + int qshell_main(int argc, char *argv[]) { if (argc < 2) {