aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
authorJohan Jansen <jnsn.johan@gmail.com>2015-01-07 23:28:20 +0100
committerJohan Jansen <jnsn.johan@gmail.com>2015-01-15 14:37:51 +0100
commit9ea086bf2d9b3d9d3d480f6ae83447b9669f3603 (patch)
tree15eb7b3ad73f74e7318774bcaaa40909c50b9d03 /src
parent10a2dd8a346a6a08a0d9b52739f20b842d460646 (diff)
downloadpx4-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.cpp74
-rw-r--r--src/modules/land_detector/FixedwingLandDetector.h52
-rw-r--r--src/modules/land_detector/LandDetector.cpp89
-rw-r--r--src/modules/land_detector/LandDetector.h70
-rw-r--r--src/modules/land_detector/MulticopterLandDetector.h3
-rw-r--r--src/modules/land_detector/land_detector_main.cpp16
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