fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

update airspeed usage to airspeed_validated (#13710)

Browse Source 
* 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>
This commit is contained in:
Silvan Fuhrer 2019-12-28 18:32:41 +01:00 committed by Daniel Agar
parent fc1341208f
commit 7bb952baed
3 changed files with 18 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/modules/land_detector/FixedwingLandDetector.cpp
View File

@ -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()
@ -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.

6
src/modules/land_detector/FixedwingLandDetector.h
View File

@ -43,7 +43,7 @@
#pragma once
#include <matrix/math.hpp>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/airspeed_validated.h>
#include "LandDetector.h"
@ -69,9 +69,9 @@ private:
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};

14
src/modules/mavlink/mavlink_messages.cpp
View File

@ -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>
@ -1398,7 +1398,7 @@ private:
MavlinkOrbSubscription *_act0_sub;
MavlinkOrbSubscription *_act1_sub;
MavlinkOrbSubscription *_airspeed_sub;
MavlinkOrbSubscription *_airspeed_validated_sub;
uint64_t _airspeed_time;
MavlinkOrbSubscription *_air_data_sub;
@ -1415,7 +1415,7 @@ protected:
_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)))
{}
@ -1424,16 +1424,16 @@ protected:
{
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));