mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-06-27 09:50:35 +08:00
Silvan Fuhrer
ebdc29bc5f
* FW attitude controller, FW position controller and VTOL attitude controller subscribe to airspeed_validated topic * add possibility to switch off the airspeed valid checks * remove airspeed valid checks from commander * clean up of VTOL transition logic * Airspeed Selector: remove dynamic allocation of airspeed validators (depending on number of connected sensors) but do it statically for the maximum number allowed. Check for number of connected sensors not only during start up, but always when vehicle is disarmed. * Airspeed Selector: change work queue from lp to att_pos_ctrl as this module is safety-critical * add airspeed selector to px4_fmu-v2 defaults
284 lines
11 KiB
C++
284 lines
11 KiB
C++
/****************************************************************************
|
|
*
|
|
* Copyright (c) 2017 PX4 Development Team. All rights reserved.
|
|
*
|
|
* Redistribution and use in source and binary forms, with or without
|
|
* modification, are permitted provided that the following conditions
|
|
* are met:
|
|
*
|
|
* 1. Redistributions of source code must retain the above copyright
|
|
* notice, this list of conditions and the following disclaimer.
|
|
* 2. Redistributions in binary form must reproduce the above copyright
|
|
* notice, this list of conditions and the following disclaimer in
|
|
* the documentation and/or other materials provided with the
|
|
* distribution.
|
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
|
* used to endorse or promote products derived from this software
|
|
* without specific prior written permission.
|
|
*
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
* POSSIBILITY OF SUCH DAMAGE.
|
|
*
|
|
****************************************************************************/
|
|
|
|
#ifndef COMMANDER_HPP_
|
|
#define COMMANDER_HPP_
|
|
|
|
#include "state_machine_helper.h"
|
|
#include "failure_detector/FailureDetector.hpp"
|
|
|
|
#include <lib/controllib/blocks.hpp>
|
|
#include <lib/mathlib/mathlib.h>
|
|
#include <px4_platform_common/module.h>
|
|
#include <px4_platform_common/module_params.h>
|
|
#include <lib/hysteresis/hysteresis.h>
|
|
|
|
// publications
|
|
#include <uORB/Publication.hpp>
|
|
#include <uORB/PublicationQueued.hpp>
|
|
#include <uORB/topics/actuator_armed.h>
|
|
#include <uORB/topics/home_position.h>
|
|
#include <uORB/topics/vehicle_command_ack.h>
|
|
#include <uORB/topics/vehicle_control_mode.h>
|
|
#include <uORB/topics/vehicle_status.h>
|
|
#include <uORB/topics/vehicle_status_flags.h>
|
|
#include <uORB/topics/test_motor.h>
|
|
|
|
// subscriptions
|
|
#include <uORB/Subscription.hpp>
|
|
#include <uORB/topics/airspeed.h>
|
|
#include <uORB/topics/estimator_status.h>
|
|
#include <uORB/topics/iridiumsbd_status.h>
|
|
#include <uORB/topics/mission_result.h>
|
|
#include <uORB/topics/offboard_control_mode.h>
|
|
#include <uORB/topics/parameter_update.h>
|
|
#include <uORB/topics/telemetry_status.h>
|
|
#include <uORB/topics/vehicle_acceleration.h>
|
|
#include <uORB/topics/vehicle_command.h>
|
|
#include <uORB/topics/vehicle_global_position.h>
|
|
#include <uORB/topics/vehicle_local_position.h>
|
|
#include <uORB/topics/esc_status.h>
|
|
|
|
using math::constrain;
|
|
|
|
using namespace time_literals;
|
|
|
|
class Commander : public ModuleBase<Commander>, public ModuleParams
|
|
{
|
|
public:
|
|
Commander();
|
|
|
|
/** @see ModuleBase */
|
|
static int task_spawn(int argc, char *argv[]);
|
|
|
|
/** @see ModuleBase */
|
|
static Commander *instantiate(int argc, char *argv[]);
|
|
|
|
/** @see ModuleBase */
|
|
static int custom_command(int argc, char *argv[]);
|
|
|
|
/** @see ModuleBase */
|
|
static int print_usage(const char *reason = nullptr);
|
|
|
|
/** @see ModuleBase::run() */
|
|
void run() override;
|
|
|
|
void enable_hil();
|
|
|
|
// TODO: only temporarily static until low priority thread is removed
|
|
static bool preflight_check(bool report);
|
|
|
|
void get_circuit_breaker_params();
|
|
|
|
private:
|
|
|
|
DEFINE_PARAMETERS(
|
|
|
|
(ParamInt<px4::params::NAV_DLL_ACT>) _param_nav_dll_act,
|
|
(ParamInt<px4::params::COM_DL_LOSS_T>) _param_com_dl_loss_t,
|
|
|
|
(ParamInt<px4::params::COM_HLDL_LOSS_T>) _param_com_hldl_loss_t,
|
|
(ParamInt<px4::params::COM_HLDL_REG_T>) _param_com_hldl_reg_t,
|
|
|
|
(ParamInt<px4::params::NAV_RCL_ACT>) _param_nav_rcl_act,
|
|
(ParamFloat<px4::params::COM_RC_LOSS_T>) _param_com_rc_loss_t,
|
|
|
|
(ParamFloat<px4::params::COM_HOME_H_T>) _param_com_home_h_t,
|
|
(ParamFloat<px4::params::COM_HOME_V_T>) _param_com_home_v_t,
|
|
|
|
(ParamFloat<px4::params::COM_POS_FS_EPH>) _param_com_pos_fs_eph,
|
|
(ParamFloat<px4::params::COM_POS_FS_EPV>) _param_com_pos_fs_epv, /*Not realy used for now*/
|
|
(ParamFloat<px4::params::COM_VEL_FS_EVH>) _param_com_vel_fs_evh,
|
|
(ParamInt<px4::params::COM_POSCTL_NAVL>) _param_com_posctl_navl, /* failsafe response to loss of navigation accuracy */
|
|
|
|
(ParamInt<px4::params::COM_POS_FS_DELAY>) _param_com_pos_fs_delay,
|
|
(ParamInt<px4::params::COM_POS_FS_PROB>) _param_com_pos_fs_prob,
|
|
(ParamInt<px4::params::COM_POS_FS_GAIN>) _param_com_pos_fs_gain,
|
|
|
|
(ParamInt<px4::params::COM_LOW_BAT_ACT>) _param_com_low_bat_act,
|
|
(ParamFloat<px4::params::COM_DISARM_LAND>) _param_com_disarm_land,
|
|
(ParamFloat<px4::params::COM_DISARM_PRFLT>) _param_com_disarm_preflight,
|
|
|
|
(ParamInt<px4::params::COM_OBS_AVOID>) _param_com_obs_avoid,
|
|
(ParamInt<px4::params::COM_OA_BOOT_T>) _param_com_oa_boot_t,
|
|
|
|
(ParamInt<px4::params::COM_FLT_PROFILE>) _param_com_flt_profile,
|
|
|
|
|
|
(ParamFloat<px4::params::COM_OF_LOSS_T>) _param_com_of_loss_t,
|
|
(ParamInt<px4::params::COM_OBL_ACT>) _param_com_obl_act,
|
|
(ParamInt<px4::params::COM_OBL_RC_ACT>) _param_com_obl_rc_act,
|
|
|
|
(ParamInt<px4::params::COM_PREARM_MODE>) _param_com_prearm_mode,
|
|
(ParamInt<px4::params::COM_MOT_TEST_EN>) _param_com_mot_test_en,
|
|
|
|
(ParamFloat<px4::params::COM_KILL_DISARM>) _param_com_kill_disarm,
|
|
|
|
(ParamInt<px4::params::CBRK_SUPPLY_CHK>) _param_cbrk_supply_chk,
|
|
(ParamInt<px4::params::CBRK_USB_CHK>) _param_cbrk_usb_chk,
|
|
(ParamInt<px4::params::CBRK_AIRSPD_CHK>) _param_cbrk_airspd_chk,
|
|
(ParamInt<px4::params::CBRK_ENGINEFAIL>) _param_cbrk_enginefail,
|
|
(ParamInt<px4::params::CBRK_GPSFAIL>) _param_cbrk_gpsfail,
|
|
(ParamInt<px4::params::CBRK_FLIGHTTERM>) _param_cbrk_flightterm,
|
|
(ParamInt<px4::params::CBRK_VELPOSERR>) _param_cbrk_velposerr
|
|
)
|
|
|
|
enum class PrearmedMode {
|
|
DISABLED = 0,
|
|
SAFETY_BUTTON = 1,
|
|
ALWAYS = 2
|
|
};
|
|
|
|
const int64_t POSVEL_PROBATION_MIN = 1_s; /**< minimum probation duration (usec) */
|
|
const int64_t POSVEL_PROBATION_MAX = 100_s; /**< 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;
|
|
|
|
/* class variables used to check for navigation failure after takeoff */
|
|
hrt_abstime _time_at_takeoff{0}; /**< last time we were on the ground */
|
|
hrt_abstime _time_last_innov_pass{0}; /**< last time velocity or position innovations passed */
|
|
bool _nav_test_passed{false}; /**< true if the post takeoff navigation test has passed */
|
|
bool _nav_test_failed{false}; /**< true if the post takeoff navigation test has failed */
|
|
|
|
bool _geofence_loiter_on{false};
|
|
bool _geofence_rtl_on{false};
|
|
bool _geofence_warning_action_on{false};
|
|
bool _geofence_violated_prev{false};
|
|
|
|
FailureDetector _failure_detector;
|
|
bool _flight_termination_triggered{false};
|
|
|
|
bool handle_command(vehicle_status_s *status, const vehicle_command_s &cmd, actuator_armed_s *armed,
|
|
uORB::PublicationQueued<vehicle_command_ack_s> &command_ack_pub, bool *changed);
|
|
|
|
unsigned handle_command_motor_test(const vehicle_command_s &cmd);
|
|
|
|
bool set_home_position();
|
|
bool set_home_position_alt_only();
|
|
|
|
// Set the main system state based on RC and override device inputs
|
|
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, 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, bool *changed);
|
|
|
|
void update_control_mode();
|
|
|
|
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(bool *changed);
|
|
|
|
void mission_init();
|
|
|
|
void estimator_check(bool *status_changed);
|
|
|
|
void offboard_control_update(bool &status_changed);
|
|
|
|
void battery_status_check();
|
|
|
|
void esc_status_check(const esc_status_s &esc_status);
|
|
|
|
/**
|
|
* Checks the status of all available data links and handles switching between different system telemetry states.
|
|
*/
|
|
void data_link_check(bool &status_changed);
|
|
|
|
uORB::Subscription _telemetry_status_sub{ORB_ID(telemetry_status)};
|
|
|
|
hrt_abstime _datalink_last_heartbeat_gcs{0};
|
|
|
|
hrt_abstime _datalink_last_heartbeat_onboard_controller{0};
|
|
bool _onboard_controller_lost{false};
|
|
|
|
hrt_abstime _datalink_last_heartbeat_avoidance_system{0};
|
|
bool _avoidance_system_lost{false};
|
|
|
|
bool _avoidance_system_status_change{false};
|
|
uint8_t _datalink_last_status_avoidance_system{telemetry_status_s::MAV_STATE_UNINIT};
|
|
|
|
uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
|
|
|
|
hrt_abstime _high_latency_datalink_heartbeat{0};
|
|
hrt_abstime _high_latency_datalink_lost{0};
|
|
|
|
int _last_esc_online_flags{-1};
|
|
|
|
uORB::Subscription _battery_sub{ORB_ID(battery_status)};
|
|
uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE};
|
|
float _battery_current{0.0f};
|
|
|
|
systemlib::Hysteresis _auto_disarm_landed{false};
|
|
systemlib::Hysteresis _auto_disarm_killed{false};
|
|
|
|
bool _print_avoidance_msg_once{false};
|
|
|
|
// Subscriptions
|
|
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
|
|
uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
|
|
|
|
uORB::SubscriptionData<airspeed_s> _airspeed_sub{ORB_ID(airspeed)};
|
|
uORB::SubscriptionData<estimator_status_s> _estimator_status_sub{ORB_ID(estimator_status)};
|
|
uORB::SubscriptionData<mission_result_s> _mission_result_sub{ORB_ID(mission_result)};
|
|
uORB::SubscriptionData<offboard_control_mode_s> _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
|
|
uORB::SubscriptionData<vehicle_global_position_s> _global_position_sub{ORB_ID(vehicle_global_position)};
|
|
uORB::SubscriptionData<vehicle_local_position_s> _local_position_sub{ORB_ID(vehicle_local_position)};
|
|
|
|
// Publications
|
|
uORB::Publication<vehicle_control_mode_s> _control_mode_pub{ORB_ID(vehicle_control_mode)};
|
|
uORB::Publication<vehicle_status_s> _status_pub{ORB_ID(vehicle_status)};
|
|
uORB::Publication<actuator_armed_s> _armed_pub{ORB_ID(actuator_armed)};
|
|
uORB::Publication<commander_state_s> _commander_state_pub{ORB_ID(commander_state)};
|
|
uORB::Publication<vehicle_status_flags_s> _vehicle_status_flags_pub{ORB_ID(vehicle_status_flags)};
|
|
uORB::Publication<test_motor_s> _test_motor_pub{ORB_ID(test_motor)};
|
|
|
|
uORB::PublicationData<home_position_s> _home_pub{ORB_ID(home_position)};
|
|
|
|
};
|
|
|
|
#endif /* COMMANDER_HPP_ */
|