yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 09:28:58 +00:00
Code Issues Packages Projects Releases Wiki Activity

UAVCAN: Receive commands from the GCS

Browse Source
This commit is contained in:
Lorenz Meier
2015-08-23 13:50:59 +02:00
committed by Ben Dyer
parent 96d90c0fe6
commit a3b863bdd9
3 changed files with 28 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
src/modules/uavcan/uavcan_servers.cpp
View File

@@ -53,9 +53,11 @@
#include "uavcan_servers.hpp"
#include "uavcan_virtual_can_driver.hpp"
# include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp>
# include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp>
# include <uavcan_posix/firmware_version_checker.hpp>
#include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp>
#include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp>
#include <uavcan_posix/firmware_version_checker.hpp>
#include <uORB/topics/vehicle_command.h>
//todo:The Inclusion of file_server_backend is killing
// #include <sys/types.h> and leaving OK undefined
@@ -277,6 +279,10 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
Lock lock(_subnode_mutex);
/* the subscribe call needs to happen in the same thread,
* so not in the constructor */
int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
while (1) {
if (_check_fw == true) {
@@ -286,6 +292,23 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
const int spin_res = _subnode.spin(uavcan::MonotonicDuration::fromMSec(100));
/* check if new commands are pending */
bool cmd_ready;
orb_check(cmd_sub, &cmd_ready);
if (cmd_ready) {
struct vehicle_command_s cmd;
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_UAVCAN) {
warnx("received UAVCAN CONFIG command");
if (static_cast<int>(cmd.param1 + 0.5f) == 1) {
warnx("UAVCAN CONFIG: Actuator assignment requested");
}
}
}
if (spin_res < 0) {
warnx("node spin error %i", spin_res);
}