forked from Archive/PX4-Autopilot
Astyle: Run astyle to fix code formatting
This commit is contained in:
Johan Jansen
parent
10a2dd8a34
commit
9ea086bf2d
|
|
@ -71,7 +71,7 @@ MODULES += modules/navigator
|
|||
MODULES += modules/mavlink
|
||||
MODULES += modules/gpio_led
|
||||
MODULES += modules/uavcan
|
||||
MODULES += modules/land_detector
|
||||
MODULES += modules/land_detector
|
||||
|
||||
#
|
||||
# Estimation modules (EKF/ SO3 / other filters)
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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__
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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__
|
||||
|
|
@ -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 */
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue