Airmode - Add support for UAVCAN

PX4 · Feb 13, 2018 · 6e58cbc · 6e58cbc
1 parent 90128ef
commit 6e58cbc
Show file tree

Hide file tree

Showing 2 changed files with 21 additions and 0 deletions.
diff --git 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
@@ -152,6 +152,7 @@ class UavcanNode : public device::CDev
 	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 @@ class UavcanNode : public device::CDev
 
 	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];