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;