fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-19 19:09:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

cs_check: reject if not prearmed

Browse Source
This commit is contained in:
Balduin 2025-07-14 17:37:08 +02:00
parent 013862e9d6
commit d2fa519249
2 changed files with 28 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
src/modules/control_allocator/ControlAllocator.cpp
View File

@ -481,6 +481,10 @@ ControlAllocator::Run()
void ControlAllocator::preflight_check_handle_command(hrt_abstime now)
{
// Only start the check if prearmed but not armed. Otherwise, output a
// warning message and reject the command.
vehicle_command_s vehicle_command;
if (_vehicle_command_sub.update(&vehicle_command)) {
@ -489,13 +493,30 @@ void ControlAllocator::preflight_check_handle_command(hrt_abstime now)
if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_PREFLIGHT_CS_CHECK) {
if (!_armed) {
// currently this does not check prearmed status. if not prearmed, it will just do nothing.
preflight_check_start(vehicle_command, now);
result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_IN_PROGRESS;
actuator_armed_s armed;
if (_armed_sub.copy(&armed)) {
const bool prearmed = armed.prearmed;
if (prearmed) {
// currently this does not check prearmed status. if not prearmed, it will just do nothing.
preflight_check_start(vehicle_command, now);
result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_IN_PROGRESS;
} else {
result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
PX4_WARN("Control surface preflight check rejected (not prearmed)");
}
} else {
result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
PX4_WARN("Control surface preflight check rejected (failed to get prearmed state)");
}
} else {
result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
PX4_INFO("Control surface preflight check rejected (armed)");
PX4_WARN("Control surface preflight check rejected (armed)");
}
preflight_check_send_ack(result, now);

3
src/modules/control_allocator/ControlAllocator.hpp
View File

@ -68,6 +68,8 @@
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_motors.h>
#include <uORB/topics/actuator_servos.h>
#include <uORB/topics/actuator_servos_trim.h>
@ -197,6 +199,7 @@ private:
uORB::Subscription _vehicle_thrust_setpoint1_sub{ORB_ID(vehicle_thrust_setpoint), 1}; /**< vehicle thrust setpoint subscription (2. instance) */
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
// Outputs
uORB::PublicationMulti<control_allocator_status_s> _control_allocator_status_pub[2] {ORB_ID(control_allocator_status), ORB_ID(control_allocator_status)};