Add fw pos control states

F

src/modules/camera_feedback/CameraFeedback.cpp

@@ -70,28 +70,27 @@ CameraFeedback::Run()
 
 	camera_trigger_s trig{};
 
-	vehicle_attitude_s att{};
+	while (_trigger_sub.update(&trig)) {
 
-	while (_att_sub.update(&att)) {
 		// update geotagging subscriptions
 		vehicle_global_position_s gpos{};
 		_gpos_sub.copy(&gpos);
 
+		vehicle_attitude_s att{};
 		_att_sub.copy(&att);
-		_trigger_sub.copy(&trig);
 
-		// if (trig.timestamp == 0 ||
-		//     gpos.timestamp == 0 ||
-		//     att.timestamp == 0) {
+		if (trig.timestamp == 0 ||
+		    gpos.timestamp == 0 ||
+		    att.timestamp == 0) {
 
-		// 	// reject until we have valid data
-		// 	continue;
-		// }
+			// reject until we have valid data
+			continue;
+		}
 
-		// if ((_cam_cap_fback >= 1) && !trig.feedback) {
-		// 	// Ignore triggers that are not feedback when camera capture feedback is enabled
-		// 	continue;
-		// }
+		if ((_cam_cap_fback >= 1) && !trig.feedback) {
+			// Ignore triggers that are not feedback when camera capture feedback is enabled
+			continue;
+		}
 
 		camera_capture_s capture{};
 

src/modules/fw_path_navigation/FixedwingPositionControl.cpp

@@ -696,55 +696,42 @@ FixedwingPositionControl::updateManualTakeoffStatus()
 	}
 }
 
-void
-FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now)
+FW_POSCTRL_MODE
+FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now, const FW_POSCTRL_MODE &current_mode)
 {
 	/* only run position controller in fixed-wing mode and during transitions for VTOL */
 	if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.in_transition_mode) {
-		_control_mode_current = FW_POSCTRL_MODE_OTHER;
-		return; // do not publish the setpoint
+		return FW_POSCTRL_MODE_OTHER; // do not publish the setpoint
 	}
 
-	FW_POSCTRL_MODE commanded_position_control_mode = _control_mode_current;
-
 	_skipping_takeoff_detection = false;
 
-	if (_control_mode.flag_control_offboard_enabled && _position_setpoint_current_valid) {
-		if (_control_mode.flag_control_position_enabled) {
-			if (PX4_ISFINITE(_pos_sp_triplet.current.vx) && PX4_ISFINITE(_pos_sp_triplet.current.vy)
-			    && PX4_ISFINITE(_pos_sp_triplet.current.vz)) {
-				// Offboard position with velocity setpoints
-				_control_mode_current = FW_POSCTRL_MODE_AUTO_PATH;
-				return;
-
-			} else {
-				// Offboard position setpoint only
-				_control_mode_current = FW_POSCTRL_MODE_AUTO;
-				return;
-			}
-		}
+	if (_control_mode.flag_control_offboard_enabled && !_control_mode.flag_control_position_enabled
+	    && _control_mode.flag_control_velocity_enabled) {
+		return FW_POSCTRL_MODE_AUTO_VELOCITY;
 	}
 
-	if ((_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled)
-	    && _position_setpoint_current_valid) {
+
+	if (((_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled) ||
+	     _control_mode.flag_control_offboard_enabled) && _position_setpoint_current_valid) {
 
 		if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
 
 			if (_vehicle_status.is_vtol && _vehicle_status.in_transition_mode) {
-				_control_mode_current = FW_POSCTRL_MODE_AUTO;
-
 				// in this case we want the waypoint handled as a position setpoint -- a submode in control_auto()
 				_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
 
-			} else {
-				_control_mode_current = FW_POSCTRL_MODE_AUTO_TAKEOFF;
+				return FW_POSCTRL_MODE_AUTO;
 
-				if (commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_TAKEOFF && !_landed) {
+			} else {
+				if (current_mode != FW_POSCTRL_MODE_AUTO_TAKEOFF && !_landed) {
 					// skip takeoff detection when switching from any other mode, auto or manual,
 					// while already in air.
 					// TODO: find a better place for / way of doing this
 					_skipping_takeoff_detection = true;
 				}
+
+				return FW_POSCTRL_MODE_AUTO_TAKEOFF;
 			}
 
 		} else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
@@ -755,10 +742,10 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now)
 				// Straight landings are currently only possible in Missions, and there the previous WP
 				// is valid, and circular ones are used outside of Missions, as the land mode sets prev_valid=false.
 				if (_position_setpoint_previous_valid) {
-					_control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT;
+					return FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT;
 
 				} else {
-					_control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR;
+					return FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR;
 				}
 
 			} else {
@@ -767,7 +754,7 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now)
 			}
 
 		} else {
-			_control_mode_current = FW_POSCTRL_MODE_AUTO;
+			return FW_POSCTRL_MODE_AUTO;
 		}
 
 	} else if (_control_mode.flag_control_auto_enabled
@@ -777,33 +764,33 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now)
 
 		// failsafe modes engaged if position estimate is invalidated
 
-		if (commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_ALTITUDE
-		    && commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_CLIMBRATE) {
+		if (current_mode != FW_POSCTRL_MODE_AUTO_ALTITUDE
+		    && current_mode != FW_POSCTRL_MODE_AUTO_CLIMBRATE) {
 			// reset timer the first time we switch into this mode
 			_time_in_fixed_bank_loiter = now;
 		}
 
 		if (hrt_elapsed_time(&_time_in_fixed_bank_loiter) < (_param_nav_gpsf_lt.get() * 1_s)
 		    && !_vehicle_status.in_transition_mode) {
-			if (commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_ALTITUDE) {
+			if (current_mode != FW_POSCTRL_MODE_AUTO_ALTITUDE) {
 				// Need to init because last loop iteration was in a different mode
 				events::send(events::ID("fixedwing_position_control_fb_loiter"), events::Log::Critical,
 					     "Start loiter with fixed bank angle");
 			}
 
-			_control_mode_current = FW_POSCTRL_MODE_AUTO_ALTITUDE;
+			return FW_POSCTRL_MODE_AUTO_ALTITUDE;
 
 		} else {
-			if (commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_CLIMBRATE && !_vehicle_status.in_transition_mode) {
+			if (current_mode != FW_POSCTRL_MODE_AUTO_CLIMBRATE && !_vehicle_status.in_transition_mode) {
 				events::send(events::ID("fixedwing_position_control_descend"), events::Log::Critical, "Start descending");
 			}
 
-			_control_mode_current = FW_POSCTRL_MODE_AUTO_CLIMBRATE;
+			return FW_POSCTRL_MODE_AUTO_CLIMBRATE;
 		}
 
 
 	} else if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_position_enabled) {
-		if (commanded_position_control_mode != FW_POSCTRL_MODE_MANUAL_POSITION) {
+		if (current_mode != FW_POSCTRL_MODE_MANUAL_POSITION) {
 			/* Need to init because last loop iteration was in a different mode */
 			_hdg_hold_yaw = _yaw; // yaw is not controlled, so set setpoint to current yaw
 			_hdg_hold_enabled = false; // this makes sure the waypoints are reset below
@@ -815,15 +802,15 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now)
 			_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
 		}
 
-		_control_mode_current = FW_POSCTRL_MODE_MANUAL_POSITION;
+		return FW_POSCTRL_MODE_MANUAL_POSITION;
 
 	} else if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_altitude_enabled) {
 
-		_control_mode_current = FW_POSCTRL_MODE_MANUAL_ALTITUDE;
+		return FW_POSCTRL_MODE_MANUAL_ALTITUDE;
 
-	} else {
-		_control_mode_current = FW_POSCTRL_MODE_OTHER;
 	}
+
+	return FW_POSCTRL_MODE_OTHER;
 }
 
 void
@@ -880,61 +867,6 @@ FixedwingPositionControl::move_position_setpoint_for_vtol_transition(position_se
 	}
 }
 
-void
-FixedwingPositionControl::control_auto(const float control_interval, const Vector2d &curr_pos,
-				       const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr,
-				       const position_setpoint_s &pos_sp_next)
-{
-	position_setpoint_s current_sp = pos_sp_curr;
-	move_position_setpoint_for_vtol_transition(current_sp);
-
-	const uint8_t position_sp_type = handle_setpoint_type(current_sp);
-
-	_position_sp_type = position_sp_type;
-
-	if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_LOITER
-	    || current_sp.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
-		publishOrbitStatus(current_sp);
-	}
-
-	switch (position_sp_type) {
-	case position_setpoint_s::SETPOINT_TYPE_IDLE:
-		_att_sp.thrust_body[0] = 0.0f;
-		_att_sp.roll_body = 0.0f;
-		_att_sp.pitch_body = radians(_param_fw_psp_off.get());
-		break;
-
-	case position_setpoint_s::SETPOINT_TYPE_POSITION:
-		control_auto_position(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp);
-		break;
-
-	case position_setpoint_s::SETPOINT_TYPE_VELOCITY:
-		control_auto_velocity(control_interval, curr_pos, ground_speed, current_sp);
-		break;
-
-	case position_setpoint_s::SETPOINT_TYPE_LOITER:
-		control_auto_loiter(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp, pos_sp_next);
-		break;
-	}
-
-	/* Copy thrust output for publication, handle special cases */
-	if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
-
-		_att_sp.thrust_body[0] = 0.0f;
-
-	} else {
-		// when we are landed state we want the motor to spin at idle speed
-		_att_sp.thrust_body[0] = (_landed) ? min(_param_fw_thr_idle.get(), 1.f) : get_tecs_thrust();
-	}
-
-	/* Copy thrust and pitch values from tecs */
-	_att_sp.pitch_body = get_tecs_pitch();
-
-	if (!_vehicle_status.in_transition_to_fw) {
-		publishLocalPositionSetpoint(current_sp);
-	}
-}
-
 void
 FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_interval)
 {
@@ -995,10 +927,6 @@ FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp
 {
 	uint8_t position_sp_type = pos_sp_curr.type;
 
-	if (!_control_mode.flag_control_position_enabled && _control_mode.flag_control_velocity_enabled) {
-		return position_setpoint_s::SETPOINT_TYPE_VELOCITY;
-	}
-
 	Vector2d curr_wp{0, 0};
 
 	/* current waypoint (the one currently heading for) */
@@ -1038,6 +966,8 @@ void
 FixedwingPositionControl::control_auto_position(const float control_interval, const Vector2d &curr_pos,
 		const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
 {
+	move_position_setpoint_for_vtol_transition(pos_sp_curr);
+
 	const float acc_rad = _npfg.switchDistance(500.0f);
 	Vector2d curr_wp{0, 0};
 	Vector2d prev_wp{0, 0};
@@ -1113,7 +1043,17 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
 	_npfg.setAirspeedNom(target_airspeed * _eas2tas);
 	_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
 
-	navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
+	if (_control_mode.flag_control_offboard_enabled && PX4_ISFINITE(pos_sp_curr.vx) && PX4_ISFINITE(pos_sp_curr.vy)) {
+		// Navigate directly on position setpoint and path tangent
+		matrix::Vector2f velocity_2d(pos_sp_curr.vx, pos_sp_curr.vy);
+		float curvature = PX4_ISFINITE(_pos_sp_triplet.current.loiter_radius) ? 1 / _pos_sp_triplet.current.loiter_radius :
+				  0.0f;
+		navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(), ground_speed,
+				    _wind_vel, curvature);
+
+	} else {
+		navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
+	}
 
 	_att_sp.roll_body = _npfg.getRollSetpoint();
 	target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
@@ -1129,6 +1069,19 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
 				   tecs_fw_thr_max,
 				   _param_sinkrate_target.get(),
 				   _param_climbrate_target.get());
+
+	if (_landed) {
+		_att_sp.thrust_body[0] = _param_fw_thr_min.get();
+
+	} else {
+		_att_sp.thrust_body[0] = min(get_tecs_thrust(), _param_fw_thr_max.get());
+	}
+
+	_att_sp.pitch_body = get_tecs_pitch();
+
+	if (!_vehicle_status.in_transition_to_fw) {
+		publishLocalPositionSetpoint(pos_sp_curr);
+	}
 }
 
 void
@@ -1187,6 +1140,8 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 		const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr,
 		const position_setpoint_s &pos_sp_next)
 {
+	move_position_setpoint_for_vtol_transition(pos_sp_curr);
+
 	Vector2d curr_wp{0, 0};
 	Vector2d prev_wp{0, 0};
 
@@ -1286,57 +1241,20 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 				   tecs_fw_thr_max,
 				   _param_sinkrate_target.get(),
 				   _param_climbrate_target.get());
-}
 
-void
-FixedwingPositionControl::control_auto_path(const float control_interval, const Vector2d &curr_pos,
-		const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr)
-{
 
-	float tecs_fw_thr_min;
-	float tecs_fw_thr_max;
-
-	if (pos_sp_curr.gliding_enabled) {
-		/* enable gliding with this waypoint */
-		_tecs.set_speed_weight(2.0f);
-		tecs_fw_thr_min = 0.0;
-		tecs_fw_thr_max = 0.0;
+	if (_landed) {
+		_att_sp.thrust_body[0] = _param_fw_thr_min.get();
 
 	} else {
-		tecs_fw_thr_min = _param_fw_thr_min.get();
-		tecs_fw_thr_max = _param_fw_thr_max.get();
+		_att_sp.thrust_body[0] = min(get_tecs_thrust(), _param_fw_thr_max.get());
 	}
 
-	// waypoint is a plain navigation waypoint
-	float target_airspeed = adapt_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed,
-				_param_fw_airspd_min.get(), ground_speed);
+	_att_sp.pitch_body = get_tecs_pitch();
 
-	Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
-	Vector2f curr_wp_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon);
-
-	_npfg.setAirspeedNom(target_airspeed * _eas2tas);
-	_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
-
-	// Navigate directly on position setpoint and path tangent
-	matrix::Vector2f velocity_2d(pos_sp_curr.vx, pos_sp_curr.vy);
-	float curvature = PX4_ISFINITE(_pos_sp_triplet.current.loiter_radius) ? 1 / _pos_sp_triplet.current.loiter_radius :
-			  0.0f;
-	navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(), ground_speed, _wind_vel, curvature);
-
-	_att_sp.roll_body = _npfg.getRollSetpoint();
-	target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
-
-	_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
-
-	tecs_update_pitch_throttle(control_interval,
-				   pos_sp_curr.alt,
-				   target_airspeed,
-				   radians(_param_fw_p_lim_min.get()),
-				   radians(_param_fw_p_lim_max.get()),
-				   tecs_fw_thr_min,
-				   tecs_fw_thr_max,
-				   _param_sinkrate_target.get(),
-				   _param_climbrate_target.get());
+	if (!_vehicle_status.in_transition_to_fw) {
+		publishLocalPositionSetpoint(pos_sp_curr);
+	}
 }
 
 void
@@ -2336,7 +2254,7 @@ FixedwingPositionControl::Run()
 		Vector2d curr_pos(_current_latitude, _current_longitude);
 		Vector2f ground_speed(_local_pos.vx, _local_pos.vy);
 
-		set_control_mode_current(_local_pos.timestamp);
+		_control_mode_current = set_control_mode_current(_local_pos.timestamp, _control_mode_current);
 
 		update_in_air_states(_local_pos.timestamp);
 
@@ -2375,12 +2293,6 @@ FixedwingPositionControl::Run()
 		_new_landing_gear_position = landing_gear_s::GEAR_KEEP; // is overwritten in Takeoff and Land
 
 		switch (_control_mode_current) {
-		case FW_POSCTRL_MODE_AUTO: {
-				control_auto(control_interval, curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current,
-					     _pos_sp_triplet.next);
-				break;
-			}
-
 		case FW_POSCTRL_MODE_AUTO_ALTITUDE: {
 				control_auto_fixed_bank_alt_hold(control_interval);
 				break;
@@ -2402,8 +2314,14 @@ FixedwingPositionControl::Run()
 				break;
 			}
 
-		case FW_POSCTRL_MODE_AUTO_PATH: {
-				control_auto_path(control_interval, curr_pos, ground_speed, _pos_sp_triplet.current);
+		case FW_POSCTRL_MODE_AUTO_LOITER: {
+				control_auto_loiter(control_interval, curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current,
+						    _pos_sp_triplet.next);
+				break;
+			}
+
+		case FW_POSCTRL_MODE_AUTO_POSITION: {
+				control_auto_position(control_interval, curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current);
 				break;
 			}
 
@@ -2412,6 +2330,11 @@ FixedwingPositionControl::Run()
 				break;
 			}
 
+		case FW_POSCTRL_MODE_AUTO_VELOCITY: {
+				control_auto_velocity(control_interval, curr_pos, ground_speed, _pos_sp_triplet.current);
+				break;
+			}
+
 		case FW_POSCTRL_MODE_MANUAL_POSITION: {
 				control_manual_position(control_interval, curr_pos, ground_speed);
 				break;

src/modules/fw_path_navigation/FixedwingPositionControl.hpp

@@ -167,6 +167,20 @@ static constexpr float MANUAL_TOUCHDOWN_NUDGE_INPUT_DEADZONE = 0.15f;
 // [s] time interval after touchdown for ramping in runway clamping constraints (touchdown is assumed at FW_LND_TD_TIME after start of flare)
 static constexpr float POST_TOUCHDOWN_CLAMP_TIME = 0.5f;
 
+enum FW_POSCTRL_MODE {
+	FW_POSCTRL_MODE_AUTO_ALTITUDE,
+	FW_POSCTRL_MODE_AUTO_CLIMBRATE,
+	FW_POSCTRL_MODE_AUTO_TAKEOFF,
+	FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT,
+	FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR,
+	FW_POSCTRL_MODE_AUTO_LOITER,
+	FW_POSCTRL_MODE_AUTO_POSITION,
+	FW_POSCTRL_MODE_AUTO_VELOCITY,
+	FW_POSCTRL_MODE_MANUAL_POSITION,
+	FW_POSCTRL_MODE_MANUAL_ALTITUDE,
+	FW_POSCTRL_MODE_OTHER
+};
+
 class FixedwingPositionControl final : public ModuleBase<FixedwingPositionControl>, public ModuleParams,
 	public px4::WorkItem
 {
@@ -236,18 +250,7 @@ private:
 
 	uint8_t _position_sp_type{0};
 
-	enum FW_POSCTRL_MODE {
-		FW_POSCTRL_MODE_AUTO,
-		FW_POSCTRL_MODE_AUTO_ALTITUDE,
-		FW_POSCTRL_MODE_AUTO_CLIMBRATE,
-		FW_POSCTRL_MODE_AUTO_TAKEOFF,
-		FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT,
-		FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR,
-		FW_POSCTRL_MODE_AUTO_PATH,
-		FW_POSCTRL_MODE_MANUAL_POSITION,
-		FW_POSCTRL_MODE_MANUAL_ALTITUDE,
-		FW_POSCTRL_MODE_OTHER
-	} _control_mode_current{FW_POSCTRL_MODE_OTHER}; // used to check if the mode has changed
+	FW_POSCTRL_MODE _control_mode_current{FW_POSCTRL_MODE_OTHER}; // used to check if the mode has changed
 
 	enum StickConfig {
 		STICK_CONFIG_SWAP_STICKS_BIT = (1 << 0),
@@ -530,19 +533,6 @@ private:
 
 	/* automatic control methods */
 
-	/**
-	 * @brief Automatic position control for waypoints, orbits, and velocity control
-	 *
-	 * @param control_interval Time since last position control call [s]
-	 * @param curr_pos Current 2D local position vector of vehicle [m]
-	 * @param ground_speed Local 2D ground speed of vehicle [m/s]
-	 * @param pos_sp_prev previous position setpoint
-	 * @param pos_sp_curr current position setpoint
-	 * @param pos_sp_next next position setpoint
-	 */
-	void control_auto(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed,
-			  const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next);
-
 	/**
 	 * @brief Controls altitude and airspeed for a fixed-bank loiter.
 	 *
@@ -586,18 +576,6 @@ private:
 	void control_auto_loiter(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed,
 				 const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next);
 
-	/**
-	 * @brief Vehicle control for following a path.
-	 *
-	 * @param control_interval Time since last position control call [s]
-	 * @param curr_pos Current 2D local position vector of vehicle [m]
-	 * @param ground_speed Local 2D ground speed of vehicle [m/s]
-	 * @param pos_sp_prev previous position setpoint
-	 * @param pos_sp_curr current position setpoint
-	 */
-	void control_auto_path(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed,
-			       const position_setpoint_s &pos_sp_curr);
-
 	/**
 	 * @brief Controls a desired airspeed, bearing, and height rate.
 	 *
@@ -698,8 +676,10 @@ private:
 	 * May also change the position setpoint type depending on the desired behavior.
 	 *
 	 * @param now Current system time [us]
+	 * @param current_mode Current flight mode
+	 * @return FW_POSCTRL_MODE Next flight mode to transition to
 	 */
-	void set_control_mode_current(const hrt_abstime &now);
+	FW_POSCTRL_MODE set_control_mode_current(const hrt_abstime &now, const FW_POSCTRL_MODE &current_mode);
 
 	/**
 	 * @brief Compensate trim throttle for air density and vehicle weight.

src/modules/logger/logged_topics.cpp

@@ -51,7 +51,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("airspeed", 1000);
 	add_optional_topic("airspeed_validated", 200);
 	add_optional_topic("autotune_attitude_control_status", 100);
-	add_topic("camera_capture");
+	add_optional_topic("camera_capture");
 	add_optional_topic("camera_trigger");
 	add_topic("cellular_status", 200);
 	add_topic("commander_state");