fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-05 02:31:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Standardize class member variable naming convention in the MulticopterLandDetector class.

Browse Source
This commit is contained in:
mcsauder
2019-06-26 14:06:56 -04:00
committed by Daniel Agar
parent 3f0159d784
commit 6b6d82447e
2 changed files with 35 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/land_detector/MulticopterLandDetector.cpp
+25 -24
View File
@@ -92,13 +92,13 @@ MulticopterLandDetector::MulticopterLandDetector()
void MulticopterLandDetector::_update_topics() void MulticopterLandDetector::_update_topics()
{ {
_vehicleLocalPositionSub.update(&_vehicleLocalPosition); _attitudeSub.update(&_vehicle_attitude);
_vehicleLocalPositionSetpointSub.update(&_vehicleLocalPositionSetpoint);
_attitudeSub.update(&_vehicleAttitude);
_actuatorsSub.update(&_actuators); _actuatorsSub.update(&_actuators);
_sensor_bias_sub.update(&_sensors);
_vehicle_control_mode_sub.update(&_control_mode);
_battery_sub.update(&_battery); _battery_sub.update(&_battery);
_sensor_bias_sub.update(&_sensor_bias);
_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() void MulticopterLandDetector::_update_params()
@@ -125,14 +125,14 @@ bool MulticopterLandDetector::_get_freefall_state()
return false; return false;
} }
if (_sensors.timestamp == 0) { if (_sensor_bias.timestamp == 0) {
// _sensors is not valid yet, we have to assume we're not falling. // _sensors is not valid yet, we have to assume we're not falling.
return false; return false;
} }
float acc_norm = _sensors.accel_x * _sensors.accel_x float acc_norm = _sensor_bias.accel_x * _sensor_bias.accel_x
+ _sensors.accel_y * _sensors.accel_y + _sensor_bias.accel_y * _sensor_bias.accel_y
+ _sensors.accel_z * _sensors.accel_z; + _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. 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 return (acc_norm < _params.freefall_acc_threshold); //true if we are currently falling
@@ -157,24 +157,24 @@ bool MulticopterLandDetector::_get_ground_contact_state()
// Widen acceptance thresholds for landed state right after arming // Widen acceptance thresholds for landed state right after arming
// so that motor spool-up and other effects do not trigger false negatives. // 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 { } else {
// Adjust maxClimbRate if land_speed is lower than 2x maxClimbrate // Adjust maxClimbRate if land_speed is lower than 2x maxClimbrate
float maxClimbRate = ((land_speed_threshold * 0.5f) < _params.maxClimbRate) ? (0.5f * land_speed_threshold) : float maxClimbRate = ((land_speed_threshold * 0.5f) < _params.maxClimbRate) ? (0.5f * land_speed_threshold) :
_params.maxClimbRate; _params.maxClimbRate;
verticalMovement = fabsf(_vehicleLocalPosition.vz) > maxClimbRate; verticalMovement = fabsf(_vehicle_local_position.vz) > maxClimbRate;
} }
// Check if we are moving horizontally. // Check if we are moving horizontally.
_horizontal_movement = sqrtf(_vehicleLocalPosition.vx * _vehicleLocalPosition.vx _horizontal_movement = sqrtf(_vehicle_local_position.vx * _vehicle_local_position.vx
+ _vehicleLocalPosition.vy * _vehicleLocalPosition.vy) > _params.maxVelocity; + _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, // 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 // we then can assume that the vehicle hit ground
_in_descend = _is_climb_rate_enabled() _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; bool hit_ground = _in_descend && !verticalMovement;
// TODO: we need an accelerometer based check for vertical movement for flying without GPS // TODO: we need an accelerometer based check for vertical movement for flying without GPS
@@ -215,9 +215,9 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
// Next look if all rotation angles are not moving. // Next look if all rotation angles are not moving.
float maxRotationScaled = _params.maxRotation_rad_s * landThresholdFactor; float maxRotationScaled = _params.maxRotation_rad_s * landThresholdFactor;
bool rotating = (fabsf(_vehicleAttitude.rollspeed) > maxRotationScaled) || bool rotating = (fabsf(_vehicle_attitude.rollspeed) > maxRotationScaled) ||
(fabsf(_vehicleAttitude.pitchspeed) > maxRotationScaled) || (fabsf(_vehicle_attitude.pitchspeed) > maxRotationScaled) ||
(fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled); (fabsf(_vehicle_attitude.yawspeed) > maxRotationScaled);
// Return status based on armed state and throttle if no position lock is available. // Return status based on armed state and throttle if no position lock is available.
if (!_has_altitude_lock() && !rotating) { if (!_has_altitude_lock() && !rotating) {
@@ -282,22 +282,23 @@ float MulticopterLandDetector::_get_max_altitude()
bool MulticopterLandDetector::_has_altitude_lock() bool MulticopterLandDetector::_has_altitude_lock()
{ {
return _vehicleLocalPosition.timestamp != 0 && return _vehicle_local_position.timestamp != 0 &&
hrt_elapsed_time(&_vehicleLocalPosition.timestamp) < 500_ms && hrt_elapsed_time(&_vehicle_local_position.timestamp) < 500_ms &&
_vehicleLocalPosition.z_valid; _vehicle_local_position.z_valid;
} }
bool MulticopterLandDetector::_has_position_lock() 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 MulticopterLandDetector::_is_climb_rate_enabled()
{ {
bool has_updated = (_vehicleLocalPositionSetpoint.timestamp != 0) bool has_updated = (_vehicle_local_position_setpoint.timestamp != 0)
&& (hrt_elapsed_time(&_vehicleLocalPositionSetpoint.timestamp) < 500_ms); && (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() bool MulticopterLandDetector::_has_low_thrust()
src/modules/land_detector/MulticopterLandDetector.h
+10 -10
View File
@@ -123,21 +123,21 @@ private:
float low_thrust_threshold; float low_thrust_threshold;
} _params{}; } _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 _actuatorsSub{ORB_ID(actuator_controls_0)};
uORB::Subscription _attitudeSub{ORB_ID(vehicle_attitude)}; uORB::Subscription _attitudeSub{ORB_ID(vehicle_attitude)};
uORB::Subscription _battery_sub{ORB_ID(battery_status)};
uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)}; uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)};
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _battery_sub{ORB_ID(battery_status)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)};
vehicle_local_position_s _vehicleLocalPosition {}; actuator_controls_s _actuators {};
vehicle_local_position_setpoint_s _vehicleLocalPositionSetpoint {}; battery_status_s _battery {};
actuator_controls_s _actuators {}; vehicle_control_mode_s _control_mode {};
vehicle_attitude_s _vehicleAttitude {}; sensor_bias_s _sensor_bias {};
sensor_bias_s _sensors {}; vehicle_attitude_s _vehicle_attitude {};
vehicle_control_mode_s _control_mode {}; vehicle_local_position_s _vehicle_local_position {};
battery_status_s _battery {}; 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}; hrt_abstime _landed_time{0};