diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp
index 880d69523d08b1cdf545319d22a50372ed321a05..67dac4f5246ddd68171b90478c97b548feb1737b 100644
--- a/src/modules/commander/Commander.hpp
+++ b/src/modules/commander/Commander.hpp
@@ -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) */
+
+ 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 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);
+ 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);
+
+ // 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 ×tamp, const hrt_abstime &timeout, const bool valid_in, bool *valid_out,
- bool *changed);
+ void check_valid(const hrt_abstime ×tamp, 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_ */
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 54dd4241a1f8eda42f1ac70e531d91713bf750a6..47aa3920fadd9260c8f3f5ef847c73bb10616852 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -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()
}
}
- // 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();
- }
+ _local_position_sub.update();
+ _global_position_sub.update();
- // 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;
- }
+ // Check if quality checking of position accuracy and consistency is to be performed
+ 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;
-
- } 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);
- }
- }
- }
-
- /* 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);
- }
- }
+ // 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);
+
+ // 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 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);
}