fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 09:53:38 +08:00
Code Issues 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-25 08:30:58 +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
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
)
};