mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-22 18:27:35 +08:00
added single point to decide if we need quadchute, let the reason be passed from the source where we know what's going on
This commit is contained in:
@@ -285,7 +285,7 @@ void Standard::update_transition_state()
|
||||
if (_params_standard.front_trans_timeout > FLT_EPSILON) {
|
||||
if ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params_standard.front_trans_timeout * 1000000.0f)) {
|
||||
// transition timeout occured, abort transition
|
||||
_attc->abort_front_transition();
|
||||
_attc->abort_front_transition("Transition timeout");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -510,10 +510,10 @@ VtolAttitudeControl::is_fixed_wing_requested()
|
||||
* Abort front transition
|
||||
*/
|
||||
void
|
||||
VtolAttitudeControl::abort_front_transition()
|
||||
VtolAttitudeControl::abort_front_transition(char *reason)
|
||||
{
|
||||
if (!_abort_front_transition) {
|
||||
mavlink_log_critical(&_mavlink_log_pub, "Transition timeout or FW min alt occured, aborting");
|
||||
if(!_abort_front_transition) {
|
||||
mavlink_log_critical(&_mavlink_log_pub, "Abort: %s", reason);
|
||||
_abort_front_transition = true;
|
||||
_vtol_vehicle_status.vtol_transition_failsafe = true;
|
||||
}
|
||||
|
||||
@@ -110,7 +110,7 @@ public:
|
||||
|
||||
int start(); /* start the task and return OK on success */
|
||||
bool is_fixed_wing_requested();
|
||||
void abort_front_transition();
|
||||
void abort_front_transition(char *reason);
|
||||
|
||||
struct vehicle_attitude_s *get_att() {return &_v_att;}
|
||||
struct vehicle_attitude_setpoint_s *get_att_sp() {return &_v_att_sp;}
|
||||
|
||||
@@ -180,27 +180,25 @@ void VtolType::update_fw_state()
|
||||
waiting_on_tecs();
|
||||
}
|
||||
|
||||
// quadchute
|
||||
if (_params->fw_min_alt > FLT_EPSILON && _armed->armed) {
|
||||
if (-(_local_pos->z) < _params->fw_min_alt) {
|
||||
_attc->abort_front_transition();
|
||||
}
|
||||
}
|
||||
|
||||
check_quadchute_condition();
|
||||
}
|
||||
|
||||
void VtolType::update_transition_state()
|
||||
{
|
||||
// quadchute
|
||||
if (_params->fw_min_alt > FLT_EPSILON && _armed->armed) {
|
||||
if (-(_local_pos->z) < _params->fw_min_alt) {
|
||||
_attc->abort_front_transition();
|
||||
}
|
||||
}
|
||||
|
||||
check_quadchute_condition();
|
||||
}
|
||||
|
||||
bool VtolType::can_transition_on_ground()
|
||||
{
|
||||
return !_armed->armed || _land_detected->landed;
|
||||
}
|
||||
|
||||
void VtolType::check_quadchute_condition()
|
||||
{
|
||||
// fixed-wing minimum altitude
|
||||
if(_params->fw_min_alt > FLT_EPSILON && _armed->armed){
|
||||
if(-(_local_pos->z) < _params->fw_min_alt){
|
||||
_attc->abort_front_transition("Minimum altitude");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -123,6 +123,11 @@ public:
|
||||
*/
|
||||
virtual void waiting_on_tecs() {};
|
||||
|
||||
/**
|
||||
* Checks for fixed-wing failsafe condition and issues abort request if needed.
|
||||
*/
|
||||
void check_quadchute_condition();
|
||||
|
||||
/**
|
||||
* Returns true if we're allowed to do a mode transition on the ground.
|
||||
*/
|
||||
|
||||
Reference in New Issue
Block a user