mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-10 06:39:25 +08:00
feat(control_allocator): reject actuator group check in flight
according to the land detector. torque / tilt are rejected anyway because they require pre-armed && !armed, but thrust could previously be triggered in flight.
This commit is contained in:
@@ -83,6 +83,12 @@ const char *ActuatorGroupPreflightCheck::validateCommand(uint8_t group, bool is_
|
||||
return "failed to read armed state";
|
||||
}
|
||||
|
||||
vehicle_land_detected_s vehicle_land_detected;
|
||||
|
||||
if (!_vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
|
||||
return "failed to read landed state";
|
||||
}
|
||||
|
||||
// Transient: arming state mismatch. The user can change it and retry.
|
||||
ack_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
|
||||
|
||||
@@ -103,6 +109,11 @@ const char *ActuatorGroupPreflightCheck::validateCommand(uint8_t group, bool is_
|
||||
}
|
||||
}
|
||||
|
||||
// Transient: landed state mismatch
|
||||
if (!vehicle_land_detected.landed) {
|
||||
return "not landed";
|
||||
}
|
||||
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
@@ -150,14 +161,16 @@ void ActuatorGroupPreflightCheck::updateState(hrt_abstime now)
|
||||
if (!_running) { return; }
|
||||
|
||||
actuator_armed_s actuator_armed;
|
||||
vehicle_land_detected_s vehicle_land_detected;
|
||||
|
||||
if (_armed_sub.copy(&actuator_armed) && _vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
|
||||
|
||||
if (_armed_sub.copy(&actuator_armed)) {
|
||||
// Cancel if any of the conditions we depended on at start are lost
|
||||
// (thrust -> armed, torque/tilt -> pre-armed).
|
||||
// (thrust -> armed, torque/tilt -> pre-armed, always !landed).
|
||||
const bool armed_lost = actuator_armed.armed != _requires_armed;
|
||||
const bool prearmed_lost = !_requires_armed && !actuator_armed.prearmed;
|
||||
|
||||
if (armed_lost || prearmed_lost) {
|
||||
if (armed_lost || prearmed_lost || !vehicle_land_detected.landed) {
|
||||
_running = false;
|
||||
sendAck(_last_command, vehicle_command_ack_s::VEHICLE_CMD_RESULT_CANCELLED, now);
|
||||
return;
|
||||
|
||||
@@ -44,18 +44,21 @@
|
||||
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/vehicle_command_ack.h>
|
||||
#include <uORB/topics/vehicle_land_detected.h>
|
||||
|
||||
/**
|
||||
* Drives the VEHICLE_CMD_ACTUATOR_GROUP_TEST state machine: holds a fixed
|
||||
* torque, thrust or collective tilt on a single functional group for a short
|
||||
* duration so the operator can verify actuator motion before flight.
|
||||
*
|
||||
* Arming requirements (enforced both at start and continuously while running):
|
||||
* Pre-conditions (enforced at start, cancelled if not given while running):
|
||||
* - torque and collective-tilt groups: prearmed and *not* armed
|
||||
* - thrust groups: armed
|
||||
* - landed
|
||||
*
|
||||
* Losing the required state mid-check results in a CANCELLED ack.
|
||||
*/
|
||||
@@ -96,6 +99,7 @@ private:
|
||||
|
||||
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
|
||||
uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
|
||||
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
|
||||
uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
|
||||
|
||||
bool _running{false};
|
||||
|
||||
Reference in New Issue
Block a user