fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 17:33:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Astyle: Run astyle to fix code formatting

Browse Source
This commit is contained in:
Johan Jansen
2015-01-15 14:37:51 +01:00
parent 10a2dd8a34
commit 9ea086bf2d
7 changed files with 154 additions and 152 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/land_detector/FixedwingLandDetector.cpp
+37 -37
View File
@@ -39,65 +39,65 @@
*/
#include "FixedwingLandDetector.h"
#include <cmath>
#include <drivers/drv_hrt.h>
FixedwingLandDetector::FixedwingLandDetector() :
_vehicleLocalPositionSub(-1),
_vehicleLocalPosition({}),
_airspeedSub(-1),
_airspeed({}),
_vehicleLocalPositionSub(-1),
_vehicleLocalPosition({}),
_airspeedSub(-1),
_airspeed({}),
_velocity_xy_filtered(0.0f),
_velocity_z_filtered(0.0f),
_airspeed_filtered(0.0f),
_landDetectTrigger(0)
_velocity_xy_filtered(0.0f),
_velocity_z_filtered(0.0f),
_airspeed_filtered(0.0f),
_landDetectTrigger(0)
{
//ctor
//ctor
}
void FixedwingLandDetector::initialize()
{
//Subscribe to local position and airspeed data
_vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position));
_airspeedSub = orb_subscribe(ORB_ID(airspeed));
//Subscribe to local position and airspeed data
_vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position));
_airspeedSub = orb_subscribe(ORB_ID(airspeed));
}
void FixedwingLandDetector::updateSubscriptions()
{
orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition);
orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition);
orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
}
bool FixedwingLandDetector::update()
{
//First poll for new data from our subscriptions
updateSubscriptions();
//First poll for new data from our subscriptions
updateSubscriptions();
const uint64_t now = hrt_absolute_time();
bool landDetected = false;
const uint64_t now = hrt_absolute_time();
bool landDetected = false;
//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;
//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 */
if (_velocity_xy_filtered < FW_LAND_DETECTOR_VELOCITY_MAX
&& _velocity_z_filtered < FW_LAND_DETECTOR_CLIMBRATE_MAX
&& _airspeed_filtered < FW_LAND_DETECTOR_AIRSPEED_MAX) {
/* crude land detector for fixedwing */
if (_velocity_xy_filtered < FW_LAND_DETECTOR_VELOCITY_MAX
&& _velocity_z_filtered < FW_LAND_DETECTOR_CLIMBRATE_MAX
&& _airspeed_filtered < FW_LAND_DETECTOR_AIRSPEED_MAX) {
//These conditions need to be stable for a period of time before we trust them
if (now > _landDetectTrigger) {
landDetected = true;
}
//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
_landDetectTrigger = now + FW_LAND_DETECTOR_TRIGGER_TIME;
}
} else {
//reset land detect trigger
_landDetectTrigger = now + FW_LAND_DETECTOR_TRIGGER_TIME;
}
return landDetected;
return landDetected;
}