From 01a65bfcf63df1aa30bf7354d1b01c6048ee17c6 Mon Sep 17 00:00:00 2001 From: Balduin Date: Fri, 14 Feb 2025 09:45:53 +0100 Subject: [PATCH] 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 | 2 +- src/modules/commander/Commander.cpp | 56 +++++++++++++++++++ src/modules/commander/Commander.hpp | 3 + src/modules/commander/commander_params.c | 3 + .../control_allocator/ControlAllocator.cpp | 29 ++++++++++ .../control_allocator/ControlAllocator.hpp | 4 ++ 6 files changed, 96 insertions(+), 1 deletion(-) diff --git a/msg/versioned/VehicleStatus.msg b/msg/versioned/VehicleStatus.msg index a347dfce3d..eb60f0d112 100644 --- a/msg/versioned/VehicleStatus.msg +++ b/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 diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index a5e1583175..d8e6b2e7b1 100644 --- a/src/modules/commander/Commander.cpp +++ b/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(); diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 04c1a0d05a..18955393fd 100644 --- a/src/modules/commander/Commander.hpp +++ b/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) _param_com_spoolup_time, (ParamInt) _param_com_flight_uuid, (ParamInt) _param_com_takeoff_act, + (ParamBool) _param_com_do_cs_check, (ParamFloat) _param_com_cpu_max ) }; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 37dcbdc9a9..b17a89a969 100644 --- a/src/modules/commander/commander_params.c +++ b/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); diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 99e280dd71..45cd630ba1 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/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) { diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 1f91f7d17d..b7c13651b4 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/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};