introduced vtol_fw_permanent stabilization: allows vtol to be attitude-stabilized in manual mode

5 files changed, 30 insertions, 3 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index dc0594bf2..086f291f6 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1267,7 +1267,7 @@ int commander_thread_main(int argc, char *argv[])
 		if (updated) {
 			/* vtol status changed */
 			orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status);
-
+			status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab;
 			/* Make sure that this is only adjusted if vehicle realy is of type vtol*/
 			if (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR || VEHICLE_TYPE_VTOL_QUADROTOR) {
 				status.is_rotary_wing = vtol_status.vtol_in_rw_mode;
@@ -2246,8 +2246,8 @@ set_control_mode()
 	case NAVIGATION_STATE_MANUAL:
 		control_mode.flag_control_manual_enabled = true;
 		control_mode.flag_control_auto_enabled = false;
-		control_mode.flag_control_rates_enabled = status.is_rotary_wing;
-		control_mode.flag_control_attitude_enabled = status.is_rotary_wing;
+		control_mode.flag_control_rates_enabled = (status.is_rotary_wing || status.vtol_fw_permanent_stab);
+		control_mode.flag_control_attitude_enabled = (status.is_rotary_wing || status.vtol_fw_permanent_stab);
 		control_mode.flag_control_altitude_enabled = false;
 		control_mode.flag_control_climb_rate_enabled = false;
 		control_mode.flag_control_position_enabled = false;
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index b4d5c7b88..b56e81e04 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -189,6 +189,8 @@ struct vehicle_status_s {
 							  this is only true while flying as a multicopter */
 	bool is_vtol;					/**< True if the system is VTOL capable */
 
+	bool vtol_fw_permanent_stab;	/**< True if vtol should stabilize attitude for fw in manual mode */
+
 	bool condition_battery_voltage_valid;
 	bool condition_system_in_air_restore;	/**< true if we can restore in mid air */
 	bool condition_system_sensors_initialized;
diff --git a/src/modules/uORB/topics/vtol_vehicle_status.h b/src/modules/uORB/topics/vtol_vehicle_status.h
index 24ecca9fa..7b4e22bc8 100644
--- a/src/modules/uORB/topics/vtol_vehicle_status.h
+++ b/src/modules/uORB/topics/vtol_vehicle_status.h
@@ -54,6 +54,7 @@ struct vtol_vehicle_status_s {
 
 	uint64_t	timestamp;	/**< Microseconds since system boot */
 	bool vtol_in_rw_mode;	/*true: vtol vehicle is in rotating wing mode */
+	bool fw_permanent_stab;	/**< In fw mode stabilize attitude even if in manual mode*/
 };
 
 /**
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 9a80562f3..10a3950ac 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.cpp
+++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp
@@ -134,6 +134,7 @@ private:
 	struct {
 		param_t idle_pwm_mc;	//pwm value for idle in mc mode
 		param_t vtol_motor_count;
+		param_t vtol_fw_permanent_stab;	// in fw mode stabilize attitude also in manual mode
 		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
@@ -142,6 +143,7 @@ private:
 	struct {
 		param_t idle_pwm_mc;
 		param_t vtol_motor_count;
+		param_t vtol_fw_permanent_stab;
 		param_t mc_airspeed_min;
 		param_t mc_airspeed_trim;
 		param_t mc_airspeed_max;
@@ -234,9 +236,11 @@ VtolAttitudeControl::VtolAttitudeControl() :
 
 	_params.idle_pwm_mc = PWM_LOWEST_MIN;
 	_params.vtol_motor_count = 0;
+	_params.vtol_fw_permanent_stab = 0;
 
 	_params_handles.idle_pwm_mc = param_find("VT_IDLE_PWM_MC");
 	_params_handles.vtol_motor_count = param_find("VT_MOT_COUNT");
+	_params_handles.vtol_fw_permanent_stab = param_find("VT_FW_PERM_STAB");
 	_params_handles.mc_airspeed_min = param_find("VT_MC_ARSPD_MIN");
 	_params_handles.mc_airspeed_max = param_find("VT_MC_ARSPD_MAX");
 	_params_handles.mc_airspeed_trim = param_find("VT_MC_ARSPD_TRIM");
@@ -411,6 +415,9 @@ VtolAttitudeControl::parameters_update()
 	/* vtol motor count */
 	param_get(_params_handles.vtol_motor_count, &_params.vtol_motor_count);
 
+	/* vtol fw permanent stabilization */
+	param_get(_params_handles.vtol_fw_permanent_stab, &_params.vtol_fw_permanent_stab);
+
 	/* vtol mc mode min airspeed */
 	param_get(_params_handles.mc_airspeed_min, &v);
 	_params.mc_airspeed_min = v;
@@ -597,6 +604,9 @@ void VtolAttitudeControl::task_main()
 
 	parameters_update();  // initialize parameter cache
 
+	/* update vtol vehicle status*/
+	_vtol_vehicle_status.fw_permanent_stab = _params.vtol_fw_permanent_stab == 1 ? true : false;
+
 	// make sure we start with idle in mc mode
 	set_idle_mc();
 	flag_idle_mc = true;
@@ -647,6 +657,8 @@ void VtolAttitudeControl::task_main()
 			parameters_update();
 		}
 
+		_vtol_vehicle_status.fw_permanent_stab = _params.vtol_fw_permanent_stab == 1 ? true : false;
+
 		vehicle_control_mode_poll();	//Check for changes in vehicle control mode.
 		vehicle_manual_poll();			//Check for changes in manual inputs.
 		arming_status_poll();			//Check for arming status updates.
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 e21bccb0c..0ab446317 100644
--- a/src/modules/vtol_att_control/vtol_att_control_params.c
+++ b/src/modules/vtol_att_control/vtol_att_control_params.c
@@ -86,3 +86,15 @@ PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MAX,30.0f);
  */
 PARAM_DEFINE_FLOAT(VT_MC_ARSPD_TRIM,10.0f);
 
+/**
+ * Permanent stabilization in fw mode
+ *
+ * If set to one this parameter will cause permanent attitude stabilization in fw mode.
+ * This parameter has been introduced for pure convenience sake.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_INT32(VT_FW_PERM_STAB,0);
+