mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-06 12:48:54 +08:00
Code style: minor comment styling
This commit is contained in:
@@ -51,7 +51,6 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(),
|
||||
_airspeedSub(-1),
|
||||
_airspeed({}),
|
||||
_parameterSub(-1),
|
||||
|
||||
_velocity_xy_filtered(0.0f),
|
||||
_velocity_z_filtered(0.0f),
|
||||
_airspeed_filtered(0.0f),
|
||||
@@ -64,7 +63,7 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(),
|
||||
|
||||
void FixedwingLandDetector::initialize()
|
||||
{
|
||||
//Subscribe to local position and airspeed data
|
||||
// Subscribe to local position and airspeed data
|
||||
_vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position));
|
||||
_airspeedSub = orb_subscribe(ORB_ID(airspeed));
|
||||
}
|
||||
@@ -77,30 +76,30 @@ void FixedwingLandDetector::updateSubscriptions()
|
||||
|
||||
bool FixedwingLandDetector::update()
|
||||
{
|
||||
//First poll for new data from our subscriptions
|
||||
// First poll for new data from our subscriptions
|
||||
updateSubscriptions();
|
||||
|
||||
const uint64_t now = hrt_absolute_time();
|
||||
bool landDetected = false;
|
||||
|
||||
//TODO: reset filtered values on arming?
|
||||
// TODO: reset filtered values on arming?
|
||||
_velocity_xy_filtered = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx *
|
||||
_vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy);
|
||||
_velocity_z_filtered = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz);
|
||||
_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s;
|
||||
|
||||
/* crude land detector for fixedwing */
|
||||
// crude land detector for fixedwing
|
||||
if (_velocity_xy_filtered < _params.maxVelocity
|
||||
&& _velocity_z_filtered < _params.maxClimbRate
|
||||
&& _airspeed_filtered < _params.maxAirSpeed) {
|
||||
|
||||
//These conditions need to be stable for a period of time before we trust them
|
||||
// these conditions need to be stable for a period of time before we trust them
|
||||
if (now > _landDetectTrigger) {
|
||||
landDetected = true;
|
||||
}
|
||||
|
||||
} else {
|
||||
//reset land detect trigger
|
||||
// reset land detect trigger
|
||||
_landDetectTrigger = now + LAND_DETECTOR_TRIGGER_TIME;
|
||||
}
|
||||
|
||||
|
||||
@@ -49,7 +49,7 @@ LandDetector::LandDetector() :
|
||||
_taskShouldExit(false),
|
||||
_taskIsRunning(false)
|
||||
{
|
||||
//ctor
|
||||
// ctor
|
||||
}
|
||||
|
||||
LandDetector::~LandDetector()
|
||||
@@ -65,20 +65,20 @@ void LandDetector::shutdown()
|
||||
|
||||
void LandDetector::start()
|
||||
{
|
||||
//Make sure this method has not already been called by another thread
|
||||
// make sure this method has not already been called by another thread
|
||||
if (isRunning()) {
|
||||
return;
|
||||
}
|
||||
|
||||
//Advertise the first land detected uORB
|
||||
// advertise the first land detected uORB
|
||||
_landDetected.timestamp = hrt_absolute_time();
|
||||
_landDetected.landed = false;
|
||||
_landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected);
|
||||
|
||||
//Initialize land detection algorithm
|
||||
// initialize land detection algorithm
|
||||
initialize();
|
||||
|
||||
//Task is now running, keep doing so until shutdown() has been called
|
||||
// task is now running, keep doing so until shutdown() has been called
|
||||
_taskIsRunning = true;
|
||||
_taskShouldExit = false;
|
||||
|
||||
@@ -86,16 +86,16 @@ void LandDetector::start()
|
||||
|
||||
bool landDetected = update();
|
||||
|
||||
//Publish if land detection state has changed
|
||||
// publish if land detection state has changed
|
||||
if (_landDetected.landed != landDetected) {
|
||||
_landDetected.timestamp = hrt_absolute_time();
|
||||
_landDetected.landed = landDetected;
|
||||
|
||||
/* publish the land detected broadcast */
|
||||
// publish the land detected broadcast
|
||||
orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected);
|
||||
}
|
||||
|
||||
//Limit loop rate
|
||||
// limit loop rate
|
||||
usleep(1000000 / LAND_DETECTOR_UPDATE_RATE);
|
||||
}
|
||||
|
||||
@@ -107,7 +107,7 @@ bool LandDetector::orb_update(const struct orb_metadata *meta, int handle, void
|
||||
{
|
||||
bool newData = false;
|
||||
|
||||
//Check if there is new data to grab
|
||||
// check if there is new data to grab
|
||||
if (orb_check(handle, &newData) != OK) {
|
||||
return false;
|
||||
}
|
||||
|
||||
@@ -53,13 +53,11 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
|
||||
_actuatorsSub(-1),
|
||||
_armingSub(-1),
|
||||
_parameterSub(-1),
|
||||
|
||||
_vehicleGlobalPosition({}),
|
||||
_sensors({}),
|
||||
_waypoint({}),
|
||||
_actuators({}),
|
||||
_arming({}),
|
||||
|
||||
_landTimer(0)
|
||||
{
|
||||
_paramHandle.maxRotation = param_find("LNDMC_Z_VEL_MAX");
|
||||
@@ -70,7 +68,7 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
|
||||
|
||||
void MulticopterLandDetector::initialize()
|
||||
{
|
||||
//Subscribe to position, attitude, arming and velocity changes
|
||||
// subscribe to position, attitude, arming and velocity changes
|
||||
_vehicleGlobalPositionSub = orb_subscribe(ORB_ID(vehicle_global_position));
|
||||
_sensorsCombinedSub = orb_subscribe(ORB_ID(sensor_combined));
|
||||
_waypointSub = orb_subscribe(ORB_ID(position_setpoint_triplet));
|
||||
@@ -78,7 +76,7 @@ void MulticopterLandDetector::initialize()
|
||||
_armingSub = orb_subscribe(ORB_ID(actuator_armed));
|
||||
_parameterSub = orb_subscribe(ORB_ID(parameter_update));
|
||||
|
||||
//Download parameters
|
||||
// download parameters
|
||||
updateParameterCache(true);
|
||||
}
|
||||
|
||||
@@ -93,33 +91,33 @@ void MulticopterLandDetector::updateSubscriptions()
|
||||
|
||||
bool MulticopterLandDetector::update()
|
||||
{
|
||||
//First poll for new data from our subscriptions
|
||||
// first poll for new data from our subscriptions
|
||||
updateSubscriptions();
|
||||
|
||||
//Only trigger flight conditions if we are armed
|
||||
// only trigger flight conditions if we are armed
|
||||
if (!_arming.armed) {
|
||||
return true;
|
||||
}
|
||||
|
||||
const uint64_t now = hrt_absolute_time();
|
||||
|
||||
//Check if we are moving vertically
|
||||
// check if we are moving vertically
|
||||
bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > _params.maxClimbRate;
|
||||
|
||||
//Check if we are moving horizontally
|
||||
// check if we are moving horizontally
|
||||
bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n
|
||||
+ _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > _params.maxVelocity;
|
||||
|
||||
//Next look if all rotation angles are not moving
|
||||
// next look if all rotation angles are not moving
|
||||
bool rotating = sqrtf(_sensors.gyro_rad_s[0] * _sensors.gyro_rad_s[0] +
|
||||
_sensors.gyro_rad_s[1] * _sensors.gyro_rad_s[1] +
|
||||
_sensors.gyro_rad_s[2] * _sensors.gyro_rad_s[2]) > _params.maxRotation;
|
||||
|
||||
//Check if thrust output is minimal (about half of default)
|
||||
// check if thrust output is minimal (about half of default)
|
||||
bool minimalThrust = _actuators.control[3] <= _params.maxThrottle;
|
||||
|
||||
if (verticalMovement || rotating || !minimalThrust || horizontalMovement) {
|
||||
//Sensed movement, so reset the land detector
|
||||
// sensed movement, so reset the land detector
|
||||
_landTimer = now;
|
||||
return false;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user