dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

MulticopterRateControl: set timestamp last before publishing torque and thrust setpoints

This commit is contained in:
Matthias Grob 2022-11-07 17:26:14 +01:00 committed by Daniel Agar
parent 3e35f948d8
commit ae606488bd
1 changed files with 2 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -296,22 +296,20 @@ MulticopterRateControl::Run()
void MulticopterRateControl::publishTorqueSetpoint(const Vector3f &torque_sp, const hrt_abstime &timestamp_sample)
{
vehicle_torque_setpoint_s vehicle_torque_setpoint{};
vehicle_torque_setpoint.timestamp = hrt_absolute_time();
vehicle_torque_setpoint.timestamp_sample = timestamp_sample;
vehicle_torque_setpoint.xyz[0] = (PX4_ISFINITE(torque_sp(0))) ? torque_sp(0) : 0.0f;
vehicle_torque_setpoint.xyz[1] = (PX4_ISFINITE(torque_sp(1))) ? torque_sp(1) : 0.0f;
vehicle_torque_setpoint.xyz[2] = (PX4_ISFINITE(torque_sp(2))) ? torque_sp(2) : 0.0f;
vehicle_torque_setpoint.timestamp = hrt_absolute_time();
_vehicle_torque_setpoint_pub.publish(vehicle_torque_setpoint);
}
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;
_thrust_setpoint.copyTo(vehicle_thrust_setpoint.xyz);
vehicle_thrust_setpoint.timestamp = hrt_absolute_time();
_vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint);
}