forked from Archive/PX4-Autopilot
commander: rework posvel validity checks
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
Lorenz Meier
parent
40160c4488
commit
8ea0b2d3c5
|
|
@ -448,7 +448,7 @@ static bool gnssCheck(orb_advert_t *mavlink_log_pub, bool report_fail)
|
|||
return success;
|
||||
}
|
||||
|
||||
static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_fail)
|
||||
static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_fail, bool enforce_gps_required)
|
||||
{
|
||||
// Get estimator status data if available and exit with a fail recorded if not
|
||||
int sub = orb_subscribe(ORB_ID(estimator_status));
|
||||
|
|
@ -491,6 +491,31 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_
|
|||
goto out;
|
||||
}
|
||||
|
||||
// If GPS aiding is required, declare fault condition if the EKF is not using GPS
|
||||
if (enforce_gps_required) {
|
||||
if (!(status.control_mode_flags & 2)) {
|
||||
if (report_fail) {
|
||||
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF NOT USING GPS");
|
||||
}
|
||||
success = false;
|
||||
goto out;
|
||||
}
|
||||
}
|
||||
|
||||
// If GPS aiding is required, declare fault condition if the required GPS quality checks are failing
|
||||
if (enforce_gps_required) {
|
||||
if ((status.gps_check_fail_flags & (estimator_status_s::GPS_CHECK_FAIL_MIN_SAT_COUNT
|
||||
| estimator_status_s::GPS_CHECK_FAIL_MIN_GDOP
|
||||
| estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_ERR
|
||||
| estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_ERR)) > 0) {
|
||||
if (report_fail) {
|
||||
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS QUALITY CHECKS");
|
||||
}
|
||||
success = false;
|
||||
goto out;
|
||||
}
|
||||
}
|
||||
|
||||
// check magnetometer innovation test ratio
|
||||
param_get(param_find("COM_ARM_EKF_YAW"), &test_limit);
|
||||
if (status.mag_test_ratio > test_limit) {
|
||||
|
|
@ -694,7 +719,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkMag, bool checkAcc,
|
|||
int32_t estimator_type;
|
||||
param_get(param_find("SYS_MC_EST_GROUP"), &estimator_type);
|
||||
if (estimator_type == 2 && checkGNSS) {
|
||||
if (!ekf2Check(mavlink_log_pub, true, reportFailures)) {
|
||||
if (!ekf2Check(mavlink_log_pub, true, reportFailures, checkGNSS)) {
|
||||
failed = true;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -159,6 +159,18 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage
|
|||
#define HIL_ID_MIN 1000
|
||||
#define HIL_ID_MAX 1999
|
||||
|
||||
/* Controls the probation period which is the amount of time required for position and velocity checks to pass before the validity can be changed from false to true*/
|
||||
#define POSVEL_PROBATION_TAKEOFF 30E6 /**< probation duration set at takeoff (usec) */
|
||||
#define POSVEL_PROBATION_MIN 1E6 /**< minimum probation duration (usec) */
|
||||
#define POSVEL_PROBATION_MAX 100E6 /**< maximum probation duration (usec) */
|
||||
#define POSVEL_VALID_PROBATION_FACTOR 10 /**< the rate at which the probation duration is increased while checks are failing */
|
||||
|
||||
/* Probation times for position and velocity validity checks to pass if failed */
|
||||
static int64_t gpos_probation_time_us = POSVEL_PROBATION_TAKEOFF;
|
||||
static int64_t gvel_probation_time_us = POSVEL_PROBATION_TAKEOFF;
|
||||
static int64_t lpos_probation_time_us = POSVEL_PROBATION_TAKEOFF;
|
||||
static int64_t lvel_probation_time_us = POSVEL_PROBATION_TAKEOFF;
|
||||
|
||||
/* Mavlink log uORB handle */
|
||||
static orb_advert_t mavlink_log_pub = nullptr;
|
||||
|
||||
|
|
@ -179,8 +191,14 @@ static uint64_t last_print_mode_reject_time = 0;
|
|||
|
||||
static systemlib::Hysteresis auto_disarm_hysteresis(false);
|
||||
|
||||
static float eph_threshold = 5.0f;
|
||||
static float epv_threshold = 10.0f;
|
||||
static float eph_threshold = 5.0f; // Horizontal position error threshold (m)
|
||||
static float epv_threshold = 10.0f; // Vertivcal position error threshold (m)
|
||||
static float evh_threshold = 1.0f; // Horizontal velocity error threshold (m)
|
||||
|
||||
static uint64_t last_lpos_fail_time_us = 0; // Last time that the local position validity recovery check failed (usec)
|
||||
static uint64_t last_gpos_fail_time_us = 0; // Last time that the global position validity recovery check failed (usec)
|
||||
static uint64_t last_lvel_fail_time_us = 0; // Last time that the local velocity validity recovery check failed (usec)
|
||||
static uint64_t last_gvel_fail_time_us = 0; // Last time that the global velocity validity recovery check failed (usec)
|
||||
|
||||
/* pre-flight EKF checks */
|
||||
static float max_ekf_pos_ratio = 0.5f;
|
||||
|
|
@ -233,6 +251,7 @@ static bool arm_mission_required = false;
|
|||
|
||||
static bool _last_condition_global_position_valid = false;
|
||||
|
||||
static struct vehicle_land_detected_s land_detector = {};
|
||||
|
||||
/**
|
||||
* The daemon app only briefly exists to start
|
||||
|
|
@ -258,7 +277,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
|
|||
struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos,
|
||||
struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub,
|
||||
orb_advert_t *command_ack_pub, struct vehicle_command_ack_s *command_ack, struct vehicle_roi_s *roi,
|
||||
orb_advert_t *roi_pub);
|
||||
orb_advert_t *roi_pub,
|
||||
bool *changed);
|
||||
|
||||
/**
|
||||
* Mainloop of commander.
|
||||
|
|
@ -272,7 +292,13 @@ void get_circuit_breaker_params();
|
|||
|
||||
void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed);
|
||||
|
||||
transition_result_t set_main_state_rc(struct vehicle_status_s *status);
|
||||
transition_result_t set_main_state_rc(struct vehicle_status_s *status, vehicle_global_position_s *global_position, vehicle_local_position_s *local_position, bool *changed);
|
||||
|
||||
void reset_posvel_validity(vehicle_global_position_s *global_position, vehicle_local_position_s *local_position, bool *changed);
|
||||
|
||||
void check_global_posvel_validity(vehicle_global_position_s *global_position, bool *changed);
|
||||
|
||||
void check_local_posvel_validity(vehicle_local_position_s *local_position, bool *changed);
|
||||
|
||||
void set_control_mode();
|
||||
|
||||
|
|
@ -342,7 +368,7 @@ int commander_main(int argc, char *argv[])
|
|||
daemon_task = px4_task_spawn_cmd("commander",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_DEFAULT + 40,
|
||||
3600,
|
||||
3700,
|
||||
commander_thread_main,
|
||||
(char * const *)&argv[0]);
|
||||
|
||||
|
|
@ -705,7 +731,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
|
|||
struct home_position_s *home, struct vehicle_global_position_s *global_pos,
|
||||
struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub,
|
||||
orb_advert_t *command_ack_pub, struct vehicle_command_ack_s *command_ack,
|
||||
struct vehicle_roi_s *roi, orb_advert_t *roi_pub)
|
||||
struct vehicle_roi_s *roi, orb_advert_t *roi_pub, bool *changed)
|
||||
{
|
||||
/* only handle commands that are meant to be handled by this system and component */
|
||||
if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id)
|
||||
|
|
@ -770,11 +796,13 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
|
|||
|
||||
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
|
||||
/* POSCTL */
|
||||
reset_posvel_validity(global_pos, local_pos, changed);
|
||||
main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_POSCTL, main_state_prev, &status_flags, &internal_state);
|
||||
|
||||
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
|
||||
/* AUTO */
|
||||
if (custom_sub_mode > 0) {
|
||||
reset_posvel_validity(global_pos, local_pos, changed);
|
||||
switch(custom_sub_mode) {
|
||||
case PX4_CUSTOM_SUB_MODE_AUTO_LOITER:
|
||||
main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, main_state_prev, &status_flags, &internal_state);
|
||||
|
|
@ -818,6 +846,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
|
|||
main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, main_state_prev, &status_flags, &internal_state);
|
||||
|
||||
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
|
||||
reset_posvel_validity(global_pos, local_pos, changed);
|
||||
/* OFFBOARD */
|
||||
main_ret = main_state_transition(status_local, commander_state_s::MAIN_STATE_OFFBOARD, main_state_prev, &status_flags, &internal_state);
|
||||
}
|
||||
|
|
@ -1447,6 +1476,13 @@ int commander_thread_main(int argc, char *argv[])
|
|||
status_flags.circuit_breaker_engaged_gpsfailure_check = false;
|
||||
get_circuit_breaker_params();
|
||||
|
||||
/* Set position and velocity validty to false */
|
||||
status_flags.condition_global_position_valid = false;
|
||||
status_flags.condition_global_velocity_valid = false;
|
||||
status_flags.condition_local_position_valid = false;
|
||||
status_flags.condition_local_velocity_valid = false;
|
||||
status_flags.condition_local_altitude_valid = false;
|
||||
|
||||
// initialize gps failure to false if circuit breaker enabled
|
||||
if (status_flags.circuit_breaker_engaged_gpsfailure_check) {
|
||||
status_flags.gps_failure = false;
|
||||
|
|
@ -1587,7 +1623,6 @@ int commander_thread_main(int argc, char *argv[])
|
|||
|
||||
/* Subscribe to land detector */
|
||||
int land_detector_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
|
||||
struct vehicle_land_detected_s land_detector = {};
|
||||
land_detector.landed = true;
|
||||
|
||||
/*
|
||||
|
|
@ -2110,85 +2145,44 @@ int commander_thread_main(int argc, char *argv[])
|
|||
status_changed = true;
|
||||
}
|
||||
|
||||
// Check if quality checking of position accuracy and consistency is to be performed
|
||||
bool run_quality_checks = !status_flags.circuit_breaker_engaged_posfailure_check;
|
||||
|
||||
/* update global position estimate */
|
||||
orb_check(global_position_sub, &updated);
|
||||
bool gpos_updated = false;
|
||||
orb_check(global_position_sub, &gpos_updated);
|
||||
|
||||
if (updated) {
|
||||
if (gpos_updated) {
|
||||
/* position changed */
|
||||
vehicle_global_position_s gpos;
|
||||
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &gpos);
|
||||
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);
|
||||
|
||||
/* copy to global struct if valid, with hysteresis */
|
||||
|
||||
// XXX consolidate this with local position handling and timeouts after release
|
||||
// but we want a low-risk change now.
|
||||
if (status_flags.condition_global_position_valid) {
|
||||
if (gpos.eph < eph_threshold * 2.5f) {
|
||||
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);
|
||||
}
|
||||
} else {
|
||||
if (gpos.eph < eph_threshold) {
|
||||
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);
|
||||
}
|
||||
if (run_quality_checks) {
|
||||
check_global_posvel_validity(&global_position, &status_changed);
|
||||
}
|
||||
}
|
||||
|
||||
/* update local position estimate */
|
||||
orb_check(local_position_sub, &updated);
|
||||
bool lpos_updated = false;
|
||||
orb_check(local_position_sub, &lpos_updated);
|
||||
|
||||
if (updated) {
|
||||
if (lpos_updated) {
|
||||
/* position changed */
|
||||
orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
|
||||
|
||||
if (run_quality_checks) {
|
||||
check_local_posvel_validity(&local_position, &status_changed);
|
||||
}
|
||||
}
|
||||
|
||||
/* update attitude estimate */
|
||||
orb_check(attitude_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
/* position changed */
|
||||
/* attitude changed */
|
||||
orb_copy(ORB_ID(vehicle_attitude), attitude_sub, &attitude);
|
||||
}
|
||||
|
||||
//update condition_global_position_valid
|
||||
//Global positions are only published by the estimators if they are valid
|
||||
if (hrt_absolute_time() - global_position.timestamp > POSITION_TIMEOUT) {
|
||||
//We have had no good fix for POSITION_TIMEOUT amount of time
|
||||
if (status_flags.condition_global_position_valid) {
|
||||
set_tune_override(TONE_GPS_WARNING_TUNE);
|
||||
status_changed = true;
|
||||
status_flags.condition_global_position_valid = false;
|
||||
}
|
||||
} else if (global_position.timestamp != 0) {
|
||||
// Got good global position estimate
|
||||
if (!status_flags.condition_global_position_valid) {
|
||||
status_changed = true;
|
||||
status_flags.condition_global_position_valid = true;
|
||||
}
|
||||
}
|
||||
|
||||
/* update condition_local_position_valid and condition_local_altitude_valid */
|
||||
/* hysteresis for EPH */
|
||||
bool local_eph_good;
|
||||
|
||||
if (status_flags.condition_local_position_valid) {
|
||||
if (local_position.eph > eph_threshold * 2.5f) {
|
||||
local_eph_good = false;
|
||||
|
||||
} else {
|
||||
local_eph_good = true;
|
||||
}
|
||||
|
||||
} else {
|
||||
if (local_position.eph < eph_threshold) {
|
||||
local_eph_good = true;
|
||||
|
||||
} else {
|
||||
local_eph_good = false;
|
||||
}
|
||||
}
|
||||
|
||||
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid
|
||||
&& local_eph_good, &(status_flags.condition_local_position_valid), &status_changed);
|
||||
/* update condition_local_altitude_valid */
|
||||
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid,
|
||||
&(status_flags.condition_local_altitude_valid), &status_changed);
|
||||
|
||||
|
|
@ -2203,6 +2197,14 @@ int commander_thread_main(int argc, char *argv[])
|
|||
} else {
|
||||
mavlink_and_console_log_info(&mavlink_log_pub, "Takeoff detected");
|
||||
have_taken_off_since_arming = true;
|
||||
|
||||
// Set all position and velocity test probation durations to takeoff value
|
||||
// This is a larger value to give the vehicle time to complete a failsafe landing
|
||||
// if faulty sensors cause loss of navigatio shortly after takeoff.
|
||||
gpos_probation_time_us = POSVEL_PROBATION_TAKEOFF;
|
||||
gvel_probation_time_us = POSVEL_PROBATION_TAKEOFF;
|
||||
lpos_probation_time_us = POSVEL_PROBATION_TAKEOFF;
|
||||
lvel_probation_time_us = POSVEL_PROBATION_TAKEOFF;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -2414,12 +2416,8 @@ int commander_thread_main(int argc, char *argv[])
|
|||
}
|
||||
|
||||
/*
|
||||
* Check for valid position information.
|
||||
*
|
||||
* If the system has a valid position source from an onboard
|
||||
* position estimator, it is safe to operate it autonomously.
|
||||
* The flag_vector_flight_mode_ok flag indicates that a minimum
|
||||
* set of position measurements is available.
|
||||
* Check GPS fix quality. Note that this check augments the position validity
|
||||
* checks and adds an additional level of protection.
|
||||
*/
|
||||
|
||||
orb_check(gps_sub, &updated);
|
||||
|
|
@ -2454,6 +2452,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
}
|
||||
}
|
||||
|
||||
// Check fix type and data freshness
|
||||
if (gps_position.fix_type >= 3 && hrt_elapsed_time(&gps_position.timestamp) < FAILSAFE_DEFAULT_TIMEOUT) {
|
||||
/* handle the case where gps was regained */
|
||||
if (status_flags.gps_failure && !gpsIsNoisy) {
|
||||
|
|
@ -2469,6 +2468,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
status_changed = true;
|
||||
mavlink_log_critical(&mavlink_log_pub, "GPS fix lost");
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/* start mission result check */
|
||||
|
|
@ -2797,7 +2797,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
|
||||
/* evaluate the main state machine according to mode switches */
|
||||
bool first_rc_eval = (_last_sp_man.timestamp == 0) && (sp_man.timestamp > 0);
|
||||
transition_result_t main_res = set_main_state_rc(&status);
|
||||
transition_result_t main_res = set_main_state_rc(&status, &global_position, &local_position, &status_changed);
|
||||
|
||||
/* store last position lock state */
|
||||
_last_condition_global_position_valid = status_flags.condition_global_position_valid;
|
||||
|
|
@ -2968,7 +2968,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
|
||||
/* handle it */
|
||||
if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &local_position,
|
||||
&attitude, &home_pub, &command_ack_pub, &command_ack, &_roi, &roi_pub)) {
|
||||
&attitude, &home_pub, &command_ack_pub, &command_ack, &_roi, &roi_pub, &status_changed)) {
|
||||
status_changed = true;
|
||||
}
|
||||
}
|
||||
|
|
@ -3237,6 +3237,7 @@ get_circuit_breaker_params()
|
|||
status_flags.circuit_breaker_engaged_enginefailure_check = circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY);
|
||||
status_flags.circuit_breaker_engaged_gpsfailure_check = circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY);
|
||||
status_flags.circuit_breaker_flight_termination_disabled = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY);
|
||||
status_flags.circuit_breaker_engaged_posfailure_check = circuit_breaker_enabled("CBRK_VELPOSERR", CBRK_VELPOSERR_KEY);
|
||||
}
|
||||
|
||||
void
|
||||
|
|
@ -3377,7 +3378,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
|
|||
}
|
||||
|
||||
transition_result_t
|
||||
set_main_state_rc(struct vehicle_status_s *status_local)
|
||||
set_main_state_rc(struct vehicle_status_s *status_local, vehicle_global_position_s *global_position, vehicle_local_position_s *local_position, bool *changed)
|
||||
{
|
||||
/* set main state according to RC switches */
|
||||
transition_result_t res = TRANSITION_DENIED;
|
||||
|
|
@ -3426,6 +3427,10 @@ set_main_state_rc(struct vehicle_status_s *status_local)
|
|||
|
||||
_last_sp_man = sp_man;
|
||||
|
||||
// reset the position and velocity validity calculation to give the best change of being able to select
|
||||
// the desired mode
|
||||
reset_posvel_validity(global_position, local_position, changed);
|
||||
|
||||
/* offboard switch overrides main switch */
|
||||
if (sp_man.offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
|
||||
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_OFFBOARD, main_state_prev, &status_flags, &internal_state);
|
||||
|
|
@ -3751,6 +3756,216 @@ set_main_state_rc(struct vehicle_status_s *status_local)
|
|||
return res;
|
||||
}
|
||||
|
||||
void
|
||||
reset_posvel_validity(vehicle_global_position_s *global_position, vehicle_local_position_s *local_position, bool *changed)
|
||||
{
|
||||
// reset all the check probation times back to the minimum value
|
||||
gpos_probation_time_us = POSVEL_PROBATION_MIN;
|
||||
gvel_probation_time_us = POSVEL_PROBATION_MIN;
|
||||
lpos_probation_time_us = POSVEL_PROBATION_MIN;
|
||||
lvel_probation_time_us = POSVEL_PROBATION_MIN;
|
||||
|
||||
// recheck validity
|
||||
check_global_posvel_validity(global_position, changed);
|
||||
check_local_posvel_validity(local_position, changed);
|
||||
}
|
||||
|
||||
void
|
||||
check_global_posvel_validity(vehicle_global_position_s *global_position, bool *changed)
|
||||
{
|
||||
bool global_pos_inaccurate = false;
|
||||
bool global_vel_inaccurate = false;
|
||||
bool hold_fail_state = false;
|
||||
|
||||
// Check position accuracy with hysteresis in both test level and time
|
||||
bool pos_status_changed = false;
|
||||
if (status_flags.condition_global_position_valid && global_position->eph > eph_threshold * 2.5f) {
|
||||
global_pos_inaccurate = true;
|
||||
pos_status_changed = true;
|
||||
last_gpos_fail_time_us = hrt_absolute_time();
|
||||
} else if (!status_flags.condition_global_position_valid) {
|
||||
bool level_check_pass = global_position->eph < eph_threshold;
|
||||
if (!level_check_pass || hold_fail_state) {
|
||||
gpos_probation_time_us += (hrt_absolute_time() - last_gpos_fail_time_us) * POSVEL_VALID_PROBATION_FACTOR;
|
||||
last_gpos_fail_time_us = hrt_absolute_time();
|
||||
} else if (hrt_absolute_time() - last_gpos_fail_time_us > gpos_probation_time_us) {
|
||||
global_pos_inaccurate = false;
|
||||
pos_status_changed = true;
|
||||
last_gpos_fail_time_us = 0;
|
||||
}
|
||||
} else {
|
||||
gpos_probation_time_us -= (hrt_absolute_time() - last_gpos_fail_time_us);
|
||||
last_gpos_fail_time_us = hrt_absolute_time();
|
||||
}
|
||||
|
||||
// check global velocity accuracy with hysteresis in both test level and time
|
||||
bool vel_status_changed = false;
|
||||
if (status_flags.condition_global_velocity_valid && global_position->evh > evh_threshold * 2.5f) {
|
||||
global_vel_inaccurate = true;
|
||||
vel_status_changed = true;
|
||||
last_gvel_fail_time_us = hrt_absolute_time();
|
||||
} else if (!status_flags.condition_global_velocity_valid) {
|
||||
bool check_level_pass = global_position->evh < evh_threshold;
|
||||
if (!check_level_pass || hold_fail_state) {
|
||||
gvel_probation_time_us += (hrt_absolute_time() - last_gvel_fail_time_us) * POSVEL_VALID_PROBATION_FACTOR;
|
||||
last_gvel_fail_time_us = hrt_absolute_time();
|
||||
} else if (hrt_absolute_time() - last_gvel_fail_time_us > gvel_probation_time_us) {
|
||||
global_vel_inaccurate = false;
|
||||
vel_status_changed = true;
|
||||
last_gvel_fail_time_us = 0;
|
||||
}
|
||||
} else {
|
||||
gvel_probation_time_us -= (hrt_absolute_time() - last_gvel_fail_time_us);
|
||||
last_gvel_fail_time_us = hrt_absolute_time();
|
||||
}
|
||||
|
||||
bool global_data_stale = (hrt_absolute_time() - global_position->timestamp > POSITION_TIMEOUT);
|
||||
|
||||
// Set global velocity validity
|
||||
if (vel_status_changed) {
|
||||
if (status_flags.condition_global_velocity_valid
|
||||
&& (global_data_stale || global_vel_inaccurate)) {
|
||||
status_flags.condition_global_position_valid = false;
|
||||
*changed = true;
|
||||
} else if (!status_flags.condition_global_velocity_valid
|
||||
&& !global_data_stale
|
||||
&& !global_vel_inaccurate) {
|
||||
status_flags.condition_global_velocity_valid = true;
|
||||
*changed = true;
|
||||
}
|
||||
}
|
||||
|
||||
// Set global position validity
|
||||
if (pos_status_changed) {
|
||||
if (status_flags.condition_global_position_valid
|
||||
&& (global_data_stale || global_pos_inaccurate)) {
|
||||
status_flags.condition_global_position_valid = false;
|
||||
set_tune_override(TONE_GPS_WARNING_TUNE);
|
||||
*changed = true;
|
||||
} else if (!status_flags.condition_global_position_valid
|
||||
&& !global_data_stale
|
||||
&& !global_pos_inaccurate) {
|
||||
status_flags.condition_global_position_valid = true;
|
||||
*changed = true;
|
||||
}
|
||||
}
|
||||
|
||||
// constrain probation times
|
||||
if (land_detector.landed) {
|
||||
gpos_probation_time_us = POSVEL_PROBATION_MIN;
|
||||
gvel_probation_time_us = POSVEL_PROBATION_MIN;
|
||||
} else {
|
||||
if (gpos_probation_time_us < POSVEL_PROBATION_MIN) {
|
||||
gpos_probation_time_us = POSVEL_PROBATION_MIN;
|
||||
} else if (gpos_probation_time_us > POSVEL_PROBATION_MAX) {
|
||||
gpos_probation_time_us = POSVEL_PROBATION_MAX;
|
||||
}
|
||||
if (gvel_probation_time_us < POSVEL_PROBATION_MIN) {
|
||||
gvel_probation_time_us = POSVEL_PROBATION_MIN;
|
||||
} else if (gvel_probation_time_us > POSVEL_PROBATION_MAX) {
|
||||
gvel_probation_time_us = POSVEL_PROBATION_MAX;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
check_local_posvel_validity(struct vehicle_local_position_s *local_position, bool *changed)
|
||||
{
|
||||
bool local_pos_inaccurate = false;
|
||||
bool local_vel_inaccurate = false;
|
||||
bool hold_fail_state = false;
|
||||
|
||||
// Check local position accuracy with hysteresis in both test level and time
|
||||
bool pos_status_changed = false;
|
||||
if (status_flags.condition_local_position_valid && local_position->eph > eph_threshold * 2.5f) {
|
||||
local_pos_inaccurate = true;
|
||||
pos_status_changed = true;
|
||||
last_lpos_fail_time_us = hrt_absolute_time();
|
||||
} else if (!status_flags.condition_local_position_valid) {
|
||||
bool level_check_pass = local_position->xy_valid && local_position->eph < eph_threshold;
|
||||
if (!level_check_pass || hold_fail_state) {
|
||||
lpos_probation_time_us += (hrt_absolute_time() - last_lpos_fail_time_us) * POSVEL_VALID_PROBATION_FACTOR;
|
||||
last_lpos_fail_time_us = hrt_absolute_time();
|
||||
} else if (hrt_absolute_time() - last_lpos_fail_time_us > lpos_probation_time_us) {
|
||||
local_pos_inaccurate = false;
|
||||
pos_status_changed = true;
|
||||
last_lpos_fail_time_us = 0;
|
||||
}
|
||||
} else {
|
||||
lpos_probation_time_us -= (hrt_absolute_time() - last_lpos_fail_time_us);
|
||||
last_lpos_fail_time_us = hrt_absolute_time();
|
||||
}
|
||||
|
||||
// Check local velocity accuracy with hysteresis
|
||||
bool vel_status_changed = false;
|
||||
if (status_flags.condition_local_velocity_valid && local_position->evh > evh_threshold * 2.5f) {
|
||||
local_vel_inaccurate = true;
|
||||
vel_status_changed = true;
|
||||
last_lvel_fail_time_us = hrt_absolute_time();
|
||||
} else if (!status_flags.condition_local_velocity_valid) {
|
||||
bool level_check_pass = local_position->v_xy_valid && local_position->evh < evh_threshold;
|
||||
if (!level_check_pass || hold_fail_state) {
|
||||
lvel_probation_time_us += (hrt_absolute_time() - last_lvel_fail_time_us) * POSVEL_VALID_PROBATION_FACTOR;
|
||||
last_lvel_fail_time_us = hrt_absolute_time();
|
||||
} else if (hrt_absolute_time() - last_lvel_fail_time_us > lvel_probation_time_us) {
|
||||
local_vel_inaccurate = false;
|
||||
vel_status_changed = true;
|
||||
last_lvel_fail_time_us = 0;
|
||||
}
|
||||
} else {
|
||||
lvel_probation_time_us -= (hrt_absolute_time() - last_lvel_fail_time_us);
|
||||
last_lvel_fail_time_us = hrt_absolute_time();
|
||||
}
|
||||
|
||||
bool local_data_stale = (hrt_absolute_time() - local_position->timestamp > POSITION_TIMEOUT);
|
||||
|
||||
// Set local velocity validity
|
||||
if (vel_status_changed) {
|
||||
if (status_flags.condition_local_velocity_valid
|
||||
&& (local_data_stale || local_vel_inaccurate)) {
|
||||
status_flags.condition_local_velocity_valid = false;
|
||||
*changed = true;
|
||||
} else if (!status_flags.condition_local_velocity_valid
|
||||
&& !local_data_stale
|
||||
&& !local_vel_inaccurate) {
|
||||
status_flags.condition_local_velocity_valid = true;
|
||||
*changed = true;
|
||||
}
|
||||
}
|
||||
|
||||
// Set local position validity
|
||||
if (pos_status_changed) {
|
||||
if (status_flags.condition_local_position_valid
|
||||
&& (local_data_stale || !local_position->xy_valid || local_pos_inaccurate)) {
|
||||
status_flags.condition_local_position_valid = false;
|
||||
*changed = true;
|
||||
} else if (!status_flags.condition_local_position_valid
|
||||
&& !local_data_stale
|
||||
&& !local_pos_inaccurate
|
||||
&& local_position->xy_valid) {
|
||||
status_flags.condition_local_position_valid = true;
|
||||
*changed = true;
|
||||
}
|
||||
}
|
||||
|
||||
// constrain probation times
|
||||
if (land_detector.landed) {
|
||||
lpos_probation_time_us = POSVEL_PROBATION_MIN;
|
||||
lvel_probation_time_us = POSVEL_PROBATION_MIN;
|
||||
} else {
|
||||
if (lpos_probation_time_us < POSVEL_PROBATION_MIN) {
|
||||
lpos_probation_time_us = POSVEL_PROBATION_MIN;
|
||||
} else if (lpos_probation_time_us > POSVEL_PROBATION_MAX) {
|
||||
lpos_probation_time_us = POSVEL_PROBATION_MAX;
|
||||
}
|
||||
if (lvel_probation_time_us < POSVEL_PROBATION_MIN) {
|
||||
lvel_probation_time_us = POSVEL_PROBATION_MIN;
|
||||
} else if (lvel_probation_time_us > POSVEL_PROBATION_MAX) {
|
||||
lvel_probation_time_us = POSVEL_PROBATION_MAX;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
set_control_mode()
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -71,13 +71,15 @@ enum class link_loss_actions_t {
|
|||
struct status_flags_s {
|
||||
bool condition_calibration_enabled;
|
||||
bool condition_system_sensors_initialized;
|
||||
bool condition_system_prearm_error_reported; // true if errors have already been reported
|
||||
bool condition_system_hotplug_timeout; // true if the hotplug sensor search is over
|
||||
bool condition_system_prearm_error_reported; // true if errors have already been reported
|
||||
bool condition_system_hotplug_timeout; // true if the hotplug sensor search is over
|
||||
bool condition_system_returned_to_home;
|
||||
bool condition_auto_mission_available;
|
||||
bool condition_global_position_valid; // set to true by the commander app if the quality of the position estimate is good enough to use it for navigation
|
||||
bool condition_home_position_valid; // indicates a valid home position (a valid home position is not always a valid launch)
|
||||
bool condition_local_position_valid;
|
||||
bool condition_global_position_valid; // set to true by the commander app if the quality of the global position estimate is good enough to use for navigation
|
||||
bool condition_global_velocity_valid; // set to true by the commander app if the quality of the global horizontal velocity data is good enough to use for navigation
|
||||
bool condition_home_position_valid; // indicates a valid home position (a valid home position is not always a valid launch)
|
||||
bool condition_local_position_valid; // set to true by the commander app if the quality of the local position estimate is good enough to use for navigation
|
||||
bool condition_local_velocity_valid; // set to true by the commander app if the quality of the local horizontal velocity data is good enough to use for navigation
|
||||
bool condition_local_altitude_valid;
|
||||
bool condition_airspeed_valid; // set to true by the commander app if there is a valid airspeed measurement available
|
||||
bool condition_power_input_valid; // set if input power is valid
|
||||
|
|
@ -88,6 +90,7 @@ struct status_flags_s {
|
|||
bool circuit_breaker_engaged_gpsfailure_check;
|
||||
bool circuit_breaker_flight_termination_disabled;
|
||||
bool circuit_breaker_engaged_usb_check;
|
||||
bool circuit_breaker_engaged_posfailure_check; // set to true when the position valid checks have been disabled
|
||||
bool offboard_control_signal_found_once;
|
||||
bool offboard_control_signal_lost;
|
||||
bool offboard_control_set_by_command; // true if the offboard mode was set by a mavlink command and should not be overridden by RC
|
||||
|
|
@ -145,6 +148,16 @@ void set_rc_loss_nav_state(struct vehicle_status_s *status,
|
|||
struct actuator_armed_s *armed,
|
||||
status_flags_s *status_flags,
|
||||
const link_loss_actions_t link_loss_act);
|
||||
/*
|
||||
* Checks the validty of position data aaainst the requirements of the current navigation
|
||||
* mode and switches mode if position data required is not available.
|
||||
*/
|
||||
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 a mode using RC control can be used as a fallback
|
||||
const bool using_global_pos); // true when the current mode requires a global position estimate
|
||||
|
||||
void set_data_link_loss_nav_state(struct vehicle_status_s *status,
|
||||
struct actuator_armed_s *armed,
|
||||
|
|
|
|||
Loading…
Reference in New Issue