preflight check: only change nav_mode once

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.

src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp

@@ -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, bool preflight_check_running) {}
+				    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

src/modules/commander/Commander.cpp

@@ -1871,10 +1871,14 @@ void Commander::run()
 			_status_changed = true;
 		}
 
-		if (!isArmed()) {
+		// 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()) {
-				_prev_nav_state = _vehicle_status.nav_state;
-				_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_CS_PREFLIGHT_CHECK);
+				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) {

src/modules/control_allocator/ControlAllocator.cpp

@@ -450,23 +450,12 @@ ControlAllocator::Run()
 			// Do allocation
 			_control_allocation[i]->allocate();
 
-			if (_preflight_check_running) {
-
-				// alternative: specify these member variables in the general ActuatorEffectiveness,
-				// and just don't use them anywhere except TiltrotorVTOL
-				auto actuator_effectiveness_tiltrotor_vtol = dynamic_cast<ActuatorEffectivenessTiltrotorVTOL*>(_actuator_effectiveness);
-
-				if (actuator_effectiveness_tiltrotor_vtol) {
-					float collective_tilt_sp = preflight_check_get_tilt_control();
-					actuator_effectiveness_tiltrotor_vtol->_collective_tilt_normalized_setpoint = collective_tilt_sp;
-					actuator_effectiveness_tiltrotor_vtol->_collective_thrust_normalized_setpoint = 0.0f;
-				}
-			}
+			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(),
-								_preflight_check_running);
+								_preflight_check_running, preflight_check_tilt_sp, 0.0f);
 
 			if (_has_slew_rate) {
 				_control_allocation[i]->applySlewRateLimit(dt);
@@ -494,23 +483,10 @@ ControlAllocator::Run()
 	perf_end(_loop_perf);
 }
 
-// void ControlAllocator::test_individual_control_surfaces() {
-	// goal here: modify actuation at the servo level.
-
-	// in here: small state machine cycling through servos (or taking info
-	// from outside about which servo to actuate)
-	// if test running: if enough time passed: go to next thing
-	//                  if last thing: test = not running
-
-	// elsewhere (probably Run()...)
-	// set test running if right message received
-	// if test running,
-
-// }
-
 void ControlAllocator::preflight_check_update_state() {
 
-	bool tiltrotor = dynamic_cast<ActuatorEffectivenessTiltrotorVTOL*>(_actuator_effectiveness) != nullptr;
+	// 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.

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.cpp

@@ -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, bool preflight_check_running)
+		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);
 }

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp

@@ -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, bool preflight_check_running) 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"; }
 

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp

@@ -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, bool preflight_check_running)
+		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);
 }

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp

@@ -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, bool preflight_check_running) 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;

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp

@@ -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, bool preflight_check_running)
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max, bool preflight_check_running,
+		float collective_tilt_normalized_setpoint, float collective_thrust_normalized_setpoint)
 {
 	_saturation_flags = {};
 

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp

@@ -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, bool preflight_check_running) 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:

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp

@@ -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, bool preflight_check_running)
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max, bool preflight_check_running,
+		float collective_tilt_normalized_setpoint, float collective_thrust_normalized_setpoint)
 {
 	_saturation_flags = {};
 

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp

@@ -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, bool preflight_check_running) 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:

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp

@@ -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, bool preflight_check_running)
+		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

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp

@@ -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, bool preflight_check_running) 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"; }
 

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp

@@ -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, bool preflight_check_running)
+		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);
 }

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp

@@ -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, bool preflight_check_running) 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:

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp

@@ -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, bool preflight_check_running)
+		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);

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp

@@ -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, bool preflight_check_running) 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;
 

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp

@@ -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, bool preflight_check_running)
+		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);

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp

@@ -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, bool preflight_check_running) 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;

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp

@@ -126,7 +126,8 @@ 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, bool preflight_check_running)
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max, bool preflight_check_running,
+		float collective_tilt_normalized_setpoint, float collective_thrust_normalized_setpoint)
 {
 	// apply tilt
 
@@ -147,8 +148,8 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
 			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;
+				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

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp

@@ -82,15 +82,13 @@ 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, bool preflight_check_running) 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"; }
 
 	void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;
 
-	float _collective_tilt_normalized_setpoint{0.5f};
-	float _collective_thrust_normalized_setpoint{0.0f};
-
 protected:
 	bool _collective_tilt_updated{true};
 	ActuatorEffectivenessRotors _mc_rotors;

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.cpp

@@ -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, bool preflight_check_running)
+		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);
 }

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp

@@ -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, bool preflight_check_running) 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"; }