Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 16:56:25 +08:00
Code Issues Actions 4 Packages Projects Releases Wiki Activity

hitl,sitl,sih: use separate actuator_outputs_sim for SYS_CTRL_ALLOC==1

Browse Source 
- removes the need to do type-specific rescaling of pwm to normalized values
- allows to run physical output drivers alongside HIL/SIH
This commit is contained in:
Beat Küng
2022-02-09 13:36:27 +01:00
parent 1e32398217
commit 9b629a9e95
8 changed files with 264 additions and 164 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/actuator_outputs.msg
+3
View File
@@ -3,3 +3,6 @@ uint8 NUM_ACTUATOR_OUTPUTS = 16
uint8 NUM_ACTUATOR_OUTPUT_GROUPS = 4 # for sanity checking
uint32 noutputs # valid outputs
float32[16] output # output data, in natural output units
# actuator_outputs_sim is used for SITL, HITL & SIH (with an output range of [-1, 1])
# TOPICS actuator_outputs actuator_outputs_sim
src/drivers/pwm_out_sim/PWMSim.cpp
+34 -3
View File
@@ -81,9 +81,40 @@ bool
PWMSim::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
unsigned num_control_groups_updated)
{
// Nothing to do, as we are only interested in the actuator_outputs topic publication.
// That should only be published once we receive actuator_controls (important for lock-step to work correctly)
return num_control_groups_updated > 0;
// Only publish once we receive actuator_controls (important for lock-step to work correctly)
if (num_control_groups_updated > 0) {
actuator_outputs_s actuator_outputs{};
actuator_outputs.noutputs = num_outputs;
const uint32_t reversible_outputs = _mixing_output.reversibleOutputs();
for (int i = 0; i < (int)num_outputs; i++) {
if (outputs[i] != PWM_SIM_DISARMED_MAGIC) {
OutputFunction function = _mixing_output.outputFunction(i);
bool is_reversible = reversible_outputs & (1u << i);
float output = outputs[i];
if (((int)function >= (int)OutputFunction::Motor1 && (int)function <= (int)OutputFunction::MotorMax)
&& !is_reversible) {
// Scale non-reversible motors to [0, 1]
actuator_outputs.output[i] = (output - PWM_SIM_PWM_MIN_MAGIC) / (PWM_SIM_PWM_MAX_MAGIC - PWM_SIM_PWM_MIN_MAGIC);
} else {
// Scale everything else to [-1, 1]
const float pwm_center = (PWM_SIM_PWM_MAX_MAGIC + PWM_SIM_PWM_MIN_MAGIC) / 2.f;
const float pwm_delta = (PWM_SIM_PWM_MAX_MAGIC - PWM_SIM_PWM_MIN_MAGIC) / 2.f;
actuator_outputs.output[i] = (output - pwm_center) / pwm_delta;
}
}
}
actuator_outputs.timestamp = hrt_absolute_time();
_actuator_outputs_sim_pub.publish(actuator_outputs);
return true;
}
return false;
}
int
src/drivers/pwm_out_sim/PWMSim.hpp
+5
View File
@@ -43,6 +43,9 @@
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/time.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#if defined(CONFIG_ARCH_BOARD_PX4_SITL)
#define PARAM_PREFIX "PWM_MAIN"
@@ -86,5 +89,7 @@ private:
MixingOutput _mixing_output{PARAM_PREFIX, MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Publication<actuator_outputs_s> _actuator_outputs_sim_pub{ORB_ID(actuator_outputs_sim)};
};
src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp
+86 -69
View File
@@ -55,11 +55,21 @@ public:
}
private:
explicit MavlinkStreamHILActuatorControls(Mavlink *mavlink) : MavlinkStream(mavlink) {}
explicit MavlinkStreamHILActuatorControls(Mavlink *mavlink) : MavlinkStream(mavlink)
{
int32_t sys_ctrl_alloc = 0;
param_get(param_find("SYS_CTRL_ALLOC"), &sys_ctrl_alloc);
_use_dynamic_mixing = sys_ctrl_alloc >= 1;
if (_use_dynamic_mixing) {
_act_sub = uORB::Subscription{ORB_ID(actuator_outputs_sim)};
}
}
uORB::Subscription _act_sub{ORB_ID(actuator_outputs)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
bool _use_dynamic_mixing{false};
bool send() override
{
@@ -69,81 +79,88 @@ private:
mavlink_hil_actuator_controls_t msg{};
msg.time_usec = act.timestamp;
static constexpr float pwm_center = (PWM_DEFAULT_MAX + PWM_DEFAULT_MIN) / 2;
unsigned system_type = _mavlink->get_system_type();
/* scale outputs depending on system type */
if (system_type == MAV_TYPE_QUADROTOR ||
system_type == MAV_TYPE_HEXAROTOR ||
system_type == MAV_TYPE_OCTOROTOR ||
system_type == MAV_TYPE_VTOL_DUOROTOR ||
system_type == MAV_TYPE_VTOL_QUADROTOR ||
system_type == MAV_TYPE_VTOL_RESERVED2) {
/* multirotors: set number of rotor outputs depending on type */
unsigned n;
switch (system_type) {
case MAV_TYPE_QUADROTOR:
n = 4;
break;
case MAV_TYPE_HEXAROTOR:
n = 6;
break;
case MAV_TYPE_VTOL_DUOROTOR:
n = 2;
break;
case MAV_TYPE_VTOL_QUADROTOR:
n = 4;
break;
case MAV_TYPE_VTOL_RESERVED2:
n = 8;
break;
default:
n = 8;
break;
}
for (unsigned i = 0; i < 16; i++) {
if (act.output[i] > PWM_DEFAULT_MIN / 2) {
if (i < n) {
/* scale PWM out 900..2100 us to 0..1 for rotors */
msg.controls[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN);
} else {
/* scale PWM out 900..2100 us to -1..1 for other channels */
msg.controls[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
}
} else {
/* send 0 when disarmed and for disabled channels */
msg.controls[i] = 0.0f;
}
if (_use_dynamic_mixing) {
for (unsigned i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; i++) {
msg.controls[i] = act.output[i];
}
} else {
/* fixed wing: scale throttle to 0..1 and other channels to -1..1 */
for (unsigned i = 0; i < 16; i++) {
if (act.output[i] > PWM_DEFAULT_MIN / 2) {
if (i != 3) {
/* scale PWM out 900..2100 us to -1..1 for normal channels */
msg.controls[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
static constexpr float pwm_center = (PWM_DEFAULT_MAX + PWM_DEFAULT_MIN) / 2;
unsigned system_type = _mavlink->get_system_type();
/* scale outputs depending on system type */
if (system_type == MAV_TYPE_QUADROTOR ||
system_type == MAV_TYPE_HEXAROTOR ||
system_type == MAV_TYPE_OCTOROTOR ||
system_type == MAV_TYPE_VTOL_DUOROTOR ||
system_type == MAV_TYPE_VTOL_QUADROTOR ||
system_type == MAV_TYPE_VTOL_RESERVED2) {
/* multirotors: set number of rotor outputs depending on type */
unsigned n;
switch (system_type) {
case MAV_TYPE_QUADROTOR:
n = 4;
break;
case MAV_TYPE_HEXAROTOR:
n = 6;
break;
case MAV_TYPE_VTOL_DUOROTOR:
n = 2;
break;
case MAV_TYPE_VTOL_QUADROTOR:
n = 4;
break;
case MAV_TYPE_VTOL_RESERVED2:
n = 8;
break;
default:
n = 8;
break;
}
for (unsigned i = 0; i < 16; i++) {
if (act.output[i] > PWM_DEFAULT_MIN / 2) {
if (i < n) {
/* scale PWM out 900..2100 us to 0..1 for rotors */
msg.controls[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN);
} else {
/* scale PWM out 900..2100 us to -1..1 for other channels */
msg.controls[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
}
} else {
/* scale PWM out 900..2100 us to 0..1 for throttle */
msg.controls[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN);
/* send 0 when disarmed and for disabled channels */
msg.controls[i] = 0.0f;
}
}
} else {
/* set 0 for disabled channels */
msg.controls[i] = 0.0f;
} else {
/* fixed wing: scale throttle to 0..1 and other channels to -1..1 */
for (unsigned i = 0; i < 16; i++) {
if (act.output[i] > PWM_DEFAULT_MIN / 2) {
if (i != 3) {
/* scale PWM out 900..2100 us to -1..1 for normal channels */
msg.controls[i] = (act.output[i] - pwm_center) / ((PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2);
} else {
/* scale PWM out 900..2100 us to 0..1 for throttle */
msg.controls[i] = (act.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN);
}
} else {
/* set 0 for disabled channels */
msg.controls[i] = 0.0f;
}
}
}
}
src/modules/sih/sih.cpp
+30 -7
View File
@@ -73,6 +73,10 @@ Sih::Sih() :
_dist_snsr_time = task_start;
_vehicle = (VehicleType)constrain(_sih_vtype.get(), static_cast<typeof _sih_vtype.get()>(0),
static_cast<typeof _sih_vtype.get()>(2));
if (_sys_ctrl_alloc.get()) {
_actuator_out_sub = uORB::Subscription{ORB_ID(actuator_outputs_sim)};
}
}
Sih::~Sih()
@@ -99,6 +103,11 @@ bool Sih::init()
void Sih::Run()
{
if (should_exit()) {
exit_and_cleanup();
return;
}
perf_count(_loop_interval_perf);
// check for parameter updates
@@ -267,14 +276,28 @@ void Sih::read_motors()
float pwm_middle = 0.5f * (PWM_DEFAULT_MIN + PWM_DEFAULT_MAX);
if (_actuator_out_sub.update(&actuators_out)) {
for (int i = 0; i < NB_MOTORS; i++) { // saturate the motor signals
if ((_vehicle == VehicleType::FW && i < 3) || (_vehicle == VehicleType::TS
&& i > 3)) { // control surfaces in range [-1,1]
_u[i] = constrain(2.0f * (actuators_out.output[i] - pwm_middle) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN), -1.0f, 1.0f);
} else { // throttle signals in range [0,1]
float u_sp = constrain((actuators_out.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN), 0.0f, 1.0f);
_u[i] = _u[i] + _dt / _T_TAU * (u_sp - _u[i]); // first order transfer function with time constant tau
if (_sys_ctrl_alloc.get()) {
for (int i = 0; i < NB_MOTORS; i++) { // saturate the motor signals
if ((_vehicle == VehicleType::FW && i < 3) || (_vehicle == VehicleType::TS && i > 3)) {
_u[i] = actuators_out.output[i];
} else {
float u_sp = actuators_out.output[i];
_u[i] = _u[i] + _dt / _T_TAU * (u_sp - _u[i]); // first order transfer function with time constant tau
}
}
} else {
for (int i = 0; i < NB_MOTORS; i++) { // saturate the motor signals
if ((_vehicle == VehicleType::FW && i < 3) || (_vehicle == VehicleType::TS
&& i > 3)) { // control surfaces in range [-1,1]
_u[i] = constrain(2.0f * (actuators_out.output[i] - pwm_middle) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN), -1.0f, 1.0f);
} else { // throttle signals in range [0,1]
float u_sp = constrain((actuators_out.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN), 0.0f, 1.0f);
_u[i] = _u[i] + _dt / _T_TAU * (u_sp - _u[i]); // first order transfer function with time constant tau
}
}
}
}
src/modules/sih/sih.hpp
+2 -1
View File
@@ -303,6 +303,7 @@ private:
(ParamFloat<px4::params::SIH_DISTSNSR_MAX>) _sih_distance_snsr_max,
(ParamFloat<px4::params::SIH_DISTSNSR_OVR>) _sih_distance_snsr_override,
(ParamFloat<px4::params::SIH_T_TAU>) _sih_thrust_tau,
(ParamInt<px4::params::SIH_VEHICLE_TYPE>) _sih_vtype
(ParamInt<px4::params::SIH_VEHICLE_TYPE>) _sih_vtype,
(ParamBool<px4::params::SYS_CTRL_ALLOC>) _sys_ctrl_alloc
)
};
src/modules/simulator/simulator.h
+2 -3
View File
@@ -131,9 +131,7 @@ public:
#endif
private:
Simulator() : ModuleParams(nullptr)
{
}
Simulator();
~Simulator()
{
@@ -291,6 +289,7 @@ private:
float _last_magx{0.0f};
float _last_magy{0.0f};
float _last_magz{0.0f};
bool _use_dynamic_mixing{false};
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
px4::atomic<bool> _has_initialized {false};
src/modules/simulator/simulator_mavlink.cpp
+102 -81
View File
@@ -81,6 +81,14 @@ const unsigned mode_flag_custom = 1;
using namespace time_literals;
Simulator::Simulator()
: ModuleParams(nullptr)
{
int32_t sys_ctrl_alloc = 0;
param_get(param_find("SYS_CTRL_ALLOC"), &sys_ctrl_alloc);
_use_dynamic_mixing = sys_ctrl_alloc >= 1;
}
void Simulator::actuator_controls_from_outputs(mavlink_hil_actuator_controls_t *msg)
{
memset(msg, 0, sizeof(mavlink_hil_actuator_controls_t));
@@ -91,88 +99,96 @@ void Simulator::actuator_controls_from_outputs(mavlink_hil_actuator_controls_t *
int _system_type = _param_mav_type.get();
/* 'pos_thrust_motors_count' indicates number of motor channels which are configured with 0..1 range (positive thrust)
all other motors are configured for -1..1 range */
unsigned pos_thrust_motors_count;
bool is_fixed_wing;
switch (_system_type) {
case MAV_TYPE_AIRSHIP:
case MAV_TYPE_VTOL_DUOROTOR:
case MAV_TYPE_COAXIAL:
pos_thrust_motors_count = 2;
is_fixed_wing = false;
break;
case MAV_TYPE_TRICOPTER:
pos_thrust_motors_count = 3;
is_fixed_wing = false;
break;
case MAV_TYPE_QUADROTOR:
case MAV_TYPE_VTOL_QUADROTOR:
case MAV_TYPE_VTOL_TILTROTOR:
pos_thrust_motors_count = 4;
is_fixed_wing = false;
break;
case MAV_TYPE_VTOL_RESERVED2:
pos_thrust_motors_count = 5;
is_fixed_wing = false;
break;
case MAV_TYPE_HEXAROTOR:
pos_thrust_motors_count = 6;
is_fixed_wing = false;
break;
case MAV_TYPE_VTOL_RESERVED3:
// this is the tricopter VTOL / quad plane with 3 motors and 2 servos
pos_thrust_motors_count = 3;
is_fixed_wing = false;
break;
case MAV_TYPE_OCTOROTOR:
pos_thrust_motors_count = 8;
is_fixed_wing = false;
break;
case MAV_TYPE_SUBMARINE:
pos_thrust_motors_count = 0;
is_fixed_wing = false;
break;
case MAV_TYPE_FIXED_WING:
pos_thrust_motors_count = 0;
is_fixed_wing = true;
break;
default:
pos_thrust_motors_count = 0;
is_fixed_wing = false;
break;
}
for (unsigned i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; i++) {
if (!armed) {
/* send 0 when disarmed and for disabled channels */
msg->controls[i] = 0.0f;
} else if ((is_fixed_wing && i == 4) ||
(!is_fixed_wing && i < pos_thrust_motors_count)) { //multirotor, rotor channel
/* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to 0..1 for rotors */
msg->controls[i] = (_actuator_outputs.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN);
msg->controls[i] = math::constrain(msg->controls[i], 0.f, 1.f);
} else {
const float pwm_center = (PWM_DEFAULT_MAX + PWM_DEFAULT_MIN) / 2;
const float pwm_delta = (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2;
/* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to -1..1 for other channels */
msg->controls[i] = (_actuator_outputs.output[i] - pwm_center) / pwm_delta;
msg->controls[i] = math::constrain(msg->controls[i], -1.f, 1.f);
if (_use_dynamic_mixing) {
if (armed) {
for (unsigned i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; i++) {
msg->controls[i] = _actuator_outputs.output[i];
}
}
} else {
/* 'pos_thrust_motors_count' indicates number of motor channels which are configured with 0..1 range (positive thrust)
all other motors are configured for -1..1 range */
unsigned pos_thrust_motors_count;
bool is_fixed_wing;
switch (_system_type) {
case MAV_TYPE_AIRSHIP:
case MAV_TYPE_VTOL_DUOROTOR:
case MAV_TYPE_COAXIAL:
pos_thrust_motors_count = 2;
is_fixed_wing = false;
break;
case MAV_TYPE_TRICOPTER:
pos_thrust_motors_count = 3;
is_fixed_wing = false;
break;
case MAV_TYPE_QUADROTOR:
case MAV_TYPE_VTOL_QUADROTOR:
case MAV_TYPE_VTOL_TILTROTOR:
pos_thrust_motors_count = 4;
is_fixed_wing = false;
break;
case MAV_TYPE_VTOL_RESERVED2:
pos_thrust_motors_count = 5;
is_fixed_wing = false;
break;
case MAV_TYPE_HEXAROTOR:
pos_thrust_motors_count = 6;
is_fixed_wing = false;
break;
case MAV_TYPE_VTOL_RESERVED3:
// this is the tricopter VTOL / quad plane with 3 motors and 2 servos
pos_thrust_motors_count = 3;
is_fixed_wing = false;
break;
case MAV_TYPE_OCTOROTOR:
pos_thrust_motors_count = 8;
is_fixed_wing = false;
break;
case MAV_TYPE_SUBMARINE:
pos_thrust_motors_count = 0;
is_fixed_wing = false;
break;
case MAV_TYPE_FIXED_WING:
pos_thrust_motors_count = 0;
is_fixed_wing = true;
break;
default:
pos_thrust_motors_count = 0;
is_fixed_wing = false;
break;
}
for (unsigned i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; i++) {
if (!armed) {
/* send 0 when disarmed and for disabled channels */
msg->controls[i] = 0.0f;
} else if ((is_fixed_wing && i == 4) ||
(!is_fixed_wing && i < pos_thrust_motors_count)) { //multirotor, rotor channel
/* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to 0..1 for rotors */
msg->controls[i] = (_actuator_outputs.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN);
msg->controls[i] = math::constrain(msg->controls[i], 0.f, 1.f);
} else {
const float pwm_center = (PWM_DEFAULT_MAX + PWM_DEFAULT_MIN) / 2;
const float pwm_delta = (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) / 2;
/* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to -1..1 for other channels */
msg->controls[i] = (_actuator_outputs.output[i] - pwm_center) / pwm_delta;
msg->controls[i] = math::constrain(msg->controls[i], -1.f, 1.f);
}
}
}
msg->mode = mode_flag_custom;
@@ -694,7 +710,12 @@ void Simulator::send()
// Subscribe to topics.
// Only subscribe to the first actuator_outputs to fill a single HIL_ACTUATOR_CONTROLS.
_actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs), 0);
if (_use_dynamic_mixing) {
_actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs_sim), 0);
} else {
_actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs), 0);
}
// Before starting, we ought to send a heartbeat to initiate the SITL
// simulator to start sending sensor data which will set the time and