updated files to make UWB demo functional. Added actuator_controls_3 …

…again

.ci/Jenkinsfile-hardware

@@ -803,6 +803,7 @@ void printTopics() {
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener actuator_controls_0" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener actuator_controls_1" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener actuator_controls_2" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener actuator_controls_3" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener actuator_outputs" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener adc_report" || true'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener airspeed_validated" || true'

msg/ActuatorControls.msg

@@ -17,8 +17,11 @@ uint8 GROUP_INDEX_ATTITUDE = 0
 uint8 GROUP_INDEX_ATTITUDE_ALTERNATE = 1
 uint8 GROUP_INDEX_GIMBAL = 2
 
+# Are these necessary for actuator_controls_3?
+# uint8 GROUP_INDEX_MANUAL_PASSTHROUGH = 3
+# uint8 GROUP_INDEX_PAYLOAD = 6
 uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
 float32[9] control
 
-# TOPICS actuator_controls_0 actuator_controls_1 actuator_controls_2
+# TOPICS actuator_controls_0 actuator_controls_1 actuator_controls_2 actuator_controls_3
 # TOPICS actuator_controls_virtual_fw actuator_controls_virtual_mc

src/modules/logger/logged_topics.cpp

@@ -50,6 +50,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("actuator_controls_0", 50);
 	add_topic("actuator_controls_1", 100);
 	add_topic("actuator_controls_2", 100);
+	add_topic("actuator_controls_3", 100);
 	add_optional_topic("actuator_controls_status_0", 300);
 	add_topic("airspeed", 1000);
 	add_optional_topic("airspeed_validated", 200);

src/modules/mavlink/mavlink_receiver.cpp

@@ -561,6 +561,40 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
 			send_ack = true;
 		}
 
+	} else if (cmd_mavlink.command == MAV_CMD_DO_SET_ACTUATOR) {
+		// since we're only paying attention to 3 AUX outputs, the
+		// index should be 0, otherwise ignore the message
+		if (((int) vehicle_command.param7) == 0) {
+			actuator_controls_s actuator_controls{};
+			// update with existing values to avoid changing unspecified controls
+			_actuator_controls_3_sub.update(&actuator_controls);
+
+			actuator_controls.timestamp = hrt_absolute_time();
+
+			bool updated = false;
+
+			if (PX4_ISFINITE(vehicle_command.param1)) {
+				actuator_controls.control[5] = vehicle_command.param1;
+				updated = true;
+			}
+
+			if (PX4_ISFINITE(vehicle_command.param2)) {
+				actuator_controls.control[6] = vehicle_command.param2;
+				updated = true;
+			}
+
+			if (PX4_ISFINITE(vehicle_command.param3)) {
+				actuator_controls.control[7] = vehicle_command.param3;
+				updated = true;
+			}
+
+			if (updated) {
+				_actuator_controls_pubs[3].publish(actuator_controls);
+			}
+		}
+
+		_cmd_pub.publish(vehicle_command);
+
 	} else if (cmd_mavlink.command == MAV_CMD_DO_AUTOTUNE_ENABLE) {
 
 		bool has_module = true;

src/modules/mavlink/mavlink_receiver.h

@@ -289,7 +289,7 @@ class MavlinkReceiver : public ModuleParams
 	uint16_t _mavlink_status_last_packet_rx_drop_count{0};
 
 	// ORB publications
-	uORB::Publication<actuator_controls_s>			_actuator_controls_pubs[3] {ORB_ID(actuator_controls_0), ORB_ID(actuator_controls_1), ORB_ID(actuator_controls_2)};
+	uORB::Publication<actuator_controls_s>			_actuator_controls_pubs[4] {ORB_ID(actuator_controls_0), ORB_ID(actuator_controls_1), ORB_ID(actuator_controls_2), ORB_ID(actuator_controls_3)};
 	uORB::Publication<airspeed_s>				_airspeed_pub{ORB_ID(airspeed)};
 	uORB::Publication<battery_status_s>			_battery_pub{ORB_ID(battery_status)};
 	uORB::Publication<camera_status_s>			_camera_status_pub{ORB_ID(camera_status)};
@@ -352,6 +352,7 @@ class MavlinkReceiver : public ModuleParams
 	uORB::Subscription	_vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
 	uORB::Subscription	_vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
 	uORB::Subscription	_vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::Subscription 	_actuator_controls_3_sub{ORB_ID(actuator_controls_3)};
 	uORB::Subscription	_autotune_attitude_control_status_sub{ORB_ID(autotune_attitude_control_status)};
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};

src/modules/mavlink/streams/ACTUATOR_CONTROL_TARGET.hpp

@@ -53,6 +53,9 @@ class MavlinkStreamActuatorControlTarget : public MavlinkStream
 
 		case 2:
 			return "ACTUATOR_CONTROL_TARGET2";
+
+		case 3:
+			return "ACTUATOR_CONTROL_TARGET3";
 		}
 
 		return "ACTUATOR_CONTROL_TARGET";
@@ -85,6 +88,10 @@ class MavlinkStreamActuatorControlTarget : public MavlinkStream
 		case 2:
 			_act_ctrl_sub = new uORB::Subscription{ORB_ID(actuator_controls_2)};
 			break;
+
+		case 3:
+			_act_ctrl_sub = new uORB::Subscription{ORB_ID(actuator_controls_3)};
+			break;
 		}
 	}
 

src/modules/rc_update/rc_update.cpp

@@ -692,6 +692,51 @@ void RCUpdate::UpdateManualControlInput(const hrt_abstime &timestamp_sample)
 	manual_control_input.timestamp = hrt_absolute_time();
 	_manual_control_input_pub.publish(manual_control_input);
 	_last_manual_control_input_publish = manual_control_input.timestamp;
+
+	actuator_controls_s actuator_group_3{};
+	// copy in previous actuator control setpoint in case aux{1, 2, 3} isn't changed
+	_actuator_controls_3_sub.update(&actuator_group_3);
+	// populate and publish actuator_controls_3 copied from mapped manual_control_input
+	actuator_group_3.control[0] = manual_control_input.roll;
+	actuator_group_3.control[1] = manual_control_input.pitch;
+	actuator_group_3.control[2] = manual_control_input.yaw;
+	actuator_group_3.control[3] = manual_control_input.throttle;
+	actuator_group_3.control[4] = manual_control_input.flaps;
+
+	float new_aux_values[3];
+	new_aux_values[0] = manual_control_input.aux1;
+	new_aux_values[1] = manual_control_input.aux2;
+	new_aux_values[2] = manual_control_input.aux3;
+
+	// if AUX RC was already active, we update. otherwise, we check
+	// if there is a major stick movement to re-activate RC mode
+	bool major_movement[3] = {false, false, false};
+
+	// detect a big stick movement
+	for (int i = 0; i < 3; i++) {
+		if (fabsf(_last_manual_control_input[i] - new_aux_values[i]) > 0.1f) {
+			major_movement[i] = true;
+		}
+	}
+
+	for (int i = 0; i < 3; i++) {
+		// if someone else (DO_SET_ACTUATOR) updated the actuator control
+		// and we haven't had a major movement, switch back to automatic control
+		if ((fabsf(_last_manual_control_input[i] - actuator_group_3.control[5 + i])
+		     > 0.0001f) && (!major_movement[i])) {
+			_aux_already_active[i] = false;
+		}
+
+		if (_aux_already_active[i] || major_movement[i]) {
+			_aux_already_active[i] = true;
+			_last_manual_control_input[i] = new_aux_values[i];
+
+			actuator_group_3.control[5 + i] = new_aux_values[i];
+		}
+	}
+
+	actuator_group_3.timestamp = hrt_absolute_time();
+	_actuator_group_3_pub.publish(actuator_group_3);
 }
 
 int RCUpdate::task_spawn(int argc, char *argv[])

src/modules/rc_update/rc_update.h

@@ -163,11 +163,13 @@ class RCUpdate : public ModuleBase<RCUpdate>, public ModuleParams, public px4::W
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
 	uORB::Subscription _rc_parameter_map_sub{ORB_ID(rc_parameter_map)};
+	uORB::Subscription _actuator_controls_3_sub{ORB_ID(actuator_controls_3)};
 
 	uORB::Publication<rc_channels_s> _rc_channels_pub{ORB_ID(rc_channels)};
 	uORB::PublicationMulti<manual_control_setpoint_s> _manual_control_input_pub{ORB_ID(manual_control_input)};
 	uORB::Publication<manual_control_switches_s> _manual_control_switches_pub{ORB_ID(manual_control_switches)};
-
+	uORB::Publication<actuator_controls_s> _actuator_group_3_pub{ORB_ID(actuator_controls_3)};
+
 	manual_control_switches_s _manual_switches_previous{};
 	manual_control_switches_s _manual_switches_last_publish{};
 	rc_channels_s _rc{};