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

mc_rate_control: pass through 3D thrust

Browse Source
This commit is contained in:
Beat Küng 2022-05-04 10:23:46 +02:00 committed by Silvan Fuhrer
parent 923b5b15bd
commit 588e1572e3
2 changed files with 17 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
src/modules/mc_rate_control/MulticopterRateControl.cpp
View File

@ -184,16 +184,15 @@ MulticopterRateControl::Run()
math::superexpo(manual_control_setpoint.r, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())};
_rates_setpoint = man_rate_sp.emult(_acro_rate_max);
_thrust_setpoint = math::constrain(manual_control_setpoint.z, 0.0f, 1.0f);
_thrust_setpoint(2) = -math::constrain(manual_control_setpoint.z, 0.0f, 1.0f);
_thrust_setpoint(0) = _thrust_setpoint(1) = 0.f;
// publish rate setpoint
vehicle_rates_setpoint.roll = _rates_setpoint(0);
vehicle_rates_setpoint.pitch = _rates_setpoint(1);
vehicle_rates_setpoint.yaw = _rates_setpoint(2);
vehicle_rates_setpoint.thrust_body[0] = 0.0f;
vehicle_rates_setpoint.thrust_body[1] = 0.0f;
vehicle_rates_setpoint.thrust_body[2] = _thrust_setpoint;
vehicle_rates_setpoint.timestamp = hrt_absolute_time();
vehicle_rates_setpoint.roll = _rates_setpoint(0);
vehicle_rates_setpoint.pitch = _rates_setpoint(1);
vehicle_rates_setpoint.yaw = _rates_setpoint(2);
_thrust_setpoint.copyTo(vehicle_rates_setpoint.thrust_body);
vehicle_rates_setpoint.timestamp = hrt_absolute_time();
_vehicle_rates_setpoint_pub.publish(vehicle_rates_setpoint);
}
@ -203,7 +202,9 @@ MulticopterRateControl::Run()
_rates_setpoint(0) = PX4_ISFINITE(vehicle_rates_setpoint.roll) ? vehicle_rates_setpoint.roll : rates(0);
_rates_setpoint(1) = PX4_ISFINITE(vehicle_rates_setpoint.pitch) ? vehicle_rates_setpoint.pitch : rates(1);
_rates_setpoint(2) = PX4_ISFINITE(vehicle_rates_setpoint.yaw) ? vehicle_rates_setpoint.yaw : rates(2);
_thrust_setpoint = -vehicle_rates_setpoint.thrust_body[2];
_thrust_setpoint(0) = vehicle_rates_setpoint.thrust_body[0];
_thrust_setpoint(1) = vehicle_rates_setpoint.thrust_body[1];
_thrust_setpoint(2) = vehicle_rates_setpoint.thrust_body[2];
}
}
@ -251,13 +252,14 @@ MulticopterRateControl::Run()
actuators.control[actuator_controls_s::INDEX_ROLL] = PX4_ISFINITE(att_control(0)) ? att_control(0) : 0.0f;
actuators.control[actuator_controls_s::INDEX_PITCH] = PX4_ISFINITE(att_control(1)) ? att_control(1) : 0.0f;
actuators.control[actuator_controls_s::INDEX_YAW] = PX4_ISFINITE(att_control(2)) ? att_control(2) : 0.0f;
actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_thrust_setpoint) ? _thrust_setpoint : 0.0f;
actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_thrust_setpoint(2)) ? -_thrust_setpoint(
2) : 0.0f;
actuators.control[actuator_controls_s::INDEX_LANDING_GEAR] = _landing_gear;
actuators.timestamp_sample = angular_velocity.timestamp_sample;
if (!_vehicle_status.is_vtol) {
publishTorqueSetpoint(att_control, angular_velocity.timestamp_sample);
publishThrustSetpoint(_thrust_setpoint, angular_velocity.timestamp_sample);
publishThrustSetpoint(angular_velocity.timestamp_sample);
}
// scale effort by battery status if enabled
@ -307,14 +309,12 @@ void MulticopterRateControl::publishTorqueSetpoint(const Vector3f &torque_sp, co
_vehicle_torque_setpoint_pub.publish(vehicle_torque_setpoint);
}
void MulticopterRateControl::publishThrustSetpoint(const float thrust_setpoint, const hrt_abstime &timestamp_sample)
void MulticopterRateControl::publishThrustSetpoint(const hrt_abstime &timestamp_sample)
{
vehicle_thrust_setpoint_s vehicle_thrust_setpoint{};
vehicle_thrust_setpoint.timestamp = hrt_absolute_time();
vehicle_thrust_setpoint.timestamp_sample = timestamp_sample;
vehicle_thrust_setpoint.xyz[0] = 0.0f;
vehicle_thrust_setpoint.xyz[1] = 0.0f;
vehicle_thrust_setpoint.xyz[2] = PX4_ISFINITE(thrust_setpoint) ? -thrust_setpoint : 0.0f; // Z is Down
_thrust_setpoint.copyTo(vehicle_thrust_setpoint.xyz);
_vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint);
}

5
src/modules/mc_rate_control/MulticopterRateControl.hpp
View File

@ -94,8 +94,7 @@ private:
void updateActuatorControlsStatus(const actuator_controls_s &actuators, float dt);
void publishTorqueSetpoint(const matrix::Vector3f &torque_sp, const hrt_abstime &timestamp_sample);
void publishThrustSetpoint(const float thrust_setpoint, const hrt_abstime &timestamp_sample);
void publishThrustSetpoint(const hrt_abstime &timestamp_sample);
RateControl _rate_control; ///< class for rate control calculations
@ -138,7 +137,7 @@ private:
matrix::Vector3f _rates_setpoint{};
float _battery_status_scale{0.0f};
float _thrust_setpoint{0.0f};
matrix::Vector3f _thrust_setpoint{};
float _energy_integration_time{0.0f};
float _control_energy[4] {};