fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-16 17:07:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander move remaining position failsafe params to class and cleanup

Browse Source
This commit is contained in:
Daniel Agar
2018-03-15 01:15:35 -04:00
parent 439ed7d3f5
commit fc26fd99bd
2 changed files with 109 additions and 170 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.hpp
+38 -16
View File
@@ -39,6 +39,7 @@
#include <controllib/blocks.hpp>
#include <px4_module.h>
#include <px4_module_params.h>
#include <mathlib/mathlib.h>
// publications
#include <uORB/Publication.hpp>
@@ -58,9 +59,12 @@
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
using math::constrain;
using uORB::Publication;
using uORB::Subscription;
using namespace time_literals;
class Commander : public ModuleBase<Commander>, public ModuleParams
{
public:
@@ -95,35 +99,51 @@ private:
(ParamFloat<px4::params::COM_POS_FS_EPH>) _eph_threshold,
(ParamFloat<px4::params::COM_POS_FS_EPV>) _epv_threshold,
(ParamFloat<px4::params::COM_VEL_FS_EVH>) _evh_threshold,
(ParamInt<px4::params::COM_POS_FS_DELAY>) _failsafe_pos_delay,
(ParamInt<px4::params::COM_POS_FS_PROB>) _failsafe_pos_probation,
(ParamInt<px4::params::COM_POS_FS_GAIN>) _failsafe_pos_gain
)
bool handle_command(vehicle_status_s *status_local, const vehicle_command_s &cmd,
actuator_armed_s *armed_local, home_position_s *home, const vehicle_global_position_s &global_pos,
const vehicle_local_position_s &local_pos, orb_advert_t *home_pub, orb_advert_t *command_ack_pub, bool *changed);
static constexpr int64_t sec_to_usec = (1000 * 1000);
const int64_t POSVEL_PROBATION_MIN = 1 * sec_to_usec; /**< minimum probation duration (usec) */
const int64_t POSVEL_PROBATION_MAX = 100 * sec_to_usec; /**< maximum probation duration (usec) */
bool set_home_position(orb_advert_t &homePub, home_position_s &home, const vehicle_local_position_s &localPosition,
const vehicle_global_position_s &globalPosition, bool set_alt_only_to_lpos_ref);
hrt_abstime _last_gpos_fail_time_us{0}; /**< Last time that the global position validity recovery check failed (usec) */
hrt_abstime _last_lpos_fail_time_us{0}; /**< Last time that the local position validity recovery check failed (usec) */
hrt_abstime _last_lvel_fail_time_us{0}; /**< Last time that the local velocity validity recovery check failed (usec) */
// Probation times for position and velocity validity checks to pass if failed
hrt_abstime _gpos_probation_time_us = POSVEL_PROBATION_MIN;
hrt_abstime _lpos_probation_time_us = POSVEL_PROBATION_MIN;
hrt_abstime _lvel_probation_time_us = POSVEL_PROBATION_MIN;
bool handle_command(vehicle_status_s *status, const vehicle_command_s &cmd,
actuator_armed_s *armed, home_position_s *home, orb_advert_t *home_pub, orb_advert_t *command_ack_pub, bool *changed);
bool set_home_position(orb_advert_t &homePub, home_position_s &home, bool set_alt_only_to_lpos_ref);
// Set the main system state based on RC and override device inputs
transition_result_t set_main_state(const vehicle_status_s &status, const vehicle_global_position_s &global_position,
const vehicle_local_position_s &local_position, bool *changed);
transition_result_t set_main_state(const vehicle_status_s &status, bool *changed);
// Enable override (manual reversion mode) on the system
transition_result_t set_main_state_override_on(const vehicle_status_s &status_local, bool *changed);
transition_result_t set_main_state_override_on(const vehicle_status_s &status, bool *changed);
// Set the system main state based on the current RC inputs
transition_result_t set_main_state_rc(const vehicle_status_s &status_local,
const vehicle_global_position_s &global_position, const vehicle_local_position_s &local_position, bool *changed);
transition_result_t set_main_state_rc(const vehicle_status_s &status, bool *changed);
void check_valid(const hrt_abstime &timestamp, const hrt_abstime &timeout, const bool valid_in, bool *valid_out,
bool *changed);
// Set the main system state based on RC and override device inputs
transition_result_t set_main_state(vehicle_status_s *status, bool *changed);
transition_result_t set_main_state_override_on(vehicle_status_s *status, bool *changed);
transition_result_t set_main_state_rc(vehicle_status_s *status, bool *changed);
void check_valid(const hrt_abstime &timestamp, const hrt_abstime &timeout, const bool valid_in, bool *valid_out, bool *changed);
bool check_posvel_validity(const bool data_valid, const float data_accuracy, const float required_accuracy,
const hrt_abstime &data_timestamp_us, hrt_abstime *last_fail_time_us, hrt_abstime *probation_time_us, bool *valid_state,
bool *validity_changed);
void reset_posvel_validity(const vehicle_global_position_s &global_position,
const vehicle_local_position_s &local_position, bool *changed);
void reset_posvel_validity(bool *changed);
void mission_init();
@@ -152,8 +172,10 @@ private:
// publisher
orb_advert_t _vehicle_cmd_pub = nullptr;
// subscriptions
Subscription<mission_result_s> _mission_result_sub;
// Subscriptions
Subscription<mission_result_s> _mission_result_sub;
Subscription<vehicle_global_position_s> _global_position_sub;
Subscription<vehicle_local_position_s> _local_position_sub;
};
#endif /* COMMANDER_HPP_ */
src/modules/commander/commander.cpp
+71 -154
View File
@@ -130,8 +130,6 @@ typedef enum VEHICLE_MODE_FLAG
#define STICK_ON_OFF_LIMIT 0.9f
#define POSITION_TIMEOUT 1 /**< default number of seconds of position health check failure required to declare the position invalid */
#define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */
#define OFFBOARD_TIMEOUT 500000
#define DIFFPRESS_TIMEOUT 2000000
@@ -142,24 +140,6 @@ static constexpr uint64_t HOTPLUG_SENS_TIMEOUT = (8 * 1000 * 1000); /**< wait fo
#define INAIR_RESTART_HOLDOFF_INTERVAL 500000
/* 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 30 /**< probation duration set at takeoff (sec) */
static constexpr int64_t sec_to_usec = (1000 * 1000);
static constexpr int64_t POSVEL_PROBATION_MIN = 1 * sec_to_usec; /**< minimum probation duration (usec) */
static constexpr int64_t POSVEL_PROBATION_MAX = 100 * sec_to_usec; /**< maximum probation duration (usec) */
/* Parameters controlling the sensitivity of the position failsafe */
static uint64_t posctl_nav_loss_delay = POSITION_TIMEOUT * sec_to_usec;
static uint64_t posctl_nav_loss_prob = POSVEL_PROBATION_TAKEOFF * sec_to_usec;
static int32_t posctl_nav_loss_gain = 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 uint64_t gpos_probation_time_us = POSVEL_PROBATION_MIN;
static uint64_t lpos_probation_time_us = POSVEL_PROBATION_MIN;
static uint64_t lvel_probation_time_us = POSVEL_PROBATION_MIN;
/* Mavlink log uORB handle */
static orb_advert_t mavlink_log_pub = nullptr;
static orb_advert_t power_button_state_pub = nullptr;
@@ -176,12 +156,6 @@ static uint64_t last_print_mode_reject_time = 0;
static systemlib::Hysteresis auto_disarm_hysteresis(false);
static hrt_abstime last_lpos_fail_time_us = 0; // Last time that the local position validity recovery check failed (usec)
static hrt_abstime last_gpos_fail_time_us = 0; // Last time that the global position validity recovery check failed (usec)
static hrt_abstime last_lvel_fail_time_us = 0; // Last time that the local velocity validity recovery check failed (usec)
static hrt_abstime gpos_last_update_time_us = 0; // last time a global position update was received (usec)
static float min_stick_change = 0.25f;
static struct vehicle_status_s status = {};
@@ -616,16 +590,15 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, co
Commander::Commander() :
ModuleParams(nullptr),
_mission_result_sub(ORB_ID(mission_result))
_mission_result_sub(ORB_ID(mission_result)),
_global_position_sub(ORB_ID(vehicle_global_position)),
_local_position_sub(ORB_ID(vehicle_local_position))
{
}
bool
Commander::handle_command(vehicle_status_s *status_local,
const vehicle_command_s& cmd, actuator_armed_s *armed_local,
home_position_s *home, const vehicle_global_position_s& global_pos,
const vehicle_local_position_s& local_pos, orb_advert_t *home_pub,
orb_advert_t *command_ack_pub, bool *changed)
Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_s& cmd, actuator_armed_s *armed_local,
home_position_s *home, orb_advert_t *home_pub, orb_advert_t *command_ack_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)
@@ -690,13 +663,13 @@ Commander::handle_command(vehicle_status_s *status_local,
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
/* POSCTL */
reset_posvel_validity(global_pos, local_pos, changed);
reset_posvel_validity(changed);
main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_POSCTL, 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);
reset_posvel_validity(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, status_flags, &internal_state);
@@ -754,7 +727,7 @@ Commander::handle_command(vehicle_status_s *status_local,
main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_STAB, status_flags, &internal_state);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
reset_posvel_validity(global_pos, local_pos, changed);
reset_posvel_validity(changed);
/* OFFBOARD */
main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_OFFBOARD, status_flags, &internal_state);
}
@@ -844,7 +817,7 @@ Commander::handle_command(vehicle_status_s *status_local,
(hrt_absolute_time() > (commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) &&
!home->manual_home) {
set_home_position(*home_pub, *home, local_pos, global_pos, false);
set_home_position(*home_pub, *home, false);
}
}
}
@@ -881,7 +854,7 @@ Commander::handle_command(vehicle_status_s *status_local,
if (use_current) {
/* use current position */
if (set_home_position(*home_pub, *home, local_pos, global_pos, false)) {
if (set_home_position(*home_pub, *home, false)) {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
@@ -894,7 +867,7 @@ Commander::handle_command(vehicle_status_s *status_local,
const float alt = cmd.param7;
if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt)) {
const vehicle_local_position_s& local_pos = _local_position_sub.get();
if (local_pos.xy_global && local_pos.z_global) {
/* use specified position */
home->timestamp = hrt_absolute_time();
@@ -1110,10 +1083,11 @@ Commander::handle_command(vehicle_status_s *status_local,
* time the vehicle is armed with a good GPS fix.
**/
bool
Commander::set_home_position(orb_advert_t &homePub, home_position_s &home,
const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition,
bool set_alt_only_to_lpos_ref)
Commander::set_home_position(orb_advert_t &homePub, home_position_s &home, bool set_alt_only_to_lpos_ref)
{
const vehicle_local_position_s& localPosition = _local_position_sub.get();
const vehicle_global_position_s& globalPosition = _global_position_sub.get();
if (!set_alt_only_to_lpos_ref) {
//Need global and local position fix to be able to set home
if (!status_flags.condition_global_position_valid || !status_flags.condition_local_position_valid) {
@@ -1352,18 +1326,6 @@ Commander::run()
int offboard_control_mode_sub = orb_subscribe(ORB_ID(offboard_control_mode));
memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));
/* Subscribe to global position */
int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
struct vehicle_global_position_s global_position;
memset(&global_position, 0, sizeof(global_position));
/* Init EPH and EPV */
global_position.eph = 1000.0f;
global_position.epv = 1000.0f;
/* Subscribe to local position data */
int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
struct vehicle_local_position_s local_position = {};
/* Subscribe to land detector */
int land_detector_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
land_detector.landed = true;
@@ -1410,18 +1372,6 @@ Commander::run()
control_status_leds(&status, &armed, true, &battery, &cpuload);
/* Get parameter values controlling activation of position failure failsafe and convert to required units*/
int32_t val = POSITION_TIMEOUT;
param_get(param_find("COM_POS_FS_DELAY"), &val);
posctl_nav_loss_delay = math::constrain(val * sec_to_usec, POSVEL_PROBATION_MIN, POSVEL_PROBATION_MAX);
val = POSVEL_PROBATION_TAKEOFF;
param_get(param_find("COM_POS_FS_PROB"), &val);
posctl_nav_loss_prob = math::constrain(val * sec_to_usec, POSVEL_PROBATION_MIN, POSVEL_PROBATION_MAX);
param_get(param_find("COM_POS_FS_GAIN"), &posctl_nav_loss_gain);
thread_running = true;
/* update vehicle status to find out vehicle type (required for preflight checks) */
@@ -1434,9 +1384,9 @@ Commander::run()
commander_boot_timestamp = hrt_absolute_time();
// initially set to failed
last_lpos_fail_time_us = commander_boot_timestamp;
last_gpos_fail_time_us = commander_boot_timestamp;
last_lvel_fail_time_us = commander_boot_timestamp;
_last_lpos_fail_time_us = commander_boot_timestamp;
_last_gpos_fail_time_us = commander_boot_timestamp;
_last_lvel_fail_time_us = commander_boot_timestamp;
// Run preflight check
int32_t rc_in_off = 0;
@@ -1788,23 +1738,11 @@ Commander::run()
}
}
_local_position_sub.update();
_global_position_sub.update();
// 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 and check for timeout */
bool gpos_updated = false;
orb_check(global_position_sub, &gpos_updated);
if (gpos_updated) {
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);
gpos_last_update_time_us = hrt_absolute_time();
}
// Perform a separate timeout validity test on the global position data.
// This is necessary because the global position message is by definition valid if published.
if ((hrt_absolute_time() - gpos_last_update_time_us) > 1000000) {
status_flags.condition_global_position_valid = false;
}
const bool run_quality_checks = !status_flags.circuit_breaker_engaged_posfailure_check;
/* Check estimator status for signs of bad yaw induced post takeoff navigation failure
* for a short time interval after takeoff. Fixed wing vehicles can recover using GPS heading,
@@ -1829,8 +1767,11 @@ Commander::run()
} else {
// if nav status is unconfirmed, confirm yaw angle as passed after 30 seconds or achieving 5 m/s of speed
bool sufficient_time = (hrt_absolute_time() - time_at_takeoff > 30 * 1000 * 1000);
bool sufficient_speed = local_position.vx * local_position.vx + local_position.vy * local_position.vy > 25.0f;
const bool sufficient_time = (hrt_elapsed_time(&time_at_takeoff) > 30 * 1_s);
const vehicle_local_position_s& lpos = _local_position_sub.get();
const bool sufficient_speed = (lpos.vx*lpos.vx + lpos.vy*lpos.vy > 25.0f);
bool innovation_pass = estimator_status.vel_test_ratio < 1.0f && estimator_status.pos_test_ratio < 1.0f;
if (!nav_test_failed) {
@@ -1846,8 +1787,11 @@ Commander::run()
}
// if the innovation test has failed continuously, declare the nav as failed
if ((hrt_absolute_time() - time_last_innov_pass) > 1000 * 1000) {
if ((hrt_absolute_time() - time_last_innov_pass) > 1_s) {
nav_test_failed = true;
status_flags.condition_local_position_valid = false;
status_flags.condition_local_velocity_valid = false;
status_flags.condition_global_position_valid = false;
mavlink_log_emergency(&mavlink_log_pub, "CRITICAL NAVIGATION FAILURE - CHECK SENSOR CALIBRATION");
}
}
@@ -1857,43 +1801,19 @@ Commander::run()
}
/* run global position accuracy checks */
if (gpos_updated) {
if (run_quality_checks) {
// If nav is failed, then declare local position and velocity as invalid
if (nav_test_failed) {
status_flags.condition_global_position_valid = false;
// Check if quality checking of position accuracy and consistency is to be performed
if (run_quality_checks) {
// use global position message to determine validity
const vehicle_global_position_s& global_position = _global_position_sub.get();
check_posvel_validity(true, global_position.eph, _eph_threshold.get(), global_position.timestamp, &_last_gpos_fail_time_us, &_gpos_probation_time_us, &status_flags.condition_global_position_valid, &status_changed);
} else {
// use global position message to determine validity
check_posvel_validity(true, global_position.eph, _eph_threshold.get(), global_position.timestamp, &last_gpos_fail_time_us, &gpos_probation_time_us, &status_flags.condition_global_position_valid, &status_changed);
}
}
// use local position message to determine validity
const vehicle_local_position_s& local_position = _local_position_sub.get();
check_posvel_validity(local_position.xy_valid, local_position.eph, _eph_threshold.get(), local_position.timestamp, &_last_lpos_fail_time_us, &_lpos_probation_time_us, &status_flags.condition_local_position_valid, &status_changed);
check_posvel_validity(local_position.v_xy_valid, local_position.evh, _evh_threshold.get(), local_position.timestamp, &_last_lvel_fail_time_us, &_lvel_probation_time_us, &status_flags.condition_local_velocity_valid, &status_changed);
}
/* update local position estimate */
bool lpos_updated = false;
orb_check(local_position_sub, &lpos_updated);
if (lpos_updated) {
/* position changed */
orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
if (run_quality_checks) {
// If nav is failed, then declare local position and velocity as invalid
if (nav_test_failed) {
status_flags.condition_local_position_valid = false;
status_flags.condition_local_velocity_valid = false;
} else {
// use local position message to determine validity
check_posvel_validity(local_position.xy_valid, local_position.eph, _eph_threshold.get(), local_position.timestamp, &last_lpos_fail_time_us, &lpos_probation_time_us, &status_flags.condition_local_position_valid, &status_changed);
check_posvel_validity(local_position.v_xy_valid, local_position.evh, _evh_threshold.get(), local_position.timestamp, &last_lvel_fail_time_us, &lvel_probation_time_us, &status_flags.condition_local_velocity_valid, &status_changed);
}
}
}
/* update condition_local_altitude_valid */
check_valid(local_position.timestamp, posctl_nav_loss_delay, local_position.z_valid, &(status_flags.condition_local_altitude_valid), &status_changed);
check_valid(_local_position_sub.get().timestamp, _failsafe_pos_delay.get() * sec_to_usec, _local_position_sub.get().z_valid, &(status_flags.condition_local_altitude_valid), &status_changed);
/* Update land detector */
orb_check(land_detector_sub, &updated);
@@ -1914,9 +1834,9 @@ Commander::run()
// 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 navigation shortly after takeoff.
gpos_probation_time_us = posctl_nav_loss_prob;
lpos_probation_time_us = posctl_nav_loss_prob;
lvel_probation_time_us = posctl_nav_loss_prob;
_gpos_probation_time_us = _failsafe_pos_probation.get() * sec_to_usec;
_lpos_probation_time_us = _failsafe_pos_probation.get() * sec_to_usec;
_lvel_probation_time_us = _failsafe_pos_probation.get() * sec_to_usec;
}
}
@@ -2411,7 +2331,7 @@ Commander::run()
/* 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(status, global_position, local_position, &status_changed);
transition_result_t main_res = set_main_state(status, &status_changed);
/* store last position lock state */
_last_condition_global_position_valid = status_flags.condition_global_position_valid;
@@ -2544,7 +2464,7 @@ Commander::run()
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
/* handle it */
if (handle_command(&status, cmd, &armed, &_home, global_position, local_position, &home_pub, &command_ack_pub, &status_changed)) {
if (handle_command(&status, cmd, &armed, &_home, &home_pub, &command_ack_pub, &status_changed)) {
status_changed = true;
}
}
@@ -2608,12 +2528,13 @@ Commander::run()
// automatically set or update home position
if (!_home.manual_home) {
const vehicle_local_position_s& local_position = _local_position_sub.get();
if (armed.armed) {
if ((!was_armed || (was_landed && !land_detector.landed)) &&
(now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) {
/* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */
set_home_position(home_pub, _home, local_position, global_position, false);
set_home_position(home_pub, _home, false);
}
} else {
@@ -2623,19 +2544,19 @@ Commander::run()
float home_dist_xy = -1.0f;
float home_dist_z = -1.0f;
mavlink_wpm_distance_to_point_local(_home.x, _home.y, _home.z,
local_position.x, local_position.y, local_position.z,
&home_dist_xy, &home_dist_z);
local_position.x, local_position.y, local_position.z,
&home_dist_xy, &home_dist_z);
if ((home_dist_xy > local_position.eph * 2) || (home_dist_z > local_position.epv * 2)) {
/* update when disarmed, landed and moved away from current home position */
set_home_position(home_pub, _home, local_position, global_position, false);
set_home_position(home_pub, _home, false);
}
}
} else {
/* First time home position update - but only if disarmed */
set_home_position(home_pub, _home, local_position, global_position, false);
set_home_position(home_pub, _home, false);
}
}
@@ -2643,7 +2564,7 @@ Commander::run()
* This allows home atitude to be used in the calculation of height above takeoff location when GPS
* use has commenced after takeoff. */
if (!_home.valid_alt && local_position.z_global) {
set_home_position(home_pub, _home, local_position, global_position, true);
set_home_position(home_pub, _home, true);
}
}
@@ -2831,8 +2752,6 @@ Commander::run()
buzzer_deinit();
px4_close(sp_man_sub);
px4_close(offboard_control_mode_sub);
px4_close(local_position_sub);
px4_close(global_position_sub);
px4_close(safety_sub);
px4_close(cmd_sub);
px4_close(subsys_sub);
@@ -3000,19 +2919,18 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
}
transition_result_t
Commander::set_main_state(const vehicle_status_s &status_local, const vehicle_global_position_s &global_position,
const vehicle_local_position_s &local_position, bool *changed)
Commander::set_main_state(const vehicle_status_s& status_local, bool *changed)
{
if (safety.override_available && safety.override_enabled) {
return set_main_state_override_on(status_local, changed);
} else {
return set_main_state_rc(status_local, global_position, local_position, changed);
return set_main_state_rc(status_local, changed);
}
}
transition_result_t
Commander::set_main_state_override_on(const vehicle_status_s &status_local, bool *changed)
Commander::set_main_state_override_on(const vehicle_status_s& status_local, bool *changed)
{
transition_result_t res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &internal_state);
*changed = (res == TRANSITION_CHANGED);
@@ -3021,8 +2939,7 @@ Commander::set_main_state_override_on(const vehicle_status_s &status_local, bool
}
transition_result_t
Commander::set_main_state_rc(const vehicle_status_s &status_local, const vehicle_global_position_s &global_position,
const vehicle_local_position_s &local_position, bool *changed)
Commander::set_main_state_rc(const vehicle_status_s& status_local, bool *changed)
{
/* set main state according to RC switches */
transition_result_t res = TRANSITION_DENIED;
@@ -3073,7 +2990,7 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, const vehicle
// 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);
reset_posvel_validity(changed);
/* offboard switch overrides main switch */
if (sp_man.offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
@@ -3400,24 +3317,24 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, const vehicle
}
void
Commander::reset_posvel_validity(const vehicle_global_position_s &global_position,
const vehicle_local_position_s &local_position, bool *changed)
Commander::reset_posvel_validity(bool *changed)
{
// reset all the check probation times back to the minimum value
gpos_probation_time_us = POSVEL_PROBATION_MIN;
lpos_probation_time_us = POSVEL_PROBATION_MIN;
lvel_probation_time_us = POSVEL_PROBATION_MIN;
_gpos_probation_time_us = POSVEL_PROBATION_MIN;
_lpos_probation_time_us = POSVEL_PROBATION_MIN;
_lvel_probation_time_us = POSVEL_PROBATION_MIN;
const vehicle_local_position_s& local_position = _local_position_sub.get();
const vehicle_global_position_s& global_position = _global_position_sub.get();
// recheck validity
check_posvel_validity(true, global_position.eph, _eph_threshold.get(), global_position.timestamp, &last_gpos_fail_time_us, &gpos_probation_time_us, &status_flags.condition_global_position_valid, changed);
check_posvel_validity(local_position.xy_valid, local_position.eph, _eph_threshold.get(), local_position.timestamp, &last_lpos_fail_time_us, &lpos_probation_time_us, &status_flags.condition_local_position_valid, changed);
check_posvel_validity(local_position.v_xy_valid, local_position.evh, _evh_threshold.get(), local_position.timestamp, &last_lvel_fail_time_us, &lvel_probation_time_us, &status_flags.condition_local_velocity_valid, changed);
check_posvel_validity(true, global_position.eph, _eph_threshold.get(), global_position.timestamp, &_last_gpos_fail_time_us, &_gpos_probation_time_us, &status_flags.condition_global_position_valid, changed);
check_posvel_validity(local_position.xy_valid, local_position.eph, _eph_threshold.get(), local_position.timestamp, &_last_lpos_fail_time_us, &_lpos_probation_time_us, &status_flags.condition_local_position_valid, changed);
check_posvel_validity(local_position.v_xy_valid, local_position.evh, _evh_threshold.get(), local_position.timestamp, &_last_lvel_fail_time_us, &_lvel_probation_time_us, &status_flags.condition_local_velocity_valid, changed);
}
bool
Commander::check_posvel_validity(const bool data_valid, const float data_accuracy, const float required_accuracy,
const hrt_abstime &data_timestamp_us, hrt_abstime *last_fail_time_us, hrt_abstime *probation_time_us, bool *valid_state,
bool *validity_changed)
Commander::check_posvel_validity(const bool data_valid, const float data_accuracy, const float required_accuracy, const hrt_abstime& data_timestamp_us, hrt_abstime* last_fail_time_us, hrt_abstime *probation_time_us, bool *valid_state, bool *validity_changed)
{
const bool was_valid = *valid_state;
bool valid = was_valid;
@@ -3427,7 +3344,7 @@ Commander::check_posvel_validity(const bool data_valid, const float data_accurac
*probation_time_us = POSVEL_PROBATION_MIN;
}
const bool data_stale = (hrt_elapsed_time(&data_timestamp_us) > posctl_nav_loss_delay);
const bool data_stale = ((hrt_elapsed_time(&data_timestamp_us) > _failsafe_pos_delay.get() * sec_to_usec) || (data_timestamp_us == 0));
const float req_accuracy = (was_valid ? required_accuracy * 2.5f : required_accuracy);
const bool level_check_pass = data_valid && !data_stale && (data_accuracy < req_accuracy);
@@ -3454,7 +3371,7 @@ Commander::check_posvel_validity(const bool data_valid, const float data_accurac
} else {
// failed again, increase probation time
const int64_t probation_time_new = *probation_time_us + hrt_elapsed_time(last_fail_time_us) * posctl_nav_loss_gain;
const int64_t probation_time_new = *probation_time_us + hrt_elapsed_time(last_fail_time_us) * _failsafe_pos_gain.get();
*probation_time_us = math::constrain(probation_time_new, POSVEL_PROBATION_MIN, POSVEL_PROBATION_MAX);
}