diff options
author | Johan Jansen <jnsn.johan@gmail.com> | 2015-01-07 23:28:20 +0100 |
---|---|---|
committer | Johan Jansen <jnsn.johan@gmail.com> | 2015-01-15 14:37:51 +0100 |
commit | 9ea086bf2d9b3d9d3d480f6ae83447b9669f3603 (patch) | |
tree | 15eb7b3ad73f74e7318774bcaaa40909c50b9d03 /src | |
parent | 10a2dd8a346a6a08a0d9b52739f20b842d460646 (diff) | |
download | px4-firmware-9ea086bf2d9b3d9d3d480f6ae83447b9669f3603.tar.gz px4-firmware-9ea086bf2d9b3d9d3d480f6ae83447b9669f3603.tar.bz2 px4-firmware-9ea086bf2d9b3d9d3d480f6ae83447b9669f3603.zip |
Astyle: Run astyle to fix code formatting
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/land_detector/FixedwingLandDetector.cpp | 74 | ||||
-rw-r--r-- | src/modules/land_detector/FixedwingLandDetector.h | 52 | ||||
-rw-r--r-- | src/modules/land_detector/LandDetector.cpp | 89 | ||||
-rw-r--r-- | src/modules/land_detector/LandDetector.h | 70 | ||||
-rw-r--r-- | src/modules/land_detector/MulticopterLandDetector.h | 3 | ||||
-rw-r--r-- | src/modules/land_detector/land_detector_main.cpp | 16 |
6 files changed, 153 insertions, 151 deletions
diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 42735f38c..7df89257f 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -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; } diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index faeece6f3..6737af68a 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -48,41 +48,41 @@ class FixedwingLandDetector : public LandDetector { public: - FixedwingLandDetector(); + FixedwingLandDetector(); protected: - /** - * @brief blocking loop, should be run in a separate thread or task. Runs at 50Hz - **/ - bool update() override; + /** + * @brief blocking loop, should be run in a separate thread or task. Runs at 50Hz + **/ + bool update() override; - /** - * @brief Initializes the land detection algorithm - **/ - void initialize() override; + /** + * @brief Initializes the land detection algorithm + **/ + void initialize() override; - /** - * @brief polls all subscriptions and pulls any data that has changed - **/ - void updateSubscriptions(); + /** + * @brief polls all subscriptions and pulls any data that has changed + **/ + void updateSubscriptions(); - //Algorithm parameters (TODO: should probably be externalized) - static constexpr uint64_t FW_LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold + //Algorithm parameters (TODO: should probably be externalized) + static constexpr uint64_t FW_LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold before triggering a land */ - static constexpr float FW_LAND_DETECTOR_VELOCITY_MAX = 5.0f; /**< maximum horizontal movement m/s*/ - static constexpr float FW_LAND_DETECTOR_CLIMBRATE_MAX = 10.00f; /**< +- climb rate in m/s*/ - static constexpr float FW_LAND_DETECTOR_AIRSPEED_MAX = 10.00f; /**< airspeed max m/s*/ + static constexpr float FW_LAND_DETECTOR_VELOCITY_MAX = 5.0f; /**< maximum horizontal movement m/s*/ + static constexpr float FW_LAND_DETECTOR_CLIMBRATE_MAX = 10.00f; /**< +- climb rate in m/s*/ + static constexpr float FW_LAND_DETECTOR_AIRSPEED_MAX = 10.00f; /**< airspeed max m/s*/ private: - int _vehicleLocalPositionSub; /**< notification of local position */ - struct vehicle_local_position_s _vehicleLocalPosition; /**< the result from local position subscription */ - int _airspeedSub; - struct airspeed_s _airspeed; + int _vehicleLocalPositionSub; /**< notification of local position */ + struct vehicle_local_position_s _vehicleLocalPosition; /**< the result from local position subscription */ + int _airspeedSub; + struct airspeed_s _airspeed; - float _velocity_xy_filtered; - float _velocity_z_filtered; - float _airspeed_filtered; - uint64_t _landDetectTrigger; + float _velocity_xy_filtered; + float _velocity_z_filtered; + float _airspeed_filtered; + uint64_t _landDetectTrigger; }; #endif //__FIXED_WING_LAND_DETECTOR_H__ diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index a39e53818..5029ce185 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -3,75 +3,76 @@ #include <drivers/drv_hrt.h> LandDetector::LandDetector() : - _landDetectedPub(-1), - _landDetected({0, false}), - _taskShouldExit(false), - _taskIsRunning(false) + _landDetectedPub(-1), + _landDetected({0, false}), + _taskShouldExit(false), + _taskIsRunning(false) { - //Advertise the first land detected uORB - _landDetected.timestamp = hrt_absolute_time(); - _landDetected.landed = false; - _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); + //Advertise the first land detected uORB + _landDetected.timestamp = hrt_absolute_time(); + _landDetected.landed = false; + _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); } LandDetector::~LandDetector() { - _taskShouldExit = true; - close(_landDetectedPub); + _taskShouldExit = true; + close(_landDetectedPub); } void LandDetector::shutdown() { - _taskShouldExit = true; + _taskShouldExit = true; } void LandDetector::start() { - //Make sure this method has not already been called by another thread - if(isRunning()) { - return; - } + //Make sure this method has not already been called by another thread + if (isRunning()) { + return; + } - //Task is now running, keep doing so until shutdown() has been called - _taskIsRunning = true; - _taskShouldExit = false; - while(isRunning()) { + //Task is now running, keep doing so until shutdown() has been called + _taskIsRunning = true; + _taskShouldExit = false; - bool landDetected = update(); + while (isRunning()) { - //Publish if land detection state has changed - if (_landDetected.landed != landDetected) { - _landDetected.timestamp = hrt_absolute_time(); - _landDetected.landed = landDetected; + bool landDetected = update(); - /* publish the land detected broadcast */ - orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); - } + //Publish if land detection state has changed + if (_landDetected.landed != landDetected) { + _landDetected.timestamp = hrt_absolute_time(); + _landDetected.landed = landDetected; - //Limit loop rate - usleep(1000000 / LAND_DETECTOR_UPDATE_RATE); - } + /* publish the land detected broadcast */ + orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); + } - _taskIsRunning = false; - _exit(0); + //Limit loop rate + usleep(1000000 / LAND_DETECTOR_UPDATE_RATE); + } + + _taskIsRunning = false; + _exit(0); } bool LandDetector::orb_update(const struct orb_metadata *meta, int handle, void *buffer) { - bool newData = false; + bool newData = false; - //Check if there is new data to grab - if (orb_check(handle, &newData) != OK) { - return false; - } + //Check if there is new data to grab + if (orb_check(handle, &newData) != OK) { + return false; + } - if (!newData) { - return false; - } + if (!newData) { + return false; + } - if (orb_copy(meta, handle, buffer) != OK) { - return false; - } + if (orb_copy(meta, handle, buffer) != OK) { + return false; + } - return true; + return true; } diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index 6d7465691..f7120434c 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -48,54 +48,54 @@ class LandDetector { public: - LandDetector(); - virtual ~LandDetector(); + LandDetector(); + virtual ~LandDetector(); - /** - * @return true if this task is currently running - **/ - inline bool isRunning() const {return _taskIsRunning;} + /** + * @return true if this task is currently running + **/ + inline bool isRunning() const {return _taskIsRunning;} - /** - * @brief Tells the Land Detector task that it should exit - **/ - void shutdown(); + /** + * @brief Tells the Land Detector task that it should exit + **/ + void shutdown(); - /** - * @brief Blocking function that should be called from it's own task thread. This method will - * run the underlying algorithm at the desired update rate and publish if the landing state changes. - **/ - void start(); + /** + * @brief Blocking function that should be called from it's own task thread. This method will + * run the underlying algorithm at the desired update rate and publish if the landing state changes. + **/ + void start(); protected: - /** - * @brief Pure abstract method that must be overriden by sub-classes. This actually runs the underlying algorithm - * @return true if a landing was detected and this should be broadcast to the rest of the system - **/ - virtual bool update() = 0; + /** + * @brief Pure abstract method that must be overriden by sub-classes. This actually runs the underlying algorithm + * @return true if a landing was detected and this should be broadcast to the rest of the system + **/ + virtual bool update() = 0; - /** - * @brief Pure abstract method that is called by this class once for initializing the uderlying algorithm (memory allocation, - * uORB topic subscription, creating file descriptors, etc.) - **/ - virtual void initialize() = 0; + /** + * @brief Pure abstract method that is called by this class once for initializing the uderlying algorithm (memory allocation, + * uORB topic subscription, creating file descriptors, etc.) + **/ + virtual void initialize() = 0; - /** - * @brief Convinience function for polling uORB subscriptions - * @return true if there was new data and it was successfully copied - **/ - bool orb_update(const struct orb_metadata *meta, int handle, void *buffer); + /** + * @brief Convinience function for polling uORB subscriptions + * @return true if there was new data and it was successfully copied + **/ + bool orb_update(const struct orb_metadata *meta, int handle, void *buffer); - static constexpr uint32_t LAND_DETECTOR_UPDATE_RATE = 50; /**< Run algorithm at 50Hz */ + static constexpr uint32_t LAND_DETECTOR_UPDATE_RATE = 50; /**< Run algorithm at 50Hz */ protected: - orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ - struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ + orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ + struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ private: - bool _taskShouldExit; /**< true if it is requested that this task should exit */ - bool _taskIsRunning; /**< task has reached main loop and is currently running */ + bool _taskShouldExit; /**< true if it is requested that this task should exit */ + bool _taskIsRunning; /**< task has reached main loop and is currently running */ }; #endif //__LAND_DETECTOR_H__
\ No newline at end of file diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 08a8132ba..5b4172a36 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -75,7 +75,8 @@ protected: static constexpr float MC_LAND_DETECTOR_ROTATION_MAX = 0.5f; /**< max rotation in rad/sec (= 30 deg/s) */ static constexpr float MC_LAND_DETECTOR_THRUST_MAX = 0.2f; static constexpr float MC_LAND_DETECTOR_VELOCITY_MAX = 1.0f; /**< max +- horizontal movement in m/s */ - static constexpr uint32_t MC_LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold before triggering a land */ + static constexpr uint32_t MC_LAND_DETECTOR_TRIGGER_TIME = + 2000000; /**< usec that landing conditions have to hold before triggering a land */ private: int _vehicleGlobalPositionSub; /**< notification of global position */ diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index 2bc89e752..7c0ffb82a 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -51,7 +51,7 @@ #include "MulticopterLandDetector.h" //Function prototypes -static int land_detector_start(const char* mode); +static int land_detector_start(const char *mode); static void land_detector_stop(); /** @@ -110,7 +110,7 @@ static void land_detector_stop() /** * Start new task, fails if it is already running. Returns OK if successful **/ -static int land_detector_start(const char* mode) +static int land_detector_start(const char *mode) { if (land_detector_task != nullptr || _landDetectorTaskID != -1) { errx(1, "already running"); @@ -118,15 +118,15 @@ static int land_detector_start(const char* mode) } //Allocate memory - if(!strcmp(mode, "fixedwing")) { + if (!strcmp(mode, "fixedwing")) { land_detector_task = new FixedwingLandDetector(); - } - else if(!strcmp(mode, "multicopter")) { + + } else if (!strcmp(mode, "multicopter")) { land_detector_task = new MulticopterLandDetector(); - } - else { + + } else { errx(1, "[mode] must be either 'fixedwing' or 'multicopter'"); - return -1; + return -1; } //Check if alloc worked |