diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index 85f6c14c79..44ed5d99cc 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/src/modules/manual_control/ManualControl.cpp @@ -32,12 +32,6 @@ ****************************************************************************/ #include "ManualControl.hpp" - -#include -#include -#include -#include -#include #include namespace manual_control @@ -201,10 +195,10 @@ void ManualControl::Run() && _previous_switches.gear_switch != manual_control_switches_s::SWITCH_POS_NONE) { if (switches.gear_switch == manual_control_switches_s::SWITCH_POS_ON) { - publish_landing_gear(landing_gear_s::GEAR_UP); + publishLandingGear(landing_gear_s::GEAR_UP); } else if (switches.gear_switch == manual_control_switches_s::SWITCH_POS_OFF) { - publish_landing_gear(landing_gear_s::GEAR_DOWN); + publishLandingGear(landing_gear_s::GEAR_DOWN); } } @@ -335,13 +329,12 @@ void ManualControl::sendActionRequest(int8_t action, int8_t source, int8_t mode) _action_request_pub.publish(action_request); } -void ManualControl::publish_landing_gear(int8_t action) +void ManualControl::publishLandingGear(int8_t action) { landing_gear_s landing_gear{}; landing_gear.landing_gear = action; landing_gear.timestamp = hrt_absolute_time(); - uORB::Publication landing_gear_pub{ORB_ID(landing_gear)}; - landing_gear_pub.publish(landing_gear); + _landing_gear_pub.publish(landing_gear); } int ManualControl::task_spawn(int argc, char *argv[]) diff --git a/src/modules/manual_control/ManualControl.hpp b/src/modules/manual_control/ManualControl.hpp index f493498f54..ef9407a998 100644 --- a/src/modules/manual_control/ManualControl.hpp +++ b/src/modules/manual_control/ManualControl.hpp @@ -33,14 +33,16 @@ #pragma once +#include +#include +#include #include #include #include #include #include -#include -#include #include +#include #include #include #include @@ -122,9 +124,10 @@ private: void evaluateModeSlot(uint8_t mode_slot); void sendActionRequest(int8_t action, int8_t source, int8_t mode = 0); - void publish_landing_gear(int8_t action); + void publishLandingGear(int8_t action); uORB::Publication _action_request_pub{ORB_ID(action_request)}; + uORB::Publication _landing_gear_pub{ORB_ID(landing_gear)}; uORB::Publication _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index 5884d6f2a2..8e45d98677 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -154,7 +154,7 @@ MulticopterRateControl::Run() if (_landing_gear_sub.copy(&landing_gear)) { if (landing_gear.landing_gear != landing_gear_s::GEAR_KEEP) { - if (landing_gear_s::GEAR_UP && (_landed || _maybe_landed)) { + 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") } else {