Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add max_altitude and _vehicle_attitude.timestamp validity checks to MulticopterLandDetector and standardize var naming #12681

Merged
merged 1 commit into from
Sep 2, 2019
Merged

Add max_altitude and _vehicle_attitude.timestamp validity checks to MulticopterLandDetector and standardize var naming #12681

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 3 additions & 3 deletions src/modules/land_detector/LandDetector.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,16 +42,16 @@

#pragma once

#include <px4_module.h>
#include <px4_module_params.h>
#include <lib/hysteresis/hysteresis.h>
#include <parameters/param.h>
#include <perf/perf_counter.h>
#include <px4_module.h>
#include <px4_module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>

using namespace time_literals;

Expand Down
36 changes: 20 additions & 16 deletions src/modules/land_detector/MulticopterLandDetector.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -91,7 +91,7 @@ void MulticopterLandDetector::_update_topics()
_vehicle_acceleration_sub.update(&_vehicle_acceleration);
_vehicle_angular_velocity_sub.update(&_vehicle_angular_velocity);
_vehicle_attitude_sub.update(&_vehicle_attitude);
_vehicle_control_mode_sub.update(&_control_mode);
_vehicle_control_mode_sub.update(&_vehicle_control_mode);
_vehicle_local_position_sub.update(&_vehicle_local_position);
_vehicle_local_position_setpoint_sub.update(&_vehicle_local_position_setpoint);
}
Expand Down Expand Up @@ -137,20 +137,20 @@ bool MulticopterLandDetector::_get_ground_contact_state()
// Check if we are moving vertically - this might see a spike after arming due to
// throttle-up vibration. If accelerating fast the throttle thresholds will still give
// an accurate in-air indication.
bool verticalMovement;
bool vertical_movement = false;

if (hrt_elapsed_time(&_landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) {

// Widen acceptance thresholds for landed state right after arming
// so that motor spool-up and other effects do not trigger false negatives.
verticalMovement = fabsf(_vehicle_local_position.vz) > _param_lndmc_z_vel_max.get() * 2.5f;
vertical_movement = fabsf(_vehicle_local_position.vz) > _param_lndmc_z_vel_max.get() * 2.5f;

} else {

// Adjust maxClimbRate if land_speed is lower than 2x maxClimbrate
float maxClimbRate = ((land_speed_threshold * 0.5f) < _param_lndmc_z_vel_max.get()) ? (0.5f * land_speed_threshold) :
_param_lndmc_z_vel_max.get();
verticalMovement = fabsf(_vehicle_local_position.vz) > maxClimbRate;
// Adjust max_climb_rate if land_speed is lower than 2x max_climb_rate
float max_climb_rate = ((land_speed_threshold * 0.5f) < _param_lndmc_z_vel_max.get()) ? (0.5f * land_speed_threshold) :
_param_lndmc_z_vel_max.get();
vertical_movement = fabsf(_vehicle_local_position.vz) > max_climb_rate;
}

// Check if we are moving horizontally.
Expand All @@ -161,11 +161,11 @@ bool MulticopterLandDetector::_get_ground_contact_state()
// we then can assume that the vehicle hit ground
_in_descend = _is_climb_rate_enabled()
&& (_vehicle_local_position_setpoint.vz >= land_speed_threshold);
bool hit_ground = _in_descend && !verticalMovement;
bool hit_ground = _in_descend && !vertical_movement;

// TODO: we need an accelerometer based check for vertical movement for flying without GPS
if ((_has_low_thrust() || hit_ground) && (!_horizontal_movement || !_has_position_lock())
&& (!verticalMovement || !_has_altitude_lock())) {
&& (!vertical_movement || !_has_altitude_lock())) {
return true;
}

Expand All @@ -178,7 +178,7 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
const hrt_abstime now = hrt_absolute_time();

// When not armed, consider to be maybe-landed
if (!_actuator_armed.armed) {
if (!_actuator_armed.armed || (_vehicle_attitude.timestamp == 0)) {
return true;
}

Expand All @@ -199,11 +199,11 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
}

// Next look if all rotation angles are not moving.
float maxRotationScaled = math::radians(_param_lndmc_rot_max.get()) * landThresholdFactor;
float max_rotation_scaled = math::radians(_param_lndmc_rot_max.get()) * landThresholdFactor;

bool rotating = (fabsf(_vehicle_angular_velocity.xyz[0]) > maxRotationScaled) ||
(fabsf(_vehicle_angular_velocity.xyz[1]) > maxRotationScaled) ||
(fabsf(_vehicle_angular_velocity.xyz[2]) > maxRotationScaled);
bool rotating = (fabsf(_vehicle_angular_velocity.xyz[0]) > max_rotation_scaled) ||
(fabsf(_vehicle_angular_velocity.xyz[1]) > max_rotation_scaled) ||
(fabsf(_vehicle_angular_velocity.xyz[2]) > max_rotation_scaled);

// Return status based on armed state and throttle if no position lock is available.
if (!_has_altitude_lock() && !rotating) {
Expand Down Expand Up @@ -251,6 +251,10 @@ float MulticopterLandDetector::_get_max_altitude()
/* TODO: add a meaningful altitude */
float valid_altitude_max = _param_lndmc_alt_max.get();

if (valid_altitude_max < 0.0f) {
return INFINITY;
Copy link
Member

@dagar dagar Aug 26, 2019 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Does this cause vehicle_land_detected to be published constantly (~ 50 Hz)? Check uorb top.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Hi @dagar, It doesn't appear to:
image

}

if (_battery_status.warning == battery_status_s::BATTERY_WARNING_LOW) {
valid_altitude_max = _param_lndmc_alt_max.get() * 0.75f;
}
Expand Down Expand Up @@ -283,7 +287,7 @@ bool MulticopterLandDetector::_is_climb_rate_enabled()
bool has_updated = (_vehicle_local_position_setpoint.timestamp != 0)
&& (hrt_elapsed_time(&_vehicle_local_position_setpoint.timestamp) < 500_ms);

return (_control_mode.flag_control_climb_rate_enabled && has_updated
return (_vehicle_control_mode.flag_control_climb_rate_enabled && has_updated
&& PX4_ISFINITE(_vehicle_local_position_setpoint.vz));
}

Expand All @@ -303,7 +307,7 @@ bool MulticopterLandDetector::_has_minimal_thrust()
float sys_min_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * 0.1f;

// Determine the system min throttle based on flight mode
if (!_control_mode.flag_control_climb_rate_enabled) {
if (!_vehicle_control_mode.flag_control_climb_rate_enabled) {
sys_min_throttle = (_params.minManThrottle + 0.01f);
}

Expand Down
20 changes: 8 additions & 12 deletions src/modules/land_detector/MulticopterLandDetector.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,22 +42,19 @@

#pragma once

#include "LandDetector.h"
#include <math.h>
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is this just for the INFINITY?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes, that's correct.


#include <parameters/param.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_bias.h>
#include <uORB/topics/vehicle_acceleration.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>

#include "LandDetector.h"

using namespace time_literals;

Expand Down Expand Up @@ -122,7 +119,6 @@ class MulticopterLandDetector : public LandDetector

uORB::Subscription _actuator_controls_sub{ORB_ID(actuator_controls_0)};
uORB::Subscription _battery_sub{ORB_ID(battery_status)};
uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)};
uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
Expand All @@ -132,17 +128,17 @@ class MulticopterLandDetector : public LandDetector

actuator_controls_s _actuator_controls {};
battery_status_s _battery_status {};
vehicle_control_mode_s _control_mode {};
vehicle_acceleration_s _vehicle_acceleration{};
vehicle_attitude_s _vehicle_attitude {};
vehicle_angular_velocity_s _vehicle_angular_velocity{};
vehicle_attitude_s _vehicle_attitude {};
vehicle_control_mode_s _vehicle_control_mode {};
vehicle_local_position_s _vehicle_local_position {};
vehicle_local_position_setpoint_s _vehicle_local_position_setpoint {};

hrt_abstime _min_trust_start{0}; ///< timestamp when minimum trust was applied first
hrt_abstime _min_trust_start{0}; ///< timestamp when minimum trust was applied first
hrt_abstime _landed_time{0};

bool _in_descend{false}; ///< vehicle is desending
bool _in_descend{false}; ///< vehicle is desending
bool _horizontal_movement{false}; ///< vehicle is moving horizontally

DEFINE_PARAMETERS_CUSTOM_PARENT(
Expand Down