From ee41e3181f83e08b5bd80f6a7ae67626aed6043a Mon Sep 17 00:00:00 2001
From: Simone Guscetti <simone@yuneecresearch.com>
Date: Thu, 1 Nov 2018 11:06:56 +0100
Subject: [PATCH] mc_pos_ctrl: Replace landing gear logic
- It takes the input from the constrains from the active flight task
---
.../mc_pos_control/mc_pos_control_main.cpp | 29 ++++++++++---------
1 file changed, 15 insertions(+), 14 deletions(-)
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index ecdab84e7a..964c1f2ce8 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -139,6 +139,8 @@ private:
vehicle_trajectory_waypoint_s _traj_wp_avoidance_desired{}; /**< desired waypoints, inputs to an obstacle avoidance module */
landing_gear_s _landing_gear_state{};
+ int8_t _old_landing_gear_position;
+
DEFINE_PARAMETERS(
(ParamFloat<px4::params::MPC_TKO_RAMP_T>) _takeoff_ramp_time, /**< time constant for smooth takeoff ramp */
(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _vel_max_up,
@@ -810,25 +812,24 @@ MulticopterPositionControl::run()
// they might conflict with each other such as in offboard attitude control.
publish_attitude();
- if (!constraints.landing_gear) {
- if (constraints.landing_gear == vehicle_constraints_s::GEAR_UP) {
- _landing_gear_state.landing_gear = landing_gear_s::LANDING_GEAR_UP;
- }
-
- if (constraints.landing_gear == vehicle_constraints_s::GEAR_DOWN) {
- _landing_gear_state.landing_gear = landing_gear_s::LANDING_GEAR_DOWN;
- }
- }
+ if (_old_landing_gear_position != constraints.landing_gear) {
+ _landing_gear_state.landing_gear =
+ (constraints.landing_gear == vehicle_constraints_s::GEAR_UP) ?
+ landing_gear_s::LANDING_GEAR_UP :
+ landing_gear_s::LANDING_GEAR_DOWN;
- _landing_gear_state.timestamp = hrt_absolute_time();
+ _landing_gear_state.timestamp = hrt_absolute_time();
- if (_landing_gear_pub != nullptr) {
- orb_publish(ORB_ID(landing_gear), _landing_gear_pub, &_landing_gear_state);
+ if (_landing_gear_pub != nullptr) {
+ orb_publish(ORB_ID(landing_gear), _landing_gear_pub, &_landing_gear_state);
- } else {
- _landing_gear_pub = orb_advertise(ORB_ID(landing_gear), &_landing_gear_state);
+ } else {
+ _landing_gear_pub = orb_advertise(ORB_ID(landing_gear), &_landing_gear_state);
+ }
}
+ _old_landing_gear_position = constraints.landing_gear;
+
} else {
// no flighttask is active: set attitude setpoint to idle
_att_sp.roll_body = _att_sp.pitch_body = 0.0f;
--
GitLab