Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 08:36:08 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

hysteresis: remove dependency/side effect on time

Browse Source
This commit is contained in:
Julian Oes
2019-05-20 16:01:09 +02:00
parent 7b9562e3b1
commit b29e5e3adb
8 changed files with 92 additions and 87 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/hysteresis/hysteresis.cpp
+6 -6
View File
@@ -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
src/lib/hysteresis/hysteresis.h
+3 -3
View File
@@ -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:
src/lib/hysteresis/hysteresis_test.cpp
+65 -61
View File
@@ -37,16 +37,11 @@
*/
#include <gtest/gtest.h>
#include <unistd.h>
#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());
}
src/modules/commander/Commander.cpp
+4 -4
View File
@@ -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());
}
src/modules/land_detector/LandDetector.cpp
+6 -5
View File
@@ -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;
src/modules/mc_pos_control/Takeoff/Takeoff.cpp
+2 -2
View File
@@ -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:
src/modules/mc_pos_control/Takeoff/Takeoff.hpp
+2 -2
View File
@@ -39,7 +39,7 @@
#pragma once
#include <systemlib/hysteresis/hysteresis.h>
#include <lib/hysteresis/hysteresis.h>
#include <drivers/drv_hrt.h>
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.
src/modules/mc_pos_control/mc_pos_control_main.cpp
+4 -4
View File
@@ -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.