merged with caipi_airspeed_scale

2 files changed, 85 insertions, 2 deletions

diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp
index 329e2ef94..126ec5832 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.cpp
+++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp
@@ -63,6 +63,7 @@
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vtol_vehicle_status.h>
 #include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/airspeed.h>
 #include <uORB/topics/parameter_update.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
@@ -107,6 +108,7 @@ private:
 	int		_params_sub;			//parameter updates subscription
 	int		_manual_control_sp_sub;	//manual control setpoint subscription
 	int		_armed_sub;				//arming status subscription
+	int 	_airspeed_sub;			// airspeed subscription
 
 	int 	_actuator_inputs_mc;	//topic on which the mc_att_controller publishes actuator inputs
 	int 	_actuator_inputs_fw;	//topic on which the fw_att_controller publishes actuator inputs
@@ -130,18 +132,26 @@ private:
 	struct actuator_controls_s			_actuators_mc_in;	//actuator controls from mc_att_control
 	struct actuator_controls_s			_actuators_fw_in;	//actuator controls from fw_att_control
 	struct actuator_armed_s				_armed;				//actuator arming status
+	struct airspeed_s 					_airspeed;			// airspeed
 
 	struct {
 		param_t idle_pwm_mc;	//pwm value for idle in mc mode
 		param_t vtol_motor_count;
+		float mc_airspeed_min;		// min airspeed in multicoper mode (including prop-wash)
+		float mc_airspeed_trim;		// trim airspeed in multicopter mode
+		float mc_airspeed_max;		// max airpseed in multicopter mode
 	} _params;
 
 	struct {
 		param_t idle_pwm_mc;
 		param_t vtol_motor_count;
+		param_t mc_airspeed_min;
+		param_t mc_airspeed_trim;
+		param_t mc_airspeed_max;
 	} _params_handles;
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
+	perf_counter_t	_nonfinite_input_perf;		/**< performance counter for non finite input */
 
 	/* for multicopters it is usual to have a non-zero idle speed of the engines
 	 * for fixed wings we want to have an idle speed of zero since we do not want
@@ -161,6 +171,7 @@ private:
 	void 		actuator_controls_fw_poll();	//Check for changes in fw_attitude_control output
 	void 		vehicle_rates_sp_mc_poll();
 	void 		vehicle_rates_sp_fw_poll();
+	void 		vehicle_airspeed_poll();		// Check for changes in airspeed
 	void 		parameters_update_poll();		//Check if parameters have changed
 	int 		parameters_update();			//Update local paraemter cache
 	void  		fill_mc_att_control_output();	//write mc_att_control results to actuator message
@@ -169,6 +180,7 @@ private:
 	void 		fill_fw_att_rates_sp();
 	void 		set_idle_fw();
 	void 		set_idle_mc();
+	void 		scale_mc_output();
 };
 
 namespace VTOL_att_control
@@ -192,6 +204,7 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	_params_sub(-1),
 	_manual_control_sp_sub(-1),
 	_armed_sub(-1),
+	_airspeed_sub(-1),
 
 	//init publication handlers
 	_actuators_0_pub(-1),
@@ -199,7 +212,8 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	_vtol_vehicle_status_pub(-1),
 	_v_rates_sp_pub(-1),
 
-	_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control"))
+	_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control")),
+	_nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control nonfinite input"))
 {
 
 	flag_idle_mc = true;
@@ -219,12 +233,16 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	memset(&_actuators_mc_in, 0, sizeof(_actuators_mc_in));
 	memset(&_actuators_fw_in, 0, sizeof(_actuators_fw_in));
 	memset(&_armed, 0, sizeof(_armed));
+	memset(&_airspeed,0,sizeof(_airspeed));
 
 	_params.idle_pwm_mc = PWM_LOWEST_MIN;
 	_params.vtol_motor_count = 0;
 
 	_params_handles.idle_pwm_mc = param_find("IDLE_PWM_MC");
 	_params_handles.vtol_motor_count = param_find("VTOL_MOT_COUNT");
+	_params_handles.mc_airspeed_min = param_find("VTOL_MC_AIRSPEED_MIN");
+	_params_handles.mc_airspeed_max = param_find("VTOL_MC_AIRSPEED_MAX");
+	_params_handles.mc_airspeed_trim = param_find("VTOL_MC_AIRSPEED_TRIM");
 
 	/* fetch initial parameter values */
 	parameters_update();
@@ -353,6 +371,19 @@ void VtolAttitudeControl::vehicle_rates_sp_fw_poll()
 }
 
 /**
+* Check for airspeed updates.
+*/
+void
+VtolAttitudeControl::vehicle_airspeed_poll() {
+	bool updated;
+	orb_check(_airspeed_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(airspeed), _airspeed_sub , &_airspeed);
+	}
+}
+
+/**
 * Check for parameter updates.
 */
 void
@@ -376,12 +407,25 @@ VtolAttitudeControl::parameters_update_poll()
 int
 VtolAttitudeControl::parameters_update()
 {
+	float v;
 	/* idle pwm for mc mode */
 	param_get(_params_handles.idle_pwm_mc, &_params.idle_pwm_mc);
 
 	/* vtol motor count */
 	param_get(_params_handles.vtol_motor_count, &_params.vtol_motor_count);
 
+	/* vtol mc mode min airspeed */
+	param_get(_params_handles.mc_airspeed_min, &v);
+	_params.mc_airspeed_min = v;
+
+	/* vtol mc mode max airspeed */
+	param_get(_params_handles.mc_airspeed_max, &v);
+	_params.mc_airspeed_max = v;
+
+	/* vtol mc mode trim airspeed */
+	param_get(_params_handles.mc_airspeed_trim, &v);
+	_params.mc_airspeed_trim = v;
+
 	return OK;
 }
 
@@ -497,6 +541,34 @@ void VtolAttitudeControl::set_idle_mc()
 }
 
 void
+VtolAttitudeControl::scale_mc_output() {
+	// scale around tuning airspeed
+	float airspeed;
+
+	// if airspeed is not updating, we assume the normal average speed
+	if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) ||
+	    hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
+		airspeed = _params.mc_airspeed_trim;
+		if (nonfinite) {
+			perf_count(_nonfinite_input_perf);
+		}
+		} else {
+			// prevent numerical drama by requiring 0.5 m/s minimal speed
+			airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s);
+		}
+
+	/*
+	 * For scaling our actuators using anything less than the min (close to stall)
+	 * speed doesn't make any sense - its the strongest reasonable deflection we
+	 * want to do in flight and its the baseline a human pilot would choose.
+	 *
+	 * Forcing the scaling to this value allows reasonable handheld tests.
+	 */
+	float airspeed_scaling = _params.mc_airspeed_trim / ((airspeed < _params.mc_airspeed_min) ? _params.mc_airspeed_min : airspeed);
+	_actuators_mc_in.control[1] = math::constrain(_actuators_mc_in.control[1]*airspeed_scaling*airspeed_scaling,-1.0f,1.0f);
+}
+
+void
 VtolAttitudeControl::task_main_trampoline(int argc, char *argv[])
 {
 	VTOL_att_control::g_control->task_main();
@@ -516,6 +588,7 @@ void VtolAttitudeControl::task_main()
 	_params_sub            = orb_subscribe(ORB_ID(parameter_update));
 	_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	_armed_sub             = orb_subscribe(ORB_ID(actuator_armed));
+	_airspeed_sub          = orb_subscribe(ORB_ID(airspeed));
 
 	_actuator_inputs_mc    = orb_subscribe(ORB_ID(actuator_controls_virtual_mc));
 	_actuator_inputs_fw    = orb_subscribe(ORB_ID(actuator_controls_virtual_fw));
@@ -580,6 +653,7 @@ void VtolAttitudeControl::task_main()
 		vehicle_rates_sp_mc_poll();
 		vehicle_rates_sp_fw_poll();
 		parameters_update_poll();
+		vehicle_airspeed_poll();
 
 		if (_manual_control_sp.aux1 <= 0.0f) {		/* vehicle is in mc mode */
 			_vtol_vehicle_status.vtol_in_rw_mode = true;
@@ -593,6 +667,8 @@ void VtolAttitudeControl::task_main()
 			if (fds[0].revents & POLLIN) {
 				vehicle_manual_poll();	/* update remote input */
 				orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in);
+				// scale pitch control with airspeed
+				scale_mc_output();
 
 				fill_mc_att_control_output();
 				fill_mc_att_rates_sp();
diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c
index b9baa2046..44157acba 100644
--- a/src/modules/vtol_att_control/vtol_att_control_params.c
+++ b/src/modules/vtol_att_control/vtol_att_control_params.c
@@ -3,4 +3,11 @@
 // number of engines
 PARAM_DEFINE_INT32(VTOL_MOT_COUNT,0);
 // idle pwm in multicopter mode
-PARAM_DEFINE_INT32(IDLE_PWM_MC,900);
\ No newline at end of file
+PARAM_DEFINE_INT32(IDLE_PWM_MC,900);
+// min airspeed in multicopter mode
+PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MIN,2);
+// max airspeed in multicopter mode
+PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MAX,30);
+// trim airspeed in multicopter mode
+PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_TRIM,10);
+