support orbit command in fixed wing mode

Signed-off-by: RomanBapst <bapstroman@gmail.com>

commander: support orbit mode for fixed wings

Signed-off-by: RomanBapst <bapstroman@gmail.com>

FwPositionControl: publish orbit status

Signed-off-by: RomanBapst <bapstroman@gmail.com>

commander:reject orbit mode while doing a vtol transition

Signed-off-by: RomanBapst <bapstroman@gmail.com>

FixedWingPositionControl: explicitly cast waypoint for Orbit status

FixedwingPositionControl: fill missing orbit_status fields

navigator_main: handle reposition/orbit corner cases

- set orbit rotation direction correctly
- send mavlink message when orbit is rejected

FixedWingPositionControl: correctly report rotation direction in orbit_status

navigator: hack to not break orbit while doing altitude changes

Signed-off-by: RomanBapst <bapstroman@gmail.com>

navigator: set cruise throttle for orbit command

Signed-off-by: RomanBapst <bapstroman@gmail.com>

src/modules/commander/Commander.cpp

@@ -1211,13 +1211,28 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 	case vehicle_command_s::VEHICLE_CMD_DO_ORBIT:
 
-		// Switch to orbit state and let the orbit task handle the command further
-		if (TRANSITION_DENIED != main_state_transition(_status, commander_state_s::MAIN_STATE_ORBIT, _status_flags,
-				_internal_state)) {
+		transition_result_t main_ret;
+
+		if (_status.in_transition_mode) {
+			main_ret = TRANSITION_DENIED;
+
+		} else if (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+			// for fixed wings the behavior of orbit is the same as loiter
+			main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER,
+							 _status_flags, _internal_state);
+
+		} else {
+			// Switch to orbit state and let the orbit task handle the command further
+			main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_ORBIT, _status_flags,
+							 _internal_state);
+		}
+
+		if ((main_ret != TRANSITION_DENIED)) {
 			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
 		} else {
 			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+			mavlink_log_critical(&_mavlink_log_pub, "Orbit command rejected");
 		}
 
 		break;

src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp

@@ -42,7 +42,7 @@
 #pragma once
 
 #include "FlightTaskManualAltitudeSmoothVel.hpp"
-#include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/orbit_status.h>
 #include "StraightLine.hpp"
 #include <lib/slew_rate/SlewRateYaw.hpp>
@@ -114,7 +114,7 @@ private:
 	float _initial_heading = 0.f; /**< the heading of the drone when the orbit command was issued */
 	SlewRateYaw<float> _slew_rate_yaw;
 
-	uORB::Publication<orbit_status_s> _orbit_status_pub{ORB_ID(orbit_status)};
+	uORB::PublicationMulti<orbit_status_s> _orbit_status_pub{ORB_ID(orbit_status)};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise, /**< cruise speed for circle approach */

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -763,6 +763,10 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
 	_att_sp.pitch_reset_integral = false;
 	_att_sp.yaw_reset_integral = false;
 
+	if (pos_sp_curr.valid && pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
+		publishOrbitStatus(pos_sp_curr);
+	}
+
 	const uint8_t position_sp_type = handle_setpoint_type(pos_sp_curr.type, pos_sp_curr);
 
 	_type = position_sp_type; // for publication
@@ -2200,6 +2204,19 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo
 	tecs_status_publish();
 }
 
+void FixedwingPositionControl::publishOrbitStatus(const position_setpoint_s pos_sp)
+{
+	orbit_status_s orbit_status{};
+	orbit_status.timestamp = hrt_absolute_time();
+	orbit_status.radius = static_cast<float>(pos_sp.loiter_direction) * pos_sp.loiter_radius;
+	orbit_status.frame = 0; // MAV_FRAME::MAV_FRAME_GLOBAL
+	orbit_status.x = static_cast<double>(pos_sp.lat);
+	orbit_status.y = static_cast<double>(pos_sp.lon);
+	orbit_status.z = pos_sp.alt;
+	orbit_status.yaw_behaviour = orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE;
+	_orbit_status_pub.publish(orbit_status);
+}
+
 int FixedwingPositionControl::task_spawn(int argc, char *argv[])
 {
 	bool vtol = false;

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -67,6 +67,7 @@
 #include <px4_platform_common/posix.h>
 #include <px4_platform_common/px4_work_queue/WorkItem.hpp>
 #include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/airspeed_validated.h>
@@ -87,6 +88,7 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/orbit_status.h>
 #include <uORB/uORB.h>
 #include <vtol_att_control/vtol_type.h>
 
@@ -156,6 +158,7 @@ private:
 	uORB::Publication<position_controller_status_s>		_pos_ctrl_status_pub{ORB_ID(position_controller_status)};			///< navigation capabilities publication
 	uORB::Publication<position_controller_landing_status_s>	_pos_ctrl_landing_status_pub{ORB_ID(position_controller_landing_status)};	///< landing status publication
 	uORB::Publication<tecs_status_s>			_tecs_status_pub{ORB_ID(tecs_status)};						///< TECS status publication
+	uORB::PublicationMulti<orbit_status_s>			_orbit_status_pub{ORB_ID(orbit_status)};
 
 	manual_control_setpoint_s	_manual_control_setpoint {};			///< r/c channel data
 	position_setpoint_triplet_s	_pos_sp_triplet {};		///< triplet of mission items
@@ -349,6 +352,8 @@ private:
 	Vector2f 	get_nav_speed_2d(const Vector2f &ground_speed);
 	void		set_control_mode_current(bool pos_sp_curr_valid);
 
+	void publishOrbitStatus(const position_setpoint_s pos_sp);
+
 	/*
 	 * Call TECS : a wrapper function to call the TECS implementation
 	 */

src/modules/mavlink/streams/ORBIT_EXECUTION_STATUS.hpp

@@ -49,34 +49,40 @@ public:
 
 	unsigned get_size() override
 	{
-		return _orbit_status_sub.advertised() ? MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
+		return _orbit_status_subs.advertised() ? MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
 	}
 
 private:
 	explicit MavlinkStreamOrbitStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {}
 
-	uORB::Subscription _orbit_status_sub{ORB_ID(orbit_status)};
+	uORB::SubscriptionMultiArray<orbit_status_s> _orbit_status_subs{ORB_ID::orbit_status};
 
 	bool send() override
 	{
 		orbit_status_s orbit_status;
+		bool updated = false;
 
-		if (_orbit_status_sub.update(&orbit_status)) {
-			mavlink_orbit_execution_status_t msg{};
+		for (auto &orbit_sub : _orbit_status_subs) {
 
-			msg.time_usec = orbit_status.timestamp;
-			msg.radius = orbit_status.radius;
-			msg.frame = orbit_status.frame;
-			msg.x = orbit_status.x * 1e7;
-			msg.y = orbit_status.y * 1e7;
-			msg.z = orbit_status.z;
+			if (orbit_sub.update(&orbit_status)) {
+				updated = true;
+				mavlink_orbit_execution_status_t msg_orbit_execution_status{};
 
-			mavlink_msg_orbit_execution_status_send_struct(_mavlink->get_channel(), &msg);
+				msg_orbit_execution_status.time_usec = orbit_status.timestamp;
+				msg_orbit_execution_status.radius = orbit_status.radius;
+				msg_orbit_execution_status.frame = orbit_status.frame;
+				msg_orbit_execution_status.x = orbit_status.x * 1e7;
+				msg_orbit_execution_status.y = orbit_status.y * 1e7;
+				msg_orbit_execution_status.z = orbit_status.z;
 
-			return true;
+				mavlink_msg_orbit_execution_status_send_struct(_mavlink->get_channel(), &msg_orbit_execution_status);
+
+				// only one subscription should ever be active at any time, so we can exit here
+				break;
+			}
 		}
 
-		return false;
+		return updated;
 	}
 };
 

src/modules/navigator/navigator.h

@@ -441,4 +441,6 @@ private:
 	void		publish_mission_result();
 
 	void		publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t result);
+
+	bool 		geofence_allows_position(const vehicle_global_position_s &pos);
 };

src/modules/navigator/navigator_main.cpp

@@ -247,26 +247,13 @@ Navigator::run()
 
 				bool reposition_valid = true;
 
-				if (have_geofence_position_data &&
-				    ((_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&
-				     (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_WARN))) {
+				vehicle_global_position_s position_setpoint{};
+				position_setpoint.lat = cmd.param5;
+				position_setpoint.lon = cmd.param6;
+				position_setpoint.alt = PX4_ISFINITE(cmd.param7) ? cmd.param7 : get_global_position()->alt;
 
-					if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) {
-
-						vehicle_global_position_s test_reposition_validity {};
-						test_reposition_validity.lat = cmd.param5;
-						test_reposition_validity.lon = cmd.param6;
-
-						if (PX4_ISFINITE(cmd.param7)) {
-							test_reposition_validity.alt = cmd.param7;
-
-						} else {
-							test_reposition_validity.alt = get_global_position()->alt;
-						}
-
-						reposition_valid = _geofence.check(test_reposition_validity, _gps_pos, _home_pos,
-										   home_position_valid());
-					}
+				if (have_geofence_position_data) {
+					reposition_valid = geofence_allows_position(position_setpoint);
 				}
 
 				if (reposition_valid) {
@@ -279,10 +266,11 @@ Navigator::run()
 					rep->previous.lon = get_global_position()->lon;
 					rep->previous.alt = get_global_position()->alt;
 
-					rep->current.loiter_radius = get_loiter_radius();
-					rep->current.loiter_direction = 1;
+
 					rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;
 
+					bool only_alt_change_requested = false;
+
 					// If no argument for ground speed, use default value.
 					if (cmd.param1 <= 0 || !PX4_ISFINITE(cmd.param1)) {
 						rep->current.cruising_speed = get_cruising_speed();
@@ -321,6 +309,7 @@ Navigator::run()
 						rep->current.lat = curr->current.lat;
 						rep->current.lon = curr->current.lon;
 						rep->current.alt = cmd.param7;
+						only_alt_change_requested = true;
 
 					} else {
 						// All three set to NaN - pause vehicle
@@ -354,6 +343,15 @@ Navigator::run()
 						}
 					}
 
+					if (only_alt_change_requested && PX4_ISFINITE(curr->current.loiter_radius) && curr->current.loiter_radius > 0) {
+						rep->current.loiter_radius = curr->current.loiter_radius;
+						rep->current.loiter_direction = curr->current.loiter_direction;
+
+					} else {
+						rep->current.loiter_radius = get_loiter_radius();
+						rep->current.loiter_direction = 1;
+					}
+
 					rep->previous.valid = true;
 					rep->previous.timestamp = hrt_absolute_time();
 
@@ -370,6 +368,45 @@ Navigator::run()
 
 				// CMD_DO_REPOSITION is acknowledged by commander
 
+			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_ORBIT &&
+				   get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+
+				// for multicopters the orbit command is directly executed by the orbit flighttask
+
+				bool orbit_location_valid = true;
+
+				vehicle_global_position_s position_setpoint{};
+				position_setpoint.lat = PX4_ISFINITE(cmd.param5) ? cmd.param5 : get_global_position()->lat;
+				position_setpoint.lon = PX4_ISFINITE(cmd.param6) ? cmd.param6 : get_global_position()->lon;
+				position_setpoint.alt = PX4_ISFINITE(cmd.param7) ? cmd.param7 : get_global_position()->alt;
+
+				if (have_geofence_position_data) {
+					orbit_location_valid = geofence_allows_position(position_setpoint);
+				}
+
+				if (orbit_location_valid) {
+					position_setpoint_triplet_s *rep = get_reposition_triplet();
+					rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;
+					rep->current.loiter_radius = get_loiter_radius();
+					rep->current.loiter_direction = 1;
+					rep->current.cruising_throttle = get_cruising_throttle();
+
+					if (PX4_ISFINITE(cmd.param1)) {
+						rep->current.loiter_radius = fabsf(cmd.param1);
+						rep->current.loiter_direction = math::signNoZero(cmd.param1);
+					}
+
+					rep->current.lat = position_setpoint.lat;
+					rep->current.lon = position_setpoint.lon;
+					rep->current.alt = position_setpoint.alt;
+
+					rep->current.valid = true;
+					rep->current.timestamp = hrt_absolute_time();
+
+				} else {
+					mavlink_log_critical(&_mavlink_log_pub, "Orbit is outside geofence");
+				}
+
 			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) {
 				position_setpoint_triplet_s *rep = get_takeoff_triplet();
 
@@ -1512,6 +1549,20 @@ Navigator::release_gimbal_control()
 	publish_vehicle_cmd(&vcmd);
 }
 
+bool Navigator::geofence_allows_position(const vehicle_global_position_s &pos)
+{
+	if ((_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&
+	    (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_WARN)) {
+
+		if (PX4_ISFINITE(pos.lat) && PX4_ISFINITE(pos.lon)) {
+			return _geofence.check(pos, _gps_pos, _home_pos,
+					       home_position_valid());
+		}
+	}
+
+	return true;
+}
+
 int Navigator::print_usage(const char *reason)
 {
 	if (reason) {