dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

fix commander: check that offboard_control_mode data is recent 

This is a regression from https://github.com/PX4/PX4-Autopilot/pull/20172
This commit is contained in:
Beat Küng 2022-11-02 08:50:09 +01:00 committed by Daniel Agar
parent 298cc61e07
commit 6511866408
2 changed files with 6 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.cpp
View File

@ -35,11 +35,6 @@
using namespace time_literals;
OffboardChecks::OffboardChecks()
{
_offboard_available.set_hysteresis_time_from(true, _param_com_of_loss_t.get() * 1_s);
}
void OffboardChecks::checkAndReport(const Context &context, Report &reporter)
{
reporter.failsafeFlags().offboard_control_signal_lost = true;
@ -47,9 +42,10 @@ void OffboardChecks::checkAndReport(const Context &context, Report &reporter)
offboard_control_mode_s offboard_control_mode;
if (_offboard_control_mode_sub.copy(&offboard_control_mode)) {
bool offboard_available = offboard_control_mode.position || offboard_control_mode.velocity
|| offboard_control_mode.acceleration || offboard_control_mode.attitude || offboard_control_mode.body_rate
|| offboard_control_mode.actuator;
bool data_is_recent = hrt_absolute_time() < offboard_control_mode.timestamp + _param_com_of_loss_t.get() * 1_s;
bool offboard_available = (offboard_control_mode.position || offboard_control_mode.velocity
|| offboard_control_mode.acceleration || offboard_control_mode.attitude || offboard_control_mode.body_rate
|| offboard_control_mode.actuator) && data_is_recent;
if (offboard_control_mode.position && reporter.failsafeFlags().local_position_invalid) {
offboard_available = false;
@ -62,9 +58,7 @@ void OffboardChecks::checkAndReport(const Context &context, Report &reporter)
offboard_available = false;
}
_offboard_available.set_state_and_update(offboard_available, hrt_absolute_time());
// This is a mode requirement, no need to report
reporter.failsafeFlags().offboard_control_signal_lost = !_offboard_available.get_state();
reporter.failsafeFlags().offboard_control_signal_lost = !offboard_available;
}
}

4
src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.hpp
View File

@ -37,19 +37,17 @@
#include <uORB/Subscription.hpp>
#include <uORB/topics/offboard_control_mode.h>
#include <lib/hysteresis/hysteresis.h>
class OffboardChecks : public HealthAndArmingCheckBase
{
public:
OffboardChecks();
OffboardChecks() = default;
~OffboardChecks() = default;
void checkAndReport(const Context &context, Report &reporter) override;
private:
uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
systemlib::Hysteresis _offboard_available{false};
DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
(ParamFloat<px4::params::COM_OF_LOSS_T>) _param_com_of_loss_t