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.

msg/versioned/VehicleStatus.msg

@@ -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

src/modules/commander/Commander.cpp

@@ -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,26 @@ void Commander::run()
 			_status_changed = true;
 		}
 
+		if (_param_com_do_cs_check.get()) {
+
+			// directly modify user intention here.
+			// plan is for this to ultimately to be triggered by a mavlink command
+			// through Commander::handle_command
+
+			// this nice pattern stolen from handle_command
+			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");
+
+			} else {
+				// printRejectMode(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER);
+				// return vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				PX4_INFO("");
+			}
+		} // else {
+			// leave the mode again somehow...
+		// }
+
 		modeManagementUpdate();
 
 		const hrt_abstime now = hrt_absolute_time();

src/modules/commander/Commander.hpp

@@ -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);
@@ -349,6 +351,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
 	)
 };

src/modules/commander/commander_params.c

@@ -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);

src/modules/control_allocator/ControlAllocator.cpp

@@ -346,6 +346,8 @@ ControlAllocator::Run()
 
 			_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};
 
 			// Check if the current flight phase is HOVER or FIXED_WING
@@ -394,6 +396,9 @@ 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
@@ -473,6 +478,30 @@ 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_overwrite_torque_sp() {
+
+	// goal here: inject different torque setpoint.
+
+	// right here: state machine cycling through roll, pitch, yaw(, collective tilt)
+	// overwrite _torque_sp after finding out which one
+
+
+}
+
 void
 ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReason reason)
 {

src/modules/control_allocator/ControlAllocator.hpp

@@ -138,6 +138,8 @@ private:
 
 	void publish_actuator_controls();
 
+	void preflight_check_overwrite_torque_sp();
+
 	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 +203,8 @@ 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};
+
 	perf_counter_t	_loop_perf;			/**< loop duration performance counter */
 
 	bool _armed{false};