fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 01:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

hysteresis: remove dependency/side effect on time

Browse Source
This commit is contained in:
Julian Oes
2019-05-27 09:57:50 +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/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.