dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

vtol: always check for EKF2 resets, not just when QC is checked 

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2023-11-21 15:29:47 +01:00
parent eae939d86c
commit a99a7d2abd
3 changed files with 17 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/vtol_att_control/vtol_att_control_main.cpp
View File

@ -373,6 +373,8 @@ VtolAttitudeControl::Run()
_air_density = air_data.rho; _air_density = air_data.rho;
} }
_vtol_type->handleEkfResets();
// check if mc and fw sp were updated // check if mc and fw sp were updated
const bool mc_att_sp_updated = _mc_virtual_att_sp_sub.update(&_mc_virtual_att_sp); const bool mc_att_sp_updated = _mc_virtual_att_sp_sub.update(&_mc_virtual_att_sp);
const bool fw_att_sp_updated = _fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp); const bool fw_att_sp_updated = _fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp);

15
src/modules/vtol_att_control/vtol_type.cpp
View File

@ -302,12 +302,6 @@ bool VtolType::isFrontTransitionAltitudeLoss()
{ {
bool result = false; bool result = false;
// check if there is a reset in the z-direction, and if so, shift the transition start z as well
if (_local_pos->z_reset_counter != _altitude_reset_counter) {
_local_position_z_start_of_transition += _local_pos->delta_z;
_altitude_reset_counter = _local_pos->z_reset_counter;
}
// only run if param set, altitude valid and controlled, and in transition to FW or within 5s of finishing it. // only run if param set, altitude valid and controlled, and in transition to FW or within 5s of finishing it.
if (_param_vt_qc_t_alt_loss.get() > FLT_EPSILON && _local_pos->z_valid && _v_control_mode->flag_control_altitude_enabled if (_param_vt_qc_t_alt_loss.get() > FLT_EPSILON && _local_pos->z_valid && _v_control_mode->flag_control_altitude_enabled
&& (_common_vtol_mode == mode::TRANSITION_TO_FW || hrt_elapsed_time(&_trans_finished_ts) < 5_s)) { && (_common_vtol_mode == mode::TRANSITION_TO_FW || hrt_elapsed_time(&_trans_finished_ts) < 5_s)) {
@ -318,6 +312,15 @@ bool VtolType::isFrontTransitionAltitudeLoss()
return result; return result;
} }
void VtolType::handleEkfResets()
{
// check if there is a reset in the z-direction, and if so, shift the transition start z as well
if (_local_pos->z_reset_counter != _altitude_reset_counter) {
_local_position_z_start_of_transition += _local_pos->delta_z;
_altitude_reset_counter = _local_pos->z_reset_counter;
}
}
bool VtolType::isPitchExceeded() bool VtolType::isPitchExceeded()
{ {
// fixed-wing maximum pitch angle // fixed-wing maximum pitch angle

6
src/modules/vtol_att_control/vtol_type.h
View File

@ -270,6 +270,12 @@ public:
*/ */
void resetTransitionStates(); void resetTransitionStates();
/**
* @brief Handle EKF position resets.
*
*/
void handleEkfResets();
protected: protected:
VtolAttitudeControl *_attc; VtolAttitudeControl *_attc;
mode _common_vtol_mode; mode _common_vtol_mode;