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.

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

Standardize land_detector module class variable naming #12339

Merged
merged 5 commits into from
Jun 26, 2019
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
18 changes: 9 additions & 9 deletions src/modules/land_detector/FixedwingLandDetector.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -65,8 +65,8 @@ FixedwingLandDetector::FixedwingLandDetector()
void FixedwingLandDetector::_update_topics()
{
_airspeedSub.update(&_airspeed);
_sensor_bias_sub.update(&_sensors);
_local_pos_sub.update(&_local_pos);
_sensor_bias_sub.update(&_sensor_bias);
_vehicle_local_position_sub.update(&_vehicle_local_position);
}

void FixedwingLandDetector::_update_params()
Expand All @@ -82,7 +82,7 @@ float FixedwingLandDetector::_get_max_altitude()
// TODO
// This means no altitude limit as the limit
// is always current position plus 10000 meters
return roundf(-_local_pos.z + 10000);
return roundf(-_vehicle_local_position.z + 10000);
}

bool FixedwingLandDetector::_get_landed_state()
Expand All @@ -94,18 +94,18 @@ bool FixedwingLandDetector::_get_landed_state()

bool landDetected = false;

if (hrt_elapsed_time(&_local_pos.timestamp) < 500 * 1000) {
if (hrt_elapsed_time(&_vehicle_local_position.timestamp) < 500 * 1000) {

// horizontal velocity
float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_local_pos.vx * _local_pos.vx + _local_pos.vy *
_local_pos.vy);
float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicle_local_position.vx * _vehicle_local_position.vx +
_vehicle_local_position.vy * _vehicle_local_position.vy);

if (PX4_ISFINITE(val)) {
_velocity_xy_filtered = val;
}

// vertical velocity
val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_local_pos.vz);
val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_vehicle_local_position.vz);

if (PX4_ISFINITE(val)) {
_velocity_z_filtered = val;
Expand All @@ -115,8 +115,8 @@ bool FixedwingLandDetector::_get_landed_state()

// a leaking lowpass prevents biases from building up, but
// gives a mostly correct response for short impulses
const float acc_hor = sqrtf(_sensors.accel_x * _sensors.accel_x +
_sensors.accel_y * _sensors.accel_y);
const float acc_hor = sqrtf(_sensor_bias.accel_x * _sensor_bias.accel_x +
_sensor_bias.accel_y * _sensor_bias.accel_y);
_accel_horz_lp = _accel_horz_lp * 0.8f + acc_hor * 0.18f;

// crude land detector for fixedwing
Expand Down
6 changes: 3 additions & 3 deletions src/modules/land_detector/FixedwingLandDetector.h
Original file line number Diff line number Diff line change
Expand Up @@ -89,11 +89,11 @@ class FixedwingLandDetector final : public LandDetector

uORB::Subscription _airspeedSub{ORB_ID(airspeed)};
uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)};
uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position});
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position});

airspeed_s _airspeed{};
sensor_bias_s _sensors{};
vehicle_local_position_s _local_pos{};
sensor_bias_s _sensor_bias{};
vehicle_local_position_s _vehicle_local_position{};

float _velocity_xy_filtered{0.0f};
float _velocity_z_filtered{0.0f};
Expand Down
44 changes: 22 additions & 22 deletions src/modules/land_detector/LandDetector.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -57,11 +57,11 @@ LandDetector::LandDetector() :
ModuleParams(nullptr),
ScheduledWorkItem(px4::wq_configurations::hp_default)
{
_landDetected.timestamp = hrt_absolute_time();
_landDetected.freefall = false;
_landDetected.landed = true;
_landDetected.ground_contact = false;
_landDetected.maybe_landed = false;
_land_detected.timestamp = hrt_absolute_time();
_land_detected.freefall = false;
_land_detected.landed = true;
_land_detected.ground_contact = false;
_land_detected.maybe_landed = false;
}

LandDetector::~LandDetector()
Expand Down Expand Up @@ -95,30 +95,30 @@ void LandDetector::Run()
const hrt_abstime now = hrt_absolute_time();

// publish at 1 Hz, very first time, or when the result has changed
if ((hrt_elapsed_time(&_landDetected.timestamp) >= 1_s) ||
if ((hrt_elapsed_time(&_land_detected.timestamp) >= 1_s) ||
(_land_detected_pub == nullptr) ||
(_landDetected.landed != landDetected) ||
(_landDetected.freefall != freefallDetected) ||
(_landDetected.maybe_landed != maybe_landedDetected) ||
(_landDetected.ground_contact != ground_contactDetected) ||
(_landDetected.in_ground_effect != in_ground_effect) ||
(fabsf(_landDetected.alt_max - alt_max) > FLT_EPSILON)) {

if (!landDetected && _landDetected.landed) {
(_land_detected.landed != landDetected) ||
(_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)) {

if (!landDetected && _land_detected.landed) {
// We did take off
_takeoff_time = now;
}

_landDetected.timestamp = hrt_absolute_time();
_landDetected.landed = landDetected;
_landDetected.freefall = freefallDetected;
_landDetected.maybe_landed = maybe_landedDetected;
_landDetected.ground_contact = ground_contactDetected;
_landDetected.alt_max = alt_max;
_landDetected.in_ground_effect = in_ground_effect;
_land_detected.timestamp = hrt_absolute_time();
_land_detected.landed = landDetected;
_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;

int instance;
orb_publish_auto(ORB_ID(vehicle_land_detected), &_land_detected_pub, &_landDetected,
orb_publish_auto(ORB_ID(vehicle_land_detected), &_land_detected_pub, &_land_detected,
&instance, ORB_PRIO_DEFAULT);
}

Expand Down
2 changes: 1 addition & 1 deletion src/modules/land_detector/LandDetector.h
Original file line number Diff line number Diff line change
Expand Up @@ -143,7 +143,7 @@ class LandDetector : public ModuleBase<LandDetector>, ModuleParams, px4::Schedul
static constexpr uint32_t LAND_DETECTOR_UPDATE_INTERVAL = 20_ms;

orb_advert_t _land_detected_pub{nullptr};
vehicle_land_detected_s _landDetected{};
vehicle_land_detected_s _land_detected{};

LandDetectionState _state{LandDetectionState::LANDED};

Expand Down
55 changes: 28 additions & 27 deletions src/modules/land_detector/MulticopterLandDetector.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -92,13 +92,13 @@ MulticopterLandDetector::MulticopterLandDetector()

void MulticopterLandDetector::_update_topics()
{
_vehicleLocalPositionSub.update(&_vehicleLocalPosition);
_vehicleLocalPositionSetpointSub.update(&_vehicleLocalPositionSetpoint);
_attitudeSub.update(&_vehicleAttitude);
_actuatorsSub.update(&_actuators);
_sensor_bias_sub.update(&_sensors);
_vehicle_control_mode_sub.update(&_control_mode);
_actuator_controls_sub.update(&_actuator_controls);
_battery_sub.update(&_battery);
_sensor_bias_sub.update(&_sensor_bias);
_vehicle_attitude_sub.update(&_vehicle_attitude);
_vehicle_control_mode_sub.update(&_control_mode);
_vehicle_local_position_sub.update(&_vehicle_local_position);
_vehicle_local_position_setpoint_sub.update(&_vehicle_local_position_setpoint);
}

void MulticopterLandDetector::_update_params()
Expand All @@ -125,14 +125,14 @@ bool MulticopterLandDetector::_get_freefall_state()
return false;
}

if (_sensors.timestamp == 0) {
if (_sensor_bias.timestamp == 0) {
// _sensors is not valid yet, we have to assume we're not falling.
return false;
}

float acc_norm = _sensors.accel_x * _sensors.accel_x
+ _sensors.accel_y * _sensors.accel_y
+ _sensors.accel_z * _sensors.accel_z;
float acc_norm = _sensor_bias.accel_x * _sensor_bias.accel_x
+ _sensor_bias.accel_y * _sensor_bias.accel_y
+ _sensor_bias.accel_z * _sensor_bias.accel_z;
acc_norm = sqrtf(acc_norm); //norm of specific force. Should be close to 9.8 m/s^2 when landed.

return (acc_norm < _params.freefall_acc_threshold); //true if we are currently falling
Expand All @@ -157,24 +157,24 @@ bool MulticopterLandDetector::_get_ground_contact_state()

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

} else {

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

// Check if we are moving horizontally.
_horizontal_movement = sqrtf(_vehicleLocalPosition.vx * _vehicleLocalPosition.vx
+ _vehicleLocalPosition.vy * _vehicleLocalPosition.vy) > _params.maxVelocity;
_horizontal_movement = sqrtf(_vehicle_local_position.vx * _vehicle_local_position.vx
+ _vehicle_local_position.vy * _vehicle_local_position.vy) > _params.maxVelocity;

// if we have a valid velocity setpoint and the vehicle is demanded to go down but no vertical movement present,
// we then can assume that the vehicle hit ground
_in_descend = _is_climb_rate_enabled()
&& (_vehicleLocalPositionSetpoint.vz >= land_speed_threshold);
&& (_vehicle_local_position_setpoint.vz >= land_speed_threshold);
bool hit_ground = _in_descend && !verticalMovement;

// TODO: we need an accelerometer based check for vertical movement for flying without GPS
Expand Down Expand Up @@ -215,9 +215,9 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
// Next look if all rotation angles are not moving.
float maxRotationScaled = _params.maxRotation_rad_s * landThresholdFactor;

bool rotating = (fabsf(_vehicleAttitude.rollspeed) > maxRotationScaled) ||
(fabsf(_vehicleAttitude.pitchspeed) > maxRotationScaled) ||
(fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled);
bool rotating = (fabsf(_vehicle_attitude.rollspeed) > maxRotationScaled) ||
(fabsf(_vehicle_attitude.pitchspeed) > maxRotationScaled) ||
(fabsf(_vehicle_attitude.yawspeed) > maxRotationScaled);

// Return status based on armed state and throttle if no position lock is available.
if (!_has_altitude_lock() && !rotating) {
Expand Down Expand Up @@ -282,22 +282,23 @@ float MulticopterLandDetector::_get_max_altitude()

bool MulticopterLandDetector::_has_altitude_lock()
{
return _vehicleLocalPosition.timestamp != 0 &&
hrt_elapsed_time(&_vehicleLocalPosition.timestamp) < 500_ms &&
_vehicleLocalPosition.z_valid;
return _vehicle_local_position.timestamp != 0 &&
hrt_elapsed_time(&_vehicle_local_position.timestamp) < 500_ms &&
_vehicle_local_position.z_valid;
}

bool MulticopterLandDetector::_has_position_lock()
{
return _has_altitude_lock() && _vehicleLocalPosition.xy_valid;
return _has_altitude_lock() && _vehicle_local_position.xy_valid;
}

bool MulticopterLandDetector::_is_climb_rate_enabled()
{
bool has_updated = (_vehicleLocalPositionSetpoint.timestamp != 0)
&& (hrt_elapsed_time(&_vehicleLocalPositionSetpoint.timestamp) < 500_ms);
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 && PX4_ISFINITE(_vehicleLocalPositionSetpoint.vz));
return (_control_mode.flag_control_climb_rate_enabled && has_updated
&& PX4_ISFINITE(_vehicle_local_position_setpoint.vz));
}

bool MulticopterLandDetector::_has_low_thrust()
Expand All @@ -307,7 +308,7 @@ bool MulticopterLandDetector::_has_low_thrust()
_params.low_thrust_threshold;

// Check if thrust output is less than the minimum auto throttle param.
return _actuators.control[actuator_controls_s::INDEX_THROTTLE] <= sys_min_throttle;
return _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] <= sys_min_throttle;
}

bool MulticopterLandDetector::_has_minimal_thrust()
Expand All @@ -321,7 +322,7 @@ bool MulticopterLandDetector::_has_minimal_thrust()
}

// Check if thrust output is less than the minimum auto throttle param.
return _actuators.control[actuator_controls_s::INDEX_THROTTLE] <= sys_min_throttle;
return _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] <= sys_min_throttle;
}

bool MulticopterLandDetector::_get_ground_effect_state()
Expand Down
26 changes: 13 additions & 13 deletions src/modules/land_detector/MulticopterLandDetector.h
Original file line number Diff line number Diff line change
Expand Up @@ -123,21 +123,21 @@ class MulticopterLandDetector : public LandDetector
float low_thrust_threshold;
} _params{};

uORB::Subscription _vehicleLocalPositionSub{ORB_ID(vehicle_local_position)};
uORB::Subscription _vehicleLocalPositionSetpointSub{ORB_ID(vehicle_local_position_setpoint)};
uORB::Subscription _actuatorsSub{ORB_ID(actuator_controls_0)};
uORB::Subscription _attitudeSub{ORB_ID(vehicle_attitude)};
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_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _battery_sub{ORB_ID(battery_status)};

vehicle_local_position_s _vehicleLocalPosition {};
vehicle_local_position_setpoint_s _vehicleLocalPositionSetpoint {};
actuator_controls_s _actuators {};
vehicle_attitude_s _vehicleAttitude {};
sensor_bias_s _sensors {};
vehicle_control_mode_s _control_mode {};
battery_status_s _battery {};
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)};

actuator_controls_s _actuator_controls {};
battery_status_s _battery {};
vehicle_control_mode_s _control_mode {};
sensor_bias_s _sensor_bias {};
vehicle_attitude_s _vehicle_attitude {};
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 _landed_time{0};
Expand Down
2 changes: 1 addition & 1 deletion src/modules/land_detector/VtolLandDetector.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,7 @@ void VtolLandDetector::_update_topics()
{
MulticopterLandDetector::_update_topics();

_airspeedSub.update(&_airspeed);
_airspeed_sub.update(&_airspeed);
_vehicle_status_sub.update(&_vehicle_status);
}

Expand Down
6 changes: 3 additions & 3 deletions src/modules/land_detector/VtolLandDetector.h
Original file line number Diff line number Diff line change
Expand Up @@ -70,11 +70,11 @@ class VtolLandDetector : public MulticopterLandDetector
float maxAirSpeed;
} _params{};

uORB::Subscription _airspeedSub{ORB_ID(airspeed)};
uORB::Subscription _airspeed_sub{ORB_ID(airspeed)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};

airspeed_s _airspeed{};
vehicle_status_s _vehicle_status{};
airspeed_s _airspeed{};
vehicle_status_s _vehicle_status{};

bool _was_in_air{false}; /**< indicates whether the vehicle was in the air in the previous iteration */
float _airspeed_filtered{0.0f}; /**< low pass filtered airspeed */
Expand Down