yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 09:28:58 +00:00
Code Issues Packages Projects Releases Wiki Activity

commander: rework posvel validity checks

Browse Source 
Move into functions.
Reset probation time and recalculate checks if a mode change is demanded to give the operator ability to regain control as soon as possible after nav performance is regained. (+11 squashed commits)
Squashed commits:
[a4bb800] commander: enable pilot to quickly recover from loss of position accuracy
[19e16a0] commander: rework postal probation time
[f96284e] commander: rework bad pos and vel test probation time
[00d5f0c] commander: Allow EKF preflight checks to pass with moving vehicle

Separates the 'is using GPS' and the GPS quality checks.
Uses a reasonable subset of the GPS quality checks which allows checks to pass if the vehicle is moving.
[4cdfb5c] commander: remove unused variable
[349385a] commander: add EKF GPS quality checks to pre-arm checking

Only perform check if GPs checking is activated by parameter setting.
Display fault messages that makes it clear if EKF quality checks are failing or the EKF is not using GPS for another reason. We do not want to confuse this with GPS lock.
[340ae29] commander: make position invalid fail-safe more sticky

Require check to pass for 7 seconds before exiting failsafe. This is required because if GPs is failing innovation tests for a prolonged period, the EKF will periodically reset to the GPS and report good accuracy at the time of reset.
Adding this delay gives time for an underlying error condition (eg bad IMU or compass) to be re-detected.
[b04ac95] commander: Increase RAM allocation to eliminate low stack warnings
[9dca12f] commander: add missing position invalid fail-safe responses
[69f264d] commander: Update position invalid fail-safe responses

Replace separate logic for each case with a generic function
Add velocity checks.
[8e8cef1] commander: rework position validity checks

Consolidate existing checks for global and local position validity and add checking of velocity accuracy.
Enable checks to be bypassed using the CBRK_VELPOSERR parameter.
This commit is contained in:
priseborough
2017-03-08 11:03:27 +11:00
committed by Lorenz Meier
parent 40160c4488
commit 8ea0b2d3c5
4 changed files with 400 additions and 142 deletions
Download Patch File Download Diff File Expand all files Collapse all files

125
src/modules/commander/state_machine_helper.cpp
View File

@@ -69,8 +69,8 @@ static const char reason_no_offboard[] = "no offboard";
static const char reason_no_rc_and_no_offboard[] = "no RC and no offboard";
static const char reason_no_gps[] = "no gps";
static const char reason_no_gps_cmd[] = "no gps cmd";
static const char reason_no_home[] = "no home";
static const char reason_no_local_position[] = "no local position";
static const char reason_no_global_position[] = "no global position";
static const char reason_no_datalink[] = "no datalink";
// This array defines the arming state transitions. The rows are the new state, and the columns
@@ -622,18 +622,10 @@ bool set_nav_state(struct vehicle_status_s *status,
* this enables POSCTL using e.g. flow.
* For fixedwing, a global position is needed. */
} else if (((status->is_rotary_wing && !status_flags->condition_local_position_valid) ||
(!status->is_rotary_wing && !status_flags->condition_global_position_valid))
&& is_armed) {
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc);
if (status_flags->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_STAB;
}
} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, true, !status->is_rotary_wing)) {
// nothing to do - everything done in check_invalid_pos_nav_state
} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, true, status->is_rotary_wing)) {
// nothing to do - everything done in check_invalid_pos_nav_state
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
}
@@ -671,6 +663,8 @@ bool set_nav_state(struct vehicle_status_s *status,
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_gps);
} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
// nothing to do - everything done in check_invalid_pos_nav_state
} else if (status->engine_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
@@ -720,6 +714,8 @@ bool set_nav_state(struct vehicle_status_s *status,
/* also go into failsafe if just datalink is lost, and we're actually in air */
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink);
} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
// nothing to do - everything done in check_invalid_pos_nav_state
} else if (status->data_link_lost && data_link_loss_act_configured && !landed) {
set_data_link_loss_nav_state(status, armed, status_flags, data_link_loss_act);
@@ -756,20 +752,8 @@ bool set_nav_state(struct vehicle_status_s *status,
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_gps);
} else if ((!status_flags->condition_global_position_valid ||
!status_flags->condition_home_position_valid)) {
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_home);
if (status_flags->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
} else if (status_flags->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
// nothing to do - everything done in check_invalid_pos_nav_state
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
}
@@ -783,18 +767,8 @@ bool set_nav_state(struct vehicle_status_s *status,
if (status->engine_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (!status_flags->condition_global_position_valid) {
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_gps);
if (status_flags->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
} else if (status_flags->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
// nothing to do - everything done in check_invalid_pos_nav_state
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET;
@@ -809,18 +783,8 @@ bool set_nav_state(struct vehicle_status_s *status,
if (status->engine_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (!status_flags->condition_local_position_valid) {
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_local_position);
if (status_flags->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
} else if (status_flags->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {
// nothing to do - everything done in check_invalid_pos_nav_state
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF;
@@ -835,15 +799,8 @@ bool set_nav_state(struct vehicle_status_s *status,
if (status->engine_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (!status_flags->condition_local_position_valid) {
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_local_position);
if (status_flags->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {
// nothing to do - everything done in check_invalid_pos_nav_state
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
@@ -945,6 +902,54 @@ void set_rc_loss_nav_state(struct vehicle_status_s *status,
set_link_loss_nav_state(status, armed, status_flags, link_loss_act, vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER);
}
bool check_invalid_pos_nav_state(struct vehicle_status_s *status,
bool old_failsafe,
orb_advert_t *mavlink_log_pub,
status_flags_s *status_flags,
const bool use_rc, // true if we can fallback to a mode that uses RC inputs
const bool using_global_pos) // true if the current flight mode requires a global position
{
bool fallback_required = false;
if (using_global_pos && (!status_flags->condition_global_position_valid || !status_flags->condition_global_velocity_valid)) {
fallback_required = true;
} else if (!using_global_pos && (!status_flags->condition_local_position_valid || !status_flags->condition_local_velocity_valid)) {
fallback_required = true;
}
if (fallback_required) {
if (use_rc) {
// fallback to a mode that gives the operator stick control
if (status->is_rotary_wing && status_flags->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
} else if (status_flags->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_STAB;
}
} else {
// go into a descent that does not require stick control
if (status_flags->condition_local_position_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
} else if (status_flags->condition_local_altitude_valid) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
}
}
if (using_global_pos) {
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_global_position);
} else {
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_local_position);
}
}
return fallback_required;
}
void set_data_link_loss_nav_state(struct vehicle_status_s *status,
struct actuator_armed_s *armed,
status_flags_s *status_flags,