diff --git a/src/lib/hysteresis/hysteresis.cpp b/src/lib/hysteresis/hysteresis.cpp index 473867d044..e75327de8a 100644 --- a/src/lib/hysteresis/hysteresis.cpp +++ b/src/lib/hysteresis/hysteresis.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2016-2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -54,27 +54,27 @@ Hysteresis::set_hysteresis_time_from(const bool from_state, const hrt_abstime ne } void -Hysteresis::set_state_and_update(const bool new_state) +Hysteresis::set_state_and_update(const bool new_state, const hrt_abstime &now_us) { if (new_state != _state) { if (new_state != _requested_state) { _requested_state = new_state; - _last_time_to_change_state = hrt_absolute_time(); + _last_time_to_change_state = now_us; } } else { _requested_state = _state; } - update(); + update(now_us); } void -Hysteresis::update() +Hysteresis::update(const hrt_abstime &now_us) { if (_requested_state != _state) { - const hrt_abstime elapsed = hrt_elapsed_time(&_last_time_to_change_state); + const hrt_abstime elapsed = now_us - _last_time_to_change_state; if (_state && !_requested_state) { // true -> false diff --git a/src/lib/hysteresis/hysteresis.h b/src/lib/hysteresis/hysteresis.h index 2fbc271d0c..9c16ce4933 100644 --- a/src/lib/hysteresis/hysteresis.h +++ b/src/lib/hysteresis/hysteresis.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2016-2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -61,9 +61,9 @@ public: void set_hysteresis_time_from(const bool from_state, const hrt_abstime new_hysteresis_time_us); - void set_state_and_update(const bool new_state); + void set_state_and_update(const bool new_state, const hrt_abstime &now_us); - void update(); + void update(const hrt_abstime &now_us); private: diff --git a/src/lib/hysteresis/hysteresis_test.cpp b/src/lib/hysteresis/hysteresis_test.cpp index e477b834ed..5426c21a91 100644 --- a/src/lib/hysteresis/hysteresis_test.cpp +++ b/src/lib/hysteresis/hysteresis_test.cpp @@ -37,16 +37,11 @@ */ #include -#include #include "hysteresis.h" -#if defined(__PX4_DARWIN) || defined(__PX4_CYGWIN) -static const int f = 10; -#else -static const int f = 1; -#endif +static constexpr hrt_abstime SOME_START_TIME = 1558359616134000llu; TEST(Hysteresis, InitFalse) @@ -63,138 +58,147 @@ TEST(Hysteresis, InitTrue) TEST(Hysteresis, Zero) { + hrt_abstime time_us = SOME_START_TIME; + // Default is 0 hysteresis. systemlib::Hysteresis hysteresis(false); EXPECT_FALSE(hysteresis.get_state()); // Change and see result immediately. - hysteresis.set_state_and_update(true); + hysteresis.set_state_and_update(true, time_us); EXPECT_TRUE(hysteresis.get_state()); - hysteresis.set_state_and_update(false); + hysteresis.set_state_and_update(false, time_us); EXPECT_FALSE(hysteresis.get_state()); - hysteresis.set_state_and_update(true); + hysteresis.set_state_and_update(true, time_us); EXPECT_TRUE(hysteresis.get_state()); + time_us += 1000; // A wait won't change anything. - px4_usleep(1000 * f); - hysteresis.update(); + hysteresis.update(time_us); EXPECT_TRUE(hysteresis.get_state()); } TEST(Hysteresis, ChangeAfterTime) { + hrt_abstime time_us = SOME_START_TIME; systemlib::Hysteresis hysteresis(false); - hysteresis.set_hysteresis_time_from(false, 5000 * f); - hysteresis.set_hysteresis_time_from(true, 3000 * f); + hysteresis.set_hysteresis_time_from(false, 5000); + hysteresis.set_hysteresis_time_from(true, 3000); // Change to true. - hysteresis.set_state_and_update(true); + hysteresis.set_state_and_update(true, time_us); EXPECT_FALSE(hysteresis.get_state()); - px4_usleep(4000 * f); - hysteresis.update(); + time_us += 4000; + hysteresis.update(time_us); EXPECT_FALSE(hysteresis.get_state()); - px4_usleep(2000 * f); - hysteresis.update(); + time_us += 2000; + hysteresis.update(time_us); EXPECT_TRUE(hysteresis.get_state()); // Change back to false. - hysteresis.set_state_and_update(false); + hysteresis.set_state_and_update(false, time_us); EXPECT_TRUE(hysteresis.get_state()); - px4_usleep(1000 * f); - hysteresis.update(); + time_us += 1000; + hysteresis.update(time_us); EXPECT_TRUE(hysteresis.get_state()); - px4_usleep(3000 * f); - hysteresis.update(); + time_us += 3000; + hysteresis.update(time_us); EXPECT_FALSE(hysteresis.get_state()); } TEST(Hysteresis, HysteresisChanged) { + hrt_abstime time_us = SOME_START_TIME; + systemlib::Hysteresis hysteresis(false); - hysteresis.set_hysteresis_time_from(true, 2000 * f); - hysteresis.set_hysteresis_time_from(false, 5000 * f); + hysteresis.set_hysteresis_time_from(true, 2000); + hysteresis.set_hysteresis_time_from(false, 5000); // Change to true. - hysteresis.set_state_and_update(true); + hysteresis.set_state_and_update(true, time_us); EXPECT_FALSE(hysteresis.get_state()); - px4_usleep(3000 * f); - hysteresis.update(); + time_us += 3000; + hysteresis.update(time_us); EXPECT_FALSE(hysteresis.get_state()); - px4_usleep(3000 * f); - hysteresis.update(); + time_us += 3000; + hysteresis.update(time_us); EXPECT_TRUE(hysteresis.get_state()); // Change hysteresis time. - hysteresis.set_hysteresis_time_from(true, 10000 * f); + hysteresis.set_hysteresis_time_from(true, 10000); // Change back to false. - hysteresis.set_state_and_update(false); + hysteresis.set_state_and_update(false, time_us); EXPECT_TRUE(hysteresis.get_state()); - px4_usleep(7000 * f); - hysteresis.update(); + time_us += 7000; + hysteresis.update(time_us); EXPECT_TRUE(hysteresis.get_state()); - px4_usleep(5000 * f); - hysteresis.update(); + time_us += 5000; + hysteresis.update(time_us); EXPECT_FALSE(hysteresis.get_state()); } TEST(Hysteresis, ChangeAfterMultipleSets) { + hrt_abstime time_us = SOME_START_TIME; + systemlib::Hysteresis hysteresis(false); - hysteresis.set_hysteresis_time_from(true, 5000 * f); - hysteresis.set_hysteresis_time_from(false, 5000 * f); + hysteresis.set_hysteresis_time_from(true, 5000); + hysteresis.set_hysteresis_time_from(false, 5000); // Change to true. - hysteresis.set_state_and_update(true); + hysteresis.set_state_and_update(true, time_us); EXPECT_FALSE(hysteresis.get_state()); - px4_usleep(3000 * f); - hysteresis.set_state_and_update(true); + time_us += 3000; + hysteresis.set_state_and_update(true, time_us); EXPECT_FALSE(hysteresis.get_state()); - px4_usleep(3000 * f); - hysteresis.set_state_and_update(true); + time_us += 3000; + hysteresis.set_state_and_update(true, time_us); EXPECT_TRUE(hysteresis.get_state()); // Change to false. - hysteresis.set_state_and_update(false); + hysteresis.set_state_and_update(false, time_us); EXPECT_TRUE(hysteresis.get_state()); - px4_usleep(3000 * f); - hysteresis.set_state_and_update(false); + time_us += 3000; + hysteresis.set_state_and_update(false, time_us); EXPECT_TRUE(hysteresis.get_state()); - px4_usleep(3000 * f); - hysteresis.set_state_and_update(false); + time_us += 3000; + hysteresis.set_state_and_update(false, time_us); EXPECT_FALSE(hysteresis.get_state()); } TEST(Hysteresis, TakeChangeBack) { + hrt_abstime time_us = SOME_START_TIME; + systemlib::Hysteresis hysteresis(false); - hysteresis.set_hysteresis_time_from(false, 5000 * f); + hysteresis.set_hysteresis_time_from(false, 5000); // Change to true. - hysteresis.set_state_and_update(true); + hysteresis.set_state_and_update(true, time_us); EXPECT_FALSE(hysteresis.get_state()); - px4_usleep(3000 * f); - hysteresis.update(); + time_us += 3000; + hysteresis.update(time_us); EXPECT_FALSE(hysteresis.get_state()); // Change your mind to false. - hysteresis.set_state_and_update(false); + hysteresis.set_state_and_update(false, time_us); EXPECT_FALSE(hysteresis.get_state()); - px4_usleep(6000 * f); - hysteresis.update(); + time_us += 6000; + hysteresis.update(time_us); EXPECT_FALSE(hysteresis.get_state()); // And true again - hysteresis.set_state_and_update(true); + hysteresis.set_state_and_update(true, time_us); EXPECT_FALSE(hysteresis.get_state()); - px4_usleep(3000 * f); - hysteresis.update(); + time_us += 3000; + hysteresis.update(time_us); EXPECT_FALSE(hysteresis.get_state()); - px4_usleep(3000 * f); - hysteresis.update(); + time_us += 3000; + hysteresis.update(time_us); EXPECT_TRUE(hysteresis.get_state()); // The other directory is immediate. - hysteresis.set_state_and_update(false); + hysteresis.set_state_and_update(false, time_us); EXPECT_FALSE(hysteresis.get_state()); } diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 0ac7f80ded..15a7c199d6 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1715,7 +1715,7 @@ Commander::run() _auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_land.get() * 1_s); } - _auto_disarm_landed.set_state_and_update(land_detector.landed); + _auto_disarm_landed.set_state_and_update(land_detector.landed, hrt_absolute_time()); if (_auto_disarm_landed.get_state()) { arm_disarm(false, &mavlink_log_pub, "Auto disarm on land"); @@ -1724,15 +1724,15 @@ Commander::run() // Auto disarm after 5 seconds if kill switch is engaged - _auto_disarm_killed.set_state_and_update(armed.manual_lockdown); + _auto_disarm_killed.set_state_and_update(armed.manual_lockdown, hrt_absolute_time()); if (_auto_disarm_killed.get_state()) { arm_disarm(false, &mavlink_log_pub, "Kill-switch still engaged, disarming"); } } else { - _auto_disarm_landed.set_state_and_update(false); - _auto_disarm_killed.set_state_and_update(false); + _auto_disarm_landed.set_state_and_update(false, hrt_absolute_time()); + _auto_disarm_killed.set_state_and_update(false, hrt_absolute_time()); } diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 1aa977112a..96f7d9da58 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -207,11 +207,12 @@ void LandDetector::_update_state() { /* when we are landed we also have ground contact for sure but only one output state can be true at a particular time * with higher priority for landed */ - _freefall_hysteresis.set_state_and_update(_get_freefall_state()); - _landed_hysteresis.set_state_and_update(_get_landed_state()); - _maybe_landed_hysteresis.set_state_and_update(_get_maybe_landed_state()); - _ground_contact_hysteresis.set_state_and_update(_get_ground_contact_state()); - _ground_effect_hysteresis.set_state_and_update(_get_ground_effect_state()); + const hrt_abstime now_us = hrt_absolute_time(); + _freefall_hysteresis.set_state_and_update(_get_freefall_state(), now_us); + _landed_hysteresis.set_state_and_update(_get_landed_state(), now_us); + _maybe_landed_hysteresis.set_state_and_update(_get_maybe_landed_state(), now_us); + _ground_contact_hysteresis.set_state_and_update(_get_ground_contact_state(), now_us); + _ground_effect_hysteresis.set_state_and_update(_get_ground_effect_state(), now_us); if (_freefall_hysteresis.get_state()) { _state = LandDetectionState::FREEFALL; diff --git a/src/modules/mc_pos_control/Takeoff/Takeoff.cpp b/src/modules/mc_pos_control/Takeoff/Takeoff.cpp index 70af191343..944b4cd078 100644 --- a/src/modules/mc_pos_control/Takeoff/Takeoff.cpp +++ b/src/modules/mc_pos_control/Takeoff/Takeoff.cpp @@ -45,9 +45,9 @@ void Takeoff::generateInitialRampValue(const float hover_thrust, float velocity_ } void Takeoff::updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff, - const float takeoff_desired_vz, const bool skip_takeoff) + const float takeoff_desired_vz, const bool skip_takeoff, const hrt_abstime &now_us) { - _spoolup_time_hysteresis.set_state_and_update(armed); + _spoolup_time_hysteresis.set_state_and_update(armed, now_us); switch (_takeoff_state) { case TakeoffState::disarmed: diff --git a/src/modules/mc_pos_control/Takeoff/Takeoff.hpp b/src/modules/mc_pos_control/Takeoff/Takeoff.hpp index d3bb433d20..67f6e69ddc 100644 --- a/src/modules/mc_pos_control/Takeoff/Takeoff.hpp +++ b/src/modules/mc_pos_control/Takeoff/Takeoff.hpp @@ -39,7 +39,7 @@ #pragma once -#include +#include #include using namespace time_literals; @@ -76,7 +76,7 @@ public: * @return true if setpoint has updated correctly */ void updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff, - const float takeoff_desired_vz, const bool skip_takeoff); + const float takeoff_desired_vz, const bool skip_takeoff, const hrt_abstime &now_us); /** * Update and return the velocity constraint ramp value during takeoff. diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index dee7ad415b..bc422ca67e 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -585,7 +585,7 @@ MulticopterPositionControl::run() } // 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, !_control_mode.flag_control_climb_rate_enabled); + _takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, false, 10.f, !_control_mode.flag_control_climb_rate_enabled, time_stamp_current); // takeoff delay for motors to reach idle speed if (_takeoff.getTakeoffState() >= TakeoffState::ready_for_takeoff) { @@ -613,7 +613,7 @@ MulticopterPositionControl::run() } else { setpoint = _flight_tasks.getPositionSetpoint(); - _failsafe_land_hysteresis.set_state_and_update(false); + _failsafe_land_hysteresis.set_state_and_update(false, time_stamp_current); // 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)) && @@ -638,7 +638,7 @@ MulticopterPositionControl::run() set_vehicle_states(setpoint.vz); // handle smooth takeoff - _takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, constraints.want_takeoff, constraints.speed_up, !_control_mode.flag_control_climb_rate_enabled); + _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 = _takeoff.updateRamp(_dt, constraints.speed_up); if (_takeoff.getTakeoffState() < TakeoffState::rampup && !PX4_ISFINITE(setpoint.thrust[2])) { @@ -976,7 +976,7 @@ void MulticopterPositionControl::failsafe(vehicle_local_position_setpoint_s &setpoint, const PositionControlStates &states, const bool force, const bool warn) { - _failsafe_land_hysteresis.set_state_and_update(true); + _failsafe_land_hysteresis.set_state_and_update(true, hrt_absolute_time()); if (!_failsafe_land_hysteresis.get_state() && !force) { // just keep current setpoint and don't do anything.