FlightTaskOrbit: add parameter for orbit and set default radius to 400m

src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp

@@ -166,7 +166,7 @@ class FlightTask : public ModuleParams
 	/**
 	 * Call this whenever a parameter update notification is received (parameter_update uORB message)
 	 */
-	void handleParameterUpdate()
+	void virtual handleParameterUpdate()
 	{
 		updateParams();
 	}

src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp

@@ -39,6 +39,7 @@
 #include <mathlib/mathlib.h>
 #include <lib/ecl/geo/geo.h>
 #include <uORB/topics/orbit_status.h>
+#include <systemlib/mavlink_log.h>
 
 using namespace matrix;
 
@@ -52,6 +53,23 @@ FlightTaskOrbit::~FlightTaskOrbit()
 	orb_unadvertise(_orbit_status_pub);
 }
 
+void FlightTaskOrbit::handleParameterUpdate()
+{
+	updateParams();
+
+	if (_param_flt_orb_v_max.get() > _param_mpc_xy_vel_max.get()) {
+		_param_flt_orb_v_max.set(_param_mpc_xy_vel_max.get());
+		_param_flt_orb_v_max.commit();
+		mavlink_log_critical(&_mavlink_log_pub, "Orbit speed has been constrained by max speed")
+	}
+
+	if (_param_flt_orb_r_min.get() > _param_flt_orb_r_max.get()) {
+		_param_flt_orb_r_min.set(_param_flt_orb_r_max.get());
+		_param_flt_orb_r_min.commit();
+		mavlink_log_critical(&_mavlink_log_pub, "Orbit minimum radius has been constrained by maximum radius")
+	}
+}
+
 bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command)
 {
 	bool ret = true;
@@ -120,11 +138,11 @@ bool FlightTaskOrbit::sendTelemetry()
 bool FlightTaskOrbit::setRadius(float r)
 {
 	// clip the radius to be within range
-	r = math::constrain(r, _radius_min, _radius_max);
+	r = math::constrain(r, _param_flt_orb_r_min.get(), _param_flt_orb_r_max.get());
 
 	// small radius is more important than high velocity for safety
-	if (!checkAcceleration(r, _v, _acceleration_max)) {
-		_v = math::sign(_v) * sqrtf(_acceleration_max * r);
+	if (!checkAcceleration(r, _v, _param_flt_orb_a_max.get())) {
+		_v = math::sign(_v) * sqrtf(_param_flt_orb_a_max.get() * r);
 	}
 
 	_r = r;
@@ -133,8 +151,8 @@ bool FlightTaskOrbit::setRadius(float r)
 
 bool FlightTaskOrbit::setVelocity(const float v)
 {
-	if (fabs(v) < _velocity_max &&
-	    checkAcceleration(_r, v, _acceleration_max)) {
+	if (fabs(v) < _param_flt_orb_v_max.get() &&
+	    checkAcceleration(_r, v, _param_flt_orb_a_max.get())) {
 		_v = v;
 		return true;
 	}
@@ -150,7 +168,7 @@ bool FlightTaskOrbit::checkAcceleration(float r, float v, float a)
 bool FlightTaskOrbit::activate(vehicle_local_position_setpoint_s last_setpoint)
 {
 	bool ret = FlightTaskManualAltitudeSmooth::activate(last_setpoint);
-	_r = _radius_min;
+	_r = _param_flt_orb_r_min.get();
 	_v =  1.f;
 	_center = Vector2f(_position);
 	_center(0) -= _r;
@@ -172,8 +190,8 @@ bool FlightTaskOrbit::update()
 	FlightTaskManualAltitudeSmooth::update();
 
 	// stick input adjusts parameters within a fixed time frame
-	const float r = _r - _sticks_expo(0) * _deltatime * (_radius_max / 8.f);
-	const float v = _v - _sticks_expo(1) * _deltatime * (_velocity_max / 4.f);
+	const float r = _r - _sticks_expo(0) * _deltatime * (_param_flt_orb_r_max.get() / 8.f);
+	const float v = _v - _sticks_expo(1) * _deltatime * (_param_flt_orb_v_max.get() / 4.f);
 
 	setRadius(r);
 	setVelocity(v);

src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.hpp

@@ -53,6 +53,8 @@ class FlightTaskOrbit : public FlightTaskManualAltitudeSmooth
 	bool applyCommandParameters(const vehicle_command_s &command) override;
 	bool activate(vehicle_local_position_setpoint_s last_setpoint) override;
 	bool update() override;
+	void handleParameterUpdate() override;
+
 
 	/**
 	 * Check the feasibility of orbit parameters with respect to
@@ -90,11 +92,14 @@ class FlightTaskOrbit : public FlightTaskManualAltitudeSmooth
 	float _v = 0.f; /**< clockwise tangential velocity for orbiting in m/s */
 	matrix::Vector2f _center; /**< local frame coordinates of the center point */
 
-	// TODO: create/use parameters for limits
-	const float _radius_min = 1.f;
-	const float _radius_max = 100.f;
-	const float _velocity_max = 10.f;
-	const float _acceleration_max = 2.f;
-
 	orb_advert_t _orbit_status_pub = nullptr;
+	orb_advert_t _mavlink_log_pub{nullptr};
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualAltitudeSmooth,
+					(ParamFloat<px4::params::FLT_ORB_A_MAX>)_param_flt_orb_a_max,
+					(ParamFloat<px4::params::FLT_ORB_R_MIN>)_param_flt_orb_r_min,
+					(ParamFloat<px4::params::FLT_ORB_R_MAX>)_param_flt_orb_r_max,
+					(ParamFloat<px4::params::FLT_ORB_V_MAX>)_param_flt_orb_v_max,
+					(ParamFloat<px4::params::MPC_XY_VEL_MAX>) _param_mpc_xy_vel_max
+				       )
 };

src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit_params.c

@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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 FlightTaksOrbit_params.c
+ * Orbit specific parameters.
+ *
+ * @author Dennis Mannhart <dennis.mannhart@gmail.com>
+ */
+
+/**
+ * Centripetal maximum acceleration
+ *
+ * @unit m/s/s
+ * @min 1.0
+ * @max 10.0
+ * @increment 0.5
+ * @decimal 1
+ * @group FlightTaskOrbit
+ */
+PARAM_DEFINE_FLOAT(FLT_ORB_A_MAX, 2.0f);
+
+/**
+ * Minimum radius of orbit
+ *
+ * @unit m
+ * @min 1.0
+ * @max 10.0
+ * @increment 0.5
+ * @decimal 1
+ * @group FlightTaskOrbit
+ */
+PARAM_DEFINE_FLOAT(FLT_ORB_R_MIN, 1.0f);
+
+/**
+ * Maximum radius of orbit
+ *
+ * @unit m
+ * @min 1.0
+ * @max 1000.0
+ * @increment 0.5
+ * @decimal 1
+ * @group FlightTaskOrbit
+ */
+PARAM_DEFINE_FLOAT(FLT_ORB_R_MAX, 400.0f);
+
+/**
+ * Maximum speed
+ *
+ * @unit m/s
+ * @min 1.0
+ * @max 15.0
+ * @increment 0.5
+ * @decimal 1
+ * @group FlightTaskOrbit
+ */
+PARAM_DEFINE_FLOAT(FLT_ORB_V_MAX, 10.0f);