Browse Source

MulticoperPositionControl: remove time_stamp_now alias for timestamp_sample of the local position

to make it explicit what is used is not a fresh hrt_absolute_time() call
by this module.
main
Matthias Grob 3 years ago
parent
commit
424fd8b304
  1. 20
      src/modules/mc_pos_control/MulticopterPositionControl.cpp

20
src/modules/mc_pos_control/MulticopterPositionControl.cpp

@ -316,9 +316,9 @@ void MulticopterPositionControl::Run() @@ -316,9 +316,9 @@ void MulticopterPositionControl::Run()
vehicle_local_position_s vehicle_local_position;
if (_local_pos_sub.update(&vehicle_local_position)) {
const hrt_abstime time_stamp_now = vehicle_local_position.timestamp_sample;
const float dt = math::constrain(((time_stamp_now - _time_stamp_last_loop) * 1e-6f), 0.002f, 0.04f);
_time_stamp_last_loop = time_stamp_now;
const float dt =
math::constrain(((vehicle_local_position.timestamp_sample - _time_stamp_last_loop) * 1e-6f), 0.002f, 0.04f);
_time_stamp_last_loop = vehicle_local_position.timestamp_sample;
// set _dt in controllib Block for BlockDerivative
setDt(dt);
@ -400,9 +400,9 @@ void MulticopterPositionControl::Run() @@ -400,9 +400,9 @@ void MulticopterPositionControl::Run()
// set failsafe setpoint if there hasn't been a new
// trajectory setpoint since position control started
if ((_setpoint.timestamp < _time_position_control_enabled)
&& (time_stamp_now > _time_position_control_enabled)) {
&& (vehicle_local_position.timestamp_sample > _time_position_control_enabled)) {
failsafe(time_stamp_now, _setpoint, states);
failsafe(vehicle_local_position.timestamp_sample, _setpoint, states);
_setpoint.timestamp = vehicle_local_position.timestamp;
}
}
@ -422,7 +422,7 @@ void MulticopterPositionControl::Run() @@ -422,7 +422,7 @@ void MulticopterPositionControl::Run()
if (_vehicle_control_mode.flag_control_offboard_enabled) {
bool want_takeoff = _vehicle_control_mode.flag_armed && _vehicle_land_detected.landed
&& (time_stamp_now < _setpoint.timestamp + 1_s);
&& (vehicle_local_position.timestamp_sample < _setpoint.timestamp + 1_s);
if (want_takeoff && PX4_ISFINITE(_setpoint.z)
&& (_setpoint.z < states.position(2))) {
@ -451,7 +451,7 @@ void MulticopterPositionControl::Run() @@ -451,7 +451,7 @@ void MulticopterPositionControl::Run()
// handle smooth takeoff
_takeoff.updateTakeoffState(_vehicle_control_mode.flag_armed, _vehicle_land_detected.landed,
_vehicle_constraints.want_takeoff,
_vehicle_constraints.speed_up, false, time_stamp_now);
_vehicle_constraints.speed_up, false, vehicle_local_position.timestamp_sample);
const bool flying = (_takeoff.getTakeoffState() >= TakeoffState::flight);
@ -515,13 +515,13 @@ void MulticopterPositionControl::Run() @@ -515,13 +515,13 @@ void MulticopterPositionControl::Run()
// Run position control
if (_control.update(dt)) {
_failsafe_land_hysteresis.set_state_and_update(false, time_stamp_now);
_failsafe_land_hysteresis.set_state_and_update(false, vehicle_local_position.timestamp_sample);
} else {
// Failsafe
vehicle_local_position_setpoint_s failsafe_setpoint{};
failsafe(time_stamp_now, failsafe_setpoint, states);
failsafe(vehicle_local_position.timestamp_sample, failsafe_setpoint, states);
// reset constraints
_vehicle_constraints = {0, NAN, NAN, false, {}};
@ -548,7 +548,7 @@ void MulticopterPositionControl::Run() @@ -548,7 +548,7 @@ void MulticopterPositionControl::Run()
} else {
// an update is necessary here because otherwise the takeoff state doesn't get skiped with non-altitude-controlled modes
_takeoff.updateTakeoffState(_vehicle_control_mode.flag_armed, _vehicle_land_detected.landed, false, 10.f, true,
time_stamp_now);
vehicle_local_position.timestamp_sample);
}
// Publish takeoff status

Loading…
Cancel
Save