vehicles: add new vehicle type: Airship (#14862)

Co-authored-by: Anton Erasmus <anton@flycloudline.com>
Co-authored-by: Beat Küng <beat-kueng@gmx.net>

src/modules/airship_att_control/CMakeLists.txt

@@ -0,0 +1,43 @@
+############################################################################
+#
+#   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__airship_att_control
+	MAIN airship_att_control
+	STACK_MAX 3500
+	COMPILE_FLAGS
+	SRCS
+		airship_att_control_main.cpp
+	DEPENDS
+		px4_work_queue
+	)

src/modules/airship_att_control/airship_att_control.hpp

@@ -0,0 +1,97 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2018 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 <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_angular_velocity.h>
+
+/**
+ * Airship attitude control app start / stop handling function
+ */
+extern "C" __EXPORT int airship_att_control_main(int argc, char *argv[]);
+
+class AirshipAttitudeControl : public ModuleBase<AirshipAttitudeControl>, public ModuleParams,
+	public px4::WorkItem
+{
+public:
+	AirshipAttitudeControl();
+
+	virtual ~AirshipAttitudeControl();
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	/** @see ModuleBase::print_status() */
+	int print_status() override;
+
+	void Run() override;
+
+	bool init();
+
+private:
+
+	/**
+	 * Check for parameter update and handle it.
+	 */
+	void		parameter_update_poll();
+
+	void		publish_actuator_controls();
+
+	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};		/**< parameter updates subscription */
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};			/**< vehicle status subscription */
+	uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)};	/**< manual control setpoint subscription */
+
+	uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
+
+	uORB::Publication<actuator_controls_s>		_actuators_0_pub;
+
+	struct manual_control_setpoint_s	_manual_control_sp {};	/**< manual control setpoint */
+	struct vehicle_status_s			_vehicle_status {};	/**< vehicle status */
+	struct actuator_controls_s		_actuators {};		/**< actuator controls */
+
+	perf_counter_t	_loop_perf;			/**< loop performance counter */
+
+};

src/modules/airship_att_control/airship_att_control_main.cpp

@@ -0,0 +1,213 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-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 airship_att_control_main.cpp
+ * Airship attitude controller.
+ *
+ * @author Anton Erasmus	<anton@flycloudline.com>
+ */
+
+#include "airship_att_control.hpp"
+
+using namespace matrix;
+
+AirshipAttitudeControl::AirshipAttitudeControl() :
+	ModuleParams(nullptr),
+	WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
+	_actuators_0_pub(ORB_ID(actuator_controls_0)),
+	_loop_perf(perf_alloc(PC_ELAPSED, "airship_att_control"))
+{
+}
+
+AirshipAttitudeControl::~AirshipAttitudeControl()
+{
+	perf_free(_loop_perf);
+}
+
+bool
+AirshipAttitudeControl::init()
+{
+	if (!_vehicle_angular_velocity_sub.registerCallback()) {
+		PX4_ERR("vehicle_angular_velocity callback registration failed!");
+		return false;
+	}
+
+	return true;
+}
+
+void
+AirshipAttitudeControl::parameter_update_poll()
+{
+	// check for parameter updates
+	if (_parameter_update_sub.updated()) {
+		// clear update
+		parameter_update_s pupdate;
+		_parameter_update_sub.copy(&pupdate);
+
+		// update parameters from storage
+		updateParams();
+	}
+}
+
+void
+AirshipAttitudeControl::publish_actuator_controls()
+{
+	// zero actuators if not armed
+	if (_vehicle_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
+		for (uint8_t i = 0 ; i < 4 ; i++) {
+			_actuators.control[i] = 0.0f;
+		}
+
+	} else {
+		_actuators.control[0] = 0.0f;
+		_actuators.control[1] = _manual_control_sp.x;
+		_actuators.control[2] = _manual_control_sp.r;
+		_actuators.control[3] = _manual_control_sp.z;
+	}
+
+	// note: _actuators.timestamp_sample is set in AirshipAttitudeControl::Run()
+	_actuators.timestamp = hrt_absolute_time();
+
+	_actuators_0_pub.publish(_actuators);
+}
+
+void
+AirshipAttitudeControl::Run()
+{
+	if (should_exit()) {
+		_vehicle_angular_velocity_sub.unregisterCallback();
+		exit_and_cleanup();
+		return;
+	}
+
+	perf_begin(_loop_perf);
+
+	/* run controller on gyro changes */
+	vehicle_angular_velocity_s angular_velocity;
+
+	if (_vehicle_angular_velocity_sub.update(&angular_velocity)) {
+
+		const Vector3f rates{angular_velocity.xyz};
+
+		_actuators.timestamp_sample = angular_velocity.timestamp_sample;
+
+		/* run the rate controller immediately after a gyro update */
+		publish_actuator_controls();
+
+		/* check for updates in manual control topic */
+		if (_manual_control_sp_sub.updated()) {
+			_manual_control_sp_sub.update(&_manual_control_sp);
+		}
+
+		/* check for updates in vehicle status topic */
+		if (_vehicle_status_sub.updated()) {
+			_vehicle_status_sub.update(&_vehicle_status);
+		}
+
+		parameter_update_poll();
+	}
+
+	perf_end(_loop_perf);
+}
+
+int AirshipAttitudeControl::task_spawn(int argc, char *argv[])
+{
+	AirshipAttitudeControl *instance = new AirshipAttitudeControl();
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init()) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+int AirshipAttitudeControl::print_status()
+{
+	PX4_INFO("Running");
+
+	perf_print_counter(_loop_perf);
+
+	print_message(_actuators);
+
+	return 0;
+}
+
+int AirshipAttitudeControl::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int AirshipAttitudeControl::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+This implements the airship attitude and rate controller. Ideally it would
+take attitude setpoints (`vehicle_attitude_setpoint`) or rate setpoints (in acro mode
+via `manual_control_setpoint` topic) as inputs and outputs actuator control messages.
+
+Currently it is feeding the `manual_control_setpoint` topic directly to the actuators.
+
+### Implementation
+To reduce control latency, the module directly polls on the gyro topic published by the IMU driver.
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("airship_att_control", "controller");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+int airship_att_control_main(int argc, char *argv[])
+{
+	return AirshipAttitudeControl::main(argc, argv);
+}

src/modules/land_detector/AirshipLandDetector.cpp

@@ -0,0 +1,62 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2017 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 AirshipLandDetector.cpp
+ * Land detection algorithm for airships
+ *
+ * @author Anton Erasmus <anton@flycloudline.com>
+ */
+
+#include "AirshipLandDetector.h"
+
+namespace land_detector
+{
+
+bool AirshipLandDetector::_get_ground_contact_state()
+{
+	return false;
+}
+
+bool AirshipLandDetector::_get_landed_state()
+{
+
+	if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) {
+		return true; // If Landing has been requested then say we have landed.
+
+	} else {
+		return !_armed;  // If we are armed we are not landed.
+	}
+}
+
+} // namespace land_detector

src/modules/land_detector/AirshipLandDetector.h

@@ -0,0 +1,60 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2017 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 AirshipLandDetector.h
+ * Land detection implementation for airships.
+ *
+ * @author Anton Erasmus <anton@flycloudline.com>
+ */
+
+#pragma once
+
+#include "LandDetector.h"
+
+namespace land_detector
+{
+
+class AirshipLandDetector : public LandDetector
+{
+public:
+	AirshipLandDetector() = default;
+	~AirshipLandDetector() override = default;
+
+protected:
+	bool _get_ground_contact_state() override;
+	bool _get_landed_state() override;
+
+};
+
+} // namespace land_detector

src/modules/land_detector/CMakeLists.txt

@@ -41,6 +41,7 @@ px4_add_module(
 		FixedwingLandDetector.cpp
 		VtolLandDetector.cpp
 		RoverLandDetector.cpp
+		AirshipLandDetector.cpp
 	DEPENDS
 		hysteresis
 	)

src/modules/land_detector/land_detector_main.cpp

@@ -49,6 +49,7 @@
 #include "MulticopterLandDetector.h"
 #include "RoverLandDetector.h"
 #include "VtolLandDetector.h"
+#include "AirshipLandDetector.h"
 
 
 namespace land_detector
@@ -77,6 +78,9 @@ int LandDetector::task_spawn(int argc, char *argv[])
 	} else if (strcmp(argv[1], "rover") == 0) {
 		obj = new RoverLandDetector();
 
+	} else if (strcmp(argv[1], "airship") == 0) {
+		obj = new AirshipLandDetector();
+
 	} else {
 		print_usage("unknown mode");
 		return PX4_ERROR;
@@ -138,7 +142,7 @@ The module runs periodically on the HP work queue.
 
 	PRINT_MODULE_USAGE_NAME("land_detector", "system");
 	PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the background task");
-	PRINT_MODULE_USAGE_ARG("fixedwing|multicopter|vtol|rover", "Select vehicle type", false);
+	PRINT_MODULE_USAGE_ARG("fixedwing|multicopter|vtol|rover|airship", "Select vehicle type", false);
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 	return 0;
 }

src/modules/simulator/simulator_mavlink.cpp

@@ -143,6 +143,25 @@ mavlink_hil_actuator_controls_t Simulator::actuator_controls_from_outputs()
 			}
 		}
 
+	} else if (_system_type == MAV_TYPE_AIRSHIP) {
+		/* airship: scale starboard and port throttle to 0..1 and other channels (tilt, tail thruster) to -1..1 */
+		for (unsigned i = 0; i < 16; i++) {
+			if (armed) {
+				if (i < 2) {
+					/* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to 0..1 for rotors */
+					msg.controls[i] = (_actuator_outputs.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN);
+
+				} else {
+					/* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to -1..1 for other channels */
+					msg.controls[i] = (_actuator_outputs.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
+				}
+
+			} else {
+				/* set 0 for disabled channels */
+				msg.controls[i] = 0.0f;
+			}
+		}
+
 	} else {
 		/* fixed wing: scale throttle to 0..1 and other channels to -1..1 */