fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 03:33:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

purge broken qurt support and atlflight boards

Browse Source
This commit is contained in:
Daniel Agar
2021-07-16 08:53:36 -04:00
parent 4a0fa08953
commit 65745a3676
116 changed files with 58 additions and 11410 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/qshell/posix/CMakeLists.txt
-42
View File
@@ -1,42 +0,0 @@
############################################################################
#
# Copyright (c) 2016 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.
#
############################################################################
px4_add_module(
MODULE drivers__qshell__posix
MAIN qshell
SRCS
#qshell_main.cpp
qshell_start_posix.cpp
qshell.cpp
DEPENDS
)
src/drivers/qshell/posix/qshell.cpp
-133
View File
@@ -1,133 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2016 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 qshell.cpp
* Send shell commands to qurt
*
* @author Nicolas de Palezieux <ndepal@gmail.com>
*/
#include <px4_platform_common/log.h>
#include <unistd.h>
#include <stdio.h>
#include <string.h>
#include <string>
#include <stdlib.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/qshell_retval.h>
#include "qshell.h"
px4::AppState QShell::appState;
int QShell::main(std::vector<std::string> argList)
{
int ret = _send_cmd(argList);
if (ret != 0) {
PX4_ERR("Could not send command");
return -1;
}
ret = _wait_for_retval();
if (ret != 0) {
PX4_ERR("Could not get return value");
return -1;
}
return 0;
}
int QShell::_send_cmd(std::vector<std::string> &argList)
{
// Let's use a sequence number to check if a return value belongs to the
// command that we just sent and not a previous one that we assumed that
// it had timed out.
++_current_sequence;
qshell_req_s qshell_req{};
std::string cmd;
for (size_t i = 0; i < argList.size(); i++) {
cmd += argList[i];
if (i < argList.size() - 1) {
cmd += " ";
}
}
if (cmd.size() >= qshell_req.MAX_STRLEN) {
PX4_ERR("Command too long: %d >= %d", (int) cmd.size(), (int) qshell_req.MAX_STRLEN);
return -1;
}
PX4_INFO("Send cmd: '%s'", cmd.c_str());
qshell_req.strlen = cmd.size();
strcpy((char *)qshell_req.cmd, cmd.c_str());
qshell_req.request_sequence = _current_sequence;
qshell_req.timestamp = hrt_absolute_time();
_qshell_req_pub.publish(qshell_req);
return 0;
}
int QShell::_wait_for_retval()
{
const hrt_abstime time_started_us = hrt_absolute_time();
while (hrt_elapsed_time(&time_started_us) < 3000000) {
qshell_retval_s retval;
if (_qshell_retval_sub.update(&retval)) {
if (retval.return_sequence != _current_sequence) {
PX4_WARN("Ignoring return value with wrong sequence");
} else {
if (retval.return_value) {
PX4_WARN("cmd returned with: %d", retval.return_value);
}
return 0;
}
}
px4_usleep(1000);
}
PX4_ERR("command timed out");
return -1;
}
src/drivers/qshell/posix/qshell.h
-71
View File
@@ -1,71 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2016 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 qshell.h
* Send shell commands to qurt
*
* @author Nicolas de Palezieux <ndepal@gmail.com>
*/
#pragma once
#include <px4_platform_common/app.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/qshell_req.h>
#include <uORB/topics/qshell_retval.h>
#include <vector>
#include <string>
class QShell
{
public:
QShell() = default;
~QShell() = default;
int main(std::vector<std::string> argList);
static px4::AppState appState; /* track requests to terminate app */
private:
int _send_cmd(std::vector<std::string> &argList);
int _wait_for_retval();
uORB::Publication<qshell_req_s> _qshell_req_pub{ORB_ID(qshell_req)};
uORB::Subscription _qshell_retval_sub{ORB_ID(qshell_retval)};
uint32_t _current_sequence{0};
};
src/drivers/qshell/posix/qshell_start_posix.cpp
-77
View File
@@ -1,77 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2016 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 qshell_start_posix.cpp
* Send shell commands to qurt
*
* @author Nicolas de Palezieux <ndepal@gmail.com>
*/
#include "qshell.h"
#include <px4_platform_common/log.h>
#include <px4_platform_common/app.h>
#include <px4_platform_common/tasks.h>
#include <stdio.h>
#include <string.h>
#include <sched.h>
#include <cstdlib>
#include <string>
#include <vector>
//using namespace px4;
extern "C" __EXPORT int qshell_main(int argc, char *argv[]);
static void usage()
{
PX4_DEBUG("usage: qshell cmd [args]");
}
int qshell_main(int argc, char *argv[])
{
if (argc < 2) {
usage();
return 1;
}
std::vector<std::string> argList;
for (int i = 1; i < argc; i++) {
argList.push_back(argv[i]);
}
QShell qshell;
return qshell.main(argList);
}
src/drivers/qshell/qurt/CMakeLists.txt
-41
View File
@@ -1,41 +0,0 @@
############################################################################
#
# Copyright (c) 2016 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.
#
############################################################################
px4_add_module(
MODULE drivers__qshell__qurt
MAIN qshell
SRCS
qshell_main.cpp
qshell_start_qurt.cpp
qshell.cpp
DEPENDS
)
src/drivers/qshell/qurt/qshell.cpp
-184
View File
@@ -1,184 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2016 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 qshell.cpp
* Listener for shell commands from posix
*
* @author Nicolas de Palezieux <ndepal@gmail.com>
*/
#include "qshell.h"
#include <px4_platform_common/log.h>
#include <px4_platform_common/time.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/defines.h>
#include <dspal_platform.h>
#include <unistd.h>
#include <stdio.h>
#include <string.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <signal.h>
#include <stdio.h>
#include <stdlib.h>
#include <uORB/topics/qshell_retval.h>
#include <drivers/drv_hrt.h>
#define MAX_ARGS 8 // max number of whitespace separated args after app name
px4::AppState QShell::appState;
QShell::QShell()
{
init_app_map(m_apps);
}
int QShell::main()
{
appState.setRunning(true);
int sub_qshell_req = orb_subscribe(ORB_ID(qshell_req));
if (sub_qshell_req == PX4_ERROR) {
PX4_ERR("Error subscribing to qshell_req topic");
return -1;
}
px4_pollfd_struct_t fds[1] = {};
fds[0].fd = sub_qshell_req;
fds[0].events = POLLIN;
while (!appState.exitRequested()) {
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000);
if (pret > 0 && fds[0].revents & POLLIN) {
orb_copy(ORB_ID(qshell_req), sub_qshell_req, &m_qshell_req);
PX4_INFO("qshell gotten: %s", m_qshell_req.cmd);
char current_char;
std::string arg;
std::vector<std::string> appargs;
for (unsigned str_idx = 0; str_idx < m_qshell_req.strlen; str_idx++) {
current_char = m_qshell_req.cmd[str_idx];
if (isspace(current_char)) { // split at spaces
if (arg.length()) {
appargs.push_back(arg);
arg = "";
}
} else {
arg += current_char;
}
}
appargs.push_back(arg); // push last argument
qshell_retval_s retval{};
retval.return_value = run_cmd(appargs);
retval.return_sequence = m_qshell_req.request_sequence;
if (retval.return_value) {
PX4_ERR("Failed to execute command: %s", m_qshell_req.cmd);
} else {
PX4_INFO("Ok executing command: %s", m_qshell_req.cmd);
}
retval.timestamp = hrt_absolute_time();
_qshell_retval_pub.publish(retval);
} else if (pret == 0) {
// Timing out is fine.
} else {
// Something is wrong.
usleep(10000);
}
}
appState.setRunning(false);
return 0;
}
int QShell::run_cmd(const std::vector<std::string> &appargs)
{
// command is appargs[0]
std::string command = appargs[0];
if (command.compare("help") == 0) {
list_builtins(m_apps);
return 0;
}
//replaces app.find with iterator code to avoid null pointer exception
for (apps_map_type::iterator it = m_apps.begin(); it != m_apps.end(); ++it) {
if (it->first == command) {
// one for command name, one for null terminator
const char *arg[MAX_ARGS + 2];
unsigned int i = 0;
if (appargs.size() > MAX_ARGS + 1) {
PX4_ERR("%d too many arguments in run_cmd", appargs.size() - (MAX_ARGS + 1));
return 1;
}
while (i < appargs.size() && appargs[i].c_str()[0] != '\0') {
arg[i] = (char *)appargs[i].c_str();
PX4_DEBUG(" arg%d = '%s'\n", i, arg[i]);
++i;
}
arg[i] = (char *)0;
//PX4_DEBUG_PRINTF(i);
if (m_apps[command] == NULL) {
PX4_ERR("Null function !!\n");
} else {
return m_apps[command](i, (char **)arg);
}
}
}
PX4_ERR("Command %s not found", command.c_str());
return 1;
}
src/drivers/qshell/qurt/qshell.h
-70
View File
@@ -1,70 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2016 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 qshell.h
* Listener for shell commands from posix
*
* @author Nicolas de Palezieux <ndepal@gmail.com>
*/
#pragma once
#include <px4_platform_common/app.h>
#include <string>
#include <vector>
#include <uORB/Publication.hpp>
#include <uORB/topics/qshell_retval.h>
#include <uORB/topics/qshell_req.h>
#include "apps.h"
class QShell
{
public:
QShell();
~QShell() {}
int main();
int run_cmd(const std::vector<std::string> &appargs);
static px4::AppState appState; /* track requests to terminate app */
private:
uORB::Publication<qshell_retval_s> _qshell_retval_pub{ORB_ID(qshell_retval)};
qshell_req_s m_qshell_req{};
apps_map_type m_apps;
};
src/drivers/qshell/qurt/qshell_main.cpp
-58
View File
@@ -1,58 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2016 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 qshell_main.cpp
* Listener for shell commands from posix
*
* @author Nicolas de Palezieux <ndepal@gmail.com>
*/
#include <px4_platform_common/log.h>
#include <px4_platform_common/app.h>
#include <px4_platform_common/init.h>
#include "qshell.h"
extern "C" __EXPORT int qshell_entry(int argc, char **argv);
int qshell_entry(int argc, char **argv)
{
px4::init(argc, argv, "qshell");
PX4_DEBUG("qshell");
QShell qshell;
qshell.main();
PX4_DEBUG("goodbye");
return 0;
}
src/drivers/qshell/qurt/qshell_start_qurt.cpp
-117
View File
@@ -1,117 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2016 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 qshell_start_qurt.cpp
* Listener for shell commands from posix
*
* @author Nicolas de Palezieux <ndepal@gmail.com>
*/
#include "qshell.h"
#include <px4_platform_common/log.h>
#include <px4_platform_common/app.h>
#include <px4_platform_common/tasks.h>
#include <stdio.h>
#include <string.h>
#include <sched.h>
static int daemon_task; /* Handle of deamon task / thread */
//using namespace px4;
extern "C" __EXPORT int qshell_main(int argc, char *argv[]);
int qshell_entry(int argc, char **argv)
{
//px4::init(argc, argv, "qshell");
PX4_DEBUG("qshell entry.....");
QShell qshell;
qshell.main();
PX4_DEBUG("goodbye");
return 0;
}
static void usage()
{
PX4_INFO("usage: qshell {start|stop|status}");
}
int qshell_main(int argc, char *argv[])
{
if (argc < 2) {
usage();
return 1;
}
if (!strcmp(argv[1], "start")) {
if (QShell::appState.isRunning()) {
PX4_INFO("already running");
/* this is not an error */
return 0;
}
PX4_DEBUG("before starting the qshell_entry task");
daemon_task = px4_task_spawn_cmd("qshell",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
8192,
qshell_entry,
(char *const *)argv);
return 0;
}
if (!strcmp(argv[1], "stop")) {
QShell::appState.requestExit();
return 0;
}
if (!strcmp(argv[1], "status")) {
if (QShell::appState.isRunning()) {
PX4_INFO("is running");
} else {
PX4_INFO("not started");
}
return 0;
}
usage();
return 1;
}
src/lib/drivers/device/CMakeLists.txt
+1 -6
View File
@@ -40,12 +40,7 @@ if (${PX4_PLATFORM} STREQUAL "nuttx")
if ("${CONFIG_SPI}" STREQUAL "y")
list(APPEND SRCS_PLATFORM nuttx/SPI.cpp)
endif()
elseif((${PX4_PLATFORM} MATCHES "qurt"))
list(APPEND SRCS_PLATFORM
qurt/I2C.cpp
qurt/SPI.cpp
)
elseif(UNIX AND NOT APPLE AND NOT (${PX4_PLATFORM} MATCHES "qurt")) #TODO: add linux PX4 platform type
elseif(UNIX AND NOT APPLE) #TODO: add linux PX4 platform type
# Linux I2Cdev and SPIdev
list(APPEND SRCS_PLATFORM
posix/I2C.cpp
src/lib/drivers/device/i2c.h
-2
View File
@@ -34,8 +34,6 @@
#ifdef __PX4_NUTTX
#include "nuttx/I2C.hpp"
#elif __PX4_QURT
#include "qurt/I2C.hpp"
#else
#include "posix/I2C.hpp"
#endif
src/lib/drivers/device/qurt/I2C.cpp
-170
View File
@@ -1,170 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2016-2020 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 I2C.cpp
*
* Base class for devices attached via the I2C bus.
*
* @todo Bus frequency changes; currently we do nothing with the value
* that is supplied. Should we just depend on the bus knowing?
*/
#include "I2C.hpp"
#include <dev_fs_lib_i2c.h>
#include <px4_platform_common/i2c_spi_buses.h>
namespace device
{
I2C::I2C(uint8_t device_type, const char *name, const int bus, const uint16_t address, const uint32_t frequency) :
CDev(name, nullptr),
_frequency(frequency)
{
// fill in _device_id fields for a I2C device
_device_id.devid_s.devtype = device_type;
_device_id.devid_s.bus_type = DeviceBusType_I2C;
_device_id.devid_s.bus = bus;
_device_id.devid_s.address = address;
}
I2C::I2C(const I2CSPIDriverConfig &config)
: I2C(config.devid_driver_index, config.module_name, config.bus, config.i2c_address, config.bus_frequency)
{
}
I2C::~I2C()
{
if (_fd >= 0) {
::close(_fd);
_fd = -1;
}
}
int
I2C::init()
{
int ret = PX4_ERROR;
// Open the actual I2C device
char dev_path[16] {};
snprintf(dev_path, sizeof(dev_path), DEV_FS_I2C_DEVICE_TYPE_STRING"%i", get_device_bus());
_fd = ::open(dev_path, O_RDWR);
if (_fd < 0) {
DEVICE_DEBUG("failed to init I2C");
ret = -ENOENT;
goto out;
}
// call the probe function to check whether the device is present
ret = probe();
if (ret != OK) {
DEVICE_DEBUG("probe failed");
goto out;
}
// do base class init, which will create device node, etc
ret = CDev::init();
if (ret != OK) {
DEVICE_DEBUG("cdev init failed");
goto out;
}
// tell the world where we are
DEVICE_DEBUG("on I2C bus %d at 0x%02x", get_device_bus(), get_device_address());
out:
if ((ret != OK) && !(_fd < 0)) {
::close(_fd);
_fd = -1;
}
return ret;
}
int
I2C::transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const unsigned recv_len)
{
int ret = PX4_ERROR;
unsigned retry_count = 0;
if (_fd < 0) {
PX4_ERR("I2C device not opened");
return PX4_ERROR;
}
do {
DEVICE_DEBUG("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len);
dspal_i2c_ioctl_slave_config slave_config{};
slave_config.slave_address = get_device_address();
slave_config.bus_frequency_in_khz = _frequency / 1000;
slave_config.byte_transer_timeout_in_usecs = 10000; // 10 ms
int ret_config = ::ioctl(_fd, I2C_IOCTL_SLAVE, &slave_config);
if (ret_config < 0) {
DEVICE_DEBUG("Could not set slave config, result: %d", ret_config);
}
dspal_i2c_ioctl_combined_write_read ioctl_write_read{};
ioctl_write_read.write_buf = (uint8_t *)send;
ioctl_write_read.write_buf_len = send_len;
ioctl_write_read.read_buf = recv;
ioctl_write_read.read_buf_len = recv_len;
int bytes_read = ::ioctl(_fd, I2C_IOCTL_RDWR, &ioctl_write_read);
if (bytes_read != (int)recv_len) {
DEVICE_DEBUG("I2C transfer failed, bytes read %d", bytes_read);
ret = PX4_ERROR;
} else {
// success
ret = PX4_OK;
break;
}
} while (retry_count++ < _retries);
return ret;
}
} // namespace device
src/lib/drivers/device/qurt/I2C.hpp
-115
View File
@@ -1,115 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2016-2020 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 I2C.hpp
*
* Base class for devices connected via I2C.
*/
#ifndef _DEVICE_I2C_H
#define _DEVICE_I2C_H
#include "../CDev.hpp"
struct I2CSPIDriverConfig;
namespace device __EXPORT
{
/**
* Abstract class for character device on I2C
*/
class __EXPORT I2C : public CDev
{
public:
// no copy, assignment, move, move assignment
I2C(const I2C &) = delete;
I2C &operator=(const I2C &) = delete;
I2C(I2C &&) = delete;
I2C &operator=(I2C &&) = delete;
virtual int init() override;
protected:
/**
* The number of times a read or write operation will be retried on
* error.
*/
uint8_t _retries{0};
/**
* @ Constructor
*
* @param device_type The device type (see drv_sensor.h)
* @param name Driver name
* @param bus I2C bus on which the device lives
* @param address I2C bus address, or zero if set_address will be used
* @param frequency I2C bus frequency for the device (currently not used)
*/
I2C(uint8_t device_type, const char *name, const int bus, const uint16_t address, const uint32_t frequency);
I2C(const I2CSPIDriverConfig &config);
virtual ~I2C();
/**
* Check for the presence of the device on the bus.
*/
virtual int probe() { return PX4_OK; }
/**
* Perform an I2C transaction to the device.
*
* At least one of send_len and recv_len must be non-zero.
*
* @param send Pointer to bytes to send.
* @param send_len Number of bytes to send.
* @param recv Pointer to buffer for bytes received.
* @param recv_len Number of bytes to receive.
* @return OK if the transfer was successful, -errno
* otherwise.
*/
int transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const unsigned recv_len);
virtual bool external() const override { return px4_i2c_bus_external(_device_id.devid_s.bus); }
private:
uint32_t _frequency{0};
int _fd{-1};
};
} // namespace device
#endif /* _DEVICE_I2C_H */
src/lib/drivers/device/qurt/SPI.cpp
-207
View File
@@ -1,207 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2019 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 SPI.cpp
*
* Base class for devices connected via SPI.
*
*/
#include "SPI.hpp"
#include <fcntl.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include "dev_fs_lib_spi.h"
#include <px4_platform_common/i2c_spi_buses.h>
#include <px4_platform_common/px4_config.h>
namespace device
{
SPI::SPI(uint8_t device_type, const char *name, int bus, uint32_t device, enum spi_mode_e mode, uint32_t frequency) :
CDev(name, nullptr),
_device(device),
_mode(mode),
_frequency(frequency)
{
_device_id.devid_s.devtype = device_type;
// fill in _device_id fields for a SPI device
_device_id.devid_s.bus_type = DeviceBusType_SPI;
_device_id.devid_s.bus = bus;
_device_id.devid_s.address = (uint8_t)device;
}
SPI::SPI(const I2CSPIDriverConfig &config)
: SPI(config.devid_driver_index, config.module_name, config.bus, config.spi_devid, config.spi_mode,
config.bus_frequency)
{
}
SPI::~SPI()
{
if (_fd >= 0) {
::close(_fd);
_fd = -1;
}
}
int
SPI::init()
{
// Open the actual SPI device
char dev_path[16];
snprintf(dev_path, sizeof(dev_path), DEV_FS_SPI_DEVICE_TYPE_STRING"%lu", PX4_SPI_DEV_ID(_device));
DEVICE_DEBUG("%s", dev_path);
_fd = ::open(dev_path, O_RDWR);
if (_fd < 0) {
PX4_ERR("could not open %s", dev_path);
return PX4_ERROR;
}
/* call the probe function to check whether the device is present */
int ret = probe();
if (ret != OK) {
DEVICE_DEBUG("probe failed");
return ret;
}
/* do base class init, which will create the device node, etc. */
ret = CDev::init();
if (ret != OK) {
DEVICE_DEBUG("cdev init failed");
return ret;
}
/* tell the world where we are */
DEVICE_DEBUG("on SPI bus %d at %d (%u KHz)", get_device_bus(), PX4_SPI_DEV_ID(_device), _frequency / 1000);
return PX4_OK;
}
int
SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
{
if ((send == nullptr) && (recv == nullptr)) {
return -EINVAL;
}
// set bus frequency
dspal_spi_ioctl_set_bus_frequency bus_freq{};
bus_freq.bus_frequency_in_hz = _frequency;
if (::ioctl(_fd, SPI_IOCTL_SET_BUS_FREQUENCY_IN_HZ, &bus_freq) < 0) {
PX4_ERR("setting bus frequency failed");
return PX4_ERROR;
}
// set bus mode
// dspal_spi_ioctl_set_spi_mode bus_mode{};
// bus_mode.eClockPolarity = SPI_CLOCK_IDLE_HIGH;
// bus_mode.eShiftMode = SPI_OUTPUT_FIRST;
// if (::ioctl(spi_fildes, SPI_IOCTL_SET_SPI_MODE, &bus_mode) < 0) {
// PX4_ERR("setting mode failed");
// return PX4_ERROR;
// }
// transfer data
dspal_spi_ioctl_read_write ioctl_write_read{};
ioctl_write_read.read_buffer = send;
ioctl_write_read.read_buffer_length = len;
ioctl_write_read.write_buffer = recv;
ioctl_write_read.write_buffer_length = len;
int result = ::ioctl(_fd, SPI_IOCTL_RDWR, &ioctl_write_read);
if (result < 0) {
PX4_ERR("transfer error %d", result);
return PX4_ERROR;
}
return PX4_OK;
}
int
SPI::transferhword(uint16_t *send, uint16_t *recv, unsigned len)
{
if ((send == nullptr) && (recv == nullptr)) {
return -EINVAL;
}
// set bus frequency
dspal_spi_ioctl_set_bus_frequency bus_freq{};
bus_freq.bus_frequency_in_hz = _frequency;
if (::ioctl(_fd, SPI_IOCTL_SET_BUS_FREQUENCY_IN_HZ, &bus_freq) < 0) {
PX4_ERR("setting bus frequency failed");
return PX4_ERROR;
}
// set bus mode
// dspal_spi_ioctl_set_spi_mode bus_mode{};
// bus_mode.eClockPolarity = SPI_CLOCK_IDLE_HIGH;
// bus_mode.eShiftMode = SPI_OUTPUT_FIRST;
// if (::ioctl(spi_fildes, SPI_IOCTL_SET_SPI_MODE, &bus_mode) < 0) {
// PX4_ERR("setting mode failed");
// return PX4_ERROR;
// }
// transfer data
dspal_spi_ioctl_read_write ioctl_write_read{};
ioctl_write_read.read_buffer = send;
ioctl_write_read.read_buffer_length = len * 2;
ioctl_write_read.write_buffer = recv;
ioctl_write_read.write_buffer_length = len * 2;
int result = ::ioctl(_fd, SPI_IOCTL_RDWR, &ioctl_write_read);
if (result < 0) {
PX4_ERR("transfer error %d", result);
return PX4_ERROR;
}
return PX4_OK;
}
} // namespace device
src/lib/drivers/device/qurt/SPI.hpp
-171
View File
@@ -1,171 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2019 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 SPI.hpp
*
* Base class for devices connected via SPI.
*/
#pragma once
#include "../CDev.hpp"
#include "dev_fs_lib_spi.h"
enum spi_mode_e {
SPIDEV_MODE0 = 0, /* CPOL=0 CHPHA=0 */
SPIDEV_MODE1 = 1, /* CPOL=0 CHPHA=1 */
SPIDEV_MODE2 = 2, /* CPOL=1 CHPHA=0 */
SPIDEV_MODE3 = 3 /* CPOL=1 CHPHA=1 */
};
struct I2CSPIDriverConfig;
namespace device __EXPORT
{
/**
* Abstract class for character device on SPI
*/
class __EXPORT SPI : public CDev
{
protected:
/**
* Constructor
*
* @param device_type The device type (see drv_sensor.h)
* @param name Driver name
* @param bus SPI bus on which the device lives
* @param device Device handle (used by SPI_SELECT)
* @param mode SPI clock/data mode
* @param frequency SPI clock frequency
*/
SPI(uint8_t device_type, const char *name, int bus, uint32_t device, enum spi_mode_e mode, uint32_t frequency);
SPI(const I2CSPIDriverConfig &config);
virtual ~SPI();
/**
* Locking modes supported by the driver.
*/
enum LockMode {
LOCK_PREEMPTION, /**< the default; lock against all forms of preemption. */
LOCK_THREADS, /**< lock only against other threads, using SPI_LOCK */
LOCK_NONE /**< perform no locking, only safe if the bus is entirely private */
};
virtual int init();
/**
* Check for the presence of the device on the bus.
*/
virtual int probe() { return PX4_OK; }
/**
* Perform a SPI transfer.
*
* If called from interrupt context, this interface does not lock
* the bus and may interfere with non-interrupt-context callers.
*
* Clients in a mixed interrupt/non-interrupt configuration must
* ensure appropriate interlocking.
*
* At least one of send or recv must be non-null.
*
* @param send Bytes to send to the device, or nullptr if
* no data is to be sent.
* @param recv Buffer for receiving bytes from the device,
* or nullptr if no bytes are to be received.
* @param len Number of bytes to transfer.
* @return OK if the exchange was successful, -errno
* otherwise.
*/
int transfer(uint8_t *send, uint8_t *recv, unsigned len);
/**
* Perform a SPI 16 bit transfer.
*
* If called from interrupt context, this interface does not lock
* the bus and may interfere with non-interrupt-context callers.
*
* Clients in a mixed interrupt/non-interrupt configuration must
* ensure appropriate interlocking.
*
* At least one of send or recv must be non-null.
*
* @param send Words to send to the device, or nullptr if
* no data is to be sent.
* @param recv Words for receiving bytes from the device,
* or nullptr if no bytes are to be received.
* @param len Number of words to transfer.
* @return OK if the exchange was successful, -errno
* otherwise.
*/
int transferhword(uint16_t *send, uint16_t *recv, unsigned len);
/**
* Set the SPI bus frequency
* This is used to change frequency on the fly. Some sensors
* (such as the MPU6000) need a lower frequency for setup
* registers and can handle higher frequency for sensor
* value registers
*
* @param frequency Frequency to set (Hz)
*/
void set_frequency(uint32_t frequency) { _frequency = frequency; }
uint32_t get_frequency() { return _frequency; }
/**
* Set the SPI bus locking mode
*
* This set the SPI locking mode. For devices competing with NuttX SPI
* drivers on a bus the right lock mode is LOCK_THREADS.
*
* @param mode Locking mode
*/
void set_lockmode(enum LockMode mode) {}
private:
int _fd{-1};
uint32_t _device;
enum spi_mode_e _mode;
uint32_t _frequency;
protected:
bool external() { return px4_spi_bus_external(get_device_bus()); }
};
} // namespace device
src/lib/drivers/device/spi.h
-2
View File
@@ -34,8 +34,6 @@
#ifdef __PX4_NUTTX
#include "nuttx/SPI.hpp"
#elif __PX4_QURT
#include "qurt/SPI.hpp"
#else
#include "posix/SPI.hpp"
#endif
src/lib/mathlib/math/test/test.cpp
+2 -9
View File
@@ -100,14 +100,9 @@ bool __EXPORT less_than_or_equal(float a, float b)
}
}
void __EXPORT float2SigExp(
const float &num,
float &sig,
int &exp)
void __EXPORT float2SigExp(const float &num, float &sig, int &exp)
{
// FIXME - This code makes no sense when exp is an int
// FIXME - isnan and isinf not defined for QuRT
#ifndef __PX4_QURT
if (!PX4_ISFINITE(num)) {
sig = 0.0f;
exp = -99;
@@ -129,8 +124,6 @@ void __EXPORT float2SigExp(
exp = floor(exp);
}
#endif
sig = num;
// cheap power since it is integer
src/lib/parameters/CMakeLists.txt
+5 -10
View File
@@ -129,17 +129,12 @@ add_custom_command(OUTPUT px4_parameters.hpp
)
set(SRCS)
if ("${CONFIG_SHMEM}" STREQUAL "1")
message(STATUS "parameters shared memory enabled")
add_definitions(-DCONFIG_SHMEM=1)
list(APPEND SRCS parameters_shmem.cpp)
list(APPEND SRCS parameters.cpp)
if(BUILD_TESTING)
list(APPEND SRCS param_translation_unit_tests.cpp)
else()
list(APPEND SRCS parameters.cpp)
if(BUILD_TESTING)
list(APPEND SRCS param_translation_unit_tests.cpp)
else()
list(APPEND SRCS param_translation.cpp)
endif()
list(APPEND SRCS param_translation.cpp)
endif()
if(${PX4_PLATFORM} STREQUAL "nuttx")
src/lib/parameters/parameters.cpp
+4 -12
View File
@@ -81,14 +81,6 @@ static const char *param_default_file = PX4_ROOTFSDIR"/eeprom/parameters";
static char *param_user_file = nullptr;
#ifdef __PX4_QURT
#define PARAM_OPEN px4_open
#define PARAM_CLOSE px4_close
#else
#define PARAM_OPEN open
#define PARAM_CLOSE close
#endif
#include <px4_platform_common/workqueue.h>
/* autosaving variables */
static hrt_abstime last_autosave_timestamp = 0;
@@ -1133,11 +1125,11 @@ int param_save_default()
while (res != OK && attempts > 0) {
// write parameters to file
int fd = PARAM_OPEN(filename, O_WRONLY | O_CREAT, PX4_O_MODE_666);
int fd = ::open(filename, O_WRONLY | O_CREAT, PX4_O_MODE_666);
if (fd > -1) {
res = param_export(fd, false, nullptr);
PARAM_CLOSE(fd);
::close(fd);
if (res != PX4_OK) {
PX4_ERR("param_export failed, retrying %d", attempts);
@@ -1170,7 +1162,7 @@ param_load_default()
return flash_param_load();
}
int fd_load = PARAM_OPEN(filename, O_RDONLY);
int fd_load = ::open(filename, O_RDONLY);
if (fd_load < 0) {
/* no parameter file is OK, otherwise this is an error */
@@ -1183,7 +1175,7 @@ param_load_default()
}
int result = param_load(fd_load);
PARAM_CLOSE(fd_load);
::close(fd_load);
if (result != 0) {
PX4_ERR("error reading parameters from '%s'", filename);
src/lib/parameters/parameters_shmem.cpp
-1423
View File
File diff suppressed because it is too large Load Diff
src/lib/perf/perf_counter.cpp
-5
View File
@@ -49,11 +49,6 @@
#include "perf_counter.h"
#ifdef __PX4_QURT
// There is presumably no dprintf on QURT. Therefore use the usual output to mini-dm.
#define dprintf(_fd, _text, ...) ((_fd) == 1 ? PX4_INFO((_text), ##__VA_ARGS__) : (void)(_fd))
#endif
/**
* Header common to all counters.
*/
src/modules/muorb/adsp/CMakeLists.txt
-38
View File
@@ -1,38 +0,0 @@
############################################################################
#
# 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.
#
############################################################################
px4_add_library(modules__muorb__adsp
px4muorb.cpp
uORBFastRpcChannel.cpp
)
target_include_directories(modules__muorb__adsp PRIVATE ${PX4_SOURCE_DIR}/platforms/common/uORB)
src/modules/muorb/adsp/px4muorb.cpp
-295
View File
@@ -1,295 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2015 Mark Charlebois. 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.
*
****************************************************************************/
#include "px4muorb.hpp"
#include "uORBFastRpcChannel.hpp"
#include "uORBManager.hpp"
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/posix.h>
#include <dspal_platform.h>
#include "uORB.h"
#include <parameters/param.h>
#include <px4_platform_common/shmem.h>
#include <px4_platform_common/log.h>
__BEGIN_DECLS
extern int dspal_main(int argc, char *argv[]);
__END_DECLS
int px4muorb_orb_initialize()
{
HAP_power_request(100, 100, 1000);
shmem_info_p = NULL;
// The uORB Manager needs to be initialized first up, otherwise the instance is nullptr.
uORB::Manager::initialize();
// Register the fastrpc muorb with uORBManager.
uORB::Manager::get_instance()->set_uorb_communicator(
uORB::FastRpcChannel::GetInstance());
// Now continue with the usual dspal startup.
const char *argv[] = { "dspal", "start" };
int argc = 2;
int rc;
rc = dspal_main(argc, (char **) argv);
return rc;
}
int px4muorb_set_absolute_time_offset(int32_t time_diff_us)
{
return hrt_set_absolute_time_offset(time_diff_us);
}
int px4muorb_get_absolute_time(uint64_t *time_us)
{
*time_us = hrt_absolute_time();
return 0;
}
/*update value and param's change bit in shared memory*/
int px4muorb_param_update_to_shmem(uint32_t param, const uint8_t *value,
int data_len_in_bytes)
{
unsigned int byte_changed, bit_changed;
union param_value_u *param_value = (union param_value_u *) value;
if (!shmem_info_p) {
init_shared_memory();
}
if (get_shmem_lock(__FILE__, __LINE__) != 0) {
PX4_INFO("Could not get shmem lock\n");
return -1;
}
shmem_info_p->params_val[param] = *param_value;
byte_changed = param / 8;
bit_changed = 1 << param % 8;
shmem_info_p->krait_changed_index[byte_changed] |= bit_changed;
release_shmem_lock(__FILE__, __LINE__);
return 0;
}
int px4muorb_param_update_index_from_shmem(unsigned char *data, int data_len_in_bytes)
{
if (!shmem_info_p) {
return -1;
}
if (get_shmem_lock(__FILE__, __LINE__) != 0) {
PX4_INFO("Could not get shmem lock\n");
return -1;
}
for (int i = 0; i < data_len_in_bytes; i++) {
data[i] = shmem_info_p->adsp_changed_index[i];
}
release_shmem_lock(__FILE__, __LINE__);
return 0;
}
int px4muorb_param_update_value_from_shmem(uint32_t param, const uint8_t *value,
int data_len_in_bytes)
{
unsigned int byte_changed, bit_changed;
union param_value_u *param_value = (union param_value_u *) value;
if (!shmem_info_p) {
return -1;
}
if (get_shmem_lock(__FILE__, __LINE__) != 0) {
PX4_INFO("Could not get shmem lock\n");
return -1;
}
*param_value = shmem_info_p->params_val[param];
/*also clear the index since we are holding the lock*/
byte_changed = param / 8;
bit_changed = 1 << param % 8;
shmem_info_p->adsp_changed_index[byte_changed] &= ~bit_changed;
release_shmem_lock(__FILE__, __LINE__);
return 0;
}
int px4muorb_topic_advertised(const char *topic_name)
{
int rc = 0;
PX4_INFO("TEST px4muorb_topic_advertised of [%s] on remote side...", topic_name);
uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance();
uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler();
if (rxHandler != nullptr) {
rc = rxHandler->process_remote_topic(topic_name, 1);
} else {
rc = -1;
}
return rc;
}
int px4muorb_topic_unadvertised(const char *topic_name)
{
int rc = 0;
PX4_INFO("TEST px4muorb_topic_unadvertised of [%s] on remote side...", topic_name);
uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance();
uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler();
if (rxHandler != nullptr) {
rc = rxHandler->process_remote_topic(topic_name, 0);
} else {
rc = -1;
}
return rc;
}
int px4muorb_add_subscriber(const char *name)
{
int rc = 0;
uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance();
channel->AddRemoteSubscriber(name);
uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler();
if (rxHandler != nullptr) {
rc = rxHandler->process_add_subscription(name, 0);
if (rc != OK) {
channel->RemoveRemoteSubscriber(name);
}
} else {
rc = -1;
}
return rc;
}
int px4muorb_remove_subscriber(const char *name)
{
int rc = 0;
uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance();
channel->RemoveRemoteSubscriber(name);
uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler();
if (rxHandler != nullptr) {
rc = rxHandler->process_remove_subscription(name);
} else {
rc = -1;
}
return rc;
}
int px4muorb_send_topic_data(const char *name, const uint8_t *data,
int data_len_in_bytes)
{
int rc = 0;
uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance();
uORBCommunicator::IChannelRxHandler *rxHandler = channel->GetRxHandler();
if (rxHandler != nullptr) {
rc = rxHandler->process_received_message(name, data_len_in_bytes,
(uint8_t *) data);
} else {
rc = -1;
}
return rc;
}
int px4muorb_is_subscriber_present(const char *topic_name, int *status)
{
int rc = 0;
int32_t local_status = 0;
uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance();
rc = channel->is_subscriber_present(topic_name, &local_status);
if (rc == 0) {
*status = (int) local_status;
}
return rc;
}
int px4muorb_receive_msg(int *msg_type, char *topic_name, int topic_name_len,
uint8_t *data, int data_len_in_bytes, int *bytes_returned)
{
int rc = 0;
int32_t local_msg_type = 0;
int32_t local_bytes_returned = 0;
uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance();
//PX4_DEBUG( "topic_namePtr: [0x%p] dataPtr: [0x%p]", topic_name, data );
rc = channel->get_data(&local_msg_type, topic_name, topic_name_len, data,
data_len_in_bytes, &local_bytes_returned);
*msg_type = (int) local_msg_type;
*bytes_returned = (int) local_bytes_returned;
return rc;
}
int px4muorb_receive_bulk_data(uint8_t *bulk_transfer_buffer,
int max_size_in_bytes, int *returned_length_in_bytes, int *topic_count)
{
int rc = 0;
int32_t local_bytes_returned = 0;
int32_t local_topic_count = 0;
uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance();
//PX4_DEBUG( "topic_namePtr: [0x%p] dataPtr: [0x%p]", topic_name, data );
rc = channel->get_bulk_data(bulk_transfer_buffer, max_size_in_bytes,
&local_bytes_returned, &local_topic_count);
*returned_length_in_bytes = (int) local_bytes_returned;
*topic_count = (int) local_topic_count;
return rc;
}
int px4muorb_unblock_recieve_msg(void)
{
int rc = 0;
uORB::FastRpcChannel *channel = uORB::FastRpcChannel::GetInstance();
rc = channel->unblock_get_data_method();
return rc;
}
src/modules/muorb/adsp/px4muorb.hpp
-71
View File
@@ -1,71 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2015 Mark Charlebois. 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.
*
****************************************************************************/
#pragma once
#include <stdint.h>
extern "C" {
int px4muorb_orb_initialize() __EXPORT;
int px4muorb_set_absolute_time_offset(int32_t time_diff_us) __EXPORT;
int px4muorb_get_absolute_time(uint64_t *time_us) __EXPORT;
int px4muorb_param_update_to_shmem(uint32_t param, const uint8_t *value, int data_len_in_bytes) __EXPORT;
int px4muorb_param_update_index_from_shmem(unsigned char *data, int data_len_in_bytes) __EXPORT;
int px4muorb_param_update_value_from_shmem(uint32_t param, const uint8_t *value, int data_len_in_bytes) __EXPORT;
int px4muorb_topic_advertised(const char *name) __EXPORT;
int px4muorb_topic_unadvertised(const char *name) __EXPORT;
int px4muorb_add_subscriber(const char *name) __EXPORT;
int px4muorb_remove_subscriber(const char *name) __EXPORT;
int px4muorb_send_topic_data(const char *name, const uint8_t *data, int data_len_in_bytes) __EXPORT;
int px4muorb_is_subscriber_present(const char *topic_name, int *status) __EXPORT;
int px4muorb_receive_msg(int *msg_type, char *topic_name, int topic_name_len, uint8_t *data, int data_len_in_bytes,
int *bytes_returned) __EXPORT;
int px4muorb_receive_bulk_data(uint8_t *_BulkTransferBuffer, int max_size_in_bytes,
int *length_in_bytes, int *topic_count) __EXPORT;
int px4muorb_unblock_recieve_msg(void) __EXPORT;
}
src/modules/muorb/adsp/uORBFastRpcChannel.cpp
-715
View File
@@ -1,715 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2015 Mark Charlebois. 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.
*
****************************************************************************/
#include "uORBFastRpcChannel.hpp"
#include <px4_platform_common/log.h>
#include <algorithm>
#include <string.h>
#include <drivers/drv_hrt.h>
// static initialization.
uORB::FastRpcChannel uORB::FastRpcChannel::_Instance;
static unsigned long _dropped_pkts;
static unsigned long _get_min = 0xFFFFFF;
static unsigned long _get_max = 0;
static unsigned long _min_q = 200;
static unsigned long _max_q = 0;
static unsigned long _avg_q = 0;
static unsigned long _count = 0;
static unsigned long _get_bulk_min = 0xFFFFFF;
static unsigned long _get_bulk_max = 0;
static unsigned long _bulk_topic_count_min = 0xFFFFFF;
static unsigned long _bulk_topic_count_max = 0;
//==============================================================================
//==============================================================================
uORB::FastRpcChannel::FastRpcChannel()
: _RxHandler(0)
, _DataQInIndex(0)
, _DataQOutIndex(0)
, _ControlQInIndex(0)
, _ControlQOutIndex(0)
{
for (int32_t i = 0; i < _MAX_MSG_QUEUE_SIZE; ++ i) {
_DataMsgQueue[i]._MaxBufferSize = 0;
_DataMsgQueue[i]._Length = 0;
_DataMsgQueue[i]._Buffer = 0;
}
_RemoteSubscribers.clear();
}
//==============================================================================
//==============================================================================
int16_t uORB::FastRpcChannel::topic_advertised(const char *messageName)
{
return control_msg_queue_add(_CONTROL_MSG_TYPE_ADVERTISE, messageName);
}
//==============================================================================
//==============================================================================
int16_t uORB::FastRpcChannel::topic_unadvertised(const char *messageName)
{
return control_msg_queue_add(_CONTROL_MSG_TYPE_UNADVERTISE, messageName);
}
//==============================================================================
//==============================================================================
int16_t uORB::FastRpcChannel::control_msg_queue_add(int32_t msgtype, const char *messageName)
{
int16_t rc = 0;
hrt_abstime t1, t2;
static hrt_abstime check_time = 0;
t1 = hrt_absolute_time();
_QueueMutex.lock();
bool overwriteData = false;
if (IsControlQFull()) {
// queue is full. Overwrite the oldest data.
_ControlQOutIndex++;
if (_ControlQOutIndex == _MAX_MSG_QUEUE_SIZE) {
_ControlQOutIndex = 0;
}
overwriteData = true;
_dropped_pkts++;
}
_ControlMsgQueue[ _ControlQInIndex ]._Type = msgtype;
_ControlMsgQueue[ _ControlQInIndex ]._MsgName = messageName;
_ControlQInIndex++;
if (_ControlQInIndex == _MAX_MSG_QUEUE_SIZE) {
_ControlQInIndex = 0;
}
// the assumption here is that each caller reads only one data from either control or data queue.
if (ControlQSize() == 1 && DataQSize() == 0) { // post it only of the queue moves from empty to available.
_DataAvailableSemaphore.post();
}
if ((unsigned long)ControlQSize() < _min_q) { _min_q = (unsigned long)ControlQSize(); }
if ((unsigned long)ControlQSize() > _max_q) { _max_q = (unsigned long)ControlQSize(); }
_count++;
_avg_q = ((double)((_avg_q * (_count - 1)) + (unsigned long)(ControlQSize()))) / (double)(_count);
_QueueMutex.unlock();
t2 = hrt_absolute_time();
if ((unsigned long)(t2 - check_time) > 10000000) {
//PX4_DEBUG("MsgName: %20s, t1: %lu, t2: %lu, dt: %lu",messageName, (unsigned long) t1, (unsigned long) t2, (unsigned long) (t2-t1));
//PX4_DEBUG("Q. Stats: min: %lu, max : %lu, avg: %lu count: %lu ", _min_q, _max_q, (unsigned long)(_avg_q * 1000.0), _count);
_min_q = _MAX_MSG_QUEUE_SIZE * 2;
_max_q = 0;
_avg_q = 0;
_count = 0;
check_time = t2;
}
return rc;
}
//==============================================================================
//==============================================================================
int16_t uORB::FastRpcChannel::add_subscription(const char *messageName, int32_t msgRateInHz)
{
int16_t rc = 0;
_Subscribers.push_back(messageName);
PX4_DEBUG("Adding message[%s] to subscriber queue...", messageName);
return rc;
}
//==============================================================================
//==============================================================================
int16_t uORB::FastRpcChannel::remove_subscription(const char *messageName)
{
int16_t rc = 0;
_Subscribers.remove(messageName);
return rc;
}
int16_t uORB::FastRpcChannel::is_subscriber_present(const char *messageName, int32_t *status)
{
int16_t rc = 0;
if (std::find(_Subscribers.begin(), _Subscribers.end(), messageName) != _Subscribers.end()) {
*status = 1;
//PX4_DEBUG("******* Found subscriber for message[%s]....", messageName);
} else {
*status = 0;
//PX4_WARN("@@@@@ Subscriber not found for[%s]...numSubscribers[%d]", messageName, _Subscribers.size());
int i = 0;
for (std::list<std::string>::iterator it = _Subscribers.begin(); it != _Subscribers.end(); ++it) {
if (*it == messageName) {
PX4_DEBUG("##### Found the message[%s] in the subscriber list-index[%d]", messageName, i);
}
++i;
}
}
return rc;
}
int16_t uORB::FastRpcChannel::unblock_get_data_method()
{
PX4_DEBUG("[unblock_get_data_method] calling post method for _DataAvailableSemaphore()");
_DataAvailableSemaphore.post();
return 0;
}
//==============================================================================
//==============================================================================
int16_t uORB::FastRpcChannel::register_handler(uORBCommunicator::IChannelRxHandler *handler)
{
_RxHandler = handler;
return 0;
}
//==============================================================================
//==============================================================================
int16_t uORB::FastRpcChannel::send_message(const char *messageName, int32_t length, uint8_t *data)
{
int16_t rc = 0;
hrt_abstime t1, t2;
static hrt_abstime check_time = 0;
if (_RemoteSubscribers.find(messageName) == _RemoteSubscribers.end()) {
//there is no-remote subscriber. So do not queue the message.
return rc;
}
t1 = hrt_absolute_time();
_QueueMutex.lock();
bool overwriteData = false;
if (IsDataQFull()) {
// queue is full. Overwrite the oldest data.
//PX4_WARN("[send_message] Queue Full Overwrite the oldest data. in[%ld] out[%ld] max[%ld]",
// _DataQInIndex, _DataQOutIndex, _MAX_MSG_QUEUE_SIZE);
_DataQOutIndex++;
if (_DataQOutIndex == _MAX_MSG_QUEUE_SIZE) {
_DataQOutIndex = 0;
}
overwriteData = true;
_dropped_pkts++;
}
// now check to see if the data queue's buffer size if large enough to memcpy the data.
// if not, delete the old buffer and re-create a new buffer of larger size.
check_and_expand_data_buffer(_DataQInIndex, length);
// now memcpy the data to the buffer.
memcpy(_DataMsgQueue[ _DataQInIndex ]._Buffer, data, length);
_DataMsgQueue[ _DataQInIndex ]._Length = length;
_DataMsgQueue[ _DataQInIndex ]._MsgName = messageName;
_DataQInIndex++;
if (_DataQInIndex == _MAX_MSG_QUEUE_SIZE) {
_DataQInIndex = 0;
}
// the assumption here is that each caller reads only one data from either control or data queue.
//if (!overwriteData) {
if (DataQSize() == 1 && ControlQSize() == 0) { // post it only of the queue moves from empty to available.
_DataAvailableSemaphore.post();
}
if ((unsigned long)DataQSize() < _min_q) { _min_q = (unsigned long)DataQSize(); }
if ((unsigned long)DataQSize() > _max_q) { _max_q = (unsigned long)DataQSize(); }
_count++;
_avg_q = ((double)((_avg_q * (_count - 1)) + (unsigned long)(DataQSize()))) / (double)(_count);
_QueueMutex.unlock();
t2 = hrt_absolute_time();
if ((unsigned long)(t2 - check_time) > 10000000) {
//PX4_DEBUG("MsgName: %20s, t1: %lu, t2: %lu, dt: %lu",messageName, (unsigned long) t1, (unsigned long) t2, (unsigned long) (t2-t1));
//PX4_DEBUG("Q. Stats: min: %lu, max : %lu, avg: %lu count: %lu ", _min_q, _max_q, (unsigned long)(_avg_q * 1000.0), _count);
_min_q = _MAX_MSG_QUEUE_SIZE * 2;
_max_q = 0;
_avg_q = 0;
_count = 0;
check_time = t2;
}
return rc;
}
//==============================================================================
//==============================================================================
void uORB::FastRpcChannel::check_and_expand_data_buffer(int32_t index, int32_t length)
{
if (_DataMsgQueue[ index ]._MaxBufferSize < length) {
// create a new buffer of size length and delete old buffer.
if (_DataMsgQueue[ index ]._Buffer != 0) {
delete _DataMsgQueue[ index ]._Buffer;
}
_DataMsgQueue[ index ]._Buffer = new uint8_t[ length ];
if (_DataMsgQueue[ index ]._Buffer == 0) {
PX4_ERR("Error[check_and_expand_data_buffer] Failed to allocate data queue buffer of size[%ld]", length);
_DataMsgQueue[ index ]._MaxBufferSize = 0;
return;
}
_DataMsgQueue[ index ]._MaxBufferSize = length;
}
}
int32_t uORB::FastRpcChannel::DataQSize()
{
int32_t rc;
rc = (_DataQInIndex - _DataQOutIndex) + _MAX_MSG_QUEUE_SIZE;
rc %= _MAX_MSG_QUEUE_SIZE;
return rc;
}
int32_t uORB::FastRpcChannel::ControlQSize()
{
int32_t rc;
rc = (_ControlQInIndex - _ControlQOutIndex) + _MAX_MSG_QUEUE_SIZE;
rc %= _MAX_MSG_QUEUE_SIZE;
return rc;
}
bool uORB::FastRpcChannel::IsControlQFull()
{
return (ControlQSize() == (_MAX_MSG_QUEUE_SIZE - 1));
}
bool uORB::FastRpcChannel::IsControlQEmpty()
{
return (ControlQSize() == 0);
}
bool uORB::FastRpcChannel::IsDataQFull()
{
return (DataQSize() == (_MAX_MSG_QUEUE_SIZE - 1));
}
bool uORB::FastRpcChannel::IsDataQEmpty()
{
return (DataQSize() == 0);
}
int16_t uORB::FastRpcChannel::get_data
(
int32_t *msg_type,
char *topic_name,
int32_t topic_name_len,
uint8_t *data,
int32_t data_len_in_bytes,
int32_t *bytes_returned
)
{
int16_t rc = 0;
PX4_DEBUG("Get data should not be called...");
return -1;
// wait for data availability
static hrt_abstime check_time = 0;
hrt_abstime t1 = hrt_absolute_time();
_DataAvailableSemaphore.wait();
// hrt_abstime t2 = hrt_absolute_time();
_QueueMutex.lock();
if (DataQSize() != 0 || ControlQSize() != 0) {
if (ControlQSize() > 0) {
// read the first element of the Control Queue.
*msg_type = _ControlMsgQueue[ _ControlQOutIndex ]._Type;
if ((int)_ControlMsgQueue[ _ControlQOutIndex ]._MsgName.size() < (int)topic_name_len) {
memcpy
(
topic_name,
_ControlMsgQueue[ _ControlQOutIndex ]._MsgName.c_str(),
_ControlMsgQueue[ _ControlQOutIndex ]._MsgName.size()
);
topic_name[_ControlMsgQueue[ _ControlQOutIndex ]._MsgName.size()] = 0;
*bytes_returned = 0;
_ControlQOutIndex++;
if (_ControlQOutIndex == _MAX_MSG_QUEUE_SIZE) {
_ControlQOutIndex = 0;
}
} else {
PX4_ERR("Error[get_data-CONTROL]: max topic_name_len[%ld] < controlMsgLen[%d]",
topic_name_len,
_ControlMsgQueue[ _ControlQOutIndex ]._MsgName.size()
);
rc = -1;
}
} else {
// read the first element of the Control Queue.
*msg_type = _DATA_MSG_TYPE;
if (((int)_DataMsgQueue[ _DataQOutIndex ]._MsgName.size() < topic_name_len) ||
(_DataMsgQueue[ _DataQOutIndex ]._Length < data_len_in_bytes)) {
memcpy
(
topic_name,
_DataMsgQueue[ _DataQOutIndex ]._MsgName.c_str(),
_DataMsgQueue[ _DataQOutIndex ]._MsgName.size()
);
topic_name[_DataMsgQueue[ _DataQOutIndex ]._MsgName.size()] = 0;
*bytes_returned = _DataMsgQueue[ _DataQOutIndex ]._Length;
memcpy(data, _DataMsgQueue[ _DataQOutIndex ]._Buffer, _DataMsgQueue[ _DataQOutIndex ]._Length);
_DataQOutIndex++;
if (_DataQOutIndex == _MAX_MSG_QUEUE_SIZE) {
_DataQOutIndex = 0;
}
} else {
PX4_ERR("Error:[get_data-DATA] type msg max topic_name_len[%ld] > dataMsgLen[%d] ",
topic_name_len,
_DataMsgQueue[ _DataQOutIndex ]._MsgName.size()
);
PX4_ERR("Error:[get_data-DATA] Or data_buffer_len[%ld] > message_size[%ld] ",
data_len_in_bytes,
_DataMsgQueue[ _DataQOutIndex ]._Length
);
rc = -1;
}
}
} else {
PX4_ERR("[get_data] Error: Semaphore is up when there is no data on the control/data queues");
rc = -1;
}
_QueueMutex.unlock();
hrt_abstime t3 = hrt_absolute_time();
if ((unsigned long)(t3 - t1) > _get_max) { _get_max = (unsigned long)(t3 - t1); }
if ((unsigned long)(t3 - t1) < _get_min) { _get_min = (unsigned long)(t3 - t1); }
if ((unsigned long)(t3 - check_time) > 1000000) {
if (rc != 0) {
topic_name[0] = '\0';
}
/*
PX4_DEBUG("GetData: %30s: t1: %lu t2: %lu t3: %lu", topic_name, (unsigned long)t1, (unsigned long)t2,
(unsigned long)t3);
PX4_DEBUG(".... dt1: %7lu dt2: %7lu Q: %d", (unsigned long)(t2 - t1), (unsigned long)(t3 - t2), DataQSize());
PX4_DEBUG("ADSP RPC Stats: _get_min: %lu _get_max: %lu _dropped_pkts: %lu", _get_min, _get_max, _dropped_pkts);
*/
check_time = t3;
}
return rc;
}
int16_t uORB::FastRpcChannel::get_bulk_data
(
uint8_t *buffer,
int32_t max_buffer_in_bytes,
int32_t *returned_bytes,
int32_t *topic_count
)
{
int16_t rc = 0;
// wait for data availability
static hrt_abstime check_time = 0;
hrt_abstime t1 = hrt_absolute_time();
_DataAvailableSemaphore.wait();
//hrt_abstime t2 = hrt_absolute_time();
_QueueMutex.lock();
int32_t bytes_copied = 0;
int32_t copy_result = 0;
*returned_bytes = 0;
*topic_count = 0;
int32_t topic_count_to_return = 0;
if (DataQSize() != 0 || ControlQSize() != 0) {
if (DataQSize() != 0) {
//PX4_DEBUG( "get_bulk_data: QSize: %d", DataQSize() );
topic_count_to_return = DataQSize();
while (DataQSize() != 0) {
// this is a hack as we are using a counting semaphore. Should be re-implemented with cond_variable and wait.
//_DataAvailableSemaphore.wait();
if (get_msg_size_at(true, _DataQOutIndex) < (max_buffer_in_bytes - bytes_copied)) {
// there is enough space in the buffer, copy the data.
//PX4_DEBUG( "Coping Data to buffer..." );
copy_result = copy_msg_to_buffer(true, _DataQOutIndex, buffer, bytes_copied, max_buffer_in_bytes);
if (copy_result == -1) {
if (bytes_copied == 0) {
rc = -1;
}
break;
} else {
//PX4_DEBUG( "[%d] %02x %02x %02x %02x", *topic_count,\
// buffer[bytes_copied], \
// buffer[bytes_copied+1], \
// buffer[bytes_copied+2], \
// buffer[bytes_copied+3] );
bytes_copied += copy_result;
(*topic_count)++;
*returned_bytes = bytes_copied;
_DataQOutIndex++;
if (_DataQOutIndex == _MAX_MSG_QUEUE_SIZE) {
_DataQOutIndex = 0;
}
}
} else {
if (bytes_copied == 0) {
rc = -1;
PX4_WARN("ERROR: Insufficent space in data buffer, no topics returned");
} else {
PX4_DEBUG("Exiting out of the while loop...");
}
break;
}
}
}
if (ControlQSize() != 0) {
//PX4_DEBUG( "get_bulk_data: QSize: %d", ControlQSize() );
topic_count_to_return += ControlQSize();
while (ControlQSize() != 0) {
// this is a hack as we are using a counting semaphore. Should be re-implemented with cond_variable and wait.
//_DataAvailableSemaphore.wait();
if (get_msg_size_at(false, _ControlQOutIndex) < (max_buffer_in_bytes - bytes_copied)) {
// there is enough space in the buffer, copy the data.
//PX4_DEBUG( "Coping Control msg to buffer..." );
copy_result = copy_msg_to_buffer(false, _ControlQOutIndex, buffer, bytes_copied, max_buffer_in_bytes);
if (copy_result == -1) {
if (bytes_copied == 0) {
rc = -1;
}
break;
} else {
//PX4_DEBUG( "[%d] %02x %02x %02x %02x", *topic_count,\
// buffer[bytes_copied], \
// buffer[bytes_copied+1], \
// buffer[bytes_copied+2], \
// buffer[bytes_copied+3] );
bytes_copied += copy_result;
(*topic_count)++;
*returned_bytes = bytes_copied;
_ControlQOutIndex++;
if (_ControlQOutIndex == _MAX_MSG_QUEUE_SIZE) {
_ControlQOutIndex = 0;
}
}
} else {
if (bytes_copied == 0) {
rc = -1;
PX4_WARN("ERROR: Insufficent space in data buffer, no topics returned");
} else {
PX4_DEBUG("Exiting out of the while loop...");
}
break;
}
}
}
} else {
PX4_ERR("[get_data_bulk] Error: Semaphore is up when there is no data on the control/data queues");
rc = -1;
}
if (topic_count_to_return != *topic_count) {
PX4_WARN("Not sending all topics: topics_to_return:[%ld] topics_returning:[%ld]", topic_count_to_return, *topic_count);
}
_QueueMutex.unlock();
hrt_abstime t3 = hrt_absolute_time();
if ((unsigned long)(t3 - t1) > _get_bulk_max) { _get_bulk_max = (unsigned long)(t3 - t1); }
if ((unsigned long)(t3 - t1) < _get_bulk_min) { _get_bulk_min = (unsigned long)(t3 - t1); }
if ((unsigned long)(*topic_count) > _bulk_topic_count_max) { _bulk_topic_count_max = (unsigned long)(*topic_count); }
if ((unsigned long)(*topic_count) < _bulk_topic_count_min) { _bulk_topic_count_min = (unsigned long)(*topic_count); }
if ((unsigned long)(t3 - check_time) > 10000000) {
//PX4_DEBUG("GetData: t1: %lu t2: %lu t3: %lu", (unsigned long)t1, (unsigned long)t2, (unsigned long)t3);
//PX4_DEBUG(".... dt1: %7lu dt2: %7lu Q: %d", (unsigned long)(t2 - t1), (unsigned long)(t3 - t2), DataQSize());
//PX4_DEBUG("ADSP RPC Stats: _get_bulk_min: %lu _get_bulk_max: %lu _dropped_pkts: %lu", _get_bulk_min, _get_bulk_max,
// _dropped_pkts);
//PX4_DEBUG(" .... topic_count_min: %lu topic_count_max: %lu", _bulk_topic_count_min, _bulk_topic_count_max);
_get_bulk_max = 0;
_get_bulk_min = 0xFFFFFF;
_bulk_topic_count_min = 0xFFFFFF;
_bulk_topic_count_max = 0;
check_time = t3;
}
//PX4_DEBUG( "Returning topics: %d bytes_returned: %d", *topic_count, *returned_bytes );
return rc;
}
int32_t uORB::FastRpcChannel::get_msg_size_at(bool isData, int32_t index)
{
// the assumption here is that this is called within the context of semaphore,
// hence lock/unlock is not needed.
int32_t rc = 0;
if (isData) {
rc += _DataMsgQueue[ index ]._Length;
rc += _DataMsgQueue[ index ]._MsgName.size() + 1;
} else {
rc += _ControlMsgQueue[ index ]._MsgName.size() + 1;
}
rc += _PACKET_HEADER_SIZE;
return rc;
}
int32_t uORB::FastRpcChannel::copy_msg_to_buffer(bool isData, int32_t src_index, uint8_t *dst_buffer, int32_t offset,
int32_t dst_buffer_len)
{
int32_t rc = -1;
// before calling this method the following are assumed:
// * sem_lock is acquired for data protection
// * the dst_buffer is validated to
uint16_t msg_size = (isData ?
(uint16_t)(_DataMsgQueue[ src_index ]._MsgName.size()) :
(uint16_t)(_ControlMsgQueue[ src_index ]._MsgName.size()));
// compute the different offsets to pack the packets.
int32_t field_header_offset = offset;
int32_t field_topic_name_offset = field_header_offset + sizeof(struct BulkTransferHeader);
int32_t field_data_offset = field_topic_name_offset + msg_size + 1;
int16_t msg_type = isData ? _DATA_MSG_TYPE : _ControlMsgQueue[ src_index ]._Type;
struct BulkTransferHeader header = { (uint16_t)msg_type, (uint16_t)(msg_size + 1),
(uint16_t)(isData ? (_DataMsgQueue[ src_index ]._Length) : 0)
};
//PX4_DEBUG( "Offsets: header[%d] name[%d] data[%d]",
// field_header_offset,
// field_topic_name_offset,
// field_data_offset );
if (isData && (field_data_offset + _DataMsgQueue[ src_index ]._Length) < dst_buffer_len) {
memmove(&(dst_buffer[field_header_offset]), (char *)(&header), sizeof(header));
// pack the data here.
memmove
(
&(dst_buffer[field_topic_name_offset]),
_DataMsgQueue[ src_index ]._MsgName.c_str(),
_DataMsgQueue[ src_index ]._MsgName.size()
);
if (_DataMsgQueue[ src_index ]._MsgName.size() == 0) {
PX4_WARN("########## Error MsgName cannot be zero: ");
}
dst_buffer[ field_topic_name_offset + _DataMsgQueue[ src_index ]._MsgName.size()] = '\0';
memmove(&(dst_buffer[field_data_offset]), _DataMsgQueue[ src_index ]._Buffer, _DataMsgQueue[ src_index ]._Length);
rc = field_data_offset + _DataMsgQueue[ src_index ]._Length - offset;
} else if (field_data_offset < dst_buffer_len) { //This is a control message
memmove(&(dst_buffer[field_header_offset]), (char *)(&header), sizeof(header));
// pack the data here.
memmove
(
&(dst_buffer[field_topic_name_offset]),
_ControlMsgQueue[ src_index ]._MsgName.c_str(),
_ControlMsgQueue[ src_index ]._MsgName.size()
);
if (_ControlMsgQueue[ src_index ]._MsgName.size() == 0) {
PX4_WARN("########## Error MsgName cannot be zero: ");
}
dst_buffer[ field_topic_name_offset + _ControlMsgQueue[ src_index ]._MsgName.size()] = '\0';
rc = field_data_offset - offset;
} else {
PX4_WARN("Error coping the Msg to dst buffer, insuffienct space. ");
if (isData) {
PX4_WARN("Data... offset[%ld] len[%ld] data_msg_len[%ld]",
offset, dst_buffer_len, (field_data_offset - offset) + _DataMsgQueue[ src_index ]._Length);
} else {
PX4_WARN("ControlMsg... offset[%ld] len[%ld]",
offset, dst_buffer_len, (field_data_offset - offset));
}
}
return rc;
}
src/modules/muorb/adsp/uORBFastRpcChannel.hpp
-301
View File
@@ -1,301 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2015 Mark Charlebois. 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.
*
****************************************************************************/
#ifndef _uORBFastRpcChannel_hpp_
#define _uORBFastRpcChannel_hpp_
#include <stdint.h>
#include <string>
#include <list>
#include "uORB/uORBCommunicator.hpp"
#include <semaphore.h>
#include <set>
#include <px4_platform_common/sem.h>
namespace uORB
{
class FastRpcChannel;
}
class uORB::FastRpcChannel : public uORBCommunicator::IChannel
{
public:
/**
* static method to get the IChannel Implementor.
*/
static uORB::FastRpcChannel *GetInstance()
{
return &(_Instance);
}
/**
* @brief Interface to notify the remote entity of a topic being advertised.
*
* @param messageName
* This represents the uORB message name(aka topic); This message name should be
* globally unique.
* @return
* 0 = success; This means the messages is successfully sent to the receiver
* Note: This does not mean that the receiver as received it.
* otherwise = failure.
*/
virtual int16_t topic_advertised(const char *messageName);
/**
* @brief Interface to notify the remote entity of a topic being unadvertised
* and is no longer publishing messages.
*
* @param messageName
* This represents the uORB message name(aka topic); This message name should be
* globally unique.
* @return
* 0 = success; This means the messages is successfully sent to the receiver
* Note: This does not mean that the receiver as received it.
* otherwise = failure.
*/
virtual int16_t topic_unadvertised(const char *messageName);
/**
* @brief Interface to notify the remote entity of interest of a
* subscription for a message.
*
* @param messageName
* This represents the uORB message name; This message name should be
* globally unique.
* @param msgRate
* The max rate at which the subscriber can accept the messages.
* @return
* 0 = success; This means the messages is successfully sent to the receiver
* Note: This does not mean that the receiver as received it.
* otherwise = failure.
*/
virtual int16_t add_subscription(const char *messageName, int32_t msgRateInHz);
/**
* @brief Interface to notify the remote entity of removal of a subscription
*
* @param messageName
* This represents the uORB message name; This message name should be
* globally unique.
* @return
* 0 = success; This means the messages is successfully sent to the receiver
* Note: This does not necessarily mean that the receiver as received it.
* otherwise = failure.
*/
virtual int16_t remove_subscription(const char *messageName);
/**
* Register Message Handler. This is internal for the IChannel implementer*
*/
virtual int16_t register_handler(uORBCommunicator::IChannelRxHandler *handler);
//=========================================================================
// INTERFACES FOR Data messages
//=========================================================================
/**
* @brief Sends the data message over the communication link.
* @param messageName
* This represents the uORB message name; This message name should be
* globally unique.
* @param length
* The length of the data buffer to be sent.
* @param data
* The actual data to be sent.
* @return
* 0 = success; This means the messages is successfully sent to the receiver
* Note: This does not mean that the receiver as received it.
* otherwise = failure.
*/
virtual int16_t send_message(const char *messageName, int32_t length, uint8_t *data);
//Function to return the data to krait.
int16_t get_data
(
int32_t *msg_type,
char *topic_name,
int32_t topic_name_len,
uint8_t *data,
int32_t data_len_in_bytes,
int32_t *bytes_returned
);
int16_t get_bulk_data(uint8_t *buffer, int32_t max_size_in_bytes, int32_t *returned_bytes, int32_t *topic_count);
// function to check if there are subscribers for a topic on adsp.
int16_t is_subscriber_present(const char *messageName, int32_t *status);
// function to release the blocking semaphore for get_data method.
int16_t unblock_get_data_method();
uORBCommunicator::IChannelRxHandler *GetRxHandler()
{
return _RxHandler;
}
void AddRemoteSubscriber(const std::string &messageName)
{
_RemoteSubscribers.insert(messageName);
}
void RemoveRemoteSubscriber(const std::string &messageName)
{
_RemoteSubscribers.erase(messageName);
}
private: // data members
static uORB::FastRpcChannel _Instance;
uORBCommunicator::IChannelRxHandler *_RxHandler;
/// data structure to store the messages to be retrived by Krait.
static const int32_t _MAX_MSG_QUEUE_SIZE = 100;
static const int32_t _CONTROL_MSG_TYPE_ADD_SUBSCRIBER = 1;
static const int32_t _CONTROL_MSG_TYPE_REMOVE_SUBSCRIBER = 2;
static const int32_t _DATA_MSG_TYPE = 3;
static const int32_t _CONTROL_MSG_TYPE_ADVERTISE = 4;
static const int32_t _CONTROL_MSG_TYPE_UNADVERTISE = 5;
static const int32_t _PACKET_FIELD_TOPIC_NAME_LEN_SIZE_IN_BYTES = 2;
static const int32_t _PACKET_FIELD_DATA_LEN_IN_BYTES = 2;
static const int32_t _PACKET_HEADER_SIZE = 1 + //first byte is the MSG Type
_PACKET_FIELD_TOPIC_NAME_LEN_SIZE_IN_BYTES + _PACKET_FIELD_DATA_LEN_IN_BYTES;
struct FastRpcDataMsg {
int32_t _MaxBufferSize;
int32_t _Length;
uint8_t *_Buffer;
std::string _MsgName;
};
struct FastRpcControlMsg {
int32_t _Type;
std::string _MsgName;
};
struct BulkTransferHeader {
uint16_t _MsgType;
uint16_t _MsgNameLen;
uint16_t _DataLen;
};
struct FastRpcDataMsg _DataMsgQueue[ _MAX_MSG_QUEUE_SIZE ];
int32_t _DataQInIndex;
int32_t _DataQOutIndex;
struct FastRpcControlMsg _ControlMsgQueue[ _MAX_MSG_QUEUE_SIZE ];
int32_t _ControlQInIndex;
int32_t _ControlQOutIndex;
std::list<std::string> _Subscribers;
//utility classes
class Mutex
{
public:
Mutex()
{
sem_init(&_Sem, 0, 1);
}
~Mutex()
{
sem_destroy(&_Sem);
}
void lock()
{
sem_wait(&_Sem);
}
void unlock()
{
sem_post(&_Sem);
}
private:
sem_t _Sem;
Mutex(const Mutex &);
Mutex &operator=(const Mutex &);
};
class Semaphore
{
public:
Semaphore()
{
sem_init(&_Sem, 0, 0);
/* _Sem use case is a signal */
px4_sem_setprotocol(&_Sem, SEM_PRIO_NONE);
}
~Semaphore()
{
sem_destroy(&_Sem);
}
void post()
{
sem_post(&_Sem);
}
void wait()
{
sem_wait(&_Sem);
}
private:
sem_t _Sem;
Semaphore(const Semaphore &);
Semaphore &operator=(const Semaphore &);
};
Mutex _QueueMutex;
Semaphore _DataAvailableSemaphore;
private://class members.
/// constructor.
FastRpcChannel();
void check_and_expand_data_buffer(int32_t index, int32_t length);
bool IsControlQFull();
bool IsControlQEmpty();
bool IsDataQFull();
bool IsDataQEmpty();
int32_t DataQSize();
int32_t ControlQSize();
int32_t get_msg_size_at(bool isData, int32_t index);
int32_t copy_msg_to_buffer(bool isData, int32_t src_index, uint8_t *dst_buffer, int32_t offset, int32_t dst_buffer_len);
int16_t control_msg_queue_add(int32_t msgtype, const char *messageName);
std::set<std::string> _RemoteSubscribers;
};
#endif /* _uORBFastRpcChannel_hpp_ */
src/modules/muorb/krait/CMakeLists.txt
-47
View File
@@ -1,47 +0,0 @@
############################################################################
#
# 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.
#
############################################################################
include(hexagon_sdk)
px4_add_module(
MODULE modules__muorb__krait
MAIN muorb
COMPILE_FLAGS
-Wno-cast-align # TODO: fix and enable
INCLUDES
${HEXAGON_SDK_INCLUDES}
${PX4_BINARY_DIR}/platforms/posix
SRCS
uORBKraitFastRpcChannel.cpp
px4muorb_KraitRpcWrapper.cpp
muorb_main.cpp
)
src/modules/muorb/krait/muorb_main.cpp
-103
View File
@@ -1,103 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2016 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.
*
****************************************************************************/
#include <string.h>
#include "modules/uORB/uORBManager.hpp"
#include "uORBKraitFastRpcChannel.hpp"
extern "C" { __EXPORT int muorb_main(int argc, char *argv[]); }
static void usage()
{
warnx("Usage: muorb 'start', 'stop', 'status'");
}
int
muorb_main(int argc, char *argv[])
{
if (argc < 2) {
usage();
return -EINVAL;
}
/*
* Start/load the driver.
*
* XXX it would be nice to have a wrapper for this...
*/
if (!strcmp(argv[1], "start")) {
if (uORB::KraitFastRpcChannel::isInstance()) {
PX4_WARN("muorb already running");
} else {
// register the fast rpc channel with UORB.
uORB::Manager::get_instance()->set_uorb_communicator(uORB::KraitFastRpcChannel::GetInstance());
// start the KaitFastRPC channel thread.
uORB::KraitFastRpcChannel::GetInstance()->Start();
}
return OK;
}
if (!strcmp(argv[1], "stop")) {
if (uORB::KraitFastRpcChannel::isInstance()) {
uORB::KraitFastRpcChannel::GetInstance()->Stop();
} else {
PX4_WARN("muorb not running");
}
return OK;
}
/*
* Print driver information.
*/
if (!strcmp(argv[1], "status")) {
if (uORB::KraitFastRpcChannel::isInstance()) {
PX4_WARN("muorb running");
} else {
PX4_WARN("muorb not running");
}
return OK;
}
usage();
return -EINVAL;
}
src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp
-362
View File
@@ -1,362 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2016 Ramakrishna Kintada. 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.
*
****************************************************************************/
#include <time.h>
#include <unistd.h>
#include <fcntl.h>
#include "px4muorb_KraitRpcWrapper.hpp"
#include <rpcmem.h>
#include <px4muorb.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/shmem.h>
#include <drivers/drv_hrt.h>
#include <string.h>
using namespace px4muorb;
/* Flags applied to the allocation of the shared memory for RPC */
#define MUORB_KRAIT_FASTRPC_MEM_FLAGS 0
/* The ID of the HEAP to be used when allocating shared memory */
//TODO This heap id is used for test purposes. We need to find out the correct one.
#define MUORB_KRAIT_FASTRPC_HEAP_ID 22
static char *_TopicNameBuffer = 0;
static const int32_t _MAX_TOPIC_NAME_BUFFER = 256;
static uint8_t *_DataBuffer = 0;
static const uint32_t _MAX_DATA_BUFFER_SIZE = 2048;
static bool _Initialized = false;
// These numbers are based off the fact each fastrpc call for 64K packet is 94us.
// hence we are trying to allocation 64K of byte buffers.
static const uint32_t _MAX_TOPIC_DATA_BUFFER_SIZE = 1024;
static const uint32_t _MAX_TOPICS = 64;
static const uint32_t _MAX_BULK_TRANSFER_BUFFER_SIZE =
_MAX_TOPIC_DATA_BUFFER_SIZE * _MAX_TOPICS;
static uint8_t *_BulkTransferBuffer = 0;
unsigned char *adsp_changed_index = 0;
// The DSP timer can be read from this file.
#define DSP_TIMER_FILE "/sys/kernel/boot_adsp/qdsp_qtimer"
/**
* Helper function to get timer difference between time on DSP and appsproc side.
* Usually the DSP gets started around 2s before the appsproc (Linux) side and
* therefore the clocks are not in sync. We change the clock on the DSP side but
* for this we need to find the offset first and then tell code on the DSP side.
*
* @param time_diff_us: pointer to time offset to set.
* @return: 0 on success, < 0 on error.
*/
int calc_timer_diff_to_dsp_us(int32_t *time_diff_us);
int calc_timer_diff_to_dsp_us(int32_t *time_diff_us)
{
#if defined(__PX4_POSIX_EXCELSIOR)
*time_diff_us = 0;
return 0;
#endif
int fd = open(DSP_TIMER_FILE, O_RDONLY);
if (fd < 0) {
PX4_ERR("Could not open DSP timer file %s.", DSP_TIMER_FILE);
return -1;
}
char buffer[21] {};
int bytes_read = read(fd, buffer, sizeof(buffer));
if (bytes_read < 0) {
PX4_ERR("Could not read DSP timer file %s.", DSP_TIMER_FILE);
close(fd);
return -2;
}
// Do this call right after reading to avoid latency here.
timespec ts;
clock_gettime(CLOCK_MONOTONIC, &ts);
uint64_t time_appsproc = ((uint64_t) ts.tv_sec) * 1000000llu
+ (ts.tv_nsec / 1000);
close(fd);
uint64_t time_dsp;
int ret = sscanf(buffer, "%llx", &time_dsp);
if (ret < 0) {
PX4_ERR("Could not parse DSP timer.");
return -3;
}
// The clock count needs to get converted to us.
// The magic value of 19.2 was provided by Qualcomm.
time_dsp /= 19.2;
// Before casting to in32_t, check if it fits.
uint64_t abs_diff =
(time_appsproc > time_dsp) ?
(time_appsproc - time_dsp) : (time_dsp - time_appsproc);
if (abs_diff > INT32_MAX) {
PX4_ERR("Timer difference too big");
return -4;
}
*time_diff_us = time_appsproc - time_dsp;
PX4_DEBUG("found time_dsp: %llu us, time_appsproc: %llu us",
time_dsp, time_appsproc);
PX4_DEBUG("found time_diff: %li us, %.6f s",
*time_diff_us, ((double)*time_diff_us) / 1e6);
return 0;
}
bool px4muorb::KraitRpcWrapper::Initialize()
{
bool rc = true;
PX4_DEBUG("%s Now calling rpcmem_init...", __FUNCTION__);
rpcmem_init();
PX4_DEBUG("%s Now calling rpcmem_alloc...", __FUNCTION__);
_BulkTransferBuffer = (uint8_t *) rpcmem_alloc(MUORB_KRAIT_FASTRPC_HEAP_ID,
MUORB_KRAIT_FASTRPC_MEM_FLAGS,
_MAX_BULK_TRANSFER_BUFFER_SIZE * sizeof(uint8_t));
rc = (_BulkTransferBuffer != NULL) ? true : false;
if (!rc) {
PX4_ERR("%s rpcmem_alloc failed! for bulk transfer buffers",
__FUNCTION__);
return rc;
} else {
PX4_DEBUG(
"%s rpcmem_alloc passed for Bulk transfer buffers buffer_size: %d addr: %p",
__FUNCTION__, (_MAX_BULK_TRANSFER_BUFFER_SIZE * sizeof(uint8_t)), _BulkTransferBuffer);
}
_TopicNameBuffer = (char *) rpcmem_alloc(MUORB_KRAIT_FASTRPC_HEAP_ID,
MUORB_KRAIT_FASTRPC_MEM_FLAGS,
_MAX_TOPIC_NAME_BUFFER * sizeof(char));
rc = (_TopicNameBuffer != NULL) ? true : false;
if (!rc) {
PX4_ERR("%s rpcmem_alloc failed! for topic_name_buffer", __FUNCTION__);
rpcmem_free(_BulkTransferBuffer);
return rc;
} else {
PX4_DEBUG("%s rpcmem_alloc passed for topic_name_buffer", __FUNCTION__);
}
// now allocate the data buffer.
_DataBuffer = (uint8_t *) rpcmem_alloc(MUORB_KRAIT_FASTRPC_HEAP_ID,
MUORB_KRAIT_FASTRPC_MEM_FLAGS,
_MAX_DATA_BUFFER_SIZE * sizeof(uint8_t));
rc = (_DataBuffer != NULL) ? true : false;
if (!rc) {
PX4_ERR("%s rpcmem_alloc failed! for DataBuffer", __FUNCTION__);
// free the topic name buffer;
rpcmem_free(_BulkTransferBuffer);
rpcmem_free(_TopicNameBuffer);
_TopicNameBuffer = 0;
return rc;
} else {
PX4_DEBUG("%s rpcmem_alloc passed for data_buffer", __FUNCTION__);
}
adsp_changed_index = (uint8_t *) rpcmem_alloc(MUORB_KRAIT_FASTRPC_HEAP_ID,
MUORB_KRAIT_FASTRPC_MEM_FLAGS, PARAM_BUFFER_SIZE * sizeof(uint8_t));
rc = (adsp_changed_index != NULL) ? true : false;
if (!rc) {
PX4_ERR("%s rpcmem_alloc failed! for adsp_changed_index", __FUNCTION__);
} else {
memset(adsp_changed_index, 0, PARAM_BUFFER_SIZE * sizeof(uint8_t));
}
int32_t time_diff_us;
if (calc_timer_diff_to_dsp_us(&time_diff_us) != 0) {
rc = false;
return rc;
}
// call muorb initialize routine.
if (px4muorb_orb_initialize() != 0) {
PX4_ERR("%s Error calling the uorb fastrpc initalize method..",
__FUNCTION__);
rc = false;
return rc;
}
// TODO FIXME: remove this check or make it less verbose later
px4muorb_set_absolute_time_offset(time_diff_us);
uint64_t time_dsp;
px4muorb_get_absolute_time(&time_dsp);
uint64_t time_appsproc = hrt_absolute_time();
int diff = (time_dsp - time_appsproc);
PX4_DEBUG("time_dsp: %llu us, time appsproc: %llu us, diff: %d us",
time_dsp, time_appsproc, diff);
_Initialized = true;
return rc;
}
bool px4muorb::KraitRpcWrapper::Terminate()
{
if (_BulkTransferBuffer != NULL) {
rpcmem_free(_BulkTransferBuffer);
_BulkTransferBuffer = 0;
}
if (_TopicNameBuffer != NULL) {
rpcmem_free(_TopicNameBuffer);
_TopicNameBuffer = 0;
}
if (_DataBuffer != NULL) {
rpcmem_free(_DataBuffer);
_DataBuffer = 0;
}
if (adsp_changed_index != NULL) {
rpcmem_free(adsp_changed_index);
adsp_changed_index = 0;
}
_Initialized = false;
return true;
}
int32_t px4muorb::KraitRpcWrapper::TopicAdvertised(const char *topic)
{
return ((_Initialized) ? px4muorb_topic_advertised(topic) : -1);
}
int32_t px4muorb::KraitRpcWrapper::TopicUnadvertised(const char *topic)
{
return ((_Initialized) ? px4muorb_topic_unadvertised(topic) : -1);
}
int32_t px4muorb::KraitRpcWrapper::AddSubscriber(const char *topic)
{
return ((_Initialized) ? px4muorb_add_subscriber(topic) : -1);
}
int32_t px4muorb::KraitRpcWrapper::RemoveSubscriber(const char *topic)
{
return (_Initialized ? px4muorb_remove_subscriber(topic) : -1);
}
int32_t px4muorb::KraitRpcWrapper::IsSubscriberPresent(const char *topic,
int32_t *status)
{
return (_Initialized ? px4muorb_is_subscriber_present(topic, status) : -1);
}
int32_t px4muorb::KraitRpcWrapper::SendData(const char *topic,
int32_t length_in_bytes, const uint8_t *data)
{
return (_Initialized ?
px4muorb_send_topic_data(topic, data, length_in_bytes) : -1);
}
int32_t px4muorb::KraitRpcWrapper::ReceiveData(int32_t *msg_type, char **topic,
int32_t *length_in_bytes, uint8_t **data)
{
int32_t rc = -1;
if (_Initialized) {
rc = px4muorb_receive_msg(msg_type, _TopicNameBuffer,
_MAX_TOPIC_NAME_BUFFER, _DataBuffer, _MAX_DATA_BUFFER_SIZE,
length_in_bytes);
if (rc == 0) {
*topic = _TopicNameBuffer;
*data = _DataBuffer;
} else {
PX4_ERR("ERROR: Getting data from fastRPC link");
}
} else {
PX4_ERR("ERROR: FastRpcWrapper Not Initialized");
}
return rc;
}
int32_t px4muorb::KraitRpcWrapper::ReceiveBulkData(uint8_t **bulk_data,
int32_t *length_in_bytes, int32_t *topic_count)
{
int32_t rc = -1;
if (_Initialized) {
//rc = px4muorb_receive_msg( msg_type, _TopicNameBuffer, _MAX_TOPIC_NAME_BUFFER, _DataBuffer, _MAX_DATA_BUFFER_SIZE, length_in_bytes );
rc = px4muorb_receive_bulk_data(_BulkTransferBuffer,
_MAX_BULK_TRANSFER_BUFFER_SIZE, length_in_bytes, topic_count);
if (rc == 0) {
*bulk_data = _BulkTransferBuffer;
} else {
PX4_ERR("ERROR: Getting Bulk data from fastRPC link");
}
} else {
PX4_ERR("ERROR: FastRpcWrapper Not Initialized");
}
return rc;
}
int32_t px4muorb::KraitRpcWrapper::UnblockReceiveData()
{
return (_Initialized ? px4muorb_unblock_recieve_msg() : -1);
}
src/modules/muorb/krait/px4muorb_KraitRpcWrapper.hpp
-78
View File
@@ -1,78 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2016 Ramakrishna Kintada. 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.
*
****************************************************************************/
#ifndef _px4muorb_KraitRpcWrapper_hpp_
#define _px4muorb_KraitRpcWrapper_hpp_
#include <stdint.h>
namespace px4muorb
{
class KraitRpcWrapper;
}
class px4muorb::KraitRpcWrapper
{
public:
/**
* Constructor
*/
KraitRpcWrapper() = default;
/**
* destructor
*/
~KraitRpcWrapper() = default;
/**
* Initiatizes the rpc channel px4 muorb
*/
bool Initialize();
/**
* Terminate to clean up the resources. This should be called at program exit
*/
bool Terminate();
/**
* Muorb related functions to pub/sub of orb topic from krait to adsp
*/
int32_t TopicAdvertised(const char *topic);
int32_t TopicUnadvertised(const char *topic);
int32_t AddSubscriber(const char *topic);
int32_t RemoveSubscriber(const char *topic);
int32_t SendData(const char *topic, int32_t length_in_bytes, const uint8_t *data);
int32_t ReceiveData(int32_t *msg_type, char **topic, int32_t *length_in_bytes, uint8_t **data);
int32_t IsSubscriberPresent(const char *topic, int32_t *status);
int32_t ReceiveBulkData(uint8_t **bulk_data, int32_t *length_in_bytes, int32_t *topic_count);
int32_t UnblockReceiveData();
};
#endif // _px4muorb_KraitWrapper_hpp_
src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp
-375
View File
@@ -1,375 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2015 Mark Charlebois. 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.
*
****************************************************************************/
#include "uORBKraitFastRpcChannel.hpp"
#include <px4_platform_common/log.h>
#include <px4_platform_common/tasks.h>
#include <drivers/drv_hrt.h>
#include <cstdio>
#include <pthread.h>
#include <string.h>
#define LOG_TAG "uORBKraitFastRpcChannel.cpp"
uORB::KraitFastRpcChannel *uORB::KraitFastRpcChannel::_InstancePtr = nullptr;
static void DumpData(uint8_t *buffer, int32_t length, int32_t num_topics);
// static initialization.
static std::string _log_file_name = "./hex_dump.txt";
static unsigned long _snd_msg_min = 0xFFFFFF;
static unsigned long _snd_msg_max = 0;
static double _snd_msg_avg = 0.0;
static unsigned long _snd_msg_count = 0;
static unsigned long _overall_snd_min = 0xFFFFFF;
static unsigned long _overall_snd_max = 0;
static double _overall_snd_avg = 0.0;
static unsigned long _overall_snd_count = 0;
static hrt_abstime _log_check_time = 0;
static hrt_abstime _log_check_interval = 10000000;
uORB::KraitFastRpcChannel::KraitFastRpcChannel() :
_RxHandler(nullptr),
_ThreadStarted(false),
_ThreadShouldExit(false)
{
_KraitWrapper.Initialize();
}
int16_t uORB::KraitFastRpcChannel::topic_advertised(const char *messageName)
{
int16_t rc = 0;
PX4_DEBUG("Before calling TopicAdvertised for [%s]\n", messageName);
rc = _KraitWrapper.TopicAdvertised(messageName);
PX4_DEBUG("Response for TopicAdvertised for [%s], rc[%d]\n", messageName, rc);
return rc;
}
int16_t uORB::KraitFastRpcChannel::topic_unadvertised(const char *messageName)
{
int16_t rc = 0;
PX4_DEBUG("Before calling TopicUnadvertised for [%s]\n", messageName);
rc = _KraitWrapper.TopicUnadvertised(messageName);
PX4_DEBUG("Response for TopicUnadvertised for [%s], rc[%d]\n", messageName, rc);
return rc;
}
int16_t uORB::KraitFastRpcChannel::add_subscription(const char *messageName, int32_t msgRateInHz)
{
int16_t rc = 0;
//PX4_DEBUG("Before calling AddSubscriber for [%s]\n", messageName);
rc = _KraitWrapper.AddSubscriber(messageName);
//PX4_DEBUG("Response for AddSubscriber for [%s], rc[%d]\n", messageName, rc);
return rc;
}
int16_t uORB::KraitFastRpcChannel::remove_subscription(const char *messageName)
{
int16_t rc = 0;
//PX4_DEBUG("Before calling RemoveSubscriber for [%s]\n", messageName);
rc = _KraitWrapper.RemoveSubscriber(messageName);
//PX4_DEBUG("Response for RemoveSubscriber for [%s], rc[%d]\n", messageName, rc);
return rc;
}
int16_t uORB::KraitFastRpcChannel::register_handler(uORBCommunicator::IChannelRxHandler *handler)
{
_RxHandler = handler;
return 0;
}
int16_t uORB::KraitFastRpcChannel::send_message(const char *messageName, int32_t length, uint8_t *data)
{
int16_t rc = 0;
int32_t status = 0;
hrt_abstime t1, t4;
hrt_abstime t2 = 0;
hrt_abstime t3 = 0;
t1 = hrt_absolute_time();
if (_AdspSubscriberCache.find(std::string(messageName)) == _AdspSubscriberCache.end()) {
// check the status from adsp. as it is not cached.
if (_KraitWrapper.IsSubscriberPresent(messageName, &status) == 0) {
_AdspSubscriberCache[messageName] = status;
_AdspSubscriberSampleTimestamp[messageName] = hrt_absolute_time();
}
} else {
if ((hrt_absolute_time() - _AdspSubscriberSampleTimestamp[messageName]) > _SubCacheRefreshRate) {
if (_KraitWrapper.IsSubscriberPresent(messageName, &status) == 0) {
_AdspSubscriberCache[messageName] = status;
_AdspSubscriberSampleTimestamp[messageName] = hrt_absolute_time();
}
}
}
if (_AdspSubscriberCache[messageName] > 0) {// there are remote subscribers
t2 = hrt_absolute_time();
rc = _KraitWrapper.SendData(messageName, length, data);
t3 = hrt_absolute_time();
_snd_msg_count++;
//PX4_DEBUG( "***** SENDING[%s] topic to remote....\n", messageName.c_str() );
} else {
//PX4_DEBUG( "******* NO SUBSCRIBER PRESENT ON THE REMOTE FOR topic[%s] \n", messageName.c_str() );
}
t4 = hrt_absolute_time();
_overall_snd_count++;
if ((t4 - t1) < _overall_snd_min) { _overall_snd_min = (t4 - t1); }
if ((t4 - t1) > _overall_snd_max) { _overall_snd_max = (t4 - t1); }
if (_AdspSubscriberCache[messageName] > 0) {
if ((t3 - t2) < _snd_msg_min) { _snd_msg_min = (t3 - t2); }
if ((t3 - t2) > _snd_msg_max) { _snd_msg_max = (t3 - t2); }
_snd_msg_avg = ((double)((_snd_msg_avg * (_snd_msg_count - 1)) +
(unsigned long)(t3 - t2))) / (double)(_snd_msg_count);
}
_overall_snd_avg = ((double)((_overall_snd_avg * (_overall_snd_count - 1)) +
(unsigned long)(t4 - t1))) / (double)(_overall_snd_count);
if ((t4 - _log_check_time) > _log_check_interval) {
/*
PX4_DEBUG("SndMsgStats: overall_min: %lu overall_max: %lu snd_msg_min: %lu snd_msg_max: %lu",
_overall_snd_min, _overall_snd_max,
_snd_msg_min, _snd_msg_max);
PX4_DEBUG(".... overall_avg: %f (%lu) snd_msg_avg: %f (%lu)",
_overall_snd_avg, _overall_snd_count, _snd_msg_avg, _snd_msg_count);
*/
_log_check_time = t4;
_overall_snd_min = _snd_msg_min = 0xFFFFFFF;
_overall_snd_max = _snd_msg_max = 0;
_overall_snd_count = _snd_msg_count = 0;
_overall_snd_avg = _snd_msg_avg = 0.0;
}
//PX4_DEBUG( "Response for SendMessage for [%s],len[%d] rc[%d]\n", messageName.c_str(), length, rc );
return rc;
}
void uORB::KraitFastRpcChannel::Start()
{
_ThreadStarted = true;
_ThreadShouldExit = false;
pthread_attr_t recv_thread_attr;
pthread_attr_init(&recv_thread_attr);
struct sched_param param;
(void)pthread_attr_getschedparam(&recv_thread_attr, &param);
param.sched_priority = SCHED_PRIORITY_MAX - 80;
(void)pthread_attr_setschedparam(&recv_thread_attr, &param);
pthread_attr_setstacksize(&recv_thread_attr, 4096);
if (pthread_create(&_RecvThread, &recv_thread_attr, thread_start, (void *)this) != 0) {
PX4_ERR("Error creating the receive thread for muorb");
} else {
pthread_setname_np(_RecvThread, "muorb_krait_receiver");
}
pthread_attr_destroy(&recv_thread_attr);
}
void uORB::KraitFastRpcChannel::Stop()
{
_ThreadShouldExit = true;
_KraitWrapper.UnblockReceiveData();
//PX4_DEBUG("After calling UnblockReceiveData()...\n");
pthread_join(_RecvThread, NULL);
//PX4_DEBUG("*** After calling pthread_join...\n");
_ThreadStarted = false;
}
void *uORB::KraitFastRpcChannel::thread_start(void *handler)
{
if (handler != nullptr) {
((uORB::KraitFastRpcChannel *)handler)->fastrpc_recv_thread();
}
return 0;
}
void uORB::KraitFastRpcChannel::fastrpc_recv_thread()
{
// sit in while loop.
int32_t rc = 0;
int32_t data_length = 0;
uint8_t *data = nullptr;
unsigned long rpc_min, rpc_max;
unsigned long orb_min, orb_max;
double rpc_avg, orb_avg;
unsigned long count = 0;
rpc_max = orb_max = 0;
rpc_min = orb_min = 0xFFFFFFFF;
rpc_avg = orb_avg = 0.0;
int32_t num_topics = 0;
hrt_abstime check_time = 0;
while (!_ThreadShouldExit) {
hrt_abstime t1, t2, t3;
t1 = hrt_absolute_time();
rc = _KraitWrapper.ReceiveBulkData(&data, &data_length, &num_topics);
t2 = hrt_absolute_time();
if (rc == 0) {
//PX4_DEBUG( "Num of topics Received: %d", num_topics );
int32_t bytes_processed = 0;
for (int i = 0; i < num_topics; ++i) {
uint8_t *new_pkt = &(data[bytes_processed]);
struct BulkTransferHeader *header = (struct BulkTransferHeader *)new_pkt;
char *messageName = (char *)(new_pkt + sizeof(struct BulkTransferHeader));
uint16_t check_msg_len = strlen(messageName);
if (header->_MsgNameLen != (check_msg_len + 1)) {
PX4_ERR("Error: Packing error. Sent Msg Len. of[%d] but strlen returned:[%d]", header->_MsgNameLen, check_msg_len);
PX4_ERR("Error: NumTopics: %d processing topic: %d msgLen[%d] dataLen[%d] data_len[%d] bytes processed: %d",
num_topics, i, header->_MsgNameLen, header->_DataLen, data_length, bytes_processed);
DumpData(data, data_length, num_topics);
break;
}
uint8_t *topic_data = (uint8_t *)(messageName + strlen(messageName) + 1);
if (_RxHandler != nullptr) {
if (header->_MsgType == _DATA_MSG_TYPE) {
//PX4_DEBUG( "Received topic data for: [%s] len[%d]\n", messageName, data_length );
_RxHandler->process_received_message(messageName,
header->_DataLen, topic_data);
} else if (header->_MsgType == _CONTROL_MSG_TYPE_ADVERTISE) {
PX4_DEBUG("Received topic advertise message for: [%s] len[%d]\n", messageName, data_length);
_RxHandler->process_remote_topic(messageName, true);
} else if (header->_MsgType == _CONTROL_MSG_TYPE_UNADVERTISE) {
PX4_DEBUG("Received topic unadvertise message for: [%s] len[%d]\n", messageName, data_length);
_RxHandler->process_remote_topic(messageName, false);
}
}
bytes_processed += header->_MsgNameLen + header->_DataLen + sizeof(struct BulkTransferHeader);
}
} else {
PX4_DEBUG("Error: Getting data over fastRPC channel\n");
break;
}
t3 = hrt_absolute_time();
count++;
if ((unsigned long)(t2 - t1) < rpc_min) {
rpc_min = (unsigned long)(t2 - t1);
}
if ((unsigned long)(t2 - t1) > rpc_max) {
rpc_max = (unsigned long)(t2 - t1);
}
if ((unsigned long)(t3 - t2) < orb_min) {
orb_min = (unsigned long)(t3 - t2);
}
if ((unsigned long)(t3 - t2) > orb_max) {
orb_max = (unsigned long)(t3 - t2);
}
rpc_avg = ((double)((rpc_avg * (count - 1)) + (unsigned long)(t2 - t1))) / (double)(count);
orb_avg = ((double)((orb_avg * (count - 1)) + (unsigned long)(t3 - t2))) / (double)(count);
if ((unsigned long)(t3 - check_time) >= 10000000) {
//PX4_DEBUG("Krait RPC Stats : rpc_min: %lu rpc_max: %lu rpc_avg: %f", rpc_min, rpc_max, rpc_avg);
//PX4_DEBUG("Krait RPC(orb) Stats: orb_min: %lu orb_max: %lu orb_avg: %f", orb_min, orb_max, orb_avg);
check_time = t3;
rpc_max = orb_max = 0;
rpc_min = orb_min = 0xFFFFFF;
orb_avg = 0;
rpc_avg = 0;
count = 0;
}
//PX4_DEBUG("MsgName: %30s, t1: %lu, t2: %lu, t3: %lu, dt1: %lu, dt2: %lu",name, (unsigned long) t1, (unsigned long) t2, (unsigned long) t3,
// (unsigned long) (t2-t1), (unsigned long) (t3-t2));
}
PX4_DEBUG("[uORB::KraitFastRpcChannel::fastrpc_recv_thread] Exiting fastrpc_recv_thread\n");
}
void DumpData(uint8_t *buffer, int32_t length, int32_t num_topics)
{
FILE *fp = fopen(_log_file_name.c_str(), "a+");
if (fp == nullptr) {
PX4_ERR("Error unable to open log file[%s]", _log_file_name.c_str());
return;
}
fprintf(fp, "===== Data Len[%d] num_topics[%d] ======\n", length, num_topics);
for (int i = 0; i < length; i += 16) {
int remaining_chars = length - i;
remaining_chars = (remaining_chars >= 16) ? 16 : remaining_chars;
fprintf(fp, "%p - ", &(buffer[i]));
for (int j = 0; j < remaining_chars; j++) {
fprintf(fp, " %02X", buffer[i + j]);
if (j == 7) {
fprintf(fp, " -");
}
}
fprintf(fp, " ");
for (int j = 0; j < remaining_chars; j++) {
fprintf(fp, "%c", (char)buffer[i + j ]);
}
fprintf(fp, "\n");
}
fclose(fp);
}
src/modules/muorb/krait/uORBKraitFastRpcChannel.hpp
-196
View File
@@ -1,196 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2015 Mark Charlebois. 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.
*
****************************************************************************/
#ifndef _uORBKraitFastRpcChannel_hpp_
#define _uORBKraitFastRpcChannel_hpp_
#include <stdint.h>
#include <string>
#include <pthread.h>
#include "uORB/uORBCommunicator.hpp"
#include "px4muorb_KraitRpcWrapper.hpp"
#include <map>
#include "drivers/drv_hrt.h"
namespace uORB
{
class KraitFastRpcChannel;
}
class uORB::KraitFastRpcChannel : public uORBCommunicator::IChannel
{
public:
/**
* static method to get the IChannel Implementor.
*/
static uORB::KraitFastRpcChannel *GetInstance()
{
if (_InstancePtr == nullptr) {
_InstancePtr = new uORB::KraitFastRpcChannel();
}
return _InstancePtr;
}
/**
* Static method to check if there is an instance.
*/
static bool isInstance()
{
return (_InstancePtr != nullptr);
}
/**
* @brief Interface to notify the remote entity of a topic being advertised.
*
* @param messageName
* This represents the uORB message name(aka topic); This message name should be
* globally unique.
* @return
* 0 = success; This means the messages is successfully sent to the receiver
* Note: This does not mean that the receiver as received it.
* otherwise = failure.
*/
virtual int16_t topic_advertised(const char *messageName);
/**
* @brief Interface to notify the remote entity of a topic being unadvertised
* and is no longer publishing messages.
*
* @param messageName
* This represents the uORB message name(aka topic); This message name should be
* globally unique.
* @return
* 0 = success; This means the messages is successfully sent to the receiver
* Note: This does not mean that the receiver as received it.
* otherwise = failure.
*/
virtual int16_t topic_unadvertised(const char *messageName);
/**
* @brief Interface to notify the remote entity of interest of a
* subscription for a message.
*
* @param messageName
* This represents the uORB message name; This message name should be
* globally unique.
* @param msgRate
* The max rate at which the subscriber can accept the messages.
* @return
* 0 = success; This means the messages is successfully sent to the receiver
* Note: This does not mean that the receiver as received it.
* otherwise = failure.
*/
virtual int16_t add_subscription(const char *messageName, int32_t msgRateInHz);
/**
* @brief Interface to notify the remote entity of removal of a subscription
*
* @param messageName
* This represents the uORB message name; This message name should be
* globally unique.
* @return
* 0 = success; This means the messages is successfully sent to the receiver
* Note: This does not necessarily mean that the receiver as received it.
* otherwise = failure.
*/
virtual int16_t remove_subscription(const char *messageName);
/**
* Register Message Handler. This is internal for the IChannel implementer*
*/
virtual int16_t register_handler(uORBCommunicator::IChannelRxHandler *handler);
//=========================================================================
// INTERFACES FOR Data messages
//=========================================================================
/**
* @brief Sends the data message over the communication link.
* @param messageName
* This represents the uORB message name; This message name should be
* globally unique.
* @param length
* The length of the data buffer to be sent.
* @param data
* The actual data to be sent.
* @return
* 0 = success; This means the messages is successfully sent to the receiver
* Note: This does not mean that the receiver as received it.
* otherwise = failure.
*/
virtual int16_t send_message(const char *messageName, int32_t length, uint8_t *data);
void Start();
void Stop();
private: // data members
static uORB::KraitFastRpcChannel *_InstancePtr;
uORBCommunicator::IChannelRxHandler *_RxHandler;
pthread_t _RecvThread;
bool _ThreadStarted;
bool _ThreadShouldExit;
static const int32_t _CONTROL_MSG_TYPE_ADD_SUBSCRIBER = 1;
static const int32_t _CONTROL_MSG_TYPE_REMOVE_SUBSCRIBER = 2;
static const int32_t _DATA_MSG_TYPE = 3;
static const int32_t _CONTROL_MSG_TYPE_ADVERTISE = 4;
static const int32_t _CONTROL_MSG_TYPE_UNADVERTISE = 5;
struct BulkTransferHeader {
uint16_t _MsgType;
uint16_t _MsgNameLen;
uint16_t _DataLen;
};
px4muorb::KraitRpcWrapper _KraitWrapper;
std::map<std::string, int32_t> _AdspSubscriberCache;
std::map<std::string, hrt_abstime> _AdspSubscriberSampleTimestamp;
//hrt_abstime _SubCacheSampleTimestamp;
static const hrt_abstime _SubCacheRefreshRate = 1000000; // 1 second;
private://class members.
/// constructor.
KraitFastRpcChannel();
static void *thread_start(void *handler);
void fastrpc_recv_thread();
};
#endif /* _uORBKraitFastRpcChannel_hpp_ */
src/modules/muorb/test/CMakeLists.txt
-41
View File
@@ -1,41 +0,0 @@
############################################################################
#
# 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.
#
############################################################################
px4_add_module(
MODULE modules__muorb__test__muorb_test
MAIN muorb_test
SRCS
muorb_test_main.cpp
muorb_test_start_posix.cpp
muorb_test_example.cpp
DEPENDS
)
src/modules/muorb/test/muorb_test_example.cpp
-137
View File
@@ -1,137 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2015 Mark Charlebois. 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 hello_example.cpp
* Example for Linux
*
* @author Mark Charlebois <charlebm@gmail.com>
*/
#include "muorb_test_example.h"
#include <px4_platform_common/log.h>
#include <unistd.h>
#include <stdio.h>
#include <px4_platform_common/defines.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
px4::AppState MuorbTestExample::appState;
int MuorbTestExample::main()
{
int rc;
appState.setRunning(true);
rc = PingPongTest();
appState.setRunning(false);
return rc;
}
int MuorbTestExample::DefaultTest()
{
int i = 0;
uORB::Subscription sub_vc{ORB_ID(vehicle_command)};
uORB::Publication<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
uORB::Publication<esc_status_s> pub_id{ORB_ID(esc_status)};
pub_id.publish(m_esc_status);
while (!appState.exitRequested() && i < 100) {
PX4_DEBUG("[%d] Doing work...", i);
if (!pub_id.publish(m_esc_status)) {
PX4_ERR("[%d]Error publishing the esc status message for iter", i);
break;
}
if (sub_vc.updated()) {
PX4_DEBUG("[%d]Vechicle Status is updated... reading new value", i);
if (!sub_vc.copy(&m_vc)) {
PX4_ERR("[%d]Error calling orb copy for vechivle status... ", i);
break;
}
if (!vcmd_pub.publish(m_vc)) {
PX4_ERR("[%d]Error publishing the vechile command message", i);
break;
}
} else {
PX4_ERR("[%d]Error checking the updated status for vechile command... ", i);
PX4_DEBUG("[%d] VC topic is not updated", i);
break;
}
++i;
}
return 0;
}
int MuorbTestExample::PingPongTest()
{
int i = 0;
uORB::Publication<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
uORB::Subscription sub_esc_status{ORB_ID(esc_status)};
while (!appState.exitRequested()) {
PX4_INFO("[%d] Doing work...", i);
if (sub_esc_status.updated()) {
PX4_INFO("[%d]ESC status is updated... reading new value", i);
if (!sub_esc_status.copy(&m_esc_status)) {
PX4_ERR("[%d]Error calling orb copy for esc status... ", i);
break;
}
if (!vcmd_pub.publish(m_vc)) {
PX4_ERR("[%d]Error publishing the vechile command message", i);
break;
}
} else {
PX4_INFO("[%d] esc status topic is not updated", i);
}
// sleep for 1 sec.
usleep(1000000);
++i;
}
return 0;
}
src/modules/muorb/test/muorb_test_example.h
-62
View File
@@ -1,62 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2015 Mark Charlebois. 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 hello_example.h
* Example app for Linux
*
* @author Mark Charlebois <charlebm@gmail.com>
*/
#pragma once
#include <px4_platform_common/app.h>
#include "uORB/topics/esc_status.h"
#include "uORB/topics/vehicle_command.h"
class MuorbTestExample
{
public:
MuorbTestExample() {}
~MuorbTestExample() {}
int main();
static px4::AppState appState; /* track requests to terminate app */
private:
int DefaultTest();
int PingPongTest();
struct esc_status_s m_esc_status;
struct vehicle_command_s m_vc = {};
};
src/modules/muorb/test/muorb_test_main.cpp
-59
View File
@@ -1,59 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2015 Mark Charlebois. 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 hello_main.cpp
* Example for Linux
*
* @author Mark Charlebois <charlebm@gmail.com>
*/
#include <px4_platform_common/log.h>
#include <px4_platform_common/app.h>
#include <px4_platform_common/init.h>
#include "muorb_test_example.h"
#include <stdio.h>
#include "uORB/uORBManager.hpp"
#include "../../../modules/muorb/krait/uORBKraitFastRpcChannel.hpp"
int PX4_MAIN(int argc, char **argv)
{
px4::init(argc, argv, "muorb_test");
PX4_DEBUG("muorb_test");
MuorbTestExample hello;
hello.main();
PX4_DEBUG("goodbye");
return 0;
}
src/modules/muorb/test/muorb_test_start_posix.cpp
-101
View File
@@ -1,101 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2015 Mark Charlebois. 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 hello_start_linux.cpp
*
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author Mark Charlebois <mcharleb@gmail.com>
*/
#include "muorb_test_example.h"
#include <px4_platform_common/log.h>
#include <px4_platform_common/app.h>
#include <px4_platform_common/tasks.h>
#include <stdio.h>
#include <string.h>
#include <sched.h>
static int daemon_task; /* Handle of deamon task / thread */
//using namespace px4;
extern "C" __EXPORT int muorb_test_main(int argc, char *argv[]);
static void usage()
{
PX4_DEBUG("usage: muorb_test {start|stop|status}");
}
int muorb_test_main(int argc, char *argv[])
{
if (argc < 2) {
usage();
return 1;
}
if (!strcmp(argv[1], "start")) {
if (MuorbTestExample::appState.isRunning()) {
PX4_DEBUG("already running");
/* this is not an error */
return 0;
}
daemon_task = px4_task_spawn_cmd("muorb_test",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
16000,
PX4_MAIN,
(char *const *)argv);
return 0;
}
if (!strcmp(argv[1], "stop")) {
MuorbTestExample::appState.requestExit();
return 0;
}
if (!strcmp(argv[1], "status")) {
if (MuorbTestExample::appState.isRunning()) {
PX4_DEBUG("is running");
} else {
PX4_DEBUG("not started");
}
return 0;
}
usage();
return 1;
}
src/modules/simulator/CMakeLists.txt
+2 -7
View File
@@ -47,12 +47,6 @@ endif()
configure_file(simulator_config.h.in simulator_config.h @ONLY)
include_directories(${CMAKE_CURRENT_BINARY_DIR})
set(SIMULATOR_SRCS simulator.cpp)
if (NOT ${PX4_PLATFORM} STREQUAL "qurt")
list(APPEND SIMULATOR_SRCS
simulator_mavlink.cpp)
endif()
px4_add_module(
MODULE modules__simulator
MAIN simulator
@@ -63,7 +57,8 @@ px4_add_module(
INCLUDES
${PX4_SOURCE_DIR}/mavlink/include/mavlink
SRCS
${SIMULATOR_SRCS}
simulator.cpp
simulator_mavlink.cpp
DEPENDS
git_mavlink_v2
conversion