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 199dcac84c56..e34b85460aa0 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -336,6 +336,7 @@ VtolAttitudeControl::Run() _vehicle_control_mode_sub.update(&_vehicle_control_mode); _vehicle_attitude_sub.update(&_vehicle_attitude); + _global_pos_sub.update(&_global_pos); _local_pos_sub.update(&_local_pos); _local_pos_sp_sub.update(&_local_pos_sp); _pos_sp_triplet_sub.update(&_pos_sp_triplet); 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 6ef046f05f8a..2dd233088c54 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -78,6 +78,7 @@ #include #include #include +#include #include #include #include @@ -137,6 +138,7 @@ class VtolAttitudeControl : public ModuleBase, public Modul struct vehicle_control_mode_s *get_control_mode() {return &_vehicle_control_mode;} struct vehicle_land_detected_s *get_land_detected() {return &_land_detected;} struct vehicle_local_position_s *get_local_pos() {return &_local_pos;} + struct vehicle_global_position_s *get_global_pos() {return &_global_pos;} struct vehicle_local_position_setpoint_s *get_local_pos_sp() {return &_local_pos_sp;} struct vehicle_torque_setpoint_s *get_torque_setpoint_0() {return &_torque_setpoint_0;} struct vehicle_torque_setpoint_s *get_torque_setpoint_1() {return &_torque_setpoint_1;} @@ -160,6 +162,7 @@ class VtolAttitudeControl : public ModuleBase, public Modul 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 _global_pos_sub{ORB_ID(vehicle_global_position)}; uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _mc_virtual_att_sp_sub{ORB_ID(mc_virtual_attitude_setpoint)}; uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)}; @@ -201,6 +204,7 @@ class VtolAttitudeControl : public ModuleBase, public Modul vehicle_attitude_s _vehicle_attitude{}; vehicle_control_mode_s _vehicle_control_mode{}; vehicle_land_detected_s _land_detected{}; + vehicle_global_position_s _global_pos{}; vehicle_local_position_s _local_pos{}; vehicle_local_position_setpoint_s _local_pos_sp{}; vehicle_status_s _vehicle_status{}; diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 458b3927fb9c..19a4db2d3978 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -46,6 +46,7 @@ #include #include #include +#include using namespace matrix; @@ -176,10 +177,22 @@ float VtolType::update_and_get_backtransition_pitch_sp() const float track = atan2f(_local_pos->vy, _local_pos->vx); const float accel_body_forward = cosf(track) * _local_pos->ax + sinf(track) * _local_pos->ay; + const float vel_body_forward = cosf(track) * _local_pos->vx + sinf(track) * _local_pos->vy; - // increase the target deceleration setpoint provided to the controller by 20% - // to make overshooting the transition waypoint less likely in the presence of tracking errors - const float dec_sp = _param_vt_b_dec_mss.get() * 1.2f; + // Compute distance to waypoint in body x-direction -> dist_body_forward + MapProjection map_proj{_attc->get_global_pos()->lat, _attc->get_global_pos()->lon}; + + float dx, dy = 0.0; + + map_proj.project(_attc->get_pos_sp_triplet()->current.lat, _attc->get_pos_sp_triplet()->current.lon, dx, dy); + + const float dist_body_forward = cosf(track) * dx + sinf(track) * dy; + + // Add 20% factor to maximum allowed deceleration + const float max_dec_sp = _param_vt_b_dec_mss.get() * 1.2f; + + // Compute forward acceleration setpoint + float dec_sp = math::min(vel_body_forward*vel_body_forward / (2.f * dist_body_forward), max_dec_sp); // get accel error, positive means decelerating too slow, need to pitch up (must reverse dec_max, as it is a positive number) const float accel_error_forward = dec_sp + accel_body_forward;