flight_mode_manager: merge with flight_tasks

src/modules/flight_mode_manager/tasks/FlightTask/CMakeLists.txt

@@ -0,0 +1,38 @@
+############################################################################
+#
+#   Copyright (c) 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.
+#
+############################################################################
+
+px4_add_library(FlightTask
+	FlightTask.cpp
+)
+
+target_include_directories(FlightTask PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp

@@ -0,0 +1,207 @@
+#include "FlightTask.hpp"
+#include <mathlib/mathlib.h>
+#include <lib/ecl/geo/geo.h>
+
+constexpr uint64_t FlightTask::_timeout;
+// First index of empty_setpoint corresponds to time-stamp and requires a finite number.
+const vehicle_local_position_setpoint_s FlightTask::empty_setpoint = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {}};
+
+const ekf_reset_counters_s FlightTask::zero_reset_counters = {};
+const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, false, {}};
+const landing_gear_s FlightTask::empty_landing_gear_default_keep = {0, landing_gear_s::GEAR_KEEP, {}};
+
+bool FlightTask::activate(const vehicle_local_position_setpoint_s &last_setpoint)
+{
+	_resetSetpoints();
+	_setDefaultConstraints();
+	_time_stamp_activate = hrt_absolute_time();
+	_gear = empty_landing_gear_default_keep;
+	return true;
+}
+
+void FlightTask::reActivate()
+{
+	activate(getPositionSetpoint());
+}
+
+bool FlightTask::updateInitialize()
+{
+	_time_stamp_current = hrt_absolute_time();
+	_time = (_time_stamp_current - _time_stamp_activate) / 1e6f;
+	_deltatime  = math::min((_time_stamp_current - _time_stamp_last), _timeout) / 1e6f;
+	_time_stamp_last = _time_stamp_current;
+
+	_sub_vehicle_local_position.update();
+	_sub_home_position.update();
+
+	_evaluateVehicleLocalPosition();
+	_evaluateDistanceToGround();
+	return true;
+}
+
+bool FlightTask::update()
+{
+	_checkEkfResetCounters();
+	return true;
+}
+
+void FlightTask::_checkEkfResetCounters()
+{
+	// Check if a reset event has happened
+	if (_sub_vehicle_local_position.get().xy_reset_counter != _reset_counters.xy) {
+		_ekfResetHandlerPositionXY();
+		_reset_counters.xy = _sub_vehicle_local_position.get().xy_reset_counter;
+	}
+
+	if (_sub_vehicle_local_position.get().vxy_reset_counter != _reset_counters.vxy) {
+		_ekfResetHandlerVelocityXY();
+		_reset_counters.vxy = _sub_vehicle_local_position.get().vxy_reset_counter;
+	}
+
+	if (_sub_vehicle_local_position.get().z_reset_counter != _reset_counters.z) {
+		_ekfResetHandlerPositionZ();
+		_reset_counters.z = _sub_vehicle_local_position.get().z_reset_counter;
+	}
+
+	if (_sub_vehicle_local_position.get().vz_reset_counter != _reset_counters.vz) {
+		_ekfResetHandlerVelocityZ();
+		_reset_counters.vz = _sub_vehicle_local_position.get().vz_reset_counter;
+	}
+
+	if (_sub_vehicle_local_position.get().heading_reset_counter != _reset_counters.heading) {
+		_ekfResetHandlerHeading(_sub_vehicle_local_position.get().delta_heading);
+		_reset_counters.heading = _sub_vehicle_local_position.get().heading_reset_counter;
+	}
+}
+
+const vehicle_local_position_setpoint_s FlightTask::getPositionSetpoint()
+{
+	/* fill position setpoint message */
+	vehicle_local_position_setpoint_s vehicle_local_position_setpoint{};
+	vehicle_local_position_setpoint.timestamp = hrt_absolute_time();
+
+	vehicle_local_position_setpoint.x = _position_setpoint(0);
+	vehicle_local_position_setpoint.y = _position_setpoint(1);
+	vehicle_local_position_setpoint.z = _position_setpoint(2);
+
+	vehicle_local_position_setpoint.vx = _velocity_setpoint(0);
+	vehicle_local_position_setpoint.vy = _velocity_setpoint(1);
+	vehicle_local_position_setpoint.vz = _velocity_setpoint(2);
+
+	_acceleration_setpoint.copyTo(vehicle_local_position_setpoint.acceleration);
+	_jerk_setpoint.copyTo(vehicle_local_position_setpoint.jerk);
+	vehicle_local_position_setpoint.yaw = _yaw_setpoint;
+	vehicle_local_position_setpoint.yawspeed = _yawspeed_setpoint;
+
+	// deprecated, only kept for output logging
+	matrix::Vector3f(NAN, NAN, NAN).copyTo(vehicle_local_position_setpoint.thrust);
+
+	return vehicle_local_position_setpoint;
+}
+
+void FlightTask::_resetSetpoints()
+{
+	_position_setpoint.setNaN();
+	_velocity_setpoint.setNaN();
+	_acceleration_setpoint.setNaN();
+	_jerk_setpoint.setNaN();
+	_yaw_setpoint = _yawspeed_setpoint = NAN;
+}
+
+void FlightTask::_evaluateVehicleLocalPosition()
+{
+	_position.setAll(NAN);
+	_velocity.setAll(NAN);
+	_yaw = NAN;
+	_dist_to_bottom = NAN;
+
+	// Only use vehicle-local-position topic fields if the topic is received within a certain timestamp
+	if ((_time_stamp_current - _sub_vehicle_local_position.get().timestamp) < _timeout) {
+
+		// yaw
+		_yaw = _sub_vehicle_local_position.get().heading;
+
+		// position
+		if (_sub_vehicle_local_position.get().xy_valid) {
+			_position(0) = _sub_vehicle_local_position.get().x;
+			_position(1) = _sub_vehicle_local_position.get().y;
+		}
+
+		if (_sub_vehicle_local_position.get().z_valid) {
+			_position(2) = _sub_vehicle_local_position.get().z;
+		}
+
+		// velocity
+		if (_sub_vehicle_local_position.get().v_xy_valid) {
+			_velocity(0) = _sub_vehicle_local_position.get().vx;
+			_velocity(1) = _sub_vehicle_local_position.get().vy;
+		}
+
+		if (_sub_vehicle_local_position.get().v_z_valid) {
+			_velocity(2) = _sub_vehicle_local_position.get().vz;
+		}
+
+		// distance to bottom
+		if (_sub_vehicle_local_position.get().dist_bottom_valid
+		    && PX4_ISFINITE(_sub_vehicle_local_position.get().dist_bottom)) {
+			_dist_to_bottom =  _sub_vehicle_local_position.get().dist_bottom;
+		}
+
+		// global frame reference coordinates to enable conversions
+		if (_sub_vehicle_local_position.get().xy_global && _sub_vehicle_local_position.get().z_global) {
+			globallocalconverter_init(_sub_vehicle_local_position.get().ref_lat, _sub_vehicle_local_position.get().ref_lon,
+						  _sub_vehicle_local_position.get().ref_alt, _sub_vehicle_local_position.get().ref_timestamp);
+		}
+	}
+}
+
+void FlightTask::_evaluateDistanceToGround()
+{
+	// Altitude above ground is local z-position or altitude above home or distance sensor altitude depending on what's available
+	_dist_to_ground = -_position(2);
+
+	if (PX4_ISFINITE(_dist_to_bottom)) {
+		_dist_to_ground = _dist_to_bottom;
+
+	} else if (_sub_home_position.get().valid_alt) {
+		_dist_to_ground = -(_position(2) - _sub_home_position.get().z);
+	}
+}
+
+void FlightTask::_setDefaultConstraints()
+{
+	_constraints.speed_xy = _param_mpc_xy_vel_max.get();
+	_constraints.speed_up = _param_mpc_z_vel_max_up.get();
+	_constraints.speed_down = _param_mpc_z_vel_max_dn.get();
+	_constraints.tilt = NAN;
+	_constraints.min_distance_to_ground = NAN;
+	_constraints.max_distance_to_ground = NAN;
+	_constraints.want_takeoff = false;
+}
+
+bool FlightTask::_checkTakeoff()
+{
+	// position setpoint above the minimum altitude
+	bool position_triggered_takeoff = false;
+
+	if (PX4_ISFINITE(_position_setpoint(2))) {
+		// minimal altitude either 20cm or what is necessary for correct estimation e.g. optical flow
+		float min_altitude = 0.2f;
+		const float min_distance_to_ground = _sub_vehicle_local_position.get().hagl_min;
+
+		if (PX4_ISFINITE(min_distance_to_ground)) {
+			min_altitude = min_distance_to_ground + 0.05f;
+		}
+
+		position_triggered_takeoff = _position_setpoint(2) < (_position(2) - min_altitude);
+	}
+
+	// upwards velocity setpoint
+	bool velocity_triggered_takeoff = false;
+
+	if (PX4_ISFINITE(_velocity_setpoint(2))) {
+		velocity_triggered_takeoff = _velocity_setpoint(2) < -0.3f;
+	}
+
+	return position_triggered_takeoff || velocity_triggered_takeoff;
+}

src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp

@@ -0,0 +1,273 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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 FlightTask.hpp
+ *
+ * Abstract base class for different advanced flight tasks like orbit, follow me, ...
+ *
+ * @author Matthias Grob <maetugr@gmail.com>
+ */
+
+#pragma once
+
+#include <px4_platform_common/module_params.h>
+#include <drivers/drv_hrt.h>
+#include <matrix/matrix/math.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/landing_gear.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_constraints.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_trajectory_waypoint.h>
+#include <uORB/topics/home_position.h>
+#include <lib/weather_vane/WeatherVane.hpp>
+
+struct ekf_reset_counters_s {
+	uint8_t xy;
+	uint8_t vxy;
+	uint8_t z;
+	uint8_t vz;
+	uint8_t heading;
+};
+
+class FlightTask : public ModuleParams
+{
+public:
+	FlightTask() :
+		ModuleParams(nullptr)
+	{
+		_resetSetpoints();
+		_constraints = empty_constraints;
+	}
+
+	virtual ~FlightTask() = default;
+
+	/**
+	 * Call once on the event where you switch to the task
+	 * @param last_setpoint last output of the previous task
+	 * @return true on success, false on error
+	 */
+	virtual bool activate(const vehicle_local_position_setpoint_s &last_setpoint);
+
+	/**
+	 * Call this to reset an active Flight Task
+	 */
+	virtual void reActivate();
+
+	/**
+	 * To be called to adopt parameters from an arrived vehicle command
+	 * @param command received command message containing the parameters
+	 * @return true if accepted, false if declined
+	 */
+	virtual bool applyCommandParameters(const vehicle_command_s &command) { return false; }
+
+	/**
+	 * Call before activate() or update()
+	 * to initialize time and input data
+	 * @return true on success, false on error
+	 */
+	virtual bool updateInitialize();
+
+	/**
+	 * To be called regularly in the control loop cycle to execute the task
+	 * @return true on success, false on error
+	 */
+	virtual bool update();
+
+	/**
+	 * Call after update()
+	 * to constrain the generated setpoints in order to comply
+	 * with the constraints of the current mode
+	 * @return true on success, false on error
+	 */
+	virtual bool updateFinalize() { return true; };
+
+	/**
+	 * Get the output data
+	 * @return task output setpoints that get executed by the positon controller
+	 */
+	const vehicle_local_position_setpoint_s getPositionSetpoint();
+
+	const ekf_reset_counters_s getResetCounters() const { return _reset_counters; }
+	void setResetCounters(const ekf_reset_counters_s &counters) { _reset_counters = counters; }
+
+	/**
+	 * Get vehicle constraints.
+	 * The constraints can vary with task.
+	 * @return constraints
+	 */
+	const vehicle_constraints_s &getConstraints() { return _constraints; }
+
+	/**
+	 * Get landing gear position.
+	 * The constraints can vary with task.
+	 * @return landing gear
+	 */
+	const landing_gear_s &getGear() { return _gear; }
+
+	/**
+	 * Get avoidance desired waypoint
+	 * @return desired waypoints
+	 */
+	const vehicle_trajectory_waypoint_s &getAvoidanceWaypoint() { return _desired_waypoint; }
+
+	/**
+	 * Empty setpoint.
+	 * All setpoints are set to NAN.
+	 */
+	static const vehicle_local_position_setpoint_s empty_setpoint;
+
+	/**.
+	 * All counters are set to 0.
+	 */
+	static const ekf_reset_counters_s zero_reset_counters;
+
+	/**
+	 * Empty constraints.
+	 * All constraints are set to NAN.
+	 */
+	static const vehicle_constraints_s empty_constraints;
+
+	/**
+	 * default landing gear state
+	 */
+	static const landing_gear_s empty_landing_gear_default_keep;
+
+	/**
+	 * Call this whenever a parameter update notification is received (parameter_update uORB message)
+	 */
+	void handleParameterUpdate()
+	{
+		updateParams();
+	}
+
+	/**
+	 * Sets an external yaw handler which can be used by any flight task to implement a different yaw control strategy.
+	 * This method does nothing, each flighttask which wants to use the yaw handler needs to override this method.
+	 */
+	virtual void setYawHandler(WeatherVane *ext_yaw_handler) {}
+
+	void updateVelocityControllerIO(const matrix::Vector3f &vel_sp,
+					const matrix::Vector3f &acc_sp)
+	{
+		_velocity_setpoint_feedback = vel_sp;
+		_acceleration_setpoint_feedback = acc_sp;
+	}
+
+protected:
+	uORB::SubscriptionData<vehicle_local_position_s> _sub_vehicle_local_position{ORB_ID(vehicle_local_position)};
+	uORB::SubscriptionData<home_position_s> _sub_home_position{ORB_ID(home_position)};
+
+	/** Reset all setpoints to NAN */
+	void _resetSetpoints();
+
+	/** Check and update local position */
+	void _evaluateVehicleLocalPosition();
+
+	void _evaluateDistanceToGround();
+
+	/** Set constraints to default values */
+	virtual void _setDefaultConstraints();
+
+	/** Determine when to trigger a takeoff (ignored in flight) */
+	virtual bool _checkTakeoff();
+
+	/**
+	 * Monitor the EKF reset counters and
+	 * call the appropriate handling functions in case of a reset event
+	 * TODO: add the delta values to all the handlers
+	 */
+	void _checkEkfResetCounters();
+	virtual void _ekfResetHandlerPositionXY() {};
+	virtual void _ekfResetHandlerVelocityXY() {};
+	virtual void _ekfResetHandlerPositionZ() {};
+	virtual void _ekfResetHandlerVelocityZ() {};
+	virtual void _ekfResetHandlerHeading(float delta_psi) {};
+
+	/* Time abstraction */
+	static constexpr uint64_t _timeout = 500000; /**< maximal time in us before a loop or data times out */
+	float _time{}; /**< passed time in seconds since the task was activated */
+	float _deltatime{}; /**< passed time in seconds since the task was last updated */
+	hrt_abstime _time_stamp_activate{}; /**< time stamp when task was activated */
+	hrt_abstime _time_stamp_current{}; /**< time stamp at the beginning of the current task update */
+	hrt_abstime _time_stamp_last{}; /**< time stamp when task was last updated */
+
+	/* Current vehicle state */
+	matrix::Vector3f _position; /**< current vehicle position */
+	matrix::Vector3f _velocity; /**< current vehicle velocity */
+	float _yaw{}; /**< current vehicle yaw heading */
+	float _dist_to_bottom{}; /**< current height above ground level */
+	float _dist_to_ground{}; /**< equals _dist_to_bottom if valid, height above home otherwise */
+
+	/**
+	 * Setpoints which the position controller has to execute.
+	 * Setpoints that are set to NAN are not controlled. Not all setpoints can be set at the same time.
+	 * If more than one type of setpoint is set, then order of control is a as follow: position, velocity,
+	 * acceleration, thrust. The exception is _position_setpoint together with _velocity_setpoint, where the
+	 * _velocity_setpoint is used as feedforward.
+	 * _acceleration_setpoint and _jerk_setpoint are currently not supported.
+	 */
+	matrix::Vector3f _position_setpoint;
+	matrix::Vector3f _velocity_setpoint;
+	matrix::Vector3f _acceleration_setpoint;
+	matrix::Vector3f _jerk_setpoint;
+	float _yaw_setpoint{};
+	float _yawspeed_setpoint{};
+	matrix::Vector3f _velocity_setpoint_feedback;
+	matrix::Vector3f _acceleration_setpoint_feedback;
+
+	ekf_reset_counters_s _reset_counters{}; ///< Counters for estimator local position resets
+
+	/**
+	 * Vehicle constraints.
+	 * The constraints can vary with tasks.
+	 */
+	vehicle_constraints_s _constraints{};
+
+	landing_gear_s _gear{};
+
+	/**
+	 * Desired waypoints.
+	 * Goals set by the FCU to be sent to the obstacle avoidance system.
+	 */
+	vehicle_trajectory_waypoint_s _desired_waypoint{};
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(ModuleParams,
+					(ParamFloat<px4::params::MPC_XY_VEL_MAX>) _param_mpc_xy_vel_max,
+					(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) _param_mpc_z_vel_max_dn,
+					(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _param_mpc_z_vel_max_up
+				       )
+};