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

delete SYS_CTRL_ALLOC

Browse Source
This commit is contained in:
Daniel Agar
2022-09-09 09:14:09 -04:00
parent 13f9eabd70
commit 15fece7e14
17 changed files with 46 additions and 245 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/logger/logged_topics.cpp
+9 -22
View File
@@ -208,19 +208,12 @@ void LoggedTopics::add_default_topics()
//add_optional_topic("vehicle_optical_flow_vel", 100);
add_optional_topic("pps_capture");
// SYS_CTRL_ALLOC: additional dynamic control allocation logging when enabled
int32_t sys_ctrl_alloc = 0;
param_get(param_find("SYS_CTRL_ALLOC"), &sys_ctrl_alloc);
_dynamic_control_allocation = sys_ctrl_alloc >= 1;
if (_dynamic_control_allocation) {
add_topic("actuator_motors", 100);
add_topic("actuator_servos", 100);
add_topic("vehicle_angular_acceleration", 20);
add_topic_multi("vehicle_thrust_setpoint", 20, 1);
add_topic_multi("vehicle_torque_setpoint", 20, 2);
}
// additional control allocation logging
add_topic("actuator_motors", 100);
add_topic("actuator_servos", 100);
add_topic("vehicle_angular_acceleration", 20);
add_topic_multi("vehicle_thrust_setpoint", 20, 1);
add_topic_multi("vehicle_torque_setpoint", 20, 2);
// SYS_HITL: default ground truth logging for simulation
int32_t sys_hitl = 0;
@@ -294,15 +287,9 @@ void LoggedTopics::add_high_rate_topics()
add_topic("vehicle_attitude_setpoint");
add_topic("vehicle_rates_setpoint");
if (_dynamic_control_allocation) {
add_topic("actuator_motors");
add_topic("vehicle_thrust_setpoint");
add_topic("vehicle_torque_setpoint");
} else {
add_topic("actuator_controls_0");
add_topic("actuator_outputs");
}
add_topic("actuator_motors");
add_topic("vehicle_thrust_setpoint");
add_topic("vehicle_torque_setpoint");
}
void LoggedTopics::add_debug_topics()
src/modules/logger/logged_topics.h
-2
View File
@@ -185,8 +185,6 @@ private:
RequestedSubscriptionArray _subscriptions;
int _num_mission_subs{0};
float _rate_factor{1.0f};
bool _dynamic_control_allocation{false};
};
} //namespace logger
src/modules/simulation/simulator_sih/sih.cpp
+7 -25
View File
@@ -82,9 +82,7 @@ void Sih::run()
_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)};
}
_actuator_out_sub = uORB::Subscription{ORB_ID(actuator_outputs_sim)};
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
lockstep_loop();
@@ -369,32 +367,16 @@ void Sih::read_motors()
{
actuator_outputs_s actuators_out;
float pwm_middle = 0.5f * (PWM_DEFAULT_MIN + PWM_DEFAULT_MAX);
if (_actuator_out_sub.update(&actuators_out)) {
_last_actuator_output_time = actuators_out.timestamp;
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];
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
}
} 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
}
}
}
src/modules/simulation/simulator_sih/sih.hpp
+1 -2
View File
@@ -327,7 +327,6 @@ 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,
(ParamBool<px4::params::SYS_CTRL_ALLOC>) _sys_ctrl_alloc
(ParamInt<px4::params::SIH_VEHICLE_TYPE>) _sih_vtype
)
};