fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-14 18:51:35 +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-04-24 00:02:48 -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_ */