Skip to content

Commit

Permalink
update airspeed usage to airspeed_validated (#13710)
Browse files Browse the repository at this point in the history
* Mavlink: subscribe to airspeed_validated instead of airspeed topic

	This e.g. changes the way QGC displays the airspeed in case of an
	airspeed failure (0 instead of the last valid airspeed). It will
	always display the airspeed that's used currently in the control
	modules.

* FW land detector: move to subscribe to airspeed_validated instead of airspeed topic
	- the land detector checks further if the airspeed is NAN, in which case
	it sets the airspeed to 0 (min groundspeed, vz  and accel checks still
	have to pass.

* Fixed-wing land detector use airspeed_vaidated: addressed review comments
	- replaced ternary by conditional
	- set airspeed to 0 if airspeed_validated stops publishing

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
  • Loading branch information
sfuhrer authored and LorenzMeier committed Dec 30, 2019
1 parent 0726fb0 commit a62ccf7
Show file tree
Hide file tree
Showing 3 changed files with 18 additions and 12 deletions.
10 changes: 8 additions & 2 deletions src/modules/land_detector/FixedwingLandDetector.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,7 @@ FixedwingLandDetector::FixedwingLandDetector()
void FixedwingLandDetector::_update_topics()
{
LandDetector::_update_topics();
_airspeed_sub.update(&_airspeed);
_airspeed_validated_sub.update(&_airspeed_validated);
}

bool FixedwingLandDetector::_get_landed_state()
Expand Down Expand Up @@ -83,7 +83,13 @@ bool FixedwingLandDetector::_get_landed_state()
_velocity_z_filtered = val;
}

_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s;
// set _airspeed_filtered to 0 if airspeed data is invalid
if (!PX4_ISFINITE(_airspeed_validated.true_airspeed_m_s) || hrt_elapsed_time(&_airspeed_validated.timestamp) > 1_s) {
_airspeed_filtered = 0.0f;

} else {
_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed_validated.true_airspeed_m_s;
}

// A leaking lowpass prevents biases from building up, but
// gives a mostly correct response for short impulses.
Expand Down
6 changes: 3 additions & 3 deletions src/modules/land_detector/FixedwingLandDetector.h
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@
#pragma once

#include <matrix/math.hpp>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/airspeed_validated.h>

#include "LandDetector.h"

Expand All @@ -69,9 +69,9 @@ class FixedwingLandDetector final : public LandDetector
static constexpr hrt_abstime LANDED_TRIGGER_TIME_US = 2_s;
static constexpr hrt_abstime FLYING_TRIGGER_TIME_US = 0_us;

uORB::Subscription _airspeed_sub{ORB_ID(airspeed)};
uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};

airspeed_s _airspeed{};
airspeed_validated_s _airspeed_validated{};

float _airspeed_filtered{0.0f};
float _velocity_xy_filtered{0.0f};
Expand Down
14 changes: 7 additions & 7 deletions src/modules/mavlink/mavlink_messages.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,7 @@
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/airspeed_validated.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/camera_trigger.h>
#include <uORB/topics/camera_capture.h>
Expand Down Expand Up @@ -1396,7 +1396,7 @@ class MavlinkStreamVFRHUD : public MavlinkStream
MavlinkOrbSubscription *_act0_sub;
MavlinkOrbSubscription *_act1_sub;

MavlinkOrbSubscription *_airspeed_sub;
MavlinkOrbSubscription *_airspeed_validated_sub;
uint64_t _airspeed_time;

MavlinkOrbSubscription *_air_data_sub;
Expand All @@ -1413,7 +1413,7 @@ class MavlinkStreamVFRHUD : public MavlinkStream
_armed_time(0),
_act0_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))),
_act1_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_1))),
_airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))),
_airspeed_validated_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed_validated))),
_airspeed_time(0),
_air_data_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_air_data)))
{}
Expand All @@ -1422,16 +1422,16 @@ class MavlinkStreamVFRHUD : public MavlinkStream
{
vehicle_local_position_s pos = {};
actuator_armed_s armed = {};
airspeed_s airspeed = {};
airspeed_validated_s airspeed_validated = {};

bool updated = false;
updated |= _pos_sub->update(&_pos_time, &pos);
updated |= _armed_sub->update(&_armed_time, &armed);
updated |= _airspeed_sub->update(&_airspeed_time, &airspeed);
updated |= _airspeed_validated_sub->update(&_airspeed_time, &airspeed_validated);

if (updated) {
mavlink_vfr_hud_t msg = {};
msg.airspeed = airspeed.indicated_airspeed_m_s;
mavlink_vfr_hud_t msg{};
msg.airspeed = airspeed_validated.indicated_airspeed_m_s;
msg.groundspeed = sqrtf(pos.vx * pos.vx + pos.vy * pos.vy);
msg.heading = math::degrees(wrap_2pi(pos.yaw));

Expand Down

0 comments on commit a62ccf7

Please sign in to comment.