diff options
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/land_detector/FixedwingLandDetector.cpp | 13 | ||||
-rw-r--r-- | src/modules/land_detector/LandDetector.cpp | 18 | ||||
-rw-r--r-- | src/modules/land_detector/MulticopterLandDetector.cpp | 20 |
3 files changed, 24 insertions, 27 deletions
diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 74a197bd2..8e5bcf7ba 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -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; } diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 61e678b41..a4fbb9861 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -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; } diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index cc984f11f..277cb9363 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -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; } |