output drivers: init SmartLock after exit_and_cleanup

This fixes an invalid memory access when exiting the module:
exit_and_cleanup destroys the object, but lock_guard is destructed after
and accesses the lock.

src/drivers/dshot/DShot.cpp

@@ -47,7 +47,11 @@ DShot::DShot() :
 	_mixing_output.setAllDisarmedValues(DSHOT_DISARM_VALUE);
 	_mixing_output.setAllMinValues(DSHOT_MIN_THROTTLE);
 	_mixing_output.setAllMaxValues(DSHOT_MAX_THROTTLE);
-	_mixing_output.setAllFailsafeValues(UINT16_MAX);
+
+	if (_mixing_output.useDynamicMixing()) {
+		// Avoid using the PWM failsafe params
+		_mixing_output.setAllFailsafeValues(UINT16_MAX);
+	}
 }
 
 DShot::~DShot()
@@ -489,8 +493,6 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 
 void DShot::Run()
 {
-	SmartLock lock_guard(_lock);
-
 	if (should_exit()) {
 		ScheduleClear();
 		_mixing_output.unregister();
@@ -499,6 +501,8 @@ void DShot::Run()
 		return;
 	}
 
+	SmartLock lock_guard(_lock);
+
 	perf_begin(_cycle_perf);
 
 	_mixing_output.update();

src/drivers/linux_pwm_out/linux_pwm_out.cpp

@@ -126,8 +126,6 @@ bool LinuxPWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS
 
 void LinuxPWMOut::Run()
 {
-	SmartLock lock_guard(_lock);
-
 	if (should_exit()) {
 		ScheduleClear();
 		_mixing_output.unregister();
@@ -136,6 +134,8 @@ void LinuxPWMOut::Run()
 		return;
 	}
 
+	SmartLock lock_guard(_lock);
+
 	perf_begin(_cycle_perf);
 	perf_count(_interval_perf);
 

src/drivers/pca9685_pwm_out/main.cpp

@@ -376,8 +376,6 @@ bool PCA9685Wrapper::updateOutputs(bool stop_motors, uint16_t *outputs, unsigned
 
 void PCA9685Wrapper::Run()
 {
-	SmartLock lock_guard(_lock);
-
 	if (should_exit()) {
 		ScheduleClear();
 		_mixing_output.unregister();
@@ -391,6 +389,8 @@ void PCA9685Wrapper::Run()
 		return;
 	}
 
+	SmartLock lock_guard(_lock);
+
 	perf_begin(_cycle_perf);
 
 	switch (_state) {

src/drivers/pwm_out/PWMOut.cpp

@@ -439,8 +439,6 @@ bool PWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 
 void PWMOut::Run()
 {
-	SmartLock lock_guard(_lock);
-
 	if (should_exit()) {
 		ScheduleClear();
 		_mixing_output.unregister();
@@ -449,6 +447,8 @@ void PWMOut::Run()
 		return;
 	}
 
+	SmartLock lock_guard(_lock);
+
 	perf_begin(_cycle_perf);
 	perf_count(_interval_perf);
 

src/drivers/pwm_out_sim/PWMSim.cpp

@@ -58,8 +58,6 @@ PWMSim::PWMSim(bool hil_mode_enabled) :
 void
 PWMSim::Run()
 {
-	SmartLock lock_guard(_lock);
-
 	if (should_exit()) {
 		ScheduleClear();
 		_mixing_output.unregister();
@@ -68,6 +66,8 @@ PWMSim::Run()
 		return;
 	}
 
+	SmartLock lock_guard(_lock);
+
 	_mixing_output.update();
 
 	// check for parameter updates

src/drivers/px4io/px4io.cpp

@@ -525,8 +525,6 @@ void PX4IO::updateFailsafe()
 
 void PX4IO::Run()
 {
-	SmartLock lock_guard(_lock);
-
 	if (should_exit()) {
 		ScheduleClear();
 		_mixing_output.unregister();
@@ -535,6 +533,8 @@ void PX4IO::Run()
 		return;
 	}
 
+	SmartLock lock_guard(_lock);
+
 	perf_begin(_cycle_perf);
 	perf_count(_interval_perf);
 

src/drivers/rc_input/RCInput.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -48,12 +48,6 @@ RCInput::RCInput(const char *device) :
 	_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")),
 	_publish_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval"))
 {
-	// rc input, published to ORB
-	_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
-
-	// initialize it as RC lost
-	_rc_in.rc_lost = true;
-
 	// initialize raw_rc values and count
 	for (unsigned i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; i++) {
 		_raw_rc_values[i] = UINT16_MAX;
@@ -63,6 +57,10 @@ RCInput::RCInput(const char *device) :
 		strncpy(_device, device, sizeof(_device) - 1);
 		_device[sizeof(_device) - 1] = '\0';
 	}
+
+	if ((_param_rc_input_proto.get() >= 0) && (_param_rc_input_proto.get() <= RC_SCAN::RC_SCAN_GHST)) {
+		_rc_scan_state = static_cast<RC_SCAN>(_param_rc_input_proto.get());
+	}
 }
 
 RCInput::~RCInput()
@@ -112,6 +110,8 @@ RCInput::init()
 	px4_arch_unconfiggpio(GPIO_PPM_IN);
 #endif // GPIO_PPM_IN
 
+	rc_io_invert(false);
+
 	return 0;
 }
 
@@ -251,12 +251,22 @@ RCInput::fill_rc_in(uint16_t raw_rc_count_local,
 
 void RCInput::set_rc_scan_state(RC_SCAN newState)
 {
-	PX4_DEBUG("RCscan: %s failed, trying %s", RCInput::RC_SCAN_STRING[_rc_scan_state], RCInput::RC_SCAN_STRING[newState]);
-	_rc_scan_begin = 0;
-	_rc_scan_state = newState;
-	_rc_scan_locked = false;
+	if ((_param_rc_input_proto.get() > RC_SCAN::RC_SCAN_NONE)
+	    && (_param_rc_input_proto.get() <= RC_SCAN::RC_SCAN_GHST)) {
 
-	_report_lock = true;
+		_rc_scan_state = static_cast<RC_SCAN>(_param_rc_input_proto.get());
+
+	} else if (_param_rc_input_proto.get() < 0) {
+		// only auto change if RC_INPUT_PROTO set to auto (-1)
+		PX4_DEBUG("RCscan: %s failed, trying %s", RCInput::RC_SCAN_STRING[_rc_scan_state], RCInput::RC_SCAN_STRING[newState]);
+		_rc_scan_state = newState;
+
+	} else {
+		_rc_scan_state = RC_SCAN::RC_SCAN_NONE;
+	}
+
+	_rc_scan_begin = 0;
+	_rc_scan_locked = false;
 }
 
 void RCInput::rc_io_invert(bool invert)
@@ -401,8 +411,8 @@ void RCInput::Run()
 		bool rc_updated = false;
 
 		// This block scans for a supported serial RC input and locks onto the first one found
-		// Scan for 300 msec, then switch protocol
-		constexpr hrt_abstime rc_scan_max = 300_ms;
+		// Scan for 500 msec, then switch protocol
+		constexpr hrt_abstime rc_scan_max = 500_ms;
 
 		unsigned frame_drops = 0;
 
@@ -418,7 +428,13 @@ void RCInput::Run()
 			_bytes_rx += newBytes;
 		}
 
+		const bool rc_scan_locked = _rc_scan_locked;
+
 		switch (_rc_scan_state) {
+		case RC_SCAN_NONE:
+			// do nothing
+			break;
+
 		case RC_SCAN_SBUS:
 			if (_rc_scan_begin == 0) {
 				_rc_scan_begin = cycle_timestamp;
@@ -734,10 +750,19 @@ void RCInput::Run()
 			_rc_scan_locked = false;
 		}
 
-		if (_report_lock && _rc_scan_locked) {
-			_report_lock = false;
+		if (!rc_scan_locked && _rc_scan_locked) {
 			PX4_INFO("RC scan: %s RC input locked", RC_SCAN_STRING[_rc_scan_state]);
 		}
+
+		// set RC_INPUT_PROTO if RC successfully locked for > 3 seconds
+		if (!_armed && rc_updated && _rc_scan_locked
+		    && ((_rc_scan_begin != 0) && hrt_elapsed_time(&_rc_scan_begin) > 3_s)
+		    && (_param_rc_input_proto.get() < 0)
+		   ) {
+			// RC_INPUT_PROTO auto => locked selection
+			_param_rc_input_proto.set(_rc_scan_state);
+			_param_rc_input_proto.commit();
+		}
 	}
 }
 
@@ -823,6 +848,9 @@ int RCInput::print_status()
 
 	if (_rc_scan_locked) {
 		switch (_rc_scan_state) {
+		case RC_SCAN_NONE:
+			break;
+
 		case RC_SCAN_CRSF:
 			PX4_INFO("CRSF Telemetry: %s", _crsf_telemetry ? "yes" : "no");
 			break;

src/drivers/rc_input/RCInput.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2019, 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -91,21 +91,23 @@ public:
 private:
 
 	enum RC_SCAN {
-		RC_SCAN_PPM = 0,
-		RC_SCAN_SBUS,
-		RC_SCAN_DSM,
-		RC_SCAN_SUMD,
-		RC_SCAN_ST24,
-		RC_SCAN_CRSF,
-		RC_SCAN_GHST
+		RC_SCAN_NONE = 0,
+		RC_SCAN_PPM  = 1,
+		RC_SCAN_SBUS = 2,
+		RC_SCAN_DSM  = 3,
+		RC_SCAN_ST24 = 5,
+		RC_SCAN_SUMD = 4,
+		RC_SCAN_CRSF = 6,
+		RC_SCAN_GHST = 7,
 	} _rc_scan_state{RC_SCAN_SBUS};
 
-	static constexpr char const *RC_SCAN_STRING[7] {
+	static constexpr char const *RC_SCAN_STRING[] {
+		"None",
 		"PPM",
 		"SBUS",
 		"DSM",
-		"SUMD",
 		"ST24",
+		"SUMD",
 		"CRSF",
 		"GHST"
 	};
@@ -129,7 +131,6 @@ private:
 
 	bool _initialized{false};
 	bool _rc_scan_locked{false};
-	bool _report_lock{true};
 
 	static constexpr unsigned	_current_update_interval{4000}; // 250 Hz
 
@@ -168,6 +169,7 @@ private:
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::RC_RSSI_PWM_CHAN>) _param_rc_rssi_pwm_chan,
 		(ParamInt<px4::params::RC_RSSI_PWM_MIN>) _param_rc_rssi_pwm_min,
-		(ParamInt<px4::params::RC_RSSI_PWM_MAX>) _param_rc_rssi_pwm_max
+		(ParamInt<px4::params::RC_RSSI_PWM_MAX>) _param_rc_rssi_pwm_max,
+		(ParamInt<px4::params::RC_INPUT_PROTO>) _param_rc_input_proto
 	)
 };

src/drivers/rc_input/module.yaml

@@ -1,4 +1,28 @@
 module_name: RC Input Driver
+parameters:
+    - group: RC Input
+      definitions:
+        RC_INPUT_PROTO:
+            description:
+                short: RC input protocol
+                long: |
+                    Select your RC input protocol or auto to scan.
+            category: System
+            type: enum
+            values:
+                -1: Auto
+                0: None
+                1: PPM
+                2: SBUS
+                3: DSM
+                4: ST24
+                5: SUMD
+                6: CRSF
+                7: GHST
+            min: -1
+            max: 7
+            default: -1
+
 serial_config:
     - command: set RC_INPUT_ARGS "-d ${SERIAL_DEV}"
       port_config_param:

src/drivers/tap_esc/TAP_ESC.cpp

@@ -327,8 +327,6 @@ bool TAP_ESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], u
 
 void TAP_ESC::Run()
 {
-	SmartLock lock_guard(_lock);
-
 	if (should_exit()) {
 		ScheduleClear();
 		_mixing_output.unregister();
@@ -337,6 +335,8 @@ void TAP_ESC::Run()
 		return;
 	}
 
+	SmartLock lock_guard(_lock);
+
 	// push backup schedule
 	ScheduleDelayed(20_ms);
 

src/drivers/uavcan/CMakeLists.txt

@@ -51,7 +51,7 @@ if(CONFIG_ARCH_CHIP)
 		endif()
 	elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32")
 		set(UAVCAN_DRIVER "stm32")
-		set(UAVCAN_TIMER 5) # The default timer is TIM5
+		set(UAVCAN_TIMER 6) # The default timer is TIM6
 		if (DEFINED config_uavcan_timer_override)
 			set (UAVCAN_TIMER  ${config_uavcan_timer_override})
 		endif()

src/drivers/uavcan/sensors/battery.cpp

@@ -87,7 +87,7 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::
 	battery_status[instance].voltage_filtered_v = msg.voltage;
 	battery_status[instance].current_a = msg.current;
 	battery_status[instance].current_filtered_a = msg.current;
-	// battery_status[instance].current_average_a = msg.;
+	battery_status[instance].current_average_a = msg.current;
 
 	if (battery_aux_support[instance] == false) {
 		sumDischarged(battery_status[instance].timestamp, battery_status[instance].current_a);
@@ -101,11 +101,11 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::
 	battery_status[instance].connected = true;
 	battery_status[instance].source = msg.status_flags & uavcan::equipment::power::BatteryInfo::STATUS_FLAG_IN_USE;
 	// battery_status[instance].priority = msg.;
-	// battery_status[instance].capacity = msg.;
+	battery_status[instance].capacity = msg.full_charge_capacity_wh;
 	battery_status[instance].full_charge_capacity_wh = msg.full_charge_capacity_wh;
 	battery_status[instance].remaining_capacity_wh = msg.remaining_capacity_wh;
 	// battery_status[instance].cycle_count = msg.;
-	// battery_status[instance].time_remaining_s = msg.;
+	battery_status[instance].time_remaining_s = NAN;
 	// battery_status[instance].average_time_to_empty = msg.;
 	battery_status[instance].serial_number = msg.model_instance_id;
 	battery_status[instance].id = msg.getSrcNodeID().get();

src/drivers/uavcan/sensors/battery.hpp

@@ -85,5 +85,5 @@ private:
 	uint8_t _warning;
 	hrt_abstime _last_timestamp;
 	battery_status_s battery_status[battery_status_s::MAX_INSTANCES] {};
-	bool battery_aux_support[battery_status_s::MAX_INSTANCES] {};
+	bool battery_aux_support[battery_status_s::MAX_INSTANCES] {false};
 };

src/modules/commander/Commander.cpp

@@ -3868,11 +3868,16 @@ void Commander::battery_status_check()
 			}
 		}
 
-		_last_battery_mode[index] = battery.mode;
-
 		if (battery.connected) {
 			_last_connected_batteries |= 1 << index;
 
+		} else {
+			_last_connected_batteries &= ~(1 << index);
+		}
+
+		_last_battery_mode[index] = battery.mode;
+
+		if (battery.connected) {
 			if (battery.warning > worst_warning) {
 				worst_warning = battery.warning;
 			}

src/modules/commander/state_machine_helper.cpp

@@ -755,12 +755,13 @@ void set_link_loss_nav_state(vehicle_status_s &status, actuator_armed_s &armed,
 		} else {
 			if (status_flags.local_position_valid) {
 				status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
+				return;
 
 			} else if (status_flags.local_altitude_valid) {
 				status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+				return;
 			}
 
-			return;
 		}
 
 	// FALLTHROUGH

src/modules/ekf2/EKF/ekf.h

@@ -742,24 +742,20 @@ private:
 	template <size_t ...Idxs>
 	SquareMatrix24f computeKHP(const Vector24f &K, const SparseVector24f<Idxs...> &H) const
 	{
-		SquareMatrix24f KHP;
-		constexpr size_t non_zeros = sizeof...(Idxs);
-		float KH[non_zeros];
-
-		for (unsigned row = 0; row < _k_num_states; row++) {
-			for (unsigned i = 0; i < H.non_zeros(); i++) {
-				KH[i] = K(row) * H.atCompressedIndex(i);
+		// K(HP) and (KH)P are equivalent (matrix multiplication is associative)
+		// but K(HP) is computationally much less expensive
+		Vector24f HP;
+		for (unsigned i = 0; i < H.non_zeros(); i++) {
+			const size_t row = H.index(i);
+			for (unsigned col = 0; col < _k_num_states; col++) {
+				HP(col) = HP(col) + H.atCompressedIndex(i) * P(row, col);
 			}
+		}
 
-			for (unsigned column = 0; column < _k_num_states; column++) {
-				float tmp = 0.f;
-
-				for (unsigned i = 0; i < H.non_zeros(); i++) {
-					const size_t index = H.index(i);
-					tmp += KH[i] * P(index, column);
-				}
-
-				KHP(row, column) = tmp;
+		SquareMatrix24f KHP;
+		for (unsigned row = 0; row < _k_num_states; row++) {
+			for (unsigned col = 0; col < _k_num_states; col++) {
+				KHP(row, col) = K(row) * HP(col);
 			}
 		}
 

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -755,19 +755,28 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now, bool
 
 	if (((_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled) ||
 	     _control_mode.flag_control_offboard_enabled) && pos_sp_curr_valid) {
+
 		if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
-			// TAKEOFF: handle like a regular POSITION setpoint if already flying
-			if (!in_takeoff_situation() && (_airspeed >= _param_fw_airspd_min.get() || !_airspeed_valid)) {
-				// SETPOINT_TYPE_TAKEOFF -> SETPOINT_TYPE_POSITION
+
+			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;
 			}
 
-		} else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND
-			   && !_vehicle_status.in_transition_mode) {
-			_control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING;
+		} else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
+
+			if (!_vehicle_status.in_transition_mode) {
+				_control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING;
+
+			} else {
+				// 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;
@@ -827,58 +836,36 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now, bool
 }
 
 void
-FixedwingPositionControl::control_auto(const hrt_abstime &now, 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)
+FixedwingPositionControl::update_in_air_states(const hrt_abstime now)
 {
-	const float dt = math::constrain((now - _last_time_position_control_called) * 1e-6f, MIN_AUTO_TIMESTEP,
-					 MAX_AUTO_TIMESTEP);
-	_last_time_position_control_called = now;
-
-	if (_param_fw_use_npfg.get()) {
-		_npfg.setDt(dt);
-
-	} else {
-		_l1_control.set_dt(dt);
-	}
-
-	_att_sp.fw_control_yaw = false;		// by default we don't want yaw to be contoller directly with rudder
-	_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;		// by default we don't use flaps
-
 	/* save time when airplane is in air */
 	if (!_was_in_air && !_landed) {
 		_was_in_air = true;
 		_time_went_in_air = now;
-		_takeoff_ground_alt = _current_altitude;
+		_takeoff_ground_alt = _current_altitude; // XXX: is this really a good idea?
 	}
 
 	/* reset flag when airplane landed */
 	if (_landed) {
 		_was_in_air = false;
 	}
+}
 
-	/* Reset integrators if switching to this mode from a other mode in which posctl was not active */
-	if (_control_mode_current == FW_POSCTRL_MODE_OTHER) {
-		/* reset integrators */
-		_tecs.reset_state();
-	}
+float
+FixedwingPositionControl::update_position_control_mode_timestep(const hrt_abstime now)
+{
+	const float dt = math::constrain((now - _last_time_position_control_called) * 1e-6f, MIN_AUTO_TIMESTEP,
+					 MAX_AUTO_TIMESTEP);
+	_last_time_position_control_called = now;
 
-	/* reset hold yaw */
-	_hdg_hold_yaw = _yaw;
+	return dt;
+}
 
-	/* get circle mode */
-	const bool was_circle_mode = (_param_fw_use_npfg.get()) ? _npfg.circleMode() : _l1_control.circle_mode();
-
-	/* restore TECS parameters, in case changed intermittently (e.g. in landing handling) */
-	_tecs.set_speed_weight(_param_fw_t_spdweight.get());
-	_tecs.set_height_error_time_constant(_param_fw_t_h_error_tc.get());
-
-	/* Initialize attitude controller integrator reset flags to 0 */
-	_att_sp.roll_reset_integral = false;
-	_att_sp.pitch_reset_integral = false;
-	_att_sp.yaw_reset_integral = false;
-
-	position_setpoint_s current_sp = pos_sp_curr;
+void
+FixedwingPositionControl::move_position_setpoint_for_vtol_transition(position_setpoint_s &current_sp)
+{
+	// TODO: velocity, altitude, or just a heading hold position mode should be used for this, not position
+	// shifting hacks
 
 	if (_vehicle_status.in_transition_to_fw) {
 
@@ -902,6 +889,25 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
 		_transition_waypoint(0) = static_cast<double>(NAN);
 		_transition_waypoint(1) = static_cast<double>(NAN);
 	}
+}
+
+void
+FixedwingPositionControl::control_auto(const hrt_abstime &now, 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)
+{
+	const float dt = update_position_control_mode_timestep(now);
+
+	update_in_air_states(now);
+
+	_hdg_hold_yaw = _yaw;
+
+	_att_sp.roll_reset_integral = false;
+	_att_sp.pitch_reset_integral = false;
+	_att_sp.yaw_reset_integral = false;
+
+	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.type, current_sp);
 
@@ -912,11 +918,26 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
 		publishOrbitStatus(current_sp);
 	}
 
+	// update lateral guidance timesteps for slewrates
+	if (_param_fw_use_npfg.get()) {
+		_npfg.setDt(dt);
+
+	} else {
+		_l1_control.set_dt(dt);
+	}
+
+	const bool was_circle_mode = (_param_fw_use_npfg.get()) ? _npfg.circleMode() : _l1_control.circle_mode();
+
+	/* restore TECS parameters, in case changed intermittently (e.g. in landing handling) */
+	_tecs.set_speed_weight(_param_fw_t_spdweight.get());
+	_tecs.set_height_error_time_constant(_param_fw_t_h_error_tc.get());
+
 	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());
+		_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
 		break;
 
 	case position_setpoint_s::SETPOINT_TYPE_POSITION:
@@ -932,16 +953,6 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
 		break;
 	}
 
-	/* reset landing state */
-	if (position_sp_type != position_setpoint_s::SETPOINT_TYPE_LAND) {
-		reset_landing_state();
-	}
-
-	/* reset takeoff/launch state */
-	if (position_sp_type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
-		reset_takeoff_state();
-	}
-
 	if (was_circle_mode && !_l1_control.circle_mode()) {
 		/* just kicked out of loiter, reset roll integrals */
 		_att_sp.roll_reset_integral = true;
@@ -991,6 +1002,8 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const hrt_abstime &no
 	}
 
 	_att_sp.pitch_body = get_tecs_pitch();
+
+	_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
 }
 
 void
@@ -1019,6 +1032,8 @@ FixedwingPositionControl::control_auto_descend(const hrt_abstime &now)
 
 	_att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_min.get() : min(get_tecs_thrust(), _param_fw_thr_max.get());
 	_att_sp.pitch_body = get_tecs_pitch();
+
+	_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
 }
 
 uint8_t
@@ -1037,15 +1052,8 @@ FixedwingPositionControl::handle_setpoint_type(const uint8_t setpoint_type, cons
 
 	uint8_t position_sp_type = setpoint_type;
 
-	if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
-		// TAKEOFF: handle like a regular POSITION setpoint if already flying
-		if (!in_takeoff_situation() && (_airspeed >= _param_fw_airspd_min.get() || !_airspeed_valid)) {
-			// SETPOINT_TYPE_TAKEOFF -> SETPOINT_TYPE_POSITION
-			position_sp_type = position_setpoint_s::SETPOINT_TYPE_POSITION;
-		}
-
-	} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION
-		   || pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
+	if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION
+	    || pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
 
 		float dist_xy = -1.f;
 		float dist_z = -1.f;
@@ -1082,11 +1090,6 @@ FixedwingPositionControl::handle_setpoint_type(const uint8_t setpoint_type, cons
 		}
 	}
 
-	// set to type position during VTOL transitions in Land mode (to not start FW landing logic)
-	if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LAND && _vehicle_status.in_transition_mode) {
-		position_sp_type = position_setpoint_s::SETPOINT_TYPE_POSITION;
-	}
-
 	return position_sp_type;
 }
 
@@ -1145,8 +1148,7 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const fl
 	// Altitude first order hold (FOH)
 	if (pos_sp_prev.valid && PX4_ISFINITE(pos_sp_prev.alt) &&
 	    ((pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_POSITION) ||
-	     (pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_LOITER) ||
-	     (pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF))
+	     (pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_LOITER))
 	   ) {
 		const float d_curr_prev = get_distance_to_next_waypoint((double)curr_wp(0), (double)curr_wp(1),
 					  pos_sp_prev.lat, pos_sp_prev.lon);
@@ -1206,6 +1208,8 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const fl
 
 	_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
 
+	_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
+
 	tecs_update_pitch_throttle(now, position_sp_alt,
 				   target_airspeed,
 				   radians(_param_fw_p_lim_min.get()),
@@ -1269,6 +1273,8 @@ FixedwingPositionControl::control_auto_velocity(const hrt_abstime &now, const fl
 
 	_att_sp.yaw_body = _yaw;
 
+	_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
+
 	tecs_update_pitch_throttle(now, position_sp_alt,
 				   target_airspeed,
 				   radians(_param_fw_p_lim_min.get()),
@@ -1356,7 +1362,10 @@ FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const floa
 		// have to do this switch (which can cause significant altitude errors) close to the ground.
 		_tecs.set_height_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
 		airspeed_sp = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get();
-		_att_sp.apply_flaps = true;
+		_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_LAND;
+
+	} else {
+		_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
 	}
 
 	float target_airspeed = get_auto_airspeed_setpoint(now, airspeed_sp, ground_speed, dt);
@@ -1415,10 +1424,17 @@ void
 FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const Vector2d &curr_pos,
 		const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
 {
-	const float dt = math::constrain((now - _last_time_position_control_called) * 1e-6f, MIN_AUTO_TIMESTEP,
-					 MAX_AUTO_TIMESTEP);
-	_last_time_position_control_called = now;
+	const float dt = update_position_control_mode_timestep(now);
 
+	update_in_air_states(now);
+
+	_hdg_hold_yaw = _yaw;
+
+	_att_sp.roll_reset_integral = false;
+	_att_sp.pitch_reset_integral = false;
+	_att_sp.yaw_reset_integral = false;
+
+	// update lateral guidance timesteps for slewrates
 	if (_param_fw_use_npfg.get()) {
 		_npfg.setDt(dt);
 
@@ -1426,6 +1442,10 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const Vec
 		_l1_control.set_dt(dt);
 	}
 
+	/* restore TECS parameters, in case changed intermittently (e.g. in landing handling) */
+	_tecs.set_speed_weight(_param_fw_t_spdweight.get());
+	_tecs.set_height_error_time_constant(_param_fw_t_h_error_tc.get());
+
 	/* current waypoint (the one currently heading for) */
 	Vector2d curr_wp(pos_sp_curr.lat, pos_sp_curr.lon);
 	Vector2d prev_wp{0, 0}; /* previous waypoint */
@@ -1488,7 +1508,9 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const Vec
 		if (_param_fw_use_npfg.get()) {
 			_npfg.setAirspeedNom(target_airspeed * _eas2tas);
 			_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
-			_npfg.navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
+			// NOTE: current waypoint is passed twice to trigger the "point following" logic -- TODO: create
+			// point following navigation interface instead of this hack.
+			_npfg.navigateWaypoints(curr_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
 			_att_sp.roll_body = _runway_takeoff.getRoll(_npfg.getRollSetpoint());
 			target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
 
@@ -1656,10 +1678,17 @@ void
 FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vector2d &curr_pos,
 		const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
 {
-	const float dt = math::constrain((now - _last_time_position_control_called) * 1e-6f, MIN_AUTO_TIMESTEP,
-					 MAX_AUTO_TIMESTEP);
-	_last_time_position_control_called = now;
+	const float dt = update_position_control_mode_timestep(now);
 
+	update_in_air_states(now);
+
+	_hdg_hold_yaw = _yaw;
+
+	_att_sp.roll_reset_integral = false;
+	_att_sp.pitch_reset_integral = false;
+	_att_sp.yaw_reset_integral = false;
+
+	// update lateral guidance timesteps for slewrates
 	if (_param_fw_use_npfg.get()) {
 		_npfg.setDt(dt);
 
@@ -1667,6 +1696,12 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vec
 		_l1_control.set_dt(dt);
 	}
 
+	/* restore TECS parameters, in case changed intermittently (e.g. in landing handling) */
+	_tecs.set_speed_weight(_param_fw_t_spdweight.get());
+
+	// Enable tighter altitude control for landings
+	_tecs.set_height_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
+
 	/* current waypoint (the one currently heading for) */
 	Vector2d curr_wp(pos_sp_curr.lat, pos_sp_curr.lon);
 	Vector2d prev_wp{0, 0}; /* previous waypoint */
@@ -2074,9 +2109,15 @@ void
 FixedwingPositionControl::control_manual_position(const hrt_abstime &now, const Vector2d &curr_pos,
 		const Vector2f &ground_speed)
 {
-	const float dt = math::constrain((now - _last_time_position_control_called) * 1e-6f, MIN_AUTO_TIMESTEP,
-					 MAX_AUTO_TIMESTEP);
-	_last_time_position_control_called = now;
+	const float dt = update_position_control_mode_timestep(now);
+
+	// update lateral guidance timesteps for slewrates
+	if (_param_fw_use_npfg.get()) {
+		_npfg.setDt(dt);
+
+	} else {
+		_l1_control.set_dt(dt);
+	}
 
 	// if we assume that user is taking off then help by demanding altitude setpoint well above ground
 	// and set limit to pitch angle to prevent steering into ground
@@ -2391,6 +2432,8 @@ FixedwingPositionControl::Run()
 
 		set_control_mode_current(_local_pos.timestamp, _pos_sp_triplet.current.valid);
 
+		_att_sp.fw_control_yaw = false;		// by default we don't want yaw to be contoller directly with rudder
+
 		switch (_control_mode_current) {
 		case FW_POSCTRL_MODE_AUTO: {
 				control_auto(_local_pos.timestamp, curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current,
@@ -2437,11 +2480,23 @@ FixedwingPositionControl::Run()
 
 				_att_sp.thrust_body[0] = min(_att_sp.thrust_body[0], _param_fw_thr_max.get());
 
+				_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
+
+				_tecs.reset_state();
+
 				break;
 			}
 
 		}
 
+		if (_control_mode_current != FW_POSCTRL_MODE_AUTO_LANDING) {
+			reset_landing_state();
+		}
+
+		if (_control_mode_current != FW_POSCTRL_MODE_AUTO_TAKEOFF) {
+			reset_takeoff_state();
+		}
+
 		if (_control_mode_current != FW_POSCTRL_MODE_OTHER) {
 
 			if (_control_mode.flag_control_manual_enabled) {

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -350,6 +350,30 @@ private:
 	 * @param dt Time step
 	 */
 	void		update_desired_altitude(float dt);
+
+	/**
+	 * @brief Updates timing information for landed and in-air states.
+	 *
+	 * @param now Current system time [us]
+	 */
+	void update_in_air_states(const hrt_abstime now);
+
+	/**
+	 * @brief Updates the time since the last position control call.
+	 *
+	 * @param now Current system time [us]
+	 * @return Time since last position control call [s]
+	 */
+	float update_position_control_mode_timestep(const hrt_abstime now);
+
+	/**
+	 * @brief Moves the current position setpoint to a value far ahead of the current vehicle yaw when in  a VTOL
+	 * transition.
+	 *
+	 * @param[in,out] current_sp current position setpoint
+	 */
+	void move_position_setpoint_for_vtol_transition(position_setpoint_s &current_sp);
+
 	uint8_t		handle_setpoint_type(const uint8_t setpoint_type, const position_setpoint_s &pos_sp_curr);
 	void		control_auto(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed,
 				     const position_setpoint_s &pos_sp_prev,

src/modules/logger/log_writer_file.cpp

@@ -287,7 +287,9 @@ int LogWriterFile::hardfault_store_filename(const char *log_file)
 
 void LogWriterFile::stop_log(LogType type)
 {
+	lock();
 	_buffers[(int)type]._should_run = false;
+	unlock();
 	notify();
 }
 
@@ -312,8 +314,10 @@ int LogWriterFile::thread_start()
 void LogWriterFile::thread_stop()
 {
 	// this will terminate the main loop of the writer thread
-	_exit_thread = true;
+	lock();
+	_exit_thread.store(true);
 	_buffers[0]._should_run = _buffers[1]._should_run = false;
+	unlock();
 
 	notify();
 
@@ -335,10 +339,10 @@ void *LogWriterFile::run_helper(void *context)
 
 void LogWriterFile::run()
 {
-	while (!_exit_thread) {
+	while (!_exit_thread.load()) {
 		// Outer endless loop
 		// Wait for _should_run flag
-		while (!_exit_thread) {
+		while (!_exit_thread.load()) {
 			bool start = false;
 			pthread_mutex_lock(&_mtx);
 			pthread_cond_wait(&_cv, &_mtx);
@@ -350,7 +354,7 @@ void LogWriterFile::run()
 			}
 		}
 
-		if (_exit_thread) {
+		if (_exit_thread.load()) {
 			break;
 		}
 
@@ -480,7 +484,7 @@ void LogWriterFile::run()
 			 * not an issue because notify() is called regularly.
 			 * If the logger was switched off in the meantime, do not wait for data, instead run this loop
 			 * once more to write remaining data and close the file. */
-			if (_buffers[0]._should_run) {
+			if (_buffers[0]._should_run || _buffers[1]._should_run) {
 				pthread_cond_wait(&_cv, &_mtx);
 			}
 		}

src/modules/logger/log_writer_file.h

@@ -34,6 +34,7 @@
 #pragma once
 
 #include <px4_platform_common/defines.h>
+#include <px4_platform_common/atomic.h>
 #include <stdint.h>
 #include <pthread.h>
 #include <drivers/drv_hrt.h>
@@ -205,8 +206,8 @@ private:
 
 	LogFileBuffer _buffers[(int)LogType::Count];
 
-	bool 		_exit_thread = false;
-	bool		_need_reliable_transfer = false;
+	px4::atomic_bool	_exit_thread{false};
+	bool			_need_reliable_transfer{false};
 	pthread_mutex_t		_mtx;
 	pthread_cond_t		_cv;
 	pthread_t _thread = 0;