fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

VehicleControlMode: add flag_control_allocation_enabled

Browse Source 
Allows (external) modes to directly publish actuator_{motors,servos}.
This commit is contained in:
Beat Küng 2023-07-26 13:31:20 +02:00 committed by Daniel Agar
parent 22e613a24a
commit 03365658d5
4 changed files with 17 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/VehicleControlMode.msg
View File

@ -14,3 +14,4 @@ bool flag_control_position_enabled # true if position is controlled
bool flag_control_altitude_enabled # true if altitude is controlled
bool flag_control_climb_rate_enabled # true if climb rate is controlled
bool flag_control_termination_enabled # true if flighttermination is enabled
bool flag_control_allocation_enabled # true if control allocation is enabled

1
src/modules/commander/ModeUtil/control_mode.cpp
View File

@ -47,6 +47,7 @@ void getVehicleControlMode(bool armed, uint8_t nav_state, uint8_t vehicle_type,
vehicle_control_mode_s &vehicle_control_mode)
{
vehicle_control_mode.flag_armed = armed;
vehicle_control_mode.flag_control_allocation_enabled = true; // Always enabled by default
switch (nav_state) {
case vehicle_status_s::NAVIGATION_STATE_MANUAL:

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

@ -359,6 +359,14 @@ ControlAllocator::Run()
}
}
{
vehicle_control_mode_s vehicle_control_mode;
if (_vehicle_control_mode_sub.update(&vehicle_control_mode)) {
_publish_controls = vehicle_control_mode.flag_control_allocation_enabled;
}
}
// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
const hrt_abstime now = hrt_absolute_time();
const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f);
@ -641,6 +649,10 @@ ControlAllocator::publish_control_allocator_status(int matrix_index)
void
ControlAllocator::publish_actuator_controls()
{
if (!_publish_controls) {
return;
}
actuator_motors_s actuator_motors;
actuator_motors.timestamp = hrt_absolute_time();
actuator_motors.timestamp_sample = _timestamp_sample;

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

@ -73,6 +73,7 @@
#include <uORB/topics/actuator_servos_trim.h>
#include <uORB/topics/control_allocator_status.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_torque_setpoint.h>
#include <uORB/topics/vehicle_thrust_setpoint.h>
#include <uORB/topics/vehicle_status.h>
@ -186,10 +187,12 @@ private:
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _failure_detector_status_sub{ORB_ID(failure_detector_status)};
matrix::Vector3f _torque_sp;
matrix::Vector3f _thrust_sp;
bool _publish_controls{true};
// Reflects motor failures that are currently handled, not motor failures that are reported.
// For example, the system might report two motor failures, but only the first one is handled by CA