forked from Archive/PX4-Autopilot
Remove relaying of maximum altitude through land detector
This commit is contained in:
parent
2445fa8b4c
commit
bbb04ab4b8
|
@ -64,7 +64,6 @@ param set-default FW_BAT_SCALE_EN 1
|
|||
param set-default FW_THR_ALT_SCL 2.7
|
||||
param set-default FW_T_RLL2THR 20
|
||||
|
||||
param set-default LNDMC_ALT_MAX 9999
|
||||
param set-default LNDMC_XY_VEL_MAX 1
|
||||
param set-default LNDMC_Z_VEL_MAX 0.7
|
||||
|
||||
|
|
|
@ -1,5 +1,4 @@
|
|||
uint64 timestamp # time since system start (microseconds)
|
||||
float32 alt_max # maximum altitude in [m] that can be reached
|
||||
bool freefall # true if vehicle is currently in free-fall
|
||||
bool ground_contact # true if vehicle has ground contact but is not landed (1. stage)
|
||||
bool maybe_landed # true if the vehicle might have landed (2. stage)
|
||||
|
|
|
@ -501,14 +501,14 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt,
|
|||
void FlightModeManager::limitAltitude(vehicle_local_position_setpoint_s &setpoint,
|
||||
const vehicle_local_position_s &vehicle_local_position)
|
||||
{
|
||||
if (_vehicle_land_detected_sub.get().alt_max < 0.0f || !_home_position_sub.get().valid_alt
|
||||
if (_param_lndmc_alt_max.get() < 0.0f || !_home_position_sub.get().valid_alt
|
||||
|| !vehicle_local_position.z_valid || !vehicle_local_position.v_z_valid) {
|
||||
// there is no altitude limitation present or the required information not available
|
||||
return;
|
||||
}
|
||||
|
||||
// maximum altitude == minimal z-value (NED)
|
||||
const float min_z = _home_position_sub.get().z + (-_vehicle_land_detected_sub.get().alt_max);
|
||||
const float min_z = _home_position_sub.get().z + (-_param_lndmc_alt_max.get());
|
||||
|
||||
if (vehicle_local_position.z < min_z) {
|
||||
// above maximum altitude, only allow downwards flight == positive vz-setpoints (NED)
|
||||
|
|
|
@ -158,6 +158,7 @@ private:
|
|||
uORB::Publication<vehicle_constraints_s> _vehicle_constraints_pub{ORB_ID(vehicle_constraints)};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::LNDMC_ALT_MAX>) _param_lndmc_alt_max,
|
||||
(ParamInt<px4::params::MPC_POS_MODE>) _param_mpc_pos_mode
|
||||
);
|
||||
};
|
||||
|
|
|
@ -122,7 +122,6 @@ void LandDetector::Run()
|
|||
const bool ground_contactDetected = _ground_contact_hysteresis.get_state();
|
||||
const bool maybe_landedDetected = _maybe_landed_hysteresis.get_state();
|
||||
const bool landDetected = _landed_hysteresis.get_state();
|
||||
const float alt_max = _get_max_altitude() > 0.0f ? _get_max_altitude() : (float)INFINITY;
|
||||
const bool in_ground_effect = _ground_effect_hysteresis.get_state();
|
||||
|
||||
// publish at 1 Hz, very first time, or when the result has changed
|
||||
|
@ -131,8 +130,7 @@ void LandDetector::Run()
|
|||
(_land_detected.freefall != freefallDetected) ||
|
||||
(_land_detected.maybe_landed != maybe_landedDetected) ||
|
||||
(_land_detected.ground_contact != ground_contactDetected) ||
|
||||
(_land_detected.in_ground_effect != in_ground_effect) ||
|
||||
(fabsf(_land_detected.alt_max - alt_max) > FLT_EPSILON)) {
|
||||
(_land_detected.in_ground_effect != in_ground_effect)) {
|
||||
|
||||
if (!landDetected && _land_detected.landed && _takeoff_time == 0) { /* only set take off time once, until disarming */
|
||||
// We did take off
|
||||
|
@ -143,7 +141,6 @@ void LandDetector::Run()
|
|||
_land_detected.freefall = freefallDetected;
|
||||
_land_detected.maybe_landed = maybe_landedDetected;
|
||||
_land_detected.ground_contact = ground_contactDetected;
|
||||
_land_detected.alt_max = alt_max;
|
||||
_land_detected.in_ground_effect = in_ground_effect;
|
||||
_land_detected.in_descend = _get_in_descend();
|
||||
_land_detected.has_low_throttle = _get_has_low_throttle();
|
||||
|
|
|
@ -126,11 +126,6 @@ protected:
|
|||
*/
|
||||
virtual bool _get_freefall_state() { return false; }
|
||||
|
||||
/**
|
||||
* @return maximum altitude that can be reached
|
||||
*/
|
||||
virtual float _get_max_altitude() { return INFINITY; }
|
||||
|
||||
/**
|
||||
* @return true if vehicle could be in ground effect (close to ground)
|
||||
*/
|
||||
|
@ -163,7 +158,6 @@ private:
|
|||
|
||||
vehicle_land_detected_s _land_detected = {
|
||||
.timestamp = 0,
|
||||
.alt_max = -1.0f,
|
||||
.freefall = false,
|
||||
.ground_contact = true,
|
||||
.maybe_landed = true,
|
||||
|
|
|
@ -323,16 +323,6 @@ bool MulticopterLandDetector::_get_landed_state()
|
|||
return _maybe_landed_hysteresis.get_state();
|
||||
}
|
||||
|
||||
float MulticopterLandDetector::_get_max_altitude()
|
||||
{
|
||||
if (_param_lndmc_alt_max.get() < 0.0f) {
|
||||
return INFINITY;
|
||||
|
||||
} else {
|
||||
return _param_lndmc_alt_max.get();
|
||||
}
|
||||
}
|
||||
|
||||
float MulticopterLandDetector::_get_gnd_effect_altitude()
|
||||
{
|
||||
if (_param_lndmc_alt_gnd_effect.get() < 0.0f) {
|
||||
|
|
|
@ -76,7 +76,6 @@ protected:
|
|||
bool _get_horizontal_movement() override { return _horizontal_movement; }
|
||||
bool _get_vertical_movement() override { return _vertical_movement; }
|
||||
bool _get_close_to_ground_or_skipped_check() override { return _close_to_ground_or_skipped_check; }
|
||||
float _get_max_altitude() override;
|
||||
|
||||
void _set_hysteresis_factor(const int factor) override;
|
||||
private:
|
||||
|
@ -140,7 +139,6 @@ private:
|
|||
DEFINE_PARAMETERS_CUSTOM_PARENT(
|
||||
LandDetector,
|
||||
(ParamFloat<px4::params::LNDMC_TRIG_TIME>) _param_lndmc_trig_time,
|
||||
(ParamFloat<px4::params::LNDMC_ALT_MAX>) _param_lndmc_alt_max,
|
||||
(ParamFloat<px4::params::LNDMC_ROT_MAX>) _param_lndmc_rot_max,
|
||||
(ParamFloat<px4::params::LNDMC_XY_VEL_MAX>) _param_lndmc_xy_vel_max,
|
||||
(ParamFloat<px4::params::LNDMC_Z_VEL_MAX>) _param_lndmc_z_vel_max,
|
||||
|
|
|
@ -120,7 +120,6 @@ private:
|
|||
|
||||
vehicle_land_detected_s _vehicle_land_detected {
|
||||
.timestamp = 0,
|
||||
.alt_max = -1.0f,
|
||||
.freefall = false,
|
||||
.ground_contact = true,
|
||||
.maybe_landed = true,
|
||||
|
|
|
@ -770,12 +770,11 @@ MissionBlock::set_vtol_transition_item(struct mission_item_s *item, const uint8_
|
|||
void
|
||||
MissionBlock::mission_apply_limitation(mission_item_s &item)
|
||||
{
|
||||
/*
|
||||
* Limit altitude
|
||||
*/
|
||||
// Limit altitude
|
||||
const float maximum_altitude = _navigator->get_lndmc_alt_max();
|
||||
|
||||
/* do nothing if altitude max is negative */
|
||||
if (_navigator->get_land_detected()->alt_max > 0.0f) {
|
||||
if (maximum_altitude > 0.0f) {
|
||||
|
||||
/* absolute altitude */
|
||||
float altitude_abs = item.altitude_is_relative
|
||||
|
@ -783,17 +782,12 @@ MissionBlock::mission_apply_limitation(mission_item_s &item)
|
|||
: item.altitude;
|
||||
|
||||
/* limit altitude to maximum allowed altitude */
|
||||
if ((_navigator->get_land_detected()->alt_max + _navigator->get_home_position()->alt) < altitude_abs) {
|
||||
if ((maximum_altitude + _navigator->get_home_position()->alt) < altitude_abs) {
|
||||
item.altitude = item.altitude_is_relative ?
|
||||
_navigator->get_land_detected()->alt_max :
|
||||
_navigator->get_land_detected()->alt_max + _navigator->get_home_position()->alt;
|
||||
|
||||
maximum_altitude :
|
||||
maximum_altitude + _navigator->get_home_position()->alt;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Add other limitations here
|
||||
*/
|
||||
}
|
||||
|
||||
float
|
||||
|
|
|
@ -299,6 +299,7 @@ public:
|
|||
bool get_takeoff_required() const { return _param_mis_takeoff_req.get(); }
|
||||
float get_yaw_timeout() const { return _param_mis_yaw_tmt.get(); }
|
||||
float get_yaw_threshold() const { return math::radians(_param_mis_yaw_err.get()); }
|
||||
float get_lndmc_alt_max() const { return _param_lndmc_alt_max.get(); }
|
||||
|
||||
float get_vtol_back_trans_deceleration() const { return _param_back_trans_dec_mss; }
|
||||
float get_vtol_reverse_delay() const { return _param_reverse_delay; }
|
||||
|
@ -329,7 +330,10 @@ private:
|
|||
(ParamFloat<px4::params::MIS_TAKEOFF_ALT>) _param_mis_takeoff_alt,
|
||||
(ParamBool<px4::params::MIS_TAKEOFF_REQ>) _param_mis_takeoff_req,
|
||||
(ParamFloat<px4::params::MIS_YAW_TMT>) _param_mis_yaw_tmt,
|
||||
(ParamFloat<px4::params::MIS_YAW_ERR>) _param_mis_yaw_err
|
||||
(ParamFloat<px4::params::MIS_YAW_ERR>) _param_mis_yaw_err,
|
||||
|
||||
(ParamFloat<px4::params::LNDMC_ALT_MAX>) _param_lndmc_alt_max
|
||||
|
||||
)
|
||||
|
||||
struct traffic_buffer_s {
|
||||
|
|
Loading…
Reference in New Issue