|
|
|
@ -37,6 +37,7 @@
@@ -37,6 +37,7 @@
|
|
|
|
|
#include <circuit_breaker/circuit_breaker.h> |
|
|
|
|
#include <mathlib/math/Limits.hpp> |
|
|
|
|
#include <mathlib/math/Functions.hpp> |
|
|
|
|
#include <px4_platform_common/events.h> |
|
|
|
|
|
|
|
|
|
using namespace matrix; |
|
|
|
|
using namespace time_literals; |
|
|
|
@ -155,7 +156,10 @@ MulticopterRateControl::Run()
@@ -155,7 +156,10 @@ MulticopterRateControl::Run()
|
|
|
|
|
if (_landing_gear_sub.copy(&landing_gear)) { |
|
|
|
|
if (landing_gear.landing_gear != landing_gear_s::GEAR_KEEP) { |
|
|
|
|
if (landing_gear.landing_gear == landing_gear_s::GEAR_UP && (_landed || _maybe_landed)) { |
|
|
|
|
mavlink_log_critical(&_mavlink_log_pub, "Landed, unable to retract landing gear") |
|
|
|
|
mavlink_log_critical(&_mavlink_log_pub, "Landed, unable to retract landing gear\t"); |
|
|
|
|
events::send(events::ID("mc_rate_control_not_retract_landing_gear_landed"), |
|
|
|
|
{events::Log::Error, events::LogInternal::Info}, |
|
|
|
|
"Landed, unable to retract landing gear"); |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
|
_landing_gear = landing_gear.landing_gear; |
|
|
|
|