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

mc_pos_control: reuse timestamp to reduce hrt calls

Browse Source
This commit is contained in:
Matthias Grob
2019-10-26 06:11:18 +02:00
parent d0084766ff
commit be545db44f
1 changed files with 9 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+9 -9
View File
@@ -531,9 +531,9 @@ MulticopterPositionControl::Run()
parameters_update(false); parameters_update(false);
// set _dt in controllib Block - the time difference since the last loop iteration in seconds // set _dt in controllib Block - the time difference since the last loop iteration in seconds
const hrt_abstime time_stamp_current = hrt_absolute_time(); const hrt_abstime time_stamp_now = hrt_absolute_time();
setDt((time_stamp_current - _time_stamp_last_loop) / 1e6f); setDt((time_stamp_now - _time_stamp_last_loop) / 1e6f);
_time_stamp_last_loop = time_stamp_current; _time_stamp_last_loop = time_stamp_now;
const bool was_in_failsafe = _in_failsafe; const bool was_in_failsafe = _in_failsafe;
_in_failsafe = false; _in_failsafe = false;
@@ -558,7 +558,7 @@ MulticopterPositionControl::Run()
// an update is necessary here because otherwise the takeoff state doesn't get skiped with non-altitude-controlled modes // an update is necessary here because otherwise the takeoff state doesn't get skiped with non-altitude-controlled modes
_takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, false, 10.f, _takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, false, 10.f,
!_control_mode.flag_control_climb_rate_enabled, time_stamp_current); !_control_mode.flag_control_climb_rate_enabled, time_stamp_now);
// takeoff delay for motors to reach idle speed // takeoff delay for motors to reach idle speed
if (_takeoff.getTakeoffState() >= TakeoffState::ready_for_takeoff) { if (_takeoff.getTakeoffState() >= TakeoffState::ready_for_takeoff) {
@@ -588,7 +588,7 @@ MulticopterPositionControl::Run()
setpoint = _flight_tasks.getPositionSetpoint(); setpoint = _flight_tasks.getPositionSetpoint();
constraints = _flight_tasks.getConstraints(); constraints = _flight_tasks.getConstraints();
_failsafe_land_hysteresis.set_state_and_update(false, time_stamp_current); _failsafe_land_hysteresis.set_state_and_update(false, time_stamp_now);
// Check if position, velocity or thrust pairs are valid -> trigger failsaife if no pair is valid // Check if position, velocity or thrust pairs are valid -> trigger failsaife if no pair is valid
if (!(PX4_ISFINITE(setpoint.x) && PX4_ISFINITE(setpoint.y)) && if (!(PX4_ISFINITE(setpoint.x) && PX4_ISFINITE(setpoint.y)) &&
@@ -614,7 +614,7 @@ MulticopterPositionControl::Run()
// handle smooth takeoff // handle smooth takeoff
_takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, constraints.want_takeoff, _takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, constraints.want_takeoff,
constraints.speed_up, !_control_mode.flag_control_climb_rate_enabled, time_stamp_current); constraints.speed_up, !_control_mode.flag_control_climb_rate_enabled, time_stamp_now);
constraints.speed_up = _takeoff.updateRamp(_dt, constraints.speed_up); constraints.speed_up = _takeoff.updateRamp(_dt, constraints.speed_up);
if (_takeoff.getTakeoffState() < TakeoffState::rampup && !PX4_ISFINITE(setpoint.thrust[2])) { if (_takeoff.getTakeoffState() < TakeoffState::rampup && !PX4_ISFINITE(setpoint.thrust[2])) {
@@ -666,7 +666,7 @@ MulticopterPositionControl::Run()
// will remain NAN. Given that the PositionController cannot generate a position-setpoint, this type of setpoint is always equal to the input to the // will remain NAN. Given that the PositionController cannot generate a position-setpoint, this type of setpoint is always equal to the input to the
// PositionController. // PositionController.
vehicle_local_position_setpoint_s local_pos_sp{}; vehicle_local_position_setpoint_s local_pos_sp{};
local_pos_sp.timestamp = hrt_absolute_time(); local_pos_sp.timestamp = time_stamp_now;
_control.getLocalPositionSetpoint(local_pos_sp); _control.getLocalPositionSetpoint(local_pos_sp);
local_pos_sp.vx = PX4_ISFINITE(_control.getVelSp()(0)) ? _control.getVelSp()(0) : setpoint.vx; local_pos_sp.vx = PX4_ISFINITE(_control.getVelSp()(0)) ? _control.getVelSp()(0) : setpoint.vx;
local_pos_sp.vy = PX4_ISFINITE(_control.getVelSp()(1)) ? _control.getVelSp()(1) : setpoint.vy; local_pos_sp.vy = PX4_ISFINITE(_control.getVelSp()(1)) ? _control.getVelSp()(1) : setpoint.vy;
@@ -689,7 +689,7 @@ MulticopterPositionControl::Run()
} }
vehicle_attitude_setpoint_s attitude_setpoint{}; vehicle_attitude_setpoint_s attitude_setpoint{};
attitude_setpoint.timestamp = hrt_absolute_time(); attitude_setpoint.timestamp = time_stamp_now;
_control.getAttitudeSetpoint(attitude_setpoint); _control.getAttitudeSetpoint(attitude_setpoint);
@@ -709,7 +709,7 @@ MulticopterPositionControl::Run()
&& gear.landing_gear != landing_gear_s::GEAR_KEEP) { && gear.landing_gear != landing_gear_s::GEAR_KEEP) {
_landing_gear.landing_gear = gear.landing_gear; _landing_gear.landing_gear = gear.landing_gear;
_landing_gear.timestamp = hrt_absolute_time(); _landing_gear.timestamp = time_stamp_now;
_landing_gear_pub.publish(_landing_gear); _landing_gear_pub.publish(_landing_gear);
} }