Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

FW Position Controller: do not publish roll angle constrain warning in VTOL transition #22842

Merged
merged 3 commits into from
Mar 6, 2024
Merged

FW Position Controller: do not publish roll angle constrain warning in VTOL transition #22842

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
2136abc
FW Position Control: some cosmetical changes
sfuhrer Mar 5, 2024
fc95844
FW Position Control: disable roll constraining warning in VTOL transi…
sfuhrer Mar 5, 2024
3174065
FW Pos C: define magic numbers for roll constraining warning as const…
sfuhrer Mar 6, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 8 additions & 6 deletions src/modules/fw_pos_control/FixedwingPositionControl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -512,25 +512,27 @@ float FixedwingPositionControl::getCorrectedNpfgRollSetpoint()
float new_roll_setpoint(_npfg.getRollSetpoint());
const float can_run_factor(constrain(_npfg.canRun(_local_pos, _wind_valid), 0.f, 1.f));

// If the npfg was not running before, reset the user warning variables.
hrt_abstime now{hrt_absolute_time()};

if ((now - _time_since_last_npfg_call) > 2_s) {
// Warn the user when the scale is less than 90% for at least 2 seconds (disable in transition)

// If the npfg was not running before, reset the user warning variables.
if ((now - _time_since_last_npfg_call) > ROLL_WARNING_TIMEOUT) {
_need_report_npfg_uncertain_condition = true;
_time_since_first_reduced_roll = 0U;
}

// Warn the user when the scale is less than 90% for at least 2 seconds.
if ((1.f - can_run_factor) < 0.1f) {
if (_vehicle_status.in_transition_mode || can_run_factor > ROLL_WARNING_CAN_RUN_THRESHOLD) {
// NPFG reports a good condition or we are in transition, reset the user warning variables.
_need_report_npfg_uncertain_condition = true;
_time_since_first_reduced_roll = 0U;

} else if (_need_report_npfg_uncertain_condition) {
if (_time_since_first_reduced_roll == 0U) {
_time_since_first_reduced_roll = hrt_absolute_time();
_time_since_first_reduced_roll = now;
}

if ((now - _time_since_first_reduced_roll) > 2_s) {
if ((now - _time_since_first_reduced_roll) > ROLL_WARNING_TIMEOUT) {
_need_report_npfg_uncertain_condition = false;
events::send(events::ID("npfg_roll_command_uncertain"), events::Log::Warning,
"Roll command reduced due to uncertain velocity/wind estimates!");
Expand Down
7 changes: 7 additions & 0 deletions src/modules/fw_pos_control/FixedwingPositionControl.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -169,6 +169,13 @@ static constexpr float POST_TOUCHDOWN_CLAMP_TIME = 0.5f;
// [m/s] maximum reference altitude rate threshhold
static constexpr float MAX_ALT_REF_RATE_FOR_LEVEL_FLIGHT = 0.1f;

// [s] Timeout that has to pass in roll-constraining failsafe before warning is triggered
static constexpr uint64_t ROLL_WARNING_TIMEOUT = 2_s;

// [-] Can-run threshold needed to trigger the roll-constraining failsafe warning
static constexpr float ROLL_WARNING_CAN_RUN_THRESHOLD = 0.9f;


class FixedwingPositionControl final : public ModuleBase<FixedwingPositionControl>, public ModuleParams,
public px4::WorkItem
{
Expand Down
Loading