From 99ce9cc2a86c13e608c595436f592df9adcd41ba Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Tue, 13 Feb 2018 15:18:40 +0100
Subject: [PATCH] Airmode - Add support for UAVCAN
---
src/modules/uavcan/uavcan_main.cpp | 18 ++++++++++++++++++
src/modules/uavcan/uavcan_main.hpp | 3 +++
2 files changed, 21 insertions(+)
diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index 286f3b9ed0..c67db46dbe 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -417,6 +417,20 @@ int UavcanNode::get_param(int remote_node_id, const char *name)
return rv;
}
+void UavcanNode::update_params()
+{
+ param_t param_handle;
+
+ // multicopter air-mode
+ param_handle = param_find("MC_AIRMODE");
+
+ if (param_handle != PARAM_INVALID) {
+ int val;
+ param_get(param_handle, &val);
+ _airmode = val > 0;
+ }
+}
+
int UavcanNode::start_fw_server()
{
int rv = -1;
@@ -821,6 +835,8 @@ int UavcanNode::run()
while (!_task_should_exit) {
+ update_params();
+
switch (_fw_server_action) {
case Start:
_fw_server_status = start_fw_server();
@@ -908,6 +924,8 @@ int UavcanNode::run()
// but this driver could well serve multiple groups.
unsigned num_outputs_max = 8;
+ _mixers->set_airmode(_airmode);
+
// Do mixing
_outputs.noutputs = _mixers->mix(&_outputs.output[0], num_outputs_max);
diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp
index 4c0575e5c0..69cc732a87 100644
--- a/src/modules/uavcan/uavcan_main.hpp
+++ b/src/modules/uavcan/uavcan_main.hpp
@@ -152,6 +152,7 @@ private:
int request_fw_check();
int print_params(uavcan::protocol::param::GetSet::Response &resp);
int get_set_param(int nodeid, const char *name, uavcan::protocol::param::GetSet::Request &req);
+ void update_params();
void set_setget_response(uavcan::protocol::param::GetSet::Response *resp)
{
_setget_response = resp;
@@ -207,6 +208,8 @@ private:
actuator_outputs_s _outputs = {};
+ bool _airmode = false;
+
// index into _poll_fds for each _control_subs handle
uint8_t _poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN];
--
GitLab