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 44c7b3e434..24a198f0a5 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 @@ -859,7 +859,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: /** diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index f8066241c5..cff46c1a3f 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,42 +49,44 @@ 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), + _vehicleStatusSub(-1), + _armingSub(-1), + _airspeedSub(-1), + _controlState{}, + _vehicleStatus{}, + _arming{}, + _airspeed{}, + _parameterSub(-1), + _velocity_xy_filtered(0.0f), + _velocity_z_filtered(0.0f), + _airspeed_filtered(0.0f), + _accel_horz_lp(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)); + _airspeedSub = orb_subscribe(ORB_ID(airspeed)); updateParameterCache(true); } 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); + orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); } bool FixedwingLandDetector::update() @@ -99,29 +102,33 @@ 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; + + // 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; + } // 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_horz_lp < _params.maxIntVelocity) { // these conditions need to be stable for a period of time before we trust them if (now > _landDetectTrigger) { @@ -151,5 +158,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..5c5b18a9e5 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -42,11 +42,11 @@ #define __FIXED_WING_LAND_DETECTOR_H__ #include "LandDetector.h" -#include -#include +#include #include #include #include +#include #include class FixedwingLandDetector : public LandDetector @@ -83,28 +83,31 @@ 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 */ int _vehicleStatusSub; int _armingSub; - struct airspeed_s _airspeed; + 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; float _velocity_z_filtered; float _airspeed_filtered; + float _accel_horz_lp; 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 *