preflight check: first working version

modifies torque setpoints directly via the matrix c.

src/modules/commander/Commander.cpp

@@ -1878,14 +1878,15 @@ void Commander::run()
 			// through Commander::handle_command
 
 			// this nice pattern stolen from handle_command
+			// if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_CS_PREFLIGHT_CHECK, ModeChangeSource::ModeExecutor, false)) {
 			if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_CS_PREFLIGHT_CHECK)) {
 				// return vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
-				PX4_INFO("mode intention changed");
+				// PX4_INFO("mode intention changed");
 
 			} else {
 				// printRejectMode(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER);
 				// return vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
-				PX4_INFO("");
+				// PX4_INFO("mode intention not changed");
 			}
 		} // else {
 			// leave the mode again somehow...

src/modules/control_allocator/ControlAllocator.cpp

@@ -396,9 +396,6 @@ ControlAllocator::Run()
 		do_update = true;
 		_timestamp_sample = vehicle_torque_setpoint.timestamp_sample;
 
-		if (_preflight_check_running) {
-			preflight_check_overwrite_torque_sp();
-		}
 	}
 
 	// Also run allocator on thrust setpoint changes if the torque setpoint
@@ -442,6 +439,10 @@ ControlAllocator::Run()
 			}
 		}
 
+		if (_preflight_check_running) {
+			preflight_check_overwrite_torque_sp(c);
+		}
+
 		for (int i = 0; i < _num_control_allocation; ++i) {
 
 			_control_allocation[i]->setControlSetpoint(c[i]);
@@ -492,13 +493,38 @@ ControlAllocator::Run()
 
 // }
 
-void ControlAllocator::preflight_check_overwrite_torque_sp() {
+void ControlAllocator::preflight_check_overwrite_torque_sp(matrix::Vector<float, NUM_AXES> (&c)[ActuatorEffectiveness::MAX_NUM_MATRICES]) {
 
-	// goal here: inject different torque setpoint.
+	// cycle through roll, pitch, yaw, and for each one inject positive and
+	// negative torque setpoints.
 
-	// right here: state machine cycling through roll, pitch, yaw(, collective tilt)
-	// overwrite _torque_sp after finding out which one
+	// for now, no collective tilt if tiltrotor. will need to look a bit different.
 
+	int max_phase = 3 * 2;
+
+	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;
+	}
+
+	int axis = _preflight_check_phase / 2;
+	int negative = _preflight_check_phase % 2;
+
+	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;
+	}
+
+	// PX4_INFO("_torque_sp: %f, %f, %f", (double) _torque_sp(0), (double) _torque_sp(1), (double) _torque_sp(2));
 
 }
 

src/modules/control_allocator/ControlAllocator.hpp

@@ -138,7 +138,7 @@ private:
 
 	void publish_actuator_controls();
 
-	void preflight_check_overwrite_torque_sp();
+	void preflight_check_overwrite_torque_sp(matrix::Vector<float, NUM_AXES> (&c)[ActuatorEffectiveness::MAX_NUM_MATRICES]);
 
 	AllocationMethod _allocation_method_id{AllocationMethod::NONE};
 	ControlAllocation *_control_allocation[ActuatorEffectiveness::MAX_NUM_MATRICES] {}; 	///< class for control allocation calculations
@@ -204,6 +204,8 @@ private:
 	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 */