vtol_att_control: handle front transition timeout for all types

msg/vehicle_status.msg

@@ -53,9 +53,7 @@ uint8 component_id			# subsystem / component id, contains MAVLink's component ID
 
 bool is_rotary_wing			# True if system is in rotary wing configuration, so for a VTOL 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 in_transition_mode			# True if VTOL is doing a transition
-bool in_transition_to_fw		# True if VTOL is doing a transition from MC to FW
 
 bool rc_signal_lost				# true if RC reception lost
 uint8 rc_input_mode				# set to 1 to disable the RC input, 2 to enable manual control to RC in mapping.

msg/vtol_vehicle_status.msg

@@ -5,8 +5,8 @@ uint8 VEHICLE_VTOL_STATE_TRANSITION_TO_MC = 2
 uint8 VEHICLE_VTOL_STATE_MC = 3
 uint8 VEHICLE_VTOL_STATE_FW = 4
 
-bool vtol_in_rw_mode			# true: vtol vehicle is in rotating wing mode
-bool vtol_in_trans_mode
-bool in_transition_to_fw		# True if VTOL is doing a transition from MC to FW
+uint8 vtol_state
+
 bool vtol_transition_failsafe	# vtol in transition failsafe mode
+
 bool fw_permanent_stab			# In fw mode stabilize attitude even if in manual mode

src/modules/commander/commander.cpp

@@ -436,23 +436,15 @@ int commander_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "transition")) {
 
-		struct vehicle_command_s cmd = {
-			.timestamp = 0,
-			.param5 = NAN,
-			.param6 = NAN,
-			/* transition to the other mode */
-			.param1 = (float)((status.is_rotary_wing) ? vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW : vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC),
-			.param2 = NAN,
-			.param3 = NAN,
-			.param4 = NAN,
-			.param7 = NAN,
-			.command = vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION,
-			.target_system = status.system_id,
-			.target_component = status.component_id
-		};
+		vehicle_command_s vcmd = {};
+		vcmd.timestamp = hrt_absolute_time();
+		vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION;
+		vcmd.param1 = (float)((status.is_rotary_wing) ? vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW : vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC);
+		vcmd.target_system = status.system_id;
+		vcmd.target_component = status.component_id;
 
-		orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
-		(void)orb_unadvertise(h);
+		orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
+		orb_unadvertise(h);
 
 		return 0;
 	}
@@ -1349,9 +1341,6 @@ Commander::run()
 
 	/* Subscribe to vtol vehicle status topic */
 	int vtol_vehicle_status_sub = orb_subscribe(ORB_ID(vtol_vehicle_status));
-	//struct vtol_vehicle_status_s vtol_status;
-	memset(&vtol_status, 0, sizeof(vtol_status));
-	vtol_status.vtol_in_rw_mode = true;		//default for vtol is rotary wing
 
 	/* subscribe to estimator status topic */
 	int estimator_status_sub = orb_subscribe(ORB_ID(estimator_status));
@@ -1488,15 +1477,6 @@ Commander::run()
 					status.system_type = (uint8_t)system_type;
 				}
 
-				/* disable manual override for all systems that rely on electronic stabilization */
-				if (is_rotary_wing(&status) || (is_vtol(&status) && vtol_status.vtol_in_rw_mode)) {
-					status.is_rotary_wing = true;
-
-				} else {
-					status.is_rotary_wing = false;
-				}
-
-				/* set vehicle_status.is_vtol flag */
 				status.is_vtol = is_vtol(&status);
 
 				/* check and update system / component ID */
@@ -1586,7 +1566,34 @@ Commander::run()
 		orb_check(sp_man_sub, &updated);
 
 		if (updated) {
-			orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man);
+			if (orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man) == PX4_OK) {
+
+				// VTOL transition switch
+				if ((sp_man.transition_switch != _last_sp_man.transition_switch) &&
+					(sp_man.transition_switch != manual_control_setpoint_s::SWITCH_POS_NONE) &&
+					control_mode.flag_control_manual_enabled) {
+
+					uint8_t vtol_new_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_UNDEFINED;
+
+					if (sp_man.transition_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
+						vtol_new_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW;
+					} else if (sp_man.transition_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
+						vtol_new_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
+					}
+
+					if (vtol_new_state != vtol_status.vtol_state) {
+						vehicle_command_s vcmd = {};
+						vcmd.timestamp = hrt_absolute_time();
+						vcmd.param1 = vtol_new_state;
+						vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION;
+						vcmd.target_system = status.system_id;
+						vcmd.target_component = status.component_id;
+
+						orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
+						orb_unadvertise(h);
+					}
+				}
+			}
 		}
 
 		orb_check(offboard_control_mode_sub, &updated);
@@ -1699,31 +1706,27 @@ Commander::run()
 		orb_check(vtol_vehicle_status_sub, &updated);
 
 		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 really is of type vtol */
-			if (is_vtol(&status)) {
+			const uint8_t prev_vtol_state = vtol_status.vtol_state;
+			if (orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status) == PX4_OK) {
 
-				// Check if there has been any change while updating the flags
-				if (status.is_rotary_wing != vtol_status.vtol_in_rw_mode) {
-					status.is_rotary_wing = vtol_status.vtol_in_rw_mode;
-					status_changed = true;
-				}
+				status_flags.vtol_transition_failure = vtol_status.vtol_transition_failsafe;
 
-				if (status.in_transition_mode != vtol_status.vtol_in_trans_mode) {
-					status.in_transition_mode = vtol_status.vtol_in_trans_mode;
-					status_changed = true;
-				}
+				status.is_rotary_wing = true;
+				status.in_transition_mode = false;
 
-				if (status.in_transition_to_fw != vtol_status.in_transition_to_fw) {
-					status.in_transition_to_fw = vtol_status.in_transition_to_fw;
-					status_changed = true;
+				switch (vtol_status.vtol_state) {
+				case vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_FW:
+				case vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_MC:
+					status.in_transition_mode = true;
+					break;
+				case vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW:
+					status.is_rotary_wing = false;
+					break;
+				default:
+					break;
 				}
 
-				if (status_flags.vtol_transition_failure != vtol_status.vtol_transition_failsafe) {
-					status_flags.vtol_transition_failure = vtol_status.vtol_transition_failsafe;
+				if (vtol_status.vtol_state != prev_vtol_state) {
 					status_changed = true;
 				}
 
@@ -2962,6 +2965,7 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
 		 (_last_sp_man.rattitude_switch == sp_man.rattitude_switch) &&
 		 (_last_sp_man.posctl_switch == sp_man.posctl_switch) &&
 		 (_last_sp_man.loiter_switch == sp_man.loiter_switch) &&
+		 (_last_sp_man.transition_switch == sp_man.transition_switch) &&
 		 (_last_sp_man.mode_slot == sp_man.mode_slot) &&
 		 (_last_sp_man.stab_switch == sp_man.stab_switch) &&
 		 (_last_sp_man.man_switch == sp_man.man_switch)))) {
@@ -3605,9 +3609,9 @@ bool
 stabilization_required()
 {
 	return (status.is_rotary_wing ||		// is a rotary wing, or
-		status.vtol_fw_permanent_stab || 	// is a VTOL in fixed wing mode and stabilisation is on, or
-		(vtol_status.vtol_in_trans_mode && 	// is currently a VTOL transitioning AND
-		 !status.is_rotary_wing));	// is a fixed wing, ie: transitioning back to rotary wing mode
+		vtol_status.fw_permanent_stab || 	// is a VTOL in fixed wing mode and stabilisation is on, or
+		(status.in_transition_mode && 		// is currently a VTOL transitioning AND
+		 !status.is_rotary_wing));		// is a fixed wing, ie: transitioning back to rotary wing mode
 }
 
 void

src/modules/fw_att_control/FixedwingAttitudeControl.cpp

@@ -33,6 +33,8 @@
 
 #include "FixedwingAttitudeControl.hpp"
 
+using namespace time_literals;
+
 /**
  * Fixedwing attitude control app start / stop handling function
  *
@@ -372,19 +374,21 @@ FixedwingAttitudeControl::vehicle_status_poll()
 
 		/* set correct uORB ID, depending on if vehicle is VTOL or not */
 		if (!_rates_sp_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);
+			if (hrt_elapsed_time(&_vehicle_status.timestamp) < 1_s) {
+				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_type = param_find("VT_TYPE");
 
-				parameters_update();
+					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);
+				} else {
+					_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
+					_actuators_id = ORB_ID(actuator_controls_0);
+					_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
+				}
 			}
 		}
 	}

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -33,6 +33,8 @@
 
 #include "FixedwingPositionControl.hpp"
 
+using namespace time_literals;
+
 extern "C" __EXPORT int fw_pos_control_l1_main(int argc, char *argv[]);
 
 FixedwingPositionControl::FixedwingPositionControl() :
@@ -329,16 +331,18 @@ FixedwingPositionControl::vehicle_status_poll()
 
 		/* set correct uORB ID, depending on if vehicle is VTOL or not */
 		if (_attitude_setpoint_id == nullptr) {
-			if (_vehicle_status.is_vtol) {
-				_attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint);
+			if (hrt_elapsed_time(&_vehicle_status.timestamp) < 1_s) {
+				if (_vehicle_status.is_vtol) {
+					_attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint);
 
-				_parameter_handles.airspeed_trans = param_find("VT_ARSP_TRANS");
-				_parameter_handles.vtol_type = param_find("VT_TYPE");
+					_parameter_handles.airspeed_trans = param_find("VT_ARSP_TRANS");
+					_parameter_handles.vtol_type = param_find("VT_TYPE");
 
-				parameters_update();
+					parameters_update();
 
-			} else {
-				_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
+				} else {
+					_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
+				}
 			}
 		}
 	}

src/modules/logger/logger.cpp

@@ -626,7 +626,7 @@ void Logger::add_default_topics()
 	add_topic("vehicle_status_flags");
 	add_topic("vehicle_vision_attitude");
 	add_topic("vehicle_vision_position");
-	add_topic("vtol_vehicle_status", 200);
+	add_topic("vtol_vehicle_status");
 	add_topic("wind_estimate", 200);
 
 #ifdef CONFIG_ARCH_BOARD_SITL

src/modules/mavlink/mavlink_messages.cpp

@@ -3613,6 +3613,7 @@ class MavlinkStreamExtendedSysState : public MavlinkStream
 
 private:
 	MavlinkOrbSubscription *_status_sub;
+	MavlinkOrbSubscription *_vtol_status_sub;
 	MavlinkOrbSubscription *_landed_sub;
 	MavlinkOrbSubscription *_pos_sp_triplet_sub;
 	MavlinkOrbSubscription *_control_mode_sub;
@@ -3625,6 +3626,7 @@ class MavlinkStreamExtendedSysState : public MavlinkStream
 protected:
 	explicit MavlinkStreamExtendedSysState(Mavlink *mavlink) : MavlinkStream(mavlink),
 		_status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))),
+		_vtol_status_sub(_mavlink->add_orb_subscription(ORB_ID(vtol_vehicle_status))),
 		_landed_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_land_detected))),
 		_pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))),
 		_control_mode_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_control_mode))),
@@ -3642,20 +3644,10 @@ class MavlinkStreamExtendedSysState : public MavlinkStream
 
 		if (_status_sub->update(&status)) {
 			updated = true;
+			vtol_vehicle_status_s vtol_status;
 
-			if (status.is_vtol) {
-				if (!status.in_transition_mode && status.is_rotary_wing) {
-					_msg.vtol_state = MAV_VTOL_STATE_MC;
-
-				} else if (!status.in_transition_mode) {
-					_msg.vtol_state = MAV_VTOL_STATE_FW;
-
-				} else if (status.in_transition_mode && status.in_transition_to_fw) {
-					_msg.vtol_state = MAV_VTOL_STATE_TRANSITION_TO_FW;
-
-				} else if (status.in_transition_mode) {
-					_msg.vtol_state = MAV_VTOL_STATE_TRANSITION_TO_MC;
-				}
+			if (status.is_vtol && _vtol_status_sub->update(&vtol_status)) {
+				_msg.vtol_state = vtol_status.vtol_state;
 			}
 		}
 

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -61,7 +61,7 @@
 #define AXIS_COUNT 3
 
 using namespace matrix;
-
+using namespace time_literals;
 
 int MulticopterAttitudeControl::print_usage(const char *reason)
 {
@@ -279,13 +279,14 @@ MulticopterAttitudeControl::vehicle_status_poll()
 
 		/* set correct uORB ID, depending on if vehicle is VTOL or not */
 		if (_rates_sp_id == nullptr) {
-			if (_vehicle_status.is_vtol) {
-				_rates_sp_id = ORB_ID(mc_virtual_rates_setpoint);
-				_actuators_id = ORB_ID(actuator_controls_virtual_mc);
-
-			} else {
-				_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
-				_actuators_id = ORB_ID(actuator_controls_0);
+			if (hrt_elapsed_time(&_vehicle_status.timestamp) < 1_s) {
+				if (_vehicle_status.is_vtol) {
+					_rates_sp_id = ORB_ID(mc_virtual_rates_setpoint);
+					_actuators_id = ORB_ID(actuator_controls_virtual_mc);
+				} else {
+					_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
+					_actuators_id = ORB_ID(actuator_controls_0);
+				}
 			}
 		}
 	}

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -80,6 +80,8 @@
 #include "PositionControl.hpp"
 #include "Utility/ControlMath.hpp"
 
+using namespace time_literals;
+
 #define SIGMA_SINGLE_OP			0.000001f
 #define SIGMA_NORM			0.001f
 /**
@@ -656,11 +658,13 @@ MulticopterPositionControl::poll_subscriptions()
 
 		/* set correct uORB ID, depending on if vehicle is VTOL or not */
 		if (!_attitude_setpoint_id) {
-			if (_vehicle_status.is_vtol) {
-				_attitude_setpoint_id = ORB_ID(mc_virtual_attitude_setpoint);
+			if (hrt_elapsed_time(&_vehicle_status.timestamp) < 1_s) {
+				if (_vehicle_status.is_vtol) {
+					_attitude_setpoint_id = ORB_ID(mc_virtual_attitude_setpoint);
 
-			} else {
-				_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
+				} else {
+					_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
+				}
 			}
 		}
 	}

src/modules/navigator/navigator_main.cpp

@@ -1036,7 +1036,7 @@ bool
 Navigator::force_vtol()
 {
 	return _vstatus.is_vtol &&
-	       (!_vstatus.is_rotary_wing || _vstatus.in_transition_to_fw)
+	       (!_vstatus.is_rotary_wing || (_vstatus.is_rotary_wing && _vstatus.in_transition_mode))
 	       && _param_force_vtol.get();
 }
 

src/modules/sdlog2/sdlog2.c

@@ -1567,8 +1567,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 			/* --- VTOL VEHICLE STATUS --- */
 			if(copy_if_updated(ORB_ID(vtol_vehicle_status), &subs.vtol_status_sub, &buf.vtol_status)) {
 				log_msg.msg_type = LOG_VTOL_MSG;
-				log_msg.body.log_VTOL.rw_mode = buf.vtol_status.vtol_in_rw_mode;
-				log_msg.body.log_VTOL.trans_mode = buf.vtol_status.vtol_in_trans_mode;
+				log_msg.body.log_VTOL.rw_mode = (buf.vtol_status.vtol_state != VEHICLE_VTOL_STATE_FW);
+				log_msg.body.log_VTOL.trans_mode = (buf.vtol_status.vtol_state == VEHICLE_VTOL_STATE_TRANSITION_TO_FW) || (buf.vtol_status.vtol_state == VEHICLE_VTOL_STATE_TRANSITION_TO_MC);
 				log_msg.body.log_VTOL.failsafe_mode = buf.vtol_status.vtol_transition_failsafe;
 				LOGBUFFER_WRITE_AND_COUNT(VTOL);
 			}