Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Pr move land detector mavlink to airspeed validated #13710

Merged
merged 7 commits into from
Dec 28, 2019
Merged

Pr move land detector mavlink to airspeed validated #13710

Show file tree
Hide file tree
Changes from 4 commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
cb95c9c
Mavlink: subscribe to airspeed_validated instead of airspeed topic
sfuhrer Dec 9, 2019
f2a2f1f
FW land detector: move to subscribe to airspeed_validated instead of …
sfuhrer Dec 9, 2019
e0d5e06
Fixed-wing land detector use airspeed_vaidated: addressed review comm…
sfuhrer Dec 15, 2019
f9f7f0e
Merge branch 'master' into pr-move_land_detector_mavlink_to_airspeed_…
dagar Dec 24, 2019
2773f76
Merge branch 'master' into pr-move_land_detector_mavlink_to_airspeed_…
sfuhrer Dec 25, 2019
6ac7e82
Merge branch 'master' into pr-move_land_detector_mavlink_to_airspeed_…
dagar Dec 27, 2019
1ad7d4c
Merge branch 'master' into pr-move_land_detector_mavlink_to_airspeed_…
dagar Dec 28, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 8 additions & 2 deletions src/modules/land_detector/FixedwingLandDetector.cpp
View file
Open in desktop
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
View file
Open in desktop
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
View file
Open in desktop
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 @@ -1398,7 +1398,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 @@ -1415,7 +1415,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 @@ -1424,16 +1424,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