preflight check: only change nav_mode once

preflight check: only conduct if pre-armed
preflight check: optional arguments rather than members
2026-05-24 08:27:34 +08:00 · 2025-02-19 15:09:34 +01:00 · 2025-02-19 14:03:37 +01:00 · 2025-02-19 14:03:37 +01:00 · 2025-02-18 16:59:08 +01:00 · 2025-02-18 15:57:56 +01:00
28 changed files with 218 additions and 26 deletions
@@ -49,7 +49,7 @@ uint8 NAVIGATION_STATE_DESCEND = 12             # Descend mode (no position cont
 uint8 NAVIGATION_STATE_TERMINATION = 13         # Termination mode
 uint8 NAVIGATION_STATE_OFFBOARD = 14
 uint8 NAVIGATION_STATE_STAB = 15                # Stabilized mode
-uint8 NAVIGATION_STATE_FREE1 = 16
+uint8 NAVIGATION_STATE_CS_PREFLIGHT_CHECK = 16
 uint8 NAVIGATION_STATE_AUTO_TAKEOFF = 17        # Takeoff
 uint8 NAVIGATION_STATE_AUTO_LAND = 18           # Land
 uint8 NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19  # Auto Follow
@@ -198,7 +198,8 @@ public:
 	 */
 	virtual void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
 				    int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
-				    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) {}
+				    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max, bool preflight_check_running = false,
+				    float collective_tilt_normalized_setpoint = 0.0f, float collective_thrust_normalized_setpoint = 0.0f) {}

 	/**
 	 * Get a bitmask of motors to be stopped
@@ -1572,6 +1572,42 @@ void Commander::handleCommandsFromModeExecutors()
 	}
 }

+unsigned Commander::handleCommandControlTest(const vehicle_command_s &cmd)
+{
+
+	// TODO: trigger this some different way using params for dev purposes...?
+	// TODO: decode from command: do we want to test roll/pitch/yaw(/collective tilt), or
+	// individual control surfaces?
+	// TODO define these somewhere else like all the cool kids do
+	static const int TEST_MODE_INDIVIDUAL = 0;
+	static const int TEST_MODE_RPY = 1;
+
+	int test_mode = TEST_MODE_RPY;
+
+	if (test_mode == TEST_MODE_INDIVIDUAL) {
+		// _user_mode_intention.change()
+		// _user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_SERVO_TEST);
+		PX4_INFO("Not implemented");
+		return vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
+
+	} else if (test_mode == TEST_MODE_RPY) {
+
+		// this nice pattern stolen from handle_command
+		if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER)) {
+			return vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+
+		} else {
+			printRejectMode(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER);
+			return vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+		}
+
+	} else {
+		return vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
+	}
+
+	return vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+}
+
 unsigned Commander::handleCommandActuatorTest(const vehicle_command_s &cmd)
 {
 	if (isArmed() || (_safety.isButtonAvailable() && !_safety.isSafetyOff())) {
@@ -1835,6 +1871,22 @@ void Commander::run()
 			_status_changed = true;
 		}

+		// do the control surface preflight check only if pre-armed. this means we need:
+		// COM_PREARM_MODE = 1 (Safety Button) or 2 (Always).
+		if (_actuator_armed.prearmed) {
+			if (_param_com_do_cs_check.get()) {
+				if (_vehicle_status.nav_state != vehicle_status_s::NAVIGATION_STATE_CS_PREFLIGHT_CHECK) {
+					_prev_nav_state = _vehicle_status.nav_state;
+					_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_CS_PREFLIGHT_CHECK);
+				}
+
+			} else {
+				if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_CS_PREFLIGHT_CHECK) {
+					_user_mode_intention.change(_prev_nav_state);
+				}
+			}
+		}
+
 		modeManagementUpdate();

 		const hrt_abstime now = hrt_absolute_time();
@@ -157,6 +157,8 @@ private:

 	unsigned handleCommandActuatorTest(const vehicle_command_s &cmd);

+	unsigned handleCommandControlTest(const vehicle_command_s &cmd);
+
 	void executeActionRequest(const action_request_s &action_request);

 	void printRejectMode(uint8_t nav_state);
@@ -284,6 +286,8 @@ private:

 	vehicle_land_detected_s	_vehicle_land_detected{};

+	uint8_t _prev_nav_state;
+
 	// commander publications
 	actuator_armed_s        _actuator_armed{};
 	vehicle_control_mode_s  _vehicle_control_mode{};
@@ -349,6 +353,7 @@ private:
 		(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time,
 		(ParamInt<px4::params::COM_FLIGHT_UUID>)    _param_com_flight_uuid,
 		(ParamInt<px4::params::COM_TAKEOFF_ACT>)    _param_com_takeoff_act,
+		(ParamBool<px4::params::COM_DO_CS_CHECK>)   _param_com_do_cs_check,
 		(ParamFloat<px4::params::COM_CPU_MAX>)      _param_com_cpu_max
 	)
 };
@@ -178,6 +178,21 @@ void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type,
 		vehicle_control_mode.flag_control_allocation_enabled = true;
 		break;

+	// enabling literally all of these makes the allocator run nicely.
+	// enabling only allocator, it will not run.
+	// surely there is like one or two which we really need and the rest can be dropped...
+	case vehicle_status_s::NAVIGATION_STATE_CS_PREFLIGHT_CHECK:
+		vehicle_control_mode.flag_control_auto_enabled = true;
+		vehicle_control_mode.flag_control_position_enabled = true;
+		vehicle_control_mode.flag_control_velocity_enabled = true;
+		vehicle_control_mode.flag_control_altitude_enabled = true;
+		vehicle_control_mode.flag_control_climb_rate_enabled = true;
+		vehicle_control_mode.flag_control_attitude_enabled = true;
+		vehicle_control_mode.flag_control_rates_enabled = true;
+		vehicle_control_mode.flag_control_allocation_enabled = true;
+		break;
+
+
 	// vehicle_status_s::NAVIGATION_STATE_EXTERNALx: handled in ModeManagement
 	default:
 		break;
@@ -1029,3 +1029,6 @@ PARAM_DEFINE_INT32(COM_FLTT_LOW_ACT, 3);
 *
 */
 PARAM_DEFINE_INT32(COM_MODE_ARM_CHK, 0);
+
+// temporary, to test control surface preflight check.
+PARAM_DEFINE_INT32(COM_DO_CS_CHECK, 0);
@@ -345,6 +345,7 @@ ControlAllocator::Run()
 		if (_vehicle_status_sub.update(&vehicle_status)) {

 			_armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED;
+			_preflight_check_running = vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_CS_PREFLIGHT_CHECK;

 			ActuatorEffectiveness::FlightPhase flight_phase{ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT};

@@ -437,15 +438,24 @@ ControlAllocator::Run()
 			}
 		}

+		if (_preflight_check_running) {
+			preflight_check_update_state();
+			preflight_check_overwrite_torque_sp(c);
+		}
+
 		for (int i = 0; i < _num_control_allocation; ++i) {

 			_control_allocation[i]->setControlSetpoint(c[i]);

 			// Do allocation
 			_control_allocation[i]->allocate();
+
+			float preflight_check_tilt_sp = preflight_check_get_tilt_control();
+
 			_actuator_effectiveness->allocateAuxilaryControls(dt, i, _control_allocation[i]->_actuator_sp); //flaps and spoilers
 			_actuator_effectiveness->updateSetpoint(c[i], i, _control_allocation[i]->_actuator_sp,
-								_control_allocation[i]->getActuatorMin(), _control_allocation[i]->getActuatorMax());
+								_control_allocation[i]->getActuatorMin(), _control_allocation[i]->getActuatorMax(),
+								_preflight_check_running, preflight_check_tilt_sp, 0.0f);

 			if (_has_slew_rate) {
 				_control_allocation[i]->applySlewRateLimit(dt);
@@ -473,6 +483,68 @@ ControlAllocator::Run()
 	perf_end(_loop_perf);
 }

+void ControlAllocator::preflight_check_update_state() {
+
+	// bool tiltrotor = dynamic_cast<ActuatorEffectivenessTiltrotorVTOL*>(_actuator_effectiveness) != nullptr;
+	bool tiltrotor = _effectiveness_source_id == EffectivenessSource::TILTROTOR_VTOL;
+
+	// cycle through roll, pitch, yaw, and for each one inject positive and
+	// negative torque setpoints.
+
+	int n_axes = 3;
+	if (tiltrotor) {
+		n_axes = 4;
+	}
+
+	int max_phase = 2 * n_axes;
+
+	hrt_abstime now = hrt_absolute_time();
+	if (now - _last_preflight_check_update >= 500_ms) {
+		_preflight_check_phase++;
+		_preflight_check_phase %= max_phase;  // or quit once we did the whole thing once?
+		_last_preflight_check_update = now;
+	}
+}
+
+void ControlAllocator::preflight_check_overwrite_torque_sp(matrix::Vector<float, NUM_AXES> (&c)[ActuatorEffectiveness::MAX_NUM_MATRICES]) {
+
+	int axis = _preflight_check_phase / 2;
+	int negative = _preflight_check_phase % 2;
+
+	if (axis < 3) {
+		c[0](0) = 0.;
+		c[0](1) = 0.;
+		c[0](2) = 0.;
+		c[0](axis) = negative ? -1.f : 1.f;
+
+		if (_num_control_allocation > 1) {
+			c[1](0) = 0.;
+			c[1](1) = 0.;
+			c[1](2) = 0.;
+			c[1](axis) = negative ? -1.f : 1.f;
+		}
+
+	}
+
+}
+
+float ControlAllocator::preflight_check_get_tilt_control() {
+
+	int axis = _preflight_check_phase / 2;
+	int negative = _preflight_check_phase % 2;
+
+	float modified_tilt_control = 0.5f;
+
+	if (axis == 3) {
+		// axis 3 = tiltrotor.
+		// collective tilt normalised control goes from 0 to 1.
+		modified_tilt_control = negative ? 0.f : 1.f;
+	}
+
+	return modified_tilt_control;
+
+}
+
 void
 ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReason reason)
 {
@@ -138,6 +138,10 @@ private:

 	void publish_actuator_controls();

+	void preflight_check_overwrite_torque_sp(matrix::Vector<float, NUM_AXES> (&c)[ActuatorEffectiveness::MAX_NUM_MATRICES]);
+	void preflight_check_update_state();
+	float preflight_check_get_tilt_control();
+
 	AllocationMethod _allocation_method_id{AllocationMethod::NONE};
 	ControlAllocation *_control_allocation[ActuatorEffectiveness::MAX_NUM_MATRICES] {}; 	///< class for control allocation calculations
 	int _num_control_allocation{0};
@@ -201,6 +205,10 @@ private:
 	// For example, the system might report two motor failures, but only the first one is handled by CA
 	uint16_t _handled_motor_failure_bitmask{0};

+	bool _preflight_check_running{false};
+	int _preflight_check_phase{0};
+	hrt_abstime _last_preflight_check_update{0};
+
 	perf_counter_t	_loop_perf;			/**< loop duration performance counter */

 	bool _armed{false};
@@ -61,7 +61,8 @@ ActuatorEffectivenessCustom::getEffectivenessMatrix(Configuration &configuration

 void ActuatorEffectivenessCustom::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
 		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
-		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max, bool preflight_check_running,
+		float collective_tilt_normalized_setpoint, float collective_thrust_normalized_setpoint)
 {
 	stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp);
 }
@@ -47,7 +47,8 @@ public:

 	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
 			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
-			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max, bool preflight_check_running = true,
+			    float collective_tilt_normalized_setpoint = 0.0f, float collective_thrust_normalized_setpoint = 0.0f) override;

 	const char *name() const override { return "Custom"; }

@@ -63,7 +63,8 @@ ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configurat

 void ActuatorEffectivenessFixedWing::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
 		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
-		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max, bool preflight_check_running,
+		float collective_tilt_normalized_setpoint, float collective_thrust_normalized_setpoint)
 {
 	stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp);
 }
@@ -53,7 +53,8 @@ public:

 	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
 			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
-			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max, bool preflight_check_running = true,
+			    float collective_tilt_normalized_setpoint = 0.0f, float collective_thrust_normalized_setpoint = 0.0f) override;

 private:
 	ActuatorEffectivenessRotors _rotors;
@@ -130,7 +130,8 @@ bool ActuatorEffectivenessHelicopter::getEffectivenessMatrix(Configuration &conf

 void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
 		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
-		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max, bool preflight_check_running,
+		float collective_tilt_normalized_setpoint, float collective_thrust_normalized_setpoint)
 {
 	_saturation_flags = {};

@@ -80,7 +80,8 @@ public:

 	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
 			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
-			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max, bool preflight_check_running = true,
+			    float collective_tilt_normalized_setpoint = 0.0f, float collective_thrust_normalized_setpoint = 0.0f) override;

 	void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;
 private:
@@ -104,7 +104,8 @@ bool ActuatorEffectivenessHelicopterCoaxial::getEffectivenessMatrix(Configuratio

 void ActuatorEffectivenessHelicopterCoaxial::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
 		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
-		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max, bool preflight_check_running,
+		float collective_tilt_normalized_setpoint, float collective_thrust_normalized_setpoint)
 {
 	_saturation_flags = {};

@@ -71,7 +71,8 @@ public:

 	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
 			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
-			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max, bool preflight_check_running = true,
+			    float collective_tilt_normalized_setpoint = 0.0f, float collective_thrust_normalized_setpoint = 0.0f) override;

 	void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;
 private:
@@ -78,7 +78,8 @@ ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration

 void ActuatorEffectivenessMCTilt::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
 		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
-		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max, bool preflight_check_running,
+		float collective_tilt_normalized_setpoint, float collective_thrust_normalized_setpoint)
 {
 	actuator_sp += _tilt_offsets;
 	// TODO: dynamic matrix update
@@ -57,7 +57,8 @@ public:

 	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
 			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
-			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max, bool preflight_check_running = true,
+			    float collective_tilt_normalized_setpoint = 0.0f, float collective_thrust_normalized_setpoint = 0.0f) override;

 	const char *name() const override { return "MC Tilt"; }

@@ -51,7 +51,8 @@ ActuatorEffectivenessRoverAckermann::getEffectivenessMatrix(Configuration &confi

 void ActuatorEffectivenessRoverAckermann::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
 		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
-		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max, bool preflight_check_running,
+		float collective_tilt_normalized_setpoint, float collective_thrust_normalized_setpoint)
 {
 	stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp);
 }
@@ -45,7 +45,8 @@ public:

 	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
 			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
-			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max, bool preflight_check_running = true,
+			    float collective_tilt_normalized_setpoint = 0.0f, float collective_thrust_normalized_setpoint = 0.0f) override;

 	const char *name() const override { return "Rover (Ackermann)"; }
 private:
@@ -85,7 +85,8 @@ void ActuatorEffectivenessStandardVTOL::allocateAuxilaryControls(const float dt,

 void ActuatorEffectivenessStandardVTOL::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
 		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
-		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max, bool preflight_check_running,
+		float collective_tilt_normalized_setpoint, float collective_thrust_normalized_setpoint)
 {
 	if (matrix_index == 0) {
 		stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp);
@@ -77,7 +77,8 @@ public:

 	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
 			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
-			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max, bool preflight_check_running = true,
+			    float collective_tilt_normalized_setpoint = 0.0f, float collective_thrust_normalized_setpoint = 0.0f) override;

 	void setFlightPhase(const FlightPhase &flight_phase) override;

@@ -90,7 +90,8 @@ void ActuatorEffectivenessTailsitterVTOL::allocateAuxilaryControls(const float d

 void ActuatorEffectivenessTailsitterVTOL::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
 		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
-		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max, bool preflight_check_running,
+		float collective_tilt_normalized_setpoint, float collective_thrust_normalized_setpoint)
 {
 	if (matrix_index == 0) {
 		stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp);
@@ -74,7 +74,8 @@ public:

 	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
 			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
-			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max, bool preflight_check_running,
+			    float collective_tilt_normalized_setpoint, float collective_thrust_normalized_setpoint) override;


 	void setFlightPhase(const FlightPhase &flight_phase) override;
@@ -126,14 +126,31 @@ void ActuatorEffectivenessTiltrotorVTOL::allocateAuxilaryControls(const float dt

 void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
 		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
-		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max, bool preflight_check_running,
+		float collective_tilt_normalized_setpoint, float collective_thrust_normalized_setpoint)
 {
 	// apply tilt
+
+	// if preflight_check_running, we alter the behaviour in these two ways:
+	// - collective tilt and thrust setpoints are NOT taken from uOrb
+	//   message, but from class member variable, which we can arbitrarily set
+	//   before calling this function
+	// - collective tilt is added to actuator_sp even if
+	//   (throttleSpoolupFinished() || _flight_phase != FlightPhase::HOVER_FLIGHT)
+	//   evaluates to false
+
 	if (matrix_index == 0) {
 		tiltrotor_extra_controls_s tiltrotor_extra_controls;

-		if (_tiltrotor_extra_controls_sub.copy(&tiltrotor_extra_controls)) {
+		if (_tiltrotor_extra_controls_sub.copy(&tiltrotor_extra_controls) || preflight_check_running) {
+
 			float control_collective_tilt = tiltrotor_extra_controls.collective_tilt_normalized_setpoint * 2.f - 1.f;
+			float control_collective_thrust = tiltrotor_extra_controls.collective_thrust_normalized_setpoint;
+
+			if (preflight_check_running) {
+				control_collective_tilt = collective_tilt_normalized_setpoint * 2.f - 1.f;
+				control_collective_thrust = collective_thrust_normalized_setpoint;
+			}

 			// set control_collective_tilt to exactly -1 or 1 if close to these end points
 			control_collective_tilt = control_collective_tilt < -0.99f ? -1.f : control_collective_tilt;
@@ -166,7 +183,7 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
 				if (_tilts.config(i).tilt_direction == ActuatorEffectivenessTilts::TiltDirection::TowardsFront) {

 					// as long as throttle spoolup is not completed, leave the tilts in the disarmed position (in hover)
-					if (throttleSpoolupFinished() || _flight_phase != FlightPhase::HOVER_FLIGHT) {
+					if (throttleSpoolupFinished() || _flight_phase != FlightPhase::HOVER_FLIGHT || preflight_check_running) {
 						actuator_sp(i + _first_tilt_idx) += control_collective_tilt;

 					} else {
@@ -202,7 +219,7 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
 			// in FW directly use throttle sp
 			if (_flight_phase == FlightPhase::FORWARD_FLIGHT) {
 				for (int i = 0; i < _first_tilt_idx; ++i) {
-					actuator_sp(i) = tiltrotor_extra_controls.collective_thrust_normalized_setpoint;
+					actuator_sp(i) = control_collective_thrust;
 				}
 			}
 		}
@@ -82,7 +82,8 @@ public:

 	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
 			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
-			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max, bool preflight_check_running = true,
+			    float collective_tilt_normalized_setpoint = 0.0f, float collective_thrust_normalized_setpoint = 0.0f) override;

 	const char *name() const override { return "VTOL Tiltrotor"; }

@@ -57,7 +57,8 @@ bool ActuatorEffectivenessUUV::getEffectivenessMatrix(Configuration &configurati

 void ActuatorEffectivenessUUV::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
 		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
-		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max, bool preflight_check_running,
+		float collective_tilt_normalized_setpoint, float collective_thrust_normalized_setpoint)
 {
 	stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp);
 }
@@ -56,7 +56,8 @@ public:

 	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
 			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
-			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max, bool preflight_check_running = true,
+			    float collective_tilt_normalized_setpoint = 0.0f, float collective_thrust_normalized_setpoint = 0.0f) override;

 	const char *name() const override { return "UUV"; }
Author	SHA1	Message	Date
Balduin	79eac11d01	preflight check: only change nav_mode once	2025-02-19 15:09:34 +01:00
Balduin	543bab43b5	preflight check: only conduct if pre-armed	2025-02-19 14:03:37 +01:00
Balduin	fb096d1910	preflight check: optional arguments rather than members we now use optional args to give ActuatorEffectivenessTiltrotorVTOL::updateSetpoint the necessary info about the preflight check. All the other ActuatorEffectiveness*::updateSetpoint functions can be used exactly like before, by ignoring the optional arguments. Rather than the previous solution of setting a member variable only when _actuator_effectiveness is an instance of ActuatorEffectivenessTiltrotorVTOL, this also allows us to lose the ugly dynamic_cast (poor cpp man's instanceof). the other dynamic_cast is replaced by checking _effectiveness_source_id which should be equivalent.	2025-02-19 14:03:37 +01:00
Balduin	6e5f3a6099	preflight check: only conduct if not armed	2025-02-18 16:59:08 +01:00
Balduin	c28d015d9c	preflight check: replace uOrb msg with argument Previously, the approach to modify collective tilt control was to send the corresponding tiltrotor_extra_control uOrb message from ControlAllocator, which then influences ActuatorEffectivenessTiltrotorVTOL with minimal changes. This was a bit hacky and introduced potentially conflicting uOrb messages. So, with this new approach we pass the same information via argument. Specifically, the class ActuatorEffectiveness now declares updateSetpoint with an extra argument, preflight_check_running. It is only used in ActuatorEffectivenessTiltrotorVTOL, but has to be included as a "dummy" in all other classes inheriting from ActuatorEffectiveness. The argument can be used to bypass the collective tilt/thrust setpoints, instead replacing them with values from public class member variables which can be set from outside just before calling updateSetpoints. Also, slight refactor in ControlAllocator by splitting up the functions related to the preflight check into smaller parts	2025-02-18 15:57:56 +01:00
Balduin	96fc4a0673	preflight check: modify actuator_sp if running this adds a new flag to ActuatorEffectivenessTiltrotorVTOL: _preflight_check_running. We set it from ControlAllocator, and if true we always add collective tilt to the actuator setpoint.	2025-02-18 13:03:45 +01:00
Balduin	122643af25	preflight check: fix range	2025-02-18 13:03:38 +01:00
Balduin	71afdb5680	preflight check: restore old nav_state only once previously it would always change _user_mode_intention to _prev_nav_state forever. now we change it only if we are in preflight check mode to not interfere with anything else later.	2025-02-18 11:08:12 +01:00
Balduin	0253f9798e	preflight check: send tilt control always even if we are not currently testing the tilt axis. then we send zero tilt rather than nothing.	2025-02-18 11:08:06 +01:00
Balduin	ad42a5b2f9	preflight check: more self explanatory code	2025-02-17 17:46:39 +01:00
Balduin	761810884e	preflight check: always send tiltrotor controls	2025-02-17 17:24:21 +01:00
Balduin	2a779663f6	preflight check: tilt collective actuation we "intercept" the tiltrotor_extra_controls message, which is read again right after in ActuatorEffectivenessTiltrotorVTOL::updateSetpoint.	2025-02-17 11:40:41 +01:00
Balduin	7816fd14b7	preflight check: restore prev nav state	2025-02-14 13:37:31 +01:00
Balduin	64d18f0a64	preflight check: first working version modifies torque setpoints directly via the matrix c.	2025-02-14 12:17:10 +01:00
Balduin	a3cca099a8	preflight check: set vehicle_control_mode flags not sure if all of these are needed, but with all of them the allocator runs, and with only flag_control_allocation_enabled it does not.	2025-02-14 10:59:42 +01:00
Balduin	01a65bfcf6	preflight check: base business logic - currently triggered via param COM_DO_CS_CHECK. ultimately this will be replaced by a mavlink cmd - new VehicleStatus.nav_state, NAVIGATION_STATE_CS_PREFLIGHT_CHECK. this is how commander tells ControlAllocator to conduct the check - (todo) within ControlAllocator we overwrite torque setpoints to do the check. additionally we can also control servos directly from there, in a different mode if that is needed.	2025-02-14 09:45:53 +01:00