[WIP] VTOL coordinate FW and MC rate controllers only

Tools/uorb_graph/create.py

@@ -246,10 +246,8 @@ def __init__(self, module_whitelist=[], topic_blacklist=[]):
 
     ('mc_pos_control', r'mc_pos_control_main\.cpp$', r'\b_attitude_setpoint_id=([^,)]+)', r'^_attitude_setpoint_id$'),
 
-    ('mc_att_control', r'mc_att_control_main\.cpp$', r'\b_rates_sp_id=([^,)]+)', r'^_rates_sp_id$'),
     ('mc_att_control', r'mc_att_control_main\.cpp$', r'\b_actuators_id=([^,)]+)', r'^_actuators_id$'),
 
-    ('fw_att_control', r'FixedwingAttitudeControl\.cpp$', r'\b_rates_sp_id=([^,)]+)', r'^_rates_sp_id$'),
     ('fw_att_control', r'FixedwingAttitudeControl\.cpp$', r'\b_actuators_id=([^,)]+)', r'^_actuators_id$'),
     ('fw_att_control', r'FixedwingAttitudeControl\.cpp$', r'\b_attitude_setpoint_id=([^,)]+)', r'^_attitude_setpoint_id$'),
 

msg/vehicle_attitude_setpoint.msg

@@ -12,7 +12,9 @@ float32 yaw_sp_move_rate	# rad/s (commanded by user)
 float32[4] q_d			# Desired quaternion for quaternion control
 bool q_d_valid			# Set to true if quaternion vector is valid
 
-float32 thrust			# Thrust in Newton the power system should generate
+float32 thrust_x					# Throttle command in body x direction [-1,1]
+float32 thrust_y					# Throttle command in body y direction [-1,1]
+float32 thrust_z					# Throttle command in body z direction [-1,1]
 
 bool roll_reset_integral	# Reset roll integral part (navigation logic change)
 bool pitch_reset_integral	# Reset pitch integral part (navigation logic change)

msg/vehicle_rates_setpoint.msg

@@ -1,8 +1,9 @@
 uint64 timestamp	# time since system start (microseconds)
 
-float32 roll		# body angular rates in NED frame
-float32 pitch		# body angular rates in NED frame
-float32 yaw		# body angular rates in NED frame
-float32 thrust		# thrust normalized to 0..1
+float32 roll	    # body angular rates in NED frame
+float32 pitch	    # body angular rates in NED frame
+float32 yaw			# body angular rates in NED frame
 
-# TOPICS vehicle_rates_setpoint mc_virtual_rates_setpoint fw_virtual_rates_setpoint
+float32 thrust_x	   # Throttle command in body x direction [-1,1]
+float32 thrust_y	   # Throttle command in body y direction [-1,1]
+float32 thrust_z	   # Throttle command in body z direction [-1,1]

src/examples/rover_steering_control/main.cpp

@@ -182,7 +182,7 @@ void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const st
 	actuators->control[2] = yaw_err * pp.yaw_p;
 
 	/* copy throttle */
-	actuators->control[3] = att_sp->thrust;
+	actuators->control[3] = att_sp->thrust_x;
 
 	actuators->timestamp = hrt_absolute_time();
 }

src/modules/fw_att_control/FixedwingAttitudeControl.cpp

@@ -40,6 +40,9 @@
  */
 extern "C" __EXPORT int fw_att_control_main(int argc, char *argv[]);
 
+static constexpr float _airspeed_numerical_min =
+	0.5f; /**< lowest airspeed we will ever use to prevent numerical problems */
+
 FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_airspeed_sub(ORB_ID(airspeed)),
 
@@ -116,6 +119,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 
 	// initialize to invalid VTOL type
 	_parameters.vtol_type = -1;
+	_parameters.vtol_airspeed_rule = 0;
 
 	/* fetch initial parameter values */
 	parameters_update();
@@ -209,6 +213,7 @@ FixedwingAttitudeControl::parameters_update()
 
 	if (_vehicle_status.is_vtol) {
 		param_get(_parameter_handles.vtol_type, &_parameters.vtol_type);
+		param_get(_parameter_handles.vtol_airspeed_rule, &_parameters.vtol_airspeed_rule);
 	}
 
 	param_get(_parameter_handles.bat_scale_en, &_parameters.bat_scale_en);
@@ -255,16 +260,18 @@ FixedwingAttitudeControl::vehicle_control_mode_poll()
 	orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated);
 
 	if (vcontrol_mode_updated) {
-
 		orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode);
 	}
 }
 
 void
 FixedwingAttitudeControl::vehicle_manual_poll()
 {
+	bool manual_updated;
+	orb_check(_manual_sub, &manual_updated);
+
 	// only update manual if in a manual mode
-	if (_vcontrol_mode.flag_control_manual_enabled) {
+	if (_vcontrol_mode.flag_control_manual_enabled && manual_updated && !_vehicle_status.is_rotary_wing) {
 
 		// Always copy the new manual setpoint, even if it wasn't updated, to fill the _actuators with valid values
 		if (orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual) == PX4_OK) {
@@ -287,7 +294,7 @@ FixedwingAttitudeControl::vehicle_manual_poll()
 					_att_sp.pitch_body = -_manual.x * _parameters.man_pitch_max + _parameters.pitchsp_offset_rad;
 					_att_sp.pitch_body = math::constrain(_att_sp.pitch_body, -_parameters.man_pitch_max, _parameters.man_pitch_max);
 					_att_sp.yaw_body = 0.0f;
-					_att_sp.thrust = _manual.z;
+					_att_sp.thrust_x = _manual.z;
 
 					Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
 					q.copyTo(_att_sp.q_d);
@@ -310,15 +317,15 @@ FixedwingAttitudeControl::vehicle_manual_poll()
 					_rates_sp.roll = _manual.y * _parameters.acro_max_x_rate_rad;
 					_rates_sp.pitch = -_manual.x * _parameters.acro_max_y_rate_rad;
 					_rates_sp.yaw = _manual.r * _parameters.acro_max_z_rate_rad;
-					_rates_sp.thrust = _manual.z;
+					_rates_sp.thrust_x = _manual.z;
 
 					if (_rate_sp_pub != nullptr) {
 						/* publish the attitude rates setpoint */
-						orb_publish(_rates_sp_id, _rate_sp_pub, &_rates_sp);
+						orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &_rates_sp);
 
-					} else if (_rates_sp_id) {
+					} else {
 						/* advertise the attitude rates setpoint */
-						_rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp);
+						_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp);
 					}
 
 				} else {
@@ -335,14 +342,38 @@ FixedwingAttitudeControl::vehicle_manual_poll()
 }
 
 void
-FixedwingAttitudeControl::vehicle_setpoint_poll()
+FixedwingAttitudeControl::vehicle_attitude_setpoint_poll()
 {
 	/* check if there is a new setpoint */
 	bool att_sp_updated;
 	orb_check(_att_sp_sub, &att_sp_updated);
 
 	if (att_sp_updated) {
-		orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp);
+		if (orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp)) {
+			_rates_sp.thrust_x = _att_sp.thrust_x;
+			_rates_sp.thrust_y = _att_sp.thrust_y;
+			_rates_sp.thrust_z = _att_sp.thrust_z;
+		}
+	}
+}
+
+void
+FixedwingAttitudeControl::vehicle_rates_setpoint_poll()
+{
+	/* check if there is a new setpoint */
+	bool rates_sp_updated = false;
+	orb_check(_rates_sp_sub, &rates_sp_updated);
+
+	if (rates_sp_updated) {
+		orb_copy(ORB_ID(vehicle_rates_setpoint), _rates_sp_sub, &_rates_sp);
+
+		if (_parameters.vtol_type == vtol_type::TAILSITTER) {
+			float tmp = _rates_sp.roll;
+			_rates_sp.roll = -_rates_sp.yaw;
+			_rates_sp.yaw = tmp;
+		}
+
+
 	}
 }
 
@@ -368,19 +399,26 @@ FixedwingAttitudeControl::vehicle_status_poll()
 	if (vehicle_status_updated) {
 		orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
 
+		// if VTOL listen to rate setpoints only
+		if (_vehicle_status.is_vtol) {
+			if (_vehicle_status.is_rotary_wing) {
+				_vcontrol_mode.flag_control_attitude_enabled = false;
+				_vcontrol_mode.flag_control_manual_enabled = false;
+			}
+		}
+
 		/* set correct uORB ID, depending on if vehicle is VTOL or not */
-		if (!_rates_sp_id) {
+		if (!_actuators_id) {
 			if (_vehicle_status.is_vtol) {
-				_rates_sp_id = ORB_ID(fw_virtual_rates_setpoint);
 				_actuators_id = ORB_ID(actuator_controls_virtual_fw);
 				_attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint);
 
 				_parameter_handles.vtol_type = param_find("VT_TYPE");
+				_parameter_handles.vtol_airspeed_rule = param_find("VT_AIRSPD_RULE");
 
 				parameters_update();
 
 			} else {
-				_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
 				_actuators_id = ORB_ID(actuator_controls_0);
 				_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
 			}
@@ -404,6 +442,29 @@ FixedwingAttitudeControl::vehicle_land_detected_poll()
 	}
 }
 
+float FixedwingAttitudeControl::get_airspeed_scaling(float airspeed)
+{
+	// check if we have special airspeed rules for vtol
+	if (_vehicle_status.is_vtol && _parameters.vtol_airspeed_rule > 0) {
+
+		// always use minimum airspeed during hover
+		if (_parameters.vtol_airspeed_rule == 1 && _vehicle_status.is_rotary_wing &&
+		    !_vehicle_status.in_transition_mode) {
+			airspeed = _airspeed_numerical_min;
+		}
+	}
+
+	/*
+	 * 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.
+	 */
+	return _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min :
+					    airspeed);
+}
+
 void FixedwingAttitudeControl::run()
 {
 	/*
@@ -418,16 +479,23 @@ void FixedwingAttitudeControl::run()
 	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
 	_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
 	_battery_status_sub = orb_subscribe(ORB_ID(battery_status));
+	_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
 
 	parameters_update();
 
 	/* get an initial update for all sensor and status data */
-	vehicle_setpoint_poll();
+	vehicle_attitude_setpoint_poll();
 	vehicle_control_mode_poll();
 	vehicle_manual_poll();
 	vehicle_status_poll();
 	vehicle_land_detected_poll();
 
+	// set initial maximum body rate setpoints
+	_roll_ctrl.set_max_rate(_parameters.acro_max_x_rate_rad);
+	_pitch_ctrl.set_max_rate_pos(_parameters.acro_max_y_rate_rad);
+	_pitch_ctrl.set_max_rate_neg(_parameters.acro_max_y_rate_rad);
+	_yaw_ctrl.set_max_rate(_parameters.acro_max_z_rate_rad);
+
 	/* wakeup source */
 	px4_pollfd_struct_t fds[1];
 
@@ -529,7 +597,7 @@ void FixedwingAttitudeControl::run()
 			matrix::Eulerf euler_angles(R);
 
 			_airspeed_sub.update();
-			vehicle_setpoint_poll();
+			vehicle_attitude_setpoint_poll();
 			vehicle_control_mode_poll();
 			vehicle_manual_poll();
 			global_pos_poll();
@@ -560,6 +628,7 @@ void FixedwingAttitudeControl::run()
 
 			/* decide if in stabilized or full manual control */
 			if (_vcontrol_mode.flag_control_rates_enabled) {
+
 				/* scale around tuning airspeed */
 				float airspeed;
 
@@ -570,7 +639,7 @@ void FixedwingAttitudeControl::run()
 
 				if (!_parameters.airspeed_disabled && airspeed_valid) {
 					/* prevent numerical drama by requiring 0.5 m/s minimal speed */
-					airspeed = math::max(0.5f, _airspeed_sub.get().indicated_airspeed_m_s);
+					airspeed = math::max(_airspeed_numerical_min, _airspeed_sub.get().indicated_airspeed_m_s);
 
 				} else {
 					airspeed = _parameters.airspeed_trim;
@@ -580,15 +649,7 @@ void FixedwingAttitudeControl::run()
 					}
 				}
 
-				/*
-				 * 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 = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min :
-							 airspeed);
+				float airspeed_scaling = get_airspeed_scaling(airspeed);
 
 				/* Use min airspeed to calculate ground speed scaling region.
 				 * Don't scale below gspd_scaling_trim
@@ -624,10 +685,6 @@ void FixedwingAttitudeControl::run()
 					_wheel_ctrl.reset_integrator();
 				}
 
-				float roll_sp = _att_sp.roll_body;
-				float pitch_sp = _att_sp.pitch_body;
-				float yaw_sp = _att_sp.yaw_body;
-
 				/* Prepare data for attitude controllers */
 				struct ECL_ControlData control_input = {};
 				control_input.roll = euler_angles.phi();
@@ -636,9 +693,9 @@ void FixedwingAttitudeControl::run()
 				control_input.body_x_rate = _att.rollspeed;
 				control_input.body_y_rate = _att.pitchspeed;
 				control_input.body_z_rate = _att.yawspeed;
-				control_input.roll_setpoint = roll_sp;
-				control_input.pitch_setpoint = pitch_sp;
-				control_input.yaw_setpoint = yaw_sp;
+				control_input.roll_setpoint = _att_sp.roll_body;
+				control_input.pitch_setpoint = _att_sp.pitch_body;
+				control_input.yaw_setpoint = _att_sp.yaw_body;
 				control_input.airspeed_min = _parameters.airspeed_min;
 				control_input.airspeed_max = _parameters.airspeed_max;
 				control_input.airspeed = airspeed;
@@ -649,7 +706,7 @@ void FixedwingAttitudeControl::run()
 
 				/* reset body angular rate limits on mode change */
 				if ((_vcontrol_mode.flag_control_attitude_enabled != _flag_control_attitude_enabled_last) || params_updated) {
-					if (_vcontrol_mode.flag_control_attitude_enabled) {
+					if (_vcontrol_mode.flag_control_attitude_enabled || _vehicle_status.is_rotary_wing) {
 						_roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax));
 						_pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos));
 						_pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg));
@@ -693,7 +750,7 @@ void FixedwingAttitudeControl::run()
 
 				/* Run attitude controllers */
 				if (_vcontrol_mode.flag_control_attitude_enabled) {
-					if (PX4_ISFINITE(roll_sp) && PX4_ISFINITE(pitch_sp)) {
+					if (PX4_ISFINITE(_att_sp.roll_body) && PX4_ISFINITE(_att_sp.pitch_body)) {
 						_roll_ctrl.control_attitude(control_input);
 						_pitch_ctrl.control_attitude(control_input);
 						_yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude
@@ -744,8 +801,8 @@ void FixedwingAttitudeControl::run()
 						}
 
 						/* throttle passed through if it is finite and if no engine failure was detected */
-						_actuators.control[actuator_controls_s::INDEX_THROTTLE] = (PX4_ISFINITE(_att_sp.thrust)
-								&& !_vehicle_status.engine_failure) ? _att_sp.thrust : 0.0f;
+						_actuators.control[actuator_controls_s::INDEX_THROTTLE] = (PX4_ISFINITE(_att_sp.thrust_x)
+								&& !_vehicle_status.engine_failure) ? _att_sp.thrust_x : 0.0f;
 
 						/* scale effort by battery status */
 						if (_parameters.bat_scale_en &&
@@ -783,18 +840,19 @@ void FixedwingAttitudeControl::run()
 
 					if (_rate_sp_pub != nullptr) {
 						/* publish the attitude rates setpoint */
-						orb_publish(_rates_sp_id, _rate_sp_pub, &_rates_sp);
+						orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &_rates_sp);
 
-					} else if (_rates_sp_id) {
+					} else {
 						/* advertise the attitude rates setpoint */
-						_rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp);
+						_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp);
 					}
 
 				} else {
-					// pure rate control
+					vehicle_rates_setpoint_poll();
+
 					_roll_ctrl.set_bodyrate_setpoint(_rates_sp.roll);
-					_pitch_ctrl.set_bodyrate_setpoint(_rates_sp.pitch);
 					_yaw_ctrl.set_bodyrate_setpoint(_rates_sp.yaw);
+					_pitch_ctrl.set_bodyrate_setpoint(_rates_sp.pitch);
 
 					float roll_u = _roll_ctrl.control_bodyrate(control_input);
 					_actuators.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + trim_roll : trim_roll;
@@ -805,7 +863,7 @@ void FixedwingAttitudeControl::run()
 					float yaw_u = _yaw_ctrl.control_bodyrate(control_input);
 					_actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + trim_yaw : trim_yaw;
 
-					_actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_rates_sp.thrust) ? _rates_sp.thrust : 0.0f;
+					_actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_rates_sp.thrust_x) ? _rates_sp.thrust_x : 0.0f;
 				}
 
 				rate_ctrl_status_s rate_ctrl_status;