Browse Source
Conflicts: src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp src/modules/uORB/topics/vehicle_attitude.hsbg
Thomas Gubler
10 years ago
57 changed files with 2167 additions and 3269 deletions
@ -1 +1 @@
@@ -1 +1 @@
|
||||
Subproject commit dbcccb2455d759b789d549d25e1fbf489b2d3c83 |
||||
Subproject commit 574bac488f384ddaa344378e25653c27124a2b69 |
||||
File diff suppressed because it is too large
Load Diff
@ -1,247 +0,0 @@
@@ -1,247 +0,0 @@
|
||||
#pragma once |
||||
|
||||
#include "estimator_utilities.h" |
||||
|
||||
class AttPosEKF { |
||||
|
||||
public: |
||||
|
||||
AttPosEKF(); |
||||
~AttPosEKF(); |
||||
|
||||
/* ##############################################
|
||||
* |
||||
* M A I N F I L T E R P A R A M E T E R S |
||||
* |
||||
* ########################################### */ |
||||
|
||||
/*
|
||||
* parameters are defined here and initialised in |
||||
* the InitialiseParameters() (which is just 20 lines down) |
||||
*/ |
||||
|
||||
float covTimeStepMax; // maximum time allowed between covariance predictions
|
||||
float covDelAngMax; // maximum delta angle between covariance predictions
|
||||
float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
|
||||
|
||||
float yawVarScale; |
||||
float windVelSigma; |
||||
float dAngBiasSigma; |
||||
float dVelBiasSigma; |
||||
float magEarthSigma; |
||||
float magBodySigma; |
||||
float gndHgtSigma; |
||||
|
||||
float vneSigma; |
||||
float vdSigma; |
||||
float posNeSigma; |
||||
float posDSigma; |
||||
float magMeasurementSigma; |
||||
float airspeedMeasurementSigma; |
||||
|
||||
float gyroProcessNoise; |
||||
float accelProcessNoise; |
||||
|
||||
float EAS2TAS; // ratio f true to equivalent airspeed
|
||||
|
||||
void InitialiseParameters() |
||||
{ |
||||
covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
|
||||
covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
|
||||
rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
|
||||
EAS2TAS = 1.0f; |
||||
|
||||
yawVarScale = 1.0f; |
||||
windVelSigma = 0.1f; |
||||
dAngBiasSigma = 5.0e-7f; |
||||
dVelBiasSigma = 1e-4f; |
||||
magEarthSigma = 3.0e-4f; |
||||
magBodySigma = 3.0e-4f; |
||||
gndHgtSigma = 0.02f; // assume 2% terrain gradient 1-sigma
|
||||
|
||||
vneSigma = 0.2f; |
||||
vdSigma = 0.3f; |
||||
posNeSigma = 2.0f; |
||||
posDSigma = 2.0f; |
||||
|
||||
magMeasurementSigma = 0.05; |
||||
airspeedMeasurementSigma = 1.4f; |
||||
gyroProcessNoise = 1.4544411e-2f; |
||||
accelProcessNoise = 0.5f; |
||||
} |
||||
|
||||
// Global variables
|
||||
float KH[n_states][n_states]; // intermediate result used for covariance updates
|
||||
float KHP[n_states][n_states]; // intermediate result used for covariance updates
|
||||
float P[n_states][n_states]; // covariance matrix
|
||||
float Kfusion[n_states]; // Kalman gains
|
||||
float states[n_states]; // state matrix
|
||||
float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
|
||||
uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
|
||||
|
||||
float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
|
||||
float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
|
||||
float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement
|
||||
float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
|
||||
float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
|
||||
|
||||
Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
|
||||
Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
|
||||
Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad)
|
||||
Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s)
|
||||
float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
|
||||
Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
|
||||
Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)
|
||||
Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
|
||||
Vector3f dVelIMU; |
||||
Vector3f dAngIMU; |
||||
float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec)
|
||||
uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
|
||||
float innovVelPos[6]; // innovation output
|
||||
float varInnovVelPos[6]; // innovation variance output
|
||||
|
||||
float velNED[3]; // North, East, Down velocity obs (m/s)
|
||||
float posNE[2]; // North, East position obs (m)
|
||||
float hgtMea; // measured height (m)
|
||||
float posNED[3]; // North, East Down position (m)
|
||||
|
||||
float innovMag[3]; // innovation output
|
||||
float varInnovMag[3]; // innovation variance output
|
||||
Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
|
||||
float innovVtas; // innovation output
|
||||
float varInnovVtas; // innovation variance output
|
||||
float VtasMeas; // true airspeed measurement (m/s)
|
||||
float magDeclination; |
||||
float latRef; // WGS-84 latitude of reference point (rad)
|
||||
float lonRef; // WGS-84 longitude of reference point (rad)
|
||||
float hgtRef; // WGS-84 height of reference point (m)
|
||||
Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
|
||||
uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
|
||||
|
||||
// GPS input data variables
|
||||
float gpsCourse; |
||||
float gpsVelD; |
||||
float gpsLat; |
||||
float gpsLon; |
||||
float gpsHgt; |
||||
uint8_t GPSstatus; |
||||
|
||||
// Baro input
|
||||
float baroHgt; |
||||
|
||||
bool statesInitialised; |
||||
|
||||
bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused
|
||||
bool fusePosData; // this boolean causes the posNE and velNED obs to be fused
|
||||
bool fuseHgtData; // this boolean causes the hgtMea obs to be fused
|
||||
bool fuseMagData; // boolean true when magnetometer data is to be fused
|
||||
bool fuseVtasData; // boolean true when airspeed data is to be fused
|
||||
|
||||
bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying)
|
||||
bool staticMode; ///< boolean true if no position feedback is fused
|
||||
bool useAirspeed; ///< boolean true if airspeed data is being used
|
||||
bool useCompass; ///< boolean true if magnetometer data is being used
|
||||
|
||||
struct ekf_status_report current_ekf_state; |
||||
struct ekf_status_report last_ekf_error; |
||||
|
||||
bool numericalProtection; |
||||
|
||||
unsigned storeIndex; |
||||
|
||||
|
||||
void UpdateStrapdownEquationsNED(); |
||||
|
||||
void CovariancePrediction(float dt); |
||||
|
||||
void FuseVelposNED(); |
||||
|
||||
void FuseMagnetometer(); |
||||
|
||||
void FuseAirspeed(); |
||||
|
||||
void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); |
||||
|
||||
void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); |
||||
|
||||
void quatNorm(float (&quatOut)[4], const float quatIn[4]); |
||||
|
||||
// store staes along with system time stamp in msces
|
||||
void StoreStates(uint64_t timestamp_ms); |
||||
|
||||
/**
|
||||
* Recall the state vector. |
||||
* |
||||
* Recalls the vector stored at closest time to the one specified by msec |
||||
* |
||||
* @return zero on success, integer indicating the number of invalid states on failure. |
||||
* Does only copy valid states, if the statesForFusion vector was initialized |
||||
* correctly by the caller, the result can be safely used, but is a mixture |
||||
* time-wise where valid states were updated and invalid remained at the old |
||||
* value. |
||||
*/ |
||||
int RecallStates(float statesForFusion[n_states], uint64_t msec); |
||||
|
||||
void ResetStoredStates(); |
||||
|
||||
void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]); |
||||
|
||||
void calcEarthRateNED(Vector3f &omega, float latitude); |
||||
|
||||
static void eul2quat(float (&quat)[4], const float (&eul)[3]); |
||||
|
||||
static void quat2eul(float (&eul)[3], const float (&quat)[4]); |
||||
|
||||
static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); |
||||
|
||||
static void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); |
||||
|
||||
static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); |
||||
|
||||
static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]); |
||||
|
||||
static float sq(float valIn); |
||||
|
||||
void OnGroundCheck(); |
||||
|
||||
void CovarianceInit(); |
||||
|
||||
void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination); |
||||
|
||||
float ConstrainFloat(float val, float min, float max); |
||||
|
||||
void ConstrainVariances(); |
||||
|
||||
void ConstrainStates(); |
||||
|
||||
void ForceSymmetry(); |
||||
|
||||
int CheckAndBound(); |
||||
|
||||
void ResetPosition(); |
||||
|
||||
void ResetVelocity(); |
||||
|
||||
void ZeroVariables(); |
||||
|
||||
void GetFilterState(struct ekf_status_report *state); |
||||
|
||||
void GetLastErrorState(struct ekf_status_report *last_error); |
||||
|
||||
bool StatesNaN(struct ekf_status_report *err_report); |
||||
void FillErrorReport(struct ekf_status_report *err); |
||||
|
||||
void InitializeDynamic(float (&initvelNED)[3], float declination); |
||||
|
||||
protected: |
||||
|
||||
bool FilterHealthy(); |
||||
|
||||
void ResetHeight(void); |
||||
|
||||
void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat); |
||||
|
||||
}; |
||||
|
||||
uint32_t millis(); |
||||
|
||||
File diff suppressed because one or more lines are too long
@ -0,0 +1,125 @@
@@ -0,0 +1,125 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. |
||||
* |
||||
* Redistribution and use in source and binary forms, with or without |
||||
* modification, are permitted provided that the following conditions |
||||
* are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* 2. Redistributions in binary form must reproduce the above copyright |
||||
* notice, this list of conditions and the following disclaimer in |
||||
* the documentation and/or other materials provided with the |
||||
* distribution. |
||||
* 3. Neither the name PX4 nor the names of its contributors may be |
||||
* used to endorse or promote products derived from this software |
||||
* without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
/**
|
||||
* @file FixedwingLandDetector.cpp |
||||
* Land detection algorithm for fixedwings |
||||
* |
||||
* @author Johan Jansen <jnsn.johan@gmail.com> |
||||
*/ |
||||
|
||||
#include "FixedwingLandDetector.h" |
||||
|
||||
#include <cmath> |
||||
#include <drivers/drv_hrt.h> |
||||
|
||||
FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), |
||||
_paramHandle(), |
||||
_params(), |
||||
_vehicleLocalPositionSub(-1), |
||||
_vehicleLocalPosition({}), |
||||
_airspeedSub(-1), |
||||
_airspeed({}), |
||||
_parameterSub(-1), |
||||
_velocity_xy_filtered(0.0f), |
||||
_velocity_z_filtered(0.0f), |
||||
_airspeed_filtered(0.0f), |
||||
_landDetectTrigger(0) |
||||
{ |
||||
_paramHandle.maxVelocity = param_find("LNDFW_VEL_XY_MAX"); |
||||
_paramHandle.maxClimbRate = param_find("LNDFW_VEL_Z_MAX"); |
||||
_paramHandle.maxAirSpeed = param_find("LNDFW_AIRSPD_MAX"); |
||||
} |
||||
|
||||
void FixedwingLandDetector::initialize() |
||||
{ |
||||
// 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); |
||||
} |
||||
|
||||
bool FixedwingLandDetector::update() |
||||
{ |
||||
// 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?
|
||||
_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 < _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
|
||||
if (now > _landDetectTrigger) { |
||||
landDetected = true; |
||||
} |
||||
|
||||
} else { |
||||
// reset land detect trigger
|
||||
_landDetectTrigger = now + LAND_DETECTOR_TRIGGER_TIME; |
||||
} |
||||
|
||||
return landDetected; |
||||
} |
||||
|
||||
void FixedwingLandDetector::updateParameterCache(const bool force) |
||||
{ |
||||
bool updated; |
||||
parameter_update_s paramUpdate; |
||||
|
||||
orb_check(_parameterSub, &updated); |
||||
|
||||
if (updated) { |
||||
orb_copy(ORB_ID(parameter_update), _parameterSub, ¶mUpdate); |
||||
} |
||||
|
||||
if (updated || force) { |
||||
param_get(_paramHandle.maxVelocity, &_params.maxVelocity); |
||||
param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate); |
||||
param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed); |
||||
} |
||||
} |
||||
@ -0,0 +1,105 @@
@@ -0,0 +1,105 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. |
||||
* |
||||
* Redistribution and use in source and binary forms, with or without |
||||
* modification, are permitted provided that the following conditions |
||||
* are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* 2. Redistributions in binary form must reproduce the above copyright |
||||
* notice, this list of conditions and the following disclaimer in |
||||
* the documentation and/or other materials provided with the |
||||
* distribution. |
||||
* 3. Neither the name PX4 nor the names of its contributors may be |
||||
* used to endorse or promote products derived from this software |
||||
* without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
/**
|
||||
* @file FixedwingLandDetector.h |
||||
* Land detection algorithm for fixedwing |
||||
* |
||||
* @author Johan Jansen <jnsn.johan@gmail.com> |
||||
*/ |
||||
|
||||
#ifndef __FIXED_WING_LAND_DETECTOR_H__ |
||||
#define __FIXED_WING_LAND_DETECTOR_H__ |
||||
|
||||
#include "LandDetector.h" |
||||
#include <uORB/topics/vehicle_local_position.h> |
||||
#include <uORB/topics/airspeed.h> |
||||
#include <uORB/topics/parameter_update.h> |
||||
#include <systemlib/param/param.h> |
||||
|
||||
class FixedwingLandDetector : public LandDetector |
||||
{ |
||||
public: |
||||
FixedwingLandDetector(); |
||||
|
||||
protected: |
||||
/**
|
||||
* @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 polls all subscriptions and pulls any data that has changed |
||||
**/ |
||||
void updateSubscriptions(); |
||||
|
||||
private: |
||||
/**
|
||||
* @brief download and update local parameter cache |
||||
**/ |
||||
void updateParameterCache(const bool force); |
||||
|
||||
/**
|
||||
* @brief Handles for interesting parameters |
||||
**/ |
||||
struct { |
||||
param_t maxVelocity; |
||||
param_t maxClimbRate; |
||||
param_t maxAirSpeed; |
||||
} _paramHandle; |
||||
|
||||
struct { |
||||
float maxVelocity; |
||||
float maxClimbRate; |
||||
float maxAirSpeed; |
||||
} _params; |
||||
|
||||
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 _parameterSub; |
||||
|
||||
float _velocity_xy_filtered; |
||||
float _velocity_z_filtered; |
||||
float _airspeed_filtered; |
||||
uint64_t _landDetectTrigger; |
||||
}; |
||||
|
||||
#endif //__FIXED_WING_LAND_DETECTOR_H__
|
||||
@ -0,0 +1,124 @@
@@ -0,0 +1,124 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. |
||||
* |
||||
* Redistribution and use in source and binary forms, with or without |
||||
* modification, are permitted provided that the following conditions |
||||
* are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* 2. Redistributions in binary form must reproduce the above copyright |
||||
* notice, this list of conditions and the following disclaimer in |
||||
* the documentation and/or other materials provided with the |
||||
* distribution. |
||||
* 3. Neither the name PX4 nor the names of its contributors may be |
||||
* used to endorse or promote products derived from this software |
||||
* without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
/**
|
||||
* @file LandDetector.cpp |
||||
* Land detection algorithm |
||||
* |
||||
* @author Johan Jansen <jnsn.johan@gmail.com> |
||||
* @author Morten Lysgaard <morten@lysgaard.no> |
||||
*/ |
||||
|
||||
#include "LandDetector.h" |
||||
#include <unistd.h> //usleep |
||||
#include <drivers/drv_hrt.h> |
||||
|
||||
LandDetector::LandDetector() : |
||||
_landDetectedPub(-1), |
||||
_landDetected({0, false}), |
||||
_taskShouldExit(false), |
||||
_taskIsRunning(false) |
||||
{ |
||||
// ctor
|
||||
} |
||||
|
||||
LandDetector::~LandDetector() |
||||
{ |
||||
_taskShouldExit = true; |
||||
close(_landDetectedPub); |
||||
} |
||||
|
||||
void LandDetector::shutdown() |
||||
{ |
||||
_taskShouldExit = true; |
||||
} |
||||
|
||||
void LandDetector::start() |
||||
{ |
||||
// make sure this method has not already been called by another thread
|
||||
if (isRunning()) { |
||||
return; |
||||
} |
||||
|
||||
// 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(); |
||||
|
||||
// task is now running, keep doing so until shutdown() has been called
|
||||
_taskIsRunning = true; |
||||
_taskShouldExit = false; |
||||
|
||||
while (isRunning()) { |
||||
|
||||
bool landDetected = update(); |
||||
|
||||
// publish if land detection state has changed
|
||||
if (_landDetected.landed != landDetected) { |
||||
_landDetected.timestamp = hrt_absolute_time(); |
||||
_landDetected.landed = landDetected; |
||||
|
||||
// publish the land detected broadcast
|
||||
orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); |
||||
} |
||||
|
||||
// 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; |
||||
|
||||
// check if there is new data to grab
|
||||
if (orb_check(handle, &newData) != OK) { |
||||
return false; |
||||
} |
||||
|
||||
if (!newData) { |
||||
return false; |
||||
} |
||||
|
||||
if (orb_copy(meta, handle, buffer) != OK) { |
||||
return false; |
||||
} |
||||
|
||||
return true; |
||||
} |
||||
@ -0,0 +1,104 @@
@@ -0,0 +1,104 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. |
||||
* |
||||
* Redistribution and use in source and binary forms, with or without |
||||
* modification, are permitted provided that the following conditions |
||||
* are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* 2. Redistributions in binary form must reproduce the above copyright |
||||
* notice, this list of conditions and the following disclaimer in |
||||
* the documentation and/or other materials provided with the |
||||
* distribution. |
||||
* 3. Neither the name PX4 nor the names of its contributors may be |
||||
* used to endorse or promote products derived from this software |
||||
* without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
/**
|
||||
* @file LandDetector.h |
||||
* Abstract interface for land detector algorithms |
||||
* |
||||
* @author Johan Jansen <jnsn.johan@gmail.com> |
||||
*/ |
||||
|
||||
#ifndef __LAND_DETECTOR_H__ |
||||
#define __LAND_DETECTOR_H__ |
||||
|
||||
#include <uORB/uORB.h> |
||||
#include <uORB/topics/vehicle_land_detected.h> |
||||
|
||||
class LandDetector |
||||
{ |
||||
public: |
||||
|
||||
LandDetector(); |
||||
virtual ~LandDetector(); |
||||
|
||||
/**
|
||||
* @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 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 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); |
||||
|
||||
static constexpr uint32_t LAND_DETECTOR_UPDATE_RATE = 50; /**< Run algorithm at 50Hz */ |
||||
|
||||
static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold
|
||||
before triggering a land */ |
||||
|
||||
protected: |
||||
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 */ |
||||
}; |
||||
|
||||
#endif //__LAND_DETECTOR_H__
|
||||
@ -0,0 +1,145 @@
@@ -0,0 +1,145 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. |
||||
* |
||||
* Redistribution and use in source and binary forms, with or without |
||||
* modification, are permitted provided that the following conditions |
||||
* are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* 2. Redistributions in binary form must reproduce the above copyright |
||||
* notice, this list of conditions and the following disclaimer in |
||||
* the documentation and/or other materials provided with the |
||||
* distribution. |
||||
* 3. Neither the name PX4 nor the names of its contributors may be |
||||
* used to endorse or promote products derived from this software |
||||
* without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
/**
|
||||
* @file MulticopterLandDetector.cpp |
||||
* Land detection algorithm for multicopters |
||||
* |
||||
* @author Johan Jansen <jnsn.johan@gmail.com> |
||||
* @author Morten Lysgaard <morten@lysgaard.no> |
||||
*/ |
||||
|
||||
#include "MulticopterLandDetector.h" |
||||
|
||||
#include <cmath> |
||||
#include <drivers/drv_hrt.h> |
||||
|
||||
MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), |
||||
_paramHandle(), |
||||
_params(), |
||||
_vehicleGlobalPositionSub(-1), |
||||
_sensorsCombinedSub(-1), |
||||
_waypointSub(-1), |
||||
_actuatorsSub(-1), |
||||
_armingSub(-1), |
||||
_parameterSub(-1), |
||||
_vehicleGlobalPosition({}), |
||||
_sensors({}), |
||||
_waypoint({}), |
||||
_actuators({}), |
||||
_arming({}), |
||||
_landTimer(0) |
||||
{ |
||||
_paramHandle.maxRotation = param_find("LNDMC_Z_VEL_MAX"); |
||||
_paramHandle.maxVelocity = param_find("LNDMC_XY_VEL_MAX"); |
||||
_paramHandle.maxClimbRate = param_find("LNDMC_ROT_MAX"); |
||||
_paramHandle.maxThrottle = param_find("LNDMC_THR_MAX"); |
||||
} |
||||
|
||||
void MulticopterLandDetector::initialize() |
||||
{ |
||||
// 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)); |
||||
_actuatorsSub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); |
||||
_armingSub = orb_subscribe(ORB_ID(actuator_armed)); |
||||
_parameterSub = orb_subscribe(ORB_ID(parameter_update)); |
||||
|
||||
// download parameters
|
||||
updateParameterCache(true); |
||||
} |
||||
|
||||
void MulticopterLandDetector::updateSubscriptions() |
||||
{ |
||||
orb_update(ORB_ID(vehicle_global_position), _vehicleGlobalPositionSub, &_vehicleGlobalPosition); |
||||
orb_update(ORB_ID(sensor_combined), _sensorsCombinedSub, &_sensors); |
||||
orb_update(ORB_ID(position_setpoint_triplet), _waypointSub, &_waypoint); |
||||
orb_update(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _actuatorsSub, &_actuators); |
||||
orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); |
||||
} |
||||
|
||||
bool MulticopterLandDetector::update() |
||||
{ |
||||
// first poll for new data from our subscriptions
|
||||
updateSubscriptions(); |
||||
|
||||
// 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
|
||||
bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > _params.maxClimbRate; |
||||
|
||||
// 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
|
||||
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)
|
||||
bool minimalThrust = _actuators.control[3] <= _params.maxThrottle; |
||||
|
||||
if (verticalMovement || rotating || !minimalThrust || horizontalMovement) { |
||||
// sensed movement, so reset the land detector
|
||||
_landTimer = now; |
||||
return false; |
||||
} |
||||
|
||||
return now - _landTimer > LAND_DETECTOR_TRIGGER_TIME; |
||||
} |
||||
|
||||
void MulticopterLandDetector::updateParameterCache(const bool force) |
||||
{ |
||||
bool updated; |
||||
parameter_update_s paramUpdate; |
||||
|
||||
orb_check(_parameterSub, &updated); |
||||
|
||||
if (updated) { |
||||
orb_copy(ORB_ID(parameter_update), _parameterSub, ¶mUpdate); |
||||
} |
||||
|
||||
if (updated || force) { |
||||
param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate); |
||||
param_get(_paramHandle.maxVelocity, &_params.maxVelocity); |
||||
param_get(_paramHandle.maxRotation, &_params.maxRotation); |
||||
param_get(_paramHandle.maxThrottle, &_params.maxThrottle); |
||||
} |
||||
} |
||||
@ -0,0 +1,116 @@
@@ -0,0 +1,116 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. |
||||
* |
||||
* Redistribution and use in source and binary forms, with or without |
||||
* modification, are permitted provided that the following conditions |
||||
* are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* 2. Redistributions in binary form must reproduce the above copyright |
||||
* notice, this list of conditions and the following disclaimer in |
||||
* the documentation and/or other materials provided with the |
||||
* distribution. |
||||
* 3. Neither the name PX4 nor the names of its contributors may be |
||||
* used to endorse or promote products derived from this software |
||||
* without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
/**
|
||||
* @file MulticopterLandDetector.h |
||||
* Land detection algorithm for multicopters |
||||
* |
||||
* @author Johan Jansen <jnsn.johan@gmail.com> |
||||
* @author Morten Lysgaard <morten@lysgaard.no> |
||||
*/ |
||||
|
||||
#ifndef __MULTICOPTER_LAND_DETECTOR_H__ |
||||
#define __MULTICOPTER_LAND_DETECTOR_H__ |
||||
|
||||
#include "LandDetector.h" |
||||
#include <uORB/topics/vehicle_global_position.h> |
||||
#include <uORB/topics/sensor_combined.h> |
||||
#include <uORB/topics/position_setpoint_triplet.h> |
||||
#include <uORB/topics/actuator_controls.h> |
||||
#include <uORB/topics/actuator_armed.h> |
||||
#include <uORB/topics/parameter_update.h> |
||||
#include <systemlib/param/param.h> |
||||
|
||||
class MulticopterLandDetector : public LandDetector |
||||
{ |
||||
public: |
||||
MulticopterLandDetector(); |
||||
|
||||
protected: |
||||
/**
|
||||
* @brief polls all subscriptions and pulls any data that has changed |
||||
**/ |
||||
void updateSubscriptions(); |
||||
|
||||
/**
|
||||
* @brief Runs one iteration of the land detection algorithm |
||||
**/ |
||||
bool update() override; |
||||
|
||||
/**
|
||||
* @brief Initializes the land detection algorithm |
||||
**/ |
||||
void initialize() override; |
||||
|
||||
|
||||
private: |
||||
/**
|
||||
* @brief download and update local parameter cache |
||||
**/ |
||||
void updateParameterCache(const bool force); |
||||
|
||||
/**
|
||||
* @brief Handles for interesting parameters |
||||
**/ |
||||
struct { |
||||
param_t maxClimbRate; |
||||
param_t maxVelocity; |
||||
param_t maxRotation; |
||||
param_t maxThrottle; |
||||
} _paramHandle; |
||||
|
||||
struct { |
||||
float maxClimbRate; |
||||
float maxVelocity; |
||||
float maxRotation; |
||||
float maxThrottle; |
||||
} _params; |
||||
|
||||
private: |
||||
int _vehicleGlobalPositionSub; /**< notification of global position */ |
||||
int _sensorsCombinedSub; |
||||
int _waypointSub; |
||||
int _actuatorsSub; |
||||
int _armingSub; |
||||
int _parameterSub; |
||||
|
||||
struct vehicle_global_position_s _vehicleGlobalPosition; /**< the result from global position subscription */ |
||||
struct sensor_combined_s _sensors; /**< subscribe to sensor readings */ |
||||
struct position_setpoint_triplet_s _waypoint; /**< subscribe to autopilot navigation */ |
||||
struct actuator_controls_s _actuators; |
||||
struct actuator_armed_s _arming; |
||||
|
||||
uint64_t _landTimer; /**< timestamp in microseconds since a possible land was detected*/ |
||||
}; |
||||
|
||||
#endif //__MULTICOPTER_LAND_DETECTOR_H__
|
||||
@ -0,0 +1,214 @@
@@ -0,0 +1,214 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. |
||||
* |
||||
* Redistribution and use in source and binary forms, with or without |
||||
* modification, are permitted provided that the following conditions |
||||
* are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* 2. Redistributions in binary form must reproduce the above copyright |
||||
* notice, this list of conditions and the following disclaimer in |
||||
* the documentation and/or other materials provided with the |
||||
* distribution. |
||||
* 3. Neither the name PX4 nor the names of its contributors may be |
||||
* used to endorse or promote products derived from this software |
||||
* without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
/**
|
||||
* @file land_detector_main.cpp |
||||
* Land detection algorithm |
||||
* |
||||
* @author Johan Jansen <jnsn.johan@gmail.com> |
||||
*/ |
||||
|
||||
#include <unistd.h> //usleep |
||||
#include <stdio.h> |
||||
#include <string.h> |
||||
#include <stdlib.h> |
||||
#include <errno.h> |
||||
#include <drivers/drv_hrt.h> |
||||
#include <systemlib/systemlib.h> //Scheduler |
||||
#include <systemlib/err.h> //print to console |
||||
|
||||
#include "FixedwingLandDetector.h" |
||||
#include "MulticopterLandDetector.h" |
||||
|
||||
//Function prototypes
|
||||
static int land_detector_start(const char *mode); |
||||
static void land_detector_stop(); |
||||
|
||||
/**
|
||||
* land detector app start / stop handling function |
||||
* This makes the land detector module accessible from the nuttx shell |
||||
* @ingroup apps |
||||
*/ |
||||
extern "C" __EXPORT int land_detector_main(int argc, char *argv[]); |
||||
|
||||
//Private variables
|
||||
static LandDetector *land_detector_task = nullptr; |
||||
static int _landDetectorTaskID = -1; |
||||
static char _currentMode[12]; |
||||
|
||||
/**
|
||||
* Deamon thread function |
||||
**/ |
||||
static void land_detector_deamon_thread(int argc, char *argv[]) |
||||
{ |
||||
land_detector_task->start(); |
||||
} |
||||
|
||||
/**
|
||||
* Stop the task, force killing it if it doesn't stop by itself |
||||
**/ |
||||
static void land_detector_stop() |
||||
{ |
||||
if (land_detector_task == nullptr || _landDetectorTaskID == -1) { |
||||
errx(1, "not running"); |
||||
return; |
||||
} |
||||
|
||||
land_detector_task->shutdown(); |
||||
|
||||
//Wait for task to die
|
||||
int i = 0; |
||||
|
||||
do { |
||||
/* wait 20ms */ |
||||
usleep(20000); |
||||
|
||||
/* if we have given up, kill it */ |
||||
if (++i > 50) { |
||||
task_delete(_landDetectorTaskID); |
||||
break; |
||||
} |
||||
} while (land_detector_task->isRunning()); |
||||
|
||||
|
||||
delete land_detector_task; |
||||
land_detector_task = nullptr; |
||||
_landDetectorTaskID = -1; |
||||
errx(0, "land_detector has been stopped"); |
||||
} |
||||
|
||||
/**
|
||||
* Start new task, fails if it is already running. Returns OK if successful |
||||
**/ |
||||
static int land_detector_start(const char *mode) |
||||
{ |
||||
if (land_detector_task != nullptr || _landDetectorTaskID != -1) { |
||||
errx(1, "already running"); |
||||
return -1; |
||||
} |
||||
|
||||
//Allocate memory
|
||||
if (!strcmp(mode, "fixedwing")) { |
||||
land_detector_task = new FixedwingLandDetector(); |
||||
|
||||
} else if (!strcmp(mode, "multicopter")) { |
||||
land_detector_task = new MulticopterLandDetector(); |
||||
|
||||
} else { |
||||
errx(1, "[mode] must be either 'fixedwing' or 'multicopter'"); |
||||
return -1; |
||||
} |
||||
|
||||
//Check if alloc worked
|
||||
if (land_detector_task == nullptr) { |
||||
errx(1, "alloc failed"); |
||||
return -1; |
||||
} |
||||
|
||||
//Start new thread task
|
||||
_landDetectorTaskID = task_spawn_cmd("land_detector", |
||||
SCHED_DEFAULT, |
||||
SCHED_PRIORITY_DEFAULT, |
||||
1200, |
||||
(main_t)&land_detector_deamon_thread, |
||||
nullptr); |
||||
|
||||
if (_landDetectorTaskID < 0) { |
||||
errx(1, "task start failed: %d", -errno); |
||||
return -1; |
||||
} |
||||
|
||||
/* avoid memory fragmentation by not exiting start handler until the task has fully started */ |
||||
const uint32_t timeout = hrt_absolute_time() + 5000000; //5 second timeout
|
||||
|
||||
while (!land_detector_task->isRunning()) { |
||||
usleep(50000); |
||||
printf("."); |
||||
fflush(stdout); |
||||
|
||||
if (hrt_absolute_time() > timeout) { |
||||
err(1, "start failed - timeout"); |
||||
land_detector_stop(); |
||||
exit(1); |
||||
} |
||||
} |
||||
|
||||
printf("\n"); |
||||
|
||||
//Remember current active mode
|
||||
strncpy(_currentMode, mode, 12); |
||||
|
||||
exit(0); |
||||
return 0; |
||||
} |
||||
|
||||
/**
|
||||
* Main entry point for this module |
||||
**/ |
||||
int land_detector_main(int argc, char *argv[]) |
||||
{ |
||||
|
||||
if (argc < 1) { |
||||
warnx("usage: land_detector {start|stop|status} [mode]\nmode can either be 'fixedwing' or 'multicopter'"); |
||||
exit(0); |
||||
} |
||||
|
||||
if (argc >= 2 && !strcmp(argv[1], "start")) { |
||||
land_detector_start(argv[2]); |
||||
} |
||||
|
||||
if (!strcmp(argv[1], "stop")) { |
||||
land_detector_stop(); |
||||
exit(0); |
||||
} |
||||
|
||||
if (!strcmp(argv[1], "status")) { |
||||
if (land_detector_task) { |
||||
|
||||
if (land_detector_task->isRunning()) { |
||||
warnx("running (%s)", _currentMode); |
||||
|
||||
} else { |
||||
errx(1, "exists, but not running (%s)", _currentMode); |
||||
} |
||||
|
||||
exit(0); |
||||
|
||||
} else { |
||||
errx(1, "not running"); |
||||
} |
||||
} |
||||
|
||||
warn("usage: land_detector {start|stop|status} [mode]\nmode can either be 'fixedwing' or 'multicopter'"); |
||||
return 1; |
||||
} |
||||
@ -0,0 +1,104 @@
@@ -0,0 +1,104 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. |
||||
* |
||||
* Redistribution and use in source and binary forms, with or without |
||||
* modification, are permitted provided that the following conditions |
||||
* are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* 2. Redistributions in binary form must reproduce the above copyright |
||||
* notice, this list of conditions and the following disclaimer in |
||||
* the documentation and/or other materials provided with the |
||||
* distribution. |
||||
* 3. Neither the name PX4 nor the names of its contributors may be |
||||
* used to endorse or promote products derived from this software |
||||
* without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
/**
|
||||
* @file land_detector.c |
||||
* Land detector algorithm parameters. |
||||
* |
||||
* @author Johan Jansen <jnsn.johan@gmail.com> |
||||
*/ |
||||
|
||||
#include <systemlib/param/param.h> |
||||
|
||||
/**
|
||||
* Multicopter max climb rate |
||||
* |
||||
* Maximum vertical velocity allowed to trigger a land (m/s up and down) |
||||
* |
||||
* @group Land Detector |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.30f); |
||||
|
||||
/**
|
||||
* Multicopter max horizontal velocity |
||||
* |
||||
* Maximum horizontal velocity allowed to trigger a land (m/s) |
||||
* |
||||
* @group Land Detector |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.00f); |
||||
|
||||
/**
|
||||
* Multicopter max rotation |
||||
* |
||||
* Maximum allowed around each axis to trigger a land (radians per second) |
||||
* |
||||
* @group Land Detector |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 0.20f); |
||||
|
||||
/**
|
||||
* Multicopter max throttle |
||||
* |
||||
* Maximum actuator output on throttle before triggering a land |
||||
* |
||||
* @group Land Detector |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(LNDMC_THR_MAX, 0.20f); |
||||
|
||||
/**
|
||||
* Fixedwing max horizontal velocity |
||||
* |
||||
* Maximum horizontal velocity allowed to trigger a land (m/s) |
||||
* |
||||
* @group Land Detector |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 0.20f); |
||||
|
||||
/**
|
||||
* Fixedwing max climb rate |
||||
* |
||||
* Maximum vertical velocity allowed to trigger a land (m/s up and down) |
||||
* |
||||
* @group Land Detector |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 10.00f); |
||||
|
||||
/**
|
||||
* Airspeed max |
||||
* |
||||
* Maximum airspeed allowed to trigger a land (m/s) |
||||
* |
||||
* @group Land Detector |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(LNDFW_AIRSPD_MAX, 10.00f); |
||||
@ -0,0 +1,13 @@
@@ -0,0 +1,13 @@
|
||||
#
|
||||
# Land detector
|
||||
#
|
||||
|
||||
MODULE_COMMAND = land_detector
|
||||
|
||||
SRCS = land_detector_main.cpp \
|
||||
land_detector_params.c \
|
||||
LandDetector.cpp \
|
||||
MulticopterLandDetector.cpp \
|
||||
FixedwingLandDetector.cpp
|
||||
|
||||
EXTRACXXFLAGS = -Weffc++ -Os
|
||||
@ -0,0 +1,63 @@
@@ -0,0 +1,63 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. |
||||
* |
||||
* Redistribution and use in source and binary forms, with or without |
||||
* modification, are permitted provided that the following conditions |
||||
* are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* 2. Redistributions in binary form must reproduce the above copyright |
||||
* notice, this list of conditions and the following disclaimer in |
||||
* the documentation and/or other materials provided with the |
||||
* distribution. |
||||
* 3. Neither the name PX4 nor the names of its contributors may be |
||||
* used to endorse or promote products derived from this software |
||||
* without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
/**
|
||||
* @file vehicle_land_detected.h |
||||
* Land detected uORB topic |
||||
* |
||||
* @author Johan Jansen <jnsn.johan@gmail.com> |
||||
*/ |
||||
|
||||
#ifndef __TOPIC_VEHICLE_LANDED_H__ |
||||
#define __TOPIC_VEHICLE_LANDED_H__ |
||||
|
||||
#include "../uORB.h" |
||||
|
||||
/**
|
||||
* @addtogroup topics |
||||
* @{ |
||||
*/ |
||||
|
||||
struct vehicle_land_detected_s { |
||||
uint64_t timestamp; /**< timestamp of the setpoint */ |
||||
bool landed; /**< true if vehicle is currently landed on the ground*/ |
||||
}; |
||||
|
||||
/**
|
||||
* @} |
||||
*/ |
||||
|
||||
/* register this as object request broker structure */ |
||||
ORB_DECLARE(vehicle_land_detected); |
||||
|
||||
#endif //__TOPIC_VEHICLE_LANDED_H__
|
||||
Loading…
Reference in new issue