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

VTOL: remove virtual actuator_controls and instead use virtual torque/thrust topics

Browse Source 
MC/FW rate controller and auto tuner: remove actuator_controls

AirshipAttControl: remove actuator_controls

MulticopterLandDetector: remove actuator_controls

mavlink streams vfr_hud and high_latency2: remove actuator_controls

RoverPositionController: remove actuator_controls

UUVAttitudeController: remove actuator_controls

battery: use length of thrust_setpoint for throttle compensation

VehicleMagnetometer: use length of thrust_setpoint for throttle compensation

Signed-off-by: Silvan Fuhrer
This commit is contained in:
Silvan Fuhrer
2022-12-23 12:57:18 +01:00
committed by Beat Küng
parent bcd6e7adee
commit b16f16598b
47 changed files with 332 additions and 549 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_rate_control/FixedwingRateControl.cpp
+38 -76
View File
@@ -43,8 +43,9 @@ using math::radians;
FixedwingRateControl::FixedwingRateControl(bool vtol) :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
_actuator_controls_0_pub(vtol ? ORB_ID(actuator_controls_virtual_fw) : ORB_ID(actuator_controls_0)),
_actuator_controls_status_pub(vtol ? ORB_ID(actuator_controls_status_1) : ORB_ID(actuator_controls_status_0)),
_vehicle_torque_setpoint_pub(vtol ? ORB_ID(vehicle_torque_setpoint_virtual_fw) : ORB_ID(vehicle_torque_setpoint)),
_vehicle_thrust_setpoint_pub(vtol ? ORB_ID(vehicle_thrust_setpoint_virtual_fw) : ORB_ID(vehicle_thrust_setpoint)),
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
{
/* fetch initial parameter values */
@@ -123,24 +124,23 @@ FixedwingRateControl::vehicle_manual_poll()
if (_vehicle_status.is_vtol_tailsitter && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
// the controls must always be published in body (hover) frame
_actuator_controls.control[actuator_controls_s::INDEX_ROLL] =
_manual_control_setpoint.yaw * _param_fw_man_y_sc.get() + _param_trim_yaw.get();
_actuator_controls.control[actuator_controls_s::INDEX_YAW] =
-(_manual_control_setpoint.roll * _param_fw_man_r_sc.get() + _param_trim_roll.get());
_vehicle_torque_setpoint.xyz[0] = math::constrain(_manual_control_setpoint.yaw * _param_fw_man_y_sc.get() +
_param_trim_yaw.get(), -1.f, 1.f);
_vehicle_torque_setpoint.xyz[2] = math::constrain(_manual_control_setpoint.roll * _param_fw_man_r_sc.get() +
_param_trim_roll.get(), -1.f, 1.f);
} else {
_actuator_controls.control[actuator_controls_s::INDEX_ROLL] =
_manual_control_setpoint.roll * _param_fw_man_r_sc.get() + _param_trim_roll.get();
_actuator_controls.control[actuator_controls_s::INDEX_YAW] =
_manual_control_setpoint.yaw * _param_fw_man_y_sc.get() + _param_trim_yaw.get();
_vehicle_torque_setpoint.xyz[0] = math::constrain(_manual_control_setpoint.roll * _param_fw_man_r_sc.get() +
_param_trim_roll.get(), -1.f, 1.f);
_vehicle_torque_setpoint.xyz[2] = math::constrain(_manual_control_setpoint.yaw * _param_fw_man_y_sc.get() +
_param_trim_yaw.get(), -1.f, 1.f);
}
_actuator_controls.control[actuator_controls_s::INDEX_PITCH] =
-_manual_control_setpoint.pitch * _param_fw_man_p_sc.get() + _param_trim_pitch.get();
_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] = (_manual_control_setpoint.throttle + 1.f) * .5f;
_vehicle_torque_setpoint.xyz[1] = math::constrain(-_manual_control_setpoint.pitch * _param_fw_man_p_sc.get() +
_param_trim_pitch.get(), -1.f, 1.f);
_vehicle_thrust_setpoint.xyz[0] = math::constrain((_manual_control_setpoint.throttle + 1.f) * .5f, 0.f, 1.f);
}
}
}
}
@@ -350,31 +350,26 @@ void FixedwingRateControl::Run()
float roll_feedforward = _param_fw_rr_ff.get() * _airspeed_scaling * body_rates_setpoint(0);
float roll_u = angular_acceleration_setpoint(0) * _airspeed_scaling * _airspeed_scaling + roll_feedforward;
_actuator_controls.control[actuator_controls_s::INDEX_ROLL] =
(PX4_ISFINITE(roll_u)) ? math::constrain(roll_u + trim_roll, -1.f, 1.f) : trim_roll;
_vehicle_torque_setpoint.xyz[0] = PX4_ISFINITE(roll_u) ? math::constrain(roll_u + trim_roll, -1.f, 1.f) : trim_roll;
float pitch_feedforward = _param_fw_pr_ff.get() * _airspeed_scaling * body_rates_setpoint(1);
float pitch_u = angular_acceleration_setpoint(1) * _airspeed_scaling * _airspeed_scaling + pitch_feedforward;
_actuator_controls.control[actuator_controls_s::INDEX_PITCH] =
(PX4_ISFINITE(pitch_u)) ? math::constrain(pitch_u + trim_pitch, -1.f, 1.f) : trim_pitch;
_vehicle_torque_setpoint.xyz[1] = PX4_ISFINITE(pitch_u) ? math::constrain(pitch_u + trim_pitch, -1.f, 1.f) : trim_pitch;
const float yaw_feedforward = _param_fw_yr_ff.get() * _airspeed_scaling * body_rates_setpoint(2);
float yaw_u = angular_acceleration_setpoint(2) * _airspeed_scaling * _airspeed_scaling + yaw_feedforward;
_actuator_controls.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? math::constrain(yaw_u + trim_yaw,
-1.f, 1.f) : trim_yaw;
_vehicle_torque_setpoint.xyz[2] = PX4_ISFINITE(yaw_u) ? math::constrain(yaw_u + trim_yaw, -1.f, 1.f) : trim_yaw;
if (!PX4_ISFINITE(roll_u) || !PX4_ISFINITE(pitch_u) || !PX4_ISFINITE(yaw_u)) {
_rate_control.resetIntegral();
}
/* throttle passed through if it is finite */
_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] =
(PX4_ISFINITE(_rates_sp.thrust_body[0])) ? _rates_sp.thrust_body[0] : 0.0f;
_vehicle_thrust_setpoint.xyz[0] = PX4_ISFINITE(_rates_sp.thrust_body[0]) ? _rates_sp.thrust_body[0] : 0.0f;
/* scale effort by battery status */
if (_param_fw_bat_scale_en.get() &&
_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] > 0.1f) {
if (_param_fw_bat_scale_en.get() && _vehicle_thrust_setpoint.xyz[0] > 0.1f) {
if (_battery_status_sub.updated()) {
battery_status_s battery_status{};
@@ -384,7 +379,7 @@ void FixedwingRateControl::Run()
}
}
_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] *= _battery_scale;
_vehicle_thrust_setpoint.xyz[0] *= _battery_scale;
}
}
@@ -402,31 +397,28 @@ void FixedwingRateControl::Run()
// Add feed-forward from roll control output to yaw control output
// This can be used to counteract the adverse yaw effect when rolling the plane
_actuator_controls.control[actuator_controls_s::INDEX_YAW] += _param_fw_rll_to_yaw_ff.get()
* constrain(_actuator_controls.control[actuator_controls_s::INDEX_ROLL], -1.0f, 1.0f);
_vehicle_torque_setpoint.xyz[2] = math::constrain(_vehicle_torque_setpoint.xyz[2] + _param_fw_rll_to_yaw_ff.get() *
_vehicle_torque_setpoint.xyz[0], -1.f, 1.f);
// Tailsitter: rotate back to body frame from airspeed frame
if (_vehicle_status.is_vtol_tailsitter) {
const float helper = _actuator_controls.control[actuator_controls_s::INDEX_ROLL];
_actuator_controls.control[actuator_controls_s::INDEX_ROLL] =
_actuator_controls.control[actuator_controls_s::INDEX_YAW];
_actuator_controls.control[actuator_controls_s::INDEX_YAW] = -helper;
const float helper = _vehicle_torque_setpoint.xyz[0];
_vehicle_torque_setpoint.xyz[0] = _vehicle_torque_setpoint.xyz[2];
_vehicle_torque_setpoint.xyz[2] = -helper;
}
/* lazily publish the setpoint only once available */
_actuator_controls.timestamp = hrt_absolute_time();
_actuator_controls.timestamp_sample = vehicle_angular_velocity.timestamp;
/* Only publish if any of the proper modes are enabled */
if (_vcontrol_mode.flag_control_rates_enabled ||
_vcontrol_mode.flag_control_attitude_enabled ||
_vcontrol_mode.flag_control_manual_enabled) {
_actuator_controls_0_pub.publish(_actuator_controls);
{
_vehicle_thrust_setpoint.timestamp = hrt_absolute_time();
_vehicle_thrust_setpoint.timestamp_sample = angular_velocity.timestamp_sample;
_vehicle_thrust_setpoint_pub.publish(_vehicle_thrust_setpoint);
if (!_vehicle_status.is_vtol) {
publishTorqueSetpoint(angular_velocity.timestamp_sample);
publishThrustSetpoint(angular_velocity.timestamp_sample);
_vehicle_torque_setpoint.timestamp = hrt_absolute_time();
_vehicle_torque_setpoint.timestamp_sample = angular_velocity.timestamp_sample;
_vehicle_torque_setpoint_pub.publish(_vehicle_torque_setpoint);
}
}
@@ -479,44 +471,14 @@ void FixedwingRateControl::Run()
perf_end(_loop_perf);
}
void FixedwingRateControl::publishTorqueSetpoint(const hrt_abstime &timestamp_sample)
{
vehicle_torque_setpoint_s v_torque_sp = {};
v_torque_sp.timestamp = hrt_absolute_time();
v_torque_sp.timestamp_sample = timestamp_sample;
v_torque_sp.xyz[0] = _actuator_controls.control[actuator_controls_s::INDEX_ROLL];
v_torque_sp.xyz[1] = _actuator_controls.control[actuator_controls_s::INDEX_PITCH];
v_torque_sp.xyz[2] = _actuator_controls.control[actuator_controls_s::INDEX_YAW];
_vehicle_torque_setpoint_pub.publish(v_torque_sp);
}
void FixedwingRateControl::publishThrustSetpoint(const hrt_abstime &timestamp_sample)
{
vehicle_thrust_setpoint_s v_thrust_sp = {};
v_thrust_sp.timestamp = hrt_absolute_time();
v_thrust_sp.timestamp_sample = timestamp_sample;
v_thrust_sp.xyz[0] = _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE];
v_thrust_sp.xyz[1] = 0.0f;
v_thrust_sp.xyz[2] = 0.0f;
_vehicle_thrust_setpoint_pub.publish(v_thrust_sp);
}
void FixedwingRateControl::updateActuatorControlsStatus(float dt)
{
for (int i = 0; i < 4; i++) {
float control_signal;
for (int i = 0; i < 3; i++) {
if (i <= actuator_controls_status_s::INDEX_YAW) {
// We assume that the attitude is actuated by control surfaces
// consuming power only when they move
control_signal = _actuator_controls.control[i] - _control_prev[i];
_control_prev[i] = _actuator_controls.control[i];
} else {
control_signal = _actuator_controls.control[i];
}
// We assume that the attitude is actuated by control surfaces
// consuming power only when they move
const float control_signal = _vehicle_torque_setpoint.xyz[i] - _control_prev[i];
_control_prev[i] = _vehicle_torque_setpoint.xyz[i];
_control_energy[i] += control_signal * control_signal * dt;
}
@@ -526,9 +488,9 @@ void FixedwingRateControl::updateActuatorControlsStatus(float dt)
if (_energy_integration_time > 500e-3f) {
actuator_controls_status_s status;
status.timestamp = _actuator_controls.timestamp;
status.timestamp = _vehicle_torque_setpoint.timestamp;
for (int i = 0; i < 4; i++) {
for (int i = 0; i < 3; i++) {
status.control_power[i] = _control_energy[i] / _energy_integration_time;
_control_energy[i] = 0.f;
}