diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index a780808461..63f3873d58 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -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; } } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index cedad28d2b..0497695058 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -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() { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index a34fa28d53..d5bcdbe897 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -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, diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 1d91e6bcdb..d62abcf5b6 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -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,