mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-01 11:06:04 +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";
|
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.
|
// Transient: arming state mismatch. The user can change it and retry.
|
||||||
ack_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
|
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;
|
return nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -150,14 +161,16 @@ void ActuatorGroupPreflightCheck::updateState(hrt_abstime now)
|
|||||||
if (!_running) { return; }
|
if (!_running) { return; }
|
||||||
|
|
||||||
actuator_armed_s actuator_armed;
|
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
|
// 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 armed_lost = actuator_armed.armed != _requires_armed;
|
||||||
const bool prearmed_lost = !_requires_armed && !actuator_armed.prearmed;
|
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;
|
_running = false;
|
||||||
sendAck(_last_command, vehicle_command_ack_s::VEHICLE_CMD_RESULT_CANCELLED, now);
|
sendAck(_last_command, vehicle_command_ack_s::VEHICLE_CMD_RESULT_CANCELLED, now);
|
||||||
return;
|
return;
|
||||||
|
|||||||
@@ -44,18 +44,21 @@
|
|||||||
|
|
||||||
#include <uORB/Publication.hpp>
|
#include <uORB/Publication.hpp>
|
||||||
#include <uORB/Subscription.hpp>
|
#include <uORB/Subscription.hpp>
|
||||||
|
|
||||||
#include <uORB/topics/actuator_armed.h>
|
#include <uORB/topics/actuator_armed.h>
|
||||||
#include <uORB/topics/vehicle_command.h>
|
#include <uORB/topics/vehicle_command.h>
|
||||||
#include <uORB/topics/vehicle_command_ack.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
|
* 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
|
* torque, thrust or collective tilt on a single functional group for a short
|
||||||
* duration so the operator can verify actuator motion before flight.
|
* 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
|
* - torque and collective-tilt groups: prearmed and *not* armed
|
||||||
* - thrust groups: armed
|
* - thrust groups: armed
|
||||||
|
* - landed
|
||||||
*
|
*
|
||||||
* Losing the required state mid-check results in a CANCELLED ack.
|
* 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 _vehicle_command_sub{ORB_ID(vehicle_command)};
|
||||||
uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
|
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)};
|
uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
|
||||||
|
|
||||||
bool _running{false};
|
bool _running{false};
|
||||||
|
|||||||
Reference in New Issue
Block a user