fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 22:03: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/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.