From dc7369a1d0aca5c24b8523b2540f7a76a13b6aab Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Nov 2015 15:05:24 +0100 Subject: [PATCH 1/4] EKF: Report horizontal acceleration --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 4 +++- src/modules/ekf_att_pos_estimator/estimator_22states.h | 2 ++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index f4fcf78e25..5505b4c239 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -852,7 +852,9 @@ void AttitudePositionEstimatorEKF::publishControlState() /* Accelerations in Body Frame */ _ctrl_state.x_acc = _ekf->accel.x; _ctrl_state.y_acc = _ekf->accel.y; - _ctrl_state.z_acc = _ekf->accel.z; + _ctrl_state.z_acc = _ekf->accel.z - _ekf->states[13]; + + _ctrl_state.horz_acc_mag = _ekf->getAccNavMagHorizontal(); /* Velocity in Body Frame */ Vector3f v_n(_ekf->states[4], _ekf->states[5], _ekf->states[6]); diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index 6eb325d531..c0a51afc0a 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -382,6 +382,8 @@ public: void get_covariance(float c[28]); + float getAccNavMagHorizontal() { return _accNavMagHorizontal; } + protected: /** From f02dc3c95f716277a319699aeea233343574b8eb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Nov 2015 15:06:00 +0100 Subject: [PATCH 2/4] Fixed wing land detector: Use control state, use horizontal acceleration for takeoff detect --- .../land_detector/FixedwingLandDetector.cpp | 63 +++++++++++-------- .../land_detector/FixedwingLandDetector.h | 14 +++-- src/modules/land_detector/LandDetector.cpp | 7 ++- .../land_detector/land_detector_params.c | 13 ++++ 4 files changed, 63 insertions(+), 34 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index f8066241c5..7614ccd3d1 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -36,6 +36,7 @@ * Land detection algorithm for fixedwings * * @author Johan Jansen + * @author Lorenz Meier */ #include "FixedwingLandDetector.h" @@ -48,30 +49,31 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), _paramHandle(), _params(), - _vehicleLocalPositionSub(-1), - _vehicleLocalPosition( {}), - _airspeedSub(-1), - _vehicleStatusSub(-1), - _armingSub(-1), - _airspeed{}, - _vehicleStatus{}, - _arming{}, - _parameterSub(-1), - _velocity_xy_filtered(0.0f), - _velocity_z_filtered(0.0f), - _airspeed_filtered(0.0f), - _landDetectTrigger(0) + _controlStateSub(-1), + _controlState{}, + _vehicleStatusSub(-1), + _armingSub(-1), + _vehicleStatus{}, + _arming{}, + _parameterSub(-1), + _velocity_xy_filtered(0.0f), + _velocity_z_filtered(0.0f), + _airspeed_filtered(0.0f), + _accel_x_integral(0.0f), + _lastTime(0), + _lastXAccel(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"); + _paramHandle.maxIntVelocity = param_find("LNDFW_VELI_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)); + _controlStateSub = orb_subscribe(ORB_ID(control_state)); _vehicleStatusSub = orb_subscribe(ORB_ID(vehicle_status)); _armingSub = orb_subscribe(ORB_ID(actuator_armed)); @@ -80,14 +82,16 @@ void FixedwingLandDetector::initialize() void FixedwingLandDetector::updateSubscriptions() { - orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition); - orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); + orb_update(ORB_ID(control_state), _controlStateSub, &_controlState); orb_update(ORB_ID(vehicle_status), _vehicleStatusSub, &_vehicleStatus); orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); } bool FixedwingLandDetector::update() { + _lastXAccel = _controlState.x_acc; + _lastTime = _controlState.timestamp; + // First poll for new data from our subscriptions updateSubscriptions(); @@ -99,29 +103,35 @@ bool FixedwingLandDetector::update() const uint64_t now = hrt_absolute_time(); bool landDetected = false; - if (hrt_elapsed_time(&_vehicleLocalPosition.timestamp) < 500 * 1000) { - float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicleLocalPosition.vx * - _vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy); + if (hrt_elapsed_time(&_controlState.timestamp) < 500 * 1000) { + float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_controlState.x_vel * + _controlState.x_vel + _controlState.y_vel * _controlState.y_vel); if (PX4_ISFINITE(val)) { _velocity_xy_filtered = val; } - val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_vehicleLocalPosition.vz); + val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_controlState.z_vel); if (PX4_ISFINITE(val)) { _velocity_z_filtered = val; } - } - if (hrt_elapsed_time(&_airspeed.timestamp) < 500 * 1000) { - _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; + _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _controlState.airspeed; + + if (_lastTime != 0) { + /* a leaking integrator prevents biases from building up, but + * gives a mostly correct response for short impulses + */ + _accel_x_integral = _accel_x_integral * 0.8f + _controlState.horz_acc_mag * 0.18f; + } } // crude land detector for fixedwing if (_velocity_xy_filtered < _params.maxVelocity && _velocity_z_filtered < _params.maxClimbRate - && _airspeed_filtered < _params.maxAirSpeed) { + && _airspeed_filtered < _params.maxAirSpeed + && _accel_x_integral < _params.maxIntVelocity) { // these conditions need to be stable for a period of time before we trust them if (now > _landDetectTrigger) { @@ -133,6 +143,8 @@ bool FixedwingLandDetector::update() _landDetectTrigger = now + LAND_DETECTOR_TRIGGER_TIME; } + _lastTime = _controlState.timestamp; + return landDetected; } @@ -151,5 +163,6 @@ void FixedwingLandDetector::updateParameterCache(const bool force) param_get(_paramHandle.maxVelocity, &_params.maxVelocity); param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate); param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed); + param_get(_paramHandle.maxIntVelocity, &_params.maxIntVelocity); } } diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index 325faee794..cce99cb9fc 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -42,8 +42,7 @@ #define __FIXED_WING_LAND_DETECTOR_H__ #include "LandDetector.h" -#include -#include +#include #include #include #include @@ -83,21 +82,21 @@ private: param_t maxVelocity; param_t maxClimbRate; param_t maxAirSpeed; + param_t maxIntVelocity; } _paramHandle; struct { float maxVelocity; float maxClimbRate; float maxAirSpeed; + float maxIntVelocity; } _params; private: - int _vehicleLocalPositionSub; /**< notification of local position */ - struct vehicle_local_position_s _vehicleLocalPosition; /**< the result from local position subscription */ - int _airspeedSub; + int _controlStateSub; /**< notification of local position */ + struct control_state_s _controlState; /**< the result from local position subscription */ int _vehicleStatusSub; int _armingSub; - struct airspeed_s _airspeed; struct vehicle_status_s _vehicleStatus; struct actuator_armed_s _arming; int _parameterSub; @@ -105,6 +104,9 @@ private: float _velocity_xy_filtered; float _velocity_z_filtered; float _airspeed_filtered; + float _accel_x_integral; + uint64_t _lastTime; + float _lastXAccel; uint64_t _landDetectTrigger; }; diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 3040e23407..5140a2809c 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -57,14 +57,14 @@ _work{} { LandDetector::~LandDetector() { - work_cancel(LPWORK, &_work); + work_cancel(HPWORK, &_work); _taskShouldExit = true; } int LandDetector::start() { /* schedule a cycle to start things */ - work_queue(LPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, 0); + work_queue(HPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, 0); return 0; } @@ -107,10 +107,11 @@ void LandDetector::cycle() // publish the land detected broadcast orb_publish(ORB_ID(vehicle_land_detected), (orb_advert_t)_landDetectedPub, &_landDetected); + //warnx("in air status changed: %s", (_landDetected.landed) ? "LANDED" : "TAKEOFF"); } if (!_taskShouldExit) { - work_queue(LPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, + work_queue(HPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, USEC2TICK(1000000 / LAND_DETECTOR_UPDATE_RATE)); } } diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c index d9201eb18a..690467b97c 100644 --- a/src/modules/land_detector/land_detector_params.c +++ b/src/modules/land_detector/land_detector_params.c @@ -109,6 +109,19 @@ PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 5.0f); */ PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 10.0f); +/** + * Fixedwing max short-term velocity + * + * Maximum velocity integral in flight direction allowed in the landed state (m/s) + * + * @min 2 + * @max 10 + * @unit m/s + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LNDFW_VELI_MAX, 4.0f); + /** * Airspeed max * From 515d81b8d65dbab25aadf7543893621c918c9a82 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Nov 2015 15:13:56 +0100 Subject: [PATCH 3/4] Land detector: Move back to more agile raw airspeed --- src/modules/land_detector/FixedwingLandDetector.cpp | 9 +++++++-- src/modules/land_detector/FixedwingLandDetector.h | 5 ++++- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 7614ccd3d1..31c1195281 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -50,11 +50,13 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), _paramHandle(), _params(), _controlStateSub(-1), - _controlState{}, _vehicleStatusSub(-1), _armingSub(-1), + _airspeedSub(-1), + _controlState{}, _vehicleStatus{}, _arming{}, + _airspeed{}, _parameterSub(-1), _velocity_xy_filtered(0.0f), _velocity_z_filtered(0.0f), @@ -76,6 +78,7 @@ void FixedwingLandDetector::initialize() _controlStateSub = orb_subscribe(ORB_ID(control_state)); _vehicleStatusSub = orb_subscribe(ORB_ID(vehicle_status)); _armingSub = orb_subscribe(ORB_ID(actuator_armed)); + _airspeedSub = orb_subscribe(ORB_ID(airspeed)); updateParameterCache(true); } @@ -85,6 +88,7 @@ void FixedwingLandDetector::updateSubscriptions() orb_update(ORB_ID(control_state), _controlStateSub, &_controlState); orb_update(ORB_ID(vehicle_status), _vehicleStatusSub, &_vehicleStatus); orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); + orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); } bool FixedwingLandDetector::update() @@ -117,7 +121,7 @@ bool FixedwingLandDetector::update() _velocity_z_filtered = val; } - _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _controlState.airspeed; + _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; if (_lastTime != 0) { /* a leaking integrator prevents biases from building up, but @@ -125,6 +129,7 @@ bool FixedwingLandDetector::update() */ _accel_x_integral = _accel_x_integral * 0.8f + _controlState.horz_acc_mag * 0.18f; } + } // crude land detector for fixedwing diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index cce99cb9fc..32179c8698 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -46,6 +46,7 @@ #include #include #include +#include #include class FixedwingLandDetector : public LandDetector @@ -94,11 +95,13 @@ private: private: int _controlStateSub; /**< notification of local position */ - struct control_state_s _controlState; /**< the result from local position subscription */ int _vehicleStatusSub; int _armingSub; + int _airspeedSub; + struct control_state_s _controlState; /**< the result from local position subscription */ struct vehicle_status_s _vehicleStatus; struct actuator_armed_s _arming; + struct airspeed_s _airspeed; int _parameterSub; float _velocity_xy_filtered; From de46d8e872a3c6a6b6ee65669b036dfd6760b859 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Nov 2015 15:17:50 +0100 Subject: [PATCH 4/4] Land detector: Code cleanup --- .../land_detector/FixedwingLandDetector.cpp | 20 +++++-------------- .../land_detector/FixedwingLandDetector.h | 4 +--- 2 files changed, 6 insertions(+), 18 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 31c1195281..cff46c1a3f 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -61,9 +61,7 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), _velocity_xy_filtered(0.0f), _velocity_z_filtered(0.0f), _airspeed_filtered(0.0f), - _accel_x_integral(0.0f), - _lastTime(0), - _lastXAccel(0.0f), + _accel_horz_lp(0.0f), _landDetectTrigger(0) { _paramHandle.maxVelocity = param_find("LNDFW_VEL_XY_MAX"); @@ -93,9 +91,6 @@ void FixedwingLandDetector::updateSubscriptions() bool FixedwingLandDetector::update() { - _lastXAccel = _controlState.x_acc; - _lastTime = _controlState.timestamp; - // First poll for new data from our subscriptions updateSubscriptions(); @@ -123,12 +118,9 @@ bool FixedwingLandDetector::update() _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; - if (_lastTime != 0) { - /* a leaking integrator prevents biases from building up, but - * gives a mostly correct response for short impulses - */ - _accel_x_integral = _accel_x_integral * 0.8f + _controlState.horz_acc_mag * 0.18f; - } + // a leaking lowpass prevents biases from building up, but + // gives a mostly correct response for short impulses + _accel_horz_lp = _accel_horz_lp * 0.8f + _controlState.horz_acc_mag * 0.18f; } @@ -136,7 +128,7 @@ bool FixedwingLandDetector::update() if (_velocity_xy_filtered < _params.maxVelocity && _velocity_z_filtered < _params.maxClimbRate && _airspeed_filtered < _params.maxAirSpeed - && _accel_x_integral < _params.maxIntVelocity) { + && _accel_horz_lp < _params.maxIntVelocity) { // these conditions need to be stable for a period of time before we trust them if (now > _landDetectTrigger) { @@ -148,8 +140,6 @@ bool FixedwingLandDetector::update() _landDetectTrigger = now + LAND_DETECTOR_TRIGGER_TIME; } - _lastTime = _controlState.timestamp; - return landDetected; } diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index 32179c8698..5c5b18a9e5 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -107,9 +107,7 @@ private: float _velocity_xy_filtered; float _velocity_z_filtered; float _airspeed_filtered; - float _accel_x_integral; - uint64_t _lastTime; - float _lastXAccel; + float _accel_horz_lp; uint64_t _landDetectTrigger; };