From ac28c6b7e22482e0ce6d2c6aff46ca8a65a67869 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 6 Dec 2022 13:19:35 +0100 Subject: [PATCH] VTOL: take home_position into account for ground clearance (#20683) * VTOL: take home_position into account for ground clearance For approximating distance to ground in case there is no valid distance sensor data, subtract the home position altitude from the local position altitude. Signed-off-by: Silvan Fuhrer Co-authored-by: Matthias Grob --- .../vtol_att_control/vtol_att_control_main.cpp | 12 ++++++++++++ src/modules/vtol_att_control/vtol_att_control_main.h | 4 ++++ src/modules/vtol_att_control/vtol_type.cpp | 12 ++++++++++-- src/modules/vtol_att_control/vtol_type.h | 2 +- 4 files changed, 27 insertions(+), 3 deletions(-) diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index c129208fab..7b8f94cc4a 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -320,6 +320,18 @@ VtolAttitudeControl::Run() _airspeed_validated_sub.update(&_airspeed_validated); _tecs_status_sub.update(&_tecs_status); _land_detected_sub.update(&_land_detected); + + if (_home_position_sub.updated()) { + home_position_s home_position; + + if (_home_position_sub.copy(&home_position) && home_position.valid_alt) { + _home_position_z = home_position.z; + + } else { + _home_position_z = NAN; + } + } + vehicle_status_poll(); action_request_poll(); vehicle_cmd_poll(); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 2c96f3a385..2f79a185bf 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -67,6 +67,7 @@ #include #include #include +#include #include #include #include @@ -153,6 +154,7 @@ public: struct vehicle_thrust_setpoint_s *get_thrust_setpoint_0() {return &_thrust_setpoint_0;} struct vehicle_thrust_setpoint_s *get_thrust_setpoint_1() {return &_thrust_setpoint_1;} struct vtol_vehicle_status_s *get_vtol_vehicle_status() {return &_vtol_vehicle_status;} + float get_home_position_z() { return _home_position_z; } private: void Run() override; @@ -165,6 +167,7 @@ private: uORB::Subscription _action_request_sub{ORB_ID(action_request)}; uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; uORB::Subscription _fw_virtual_att_sp_sub{ORB_ID(fw_virtual_attitude_setpoint)}; + uORB::Subscription _home_position_sub{ORB_ID(home_position)}; uORB::Subscription _land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _local_pos_sp_sub{ORB_ID(vehicle_local_position_setpoint)}; uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; @@ -212,6 +215,7 @@ private: vehicle_local_position_setpoint_s _local_pos_sp{}; vehicle_status_s _vehicle_status{}; vtol_vehicle_status_s _vtol_vehicle_status{}; + float _home_position_z{NAN}; float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C}; // [kg/m^3] diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 2c8e7e4265..b5b28448bc 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -286,11 +286,19 @@ void VtolType::check_quadchute_condition() float VtolType::pusher_assist() { - // Altitude above ground is distance sensor altitude if available, otherwise local z-position - float dist_to_ground = -_local_pos->z; + // Altitude above ground is local z-position or altitude above home or distance sensor altitude depending on what's available + float dist_to_ground = 0.f; + const float home_position_z = _attc->get_home_position_z(); if (_local_pos->dist_bottom_valid) { dist_to_ground = _local_pos->dist_bottom; + + } else if (PX4_ISFINITE(home_position_z)) { + dist_to_ground = -(_local_pos->z - home_position_z); + + } else { + dist_to_ground = -_local_pos->z; + } // disable pusher assist depending on setting of forward_thrust_enable_mode: diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 8bc1a27b80..5ae921722c 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -188,7 +188,7 @@ protected: struct actuator_controls_s *_actuators_fw_in; //actuator controls from fw_att_control struct vehicle_local_position_s *_local_pos; struct vehicle_local_position_setpoint_s *_local_pos_sp; - struct airspeed_validated_s *_airspeed_validated; // airspeed + struct airspeed_validated_s *_airspeed_validated; // airspeed struct tecs_status_s *_tecs_status; struct vehicle_land_detected_s *_land_detected;