PX4 Firmware
PX4 Autopilot Software http://px4.io
Commander Class Reference

#include <Commander.hpp>

Inheritance diagram for Commander:
Collaboration diagram for Commander:

Public Member Functions

 Commander ()
 
void run () override
 
void enable_hil ()
 
void get_circuit_breaker_params ()
 

Static Public Member Functions

static int task_spawn (int argc, char *argv[])
 
static Commanderinstantiate (int argc, char *argv[])
 
static int custom_command (int argc, char *argv[])
 
static int print_usage (const char *reason=nullptr)
 
static bool preflight_check (bool report)
 

Private Member Functions

 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,(ParamFloat< px4::params::COM_VEL_FS_EVH >) _param_com_vel_fs_evh,(ParamInt< px4::params::COM_POSCTL_NAVL >) _param_com_posctl_navl,(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
 
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 ()
 This function initializes the home position an altitude of the vehicle. More...
 
bool set_home_position_alt_only ()
 
transition_result_t set_main_state (const vehicle_status_s &status, bool *changed)
 
transition_result_t set_main_state_override_on (const vehicle_status_s &status, bool *changed)
 
transition_result_t set_main_state_rc (const vehicle_status_s &status, bool *changed)
 
void update_control_mode ()
 
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 (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)
 
void data_link_check (bool &status_changed)
 Checks the status of all available data links and handles switching between different system telemetry states. More...
 

Private Attributes

const int64_t POSVEL_PROBATION_MIN = 1_s
 minimum probation duration (usec) More...
 
const int64_t POSVEL_PROBATION_MAX = 100_s
 maximum probation duration (usec) More...
 
hrt_abstime _last_gpos_fail_time_us {0}
 Last time that the global position validity recovery check failed (usec) More...
 
hrt_abstime _last_lpos_fail_time_us {0}
 Last time that the local position validity recovery check failed (usec) More...
 
hrt_abstime _last_lvel_fail_time_us {0}
 Last time that the local velocity validity recovery check failed (usec) More...
 
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
 
hrt_abstime _time_at_takeoff {0}
 last time we were on the ground More...
 
hrt_abstime _time_last_innov_pass {0}
 last time velocity or position innovations passed More...
 
bool _nav_test_passed {false}
 true if the post takeoff navigation test has passed More...
 
bool _nav_test_failed {false}
 true if the post takeoff navigation test has failed More...
 
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}
 
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}
 
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)}
 
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)}
 

Detailed Description

Definition at line 76 of file Commander.hpp.

Constructor & Destructor Documentation

◆ Commander()

Commander::Commander ( )

Definition at line 556 of file Commander.cpp.

References _auto_disarm_landed, vehicle_status_s::arming_state, vehicle_status_flags_s::avoidance_system_valid, vehicle_status_flags_s::condition_power_input_valid, vehicle_status_s::data_link_lost, hrt_absolute_time(), commander_state_s::main_state, vehicle_status_s::nav_state, vehicle_status_s::nav_state_timestamp, vehicle_status_flags_s::offboard_control_signal_lost, vehicle_status_flags_s::rc_calibration_valid, vehicle_status_s::rc_input_mode, vehicle_status_s::rc_signal_lost, and systemlib::Hysteresis::set_hysteresis_time_from().

Referenced by instantiate().

Here is the call graph for this function:
Here is the caller graph for this function:

Member Function Documentation

◆ battery_status_check()

void Commander::battery_status_check ( )
private

Definition at line 3963 of file Commander.cpp.

References _battery_current, _battery_sub, _battery_warning, actuator_armed_s::armed, battery_failsafe(), vehicle_status_flags_s::condition_battery_healthy, uORB::Subscription::copy(), hrt_elapsed_time(), mavlink_log_critical, mavlink_log_pub, shutdown_if_allowed(), and uORB::Subscription::updated().

Referenced by run().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ check_posvel_validity()

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 
)
private

Definition at line 3126 of file Commander.cpp.

References math::constrain(), hrt_absolute_time(), hrt_elapsed_time(), vehicle_land_detected_s::landed, POSVEL_PROBATION_MAX, and POSVEL_PROBATION_MIN.

Referenced by estimator_check(), and reset_posvel_validity().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ check_valid()

void Commander::check_valid ( const hrt_abstime timestamp,
const hrt_abstime timeout,
const bool  valid_in,
bool *  valid_out,
bool *  changed 
)
private

Definition at line 2544 of file Commander.cpp.

References hrt_absolute_time(), and hrt_abstime.

Referenced by estimator_check().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ custom_command()

int Commander::custom_command ( int  argc,
char *  argv[] 
)
static
See also
ModuleBase

Definition at line 3697 of file Commander.cpp.

References print_usage().

Here is the call graph for this function:

◆ data_link_check()

void Commander::data_link_check ( bool &  status_changed)
private

Checks the status of all available data links and handles switching between different system telemetry states.

Definition at line 3785 of file Commander.cpp.

References _avoidance_system_lost, _avoidance_system_status_change, _datalink_last_heartbeat_avoidance_system, _datalink_last_heartbeat_gcs, _datalink_last_heartbeat_onboard_controller, _datalink_last_status_avoidance_system, _high_latency_datalink_heartbeat, _high_latency_datalink_lost, _iridiumsbd_status_sub, _onboard_controller_lost, _print_avoidance_msg_once, _telemetry_status_sub, vehicle_status_flags_s::avoidance_system_required, vehicle_status_flags_s::avoidance_system_valid, uORB::Subscription::copy(), vehicle_status_s::data_link_lost, vehicle_status_s::data_link_lost_counter, telemetry_status_s::heartbeat_time, vehicle_status_s::high_latency_data_link_lost, hrt_absolute_time(), hrt_elapsed_time(), iridiumsbd_status_s::last_heartbeat, mavlink_log_critical, mavlink_log_info, mavlink_log_pub, telemetry_status_s::remote_component_id, telemetry_status_s::remote_system_status, telemetry_status_s::remote_type, telemetry_status_s::type, uORB::Subscription::update(), uORB::Subscription::updated(), and vehicle_status_flags_s::usb_connected.

Referenced by run().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ DEFINE_PARAMETERS()

Commander::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,
(ParamFloat< px4::params::COM_VEL_FS_EVH >)  _param_com_vel_fs_evh,
(ParamInt< px4::params::COM_POSCTL_NAVL >)  _param_com_posctl_navl,
(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 
)
inlineprivate

Definition at line 105 of file Commander.hpp.

References DISABLED.

◆ enable_hil()

void Commander::enable_hil ( )

Definition at line 3741 of file Commander.cpp.

References vehicle_status_s::hil_state.

Referenced by instantiate().

Here is the caller graph for this function:

◆ esc_status_check()

void Commander::esc_status_check ( const esc_status_s esc_status)
private

Definition at line 4202 of file Commander.cpp.

References _last_esc_online_flags, vehicle_status_flags_s::condition_escs_error, esc_status_s::esc_count, esc_status_s::esc_online_flags, mavlink_log_critical, and mavlink_log_pub.

Referenced by run().

Here is the caller graph for this function:

◆ estimator_check()

void Commander::estimator_check ( bool *  status_changed)
private

Definition at line 4014 of file Commander.cpp.

References _eph_threshold_adj, _estimator_status_sub, _global_position_sub, _gpos_probation_time_us, _last_condition_global_position_valid, _last_gpos_fail_time_us, _last_lpos_fail_time_us, _last_lvel_fail_time_us, _local_position_sub, _lpos_probation_time_us, _lvel_probation_time_us, _nav_test_failed, _nav_test_passed, _skip_pos_accuracy_check, _time_at_takeoff, _time_last_innov_pass, vehicle_status_s::arming_state, check_posvel_validity(), check_valid(), vehicle_status_flags_s::circuit_breaker_engaged_posfailure_check, vehicle_status_flags_s::condition_global_position_valid, vehicle_status_flags_s::condition_local_altitude_valid, vehicle_status_flags_s::condition_local_position_valid, vehicle_status_flags_s::condition_local_velocity_valid, estimator_status_s::control_mode_flags, vehicle_global_position_s::eph, vehicle_local_position_s::eph, vehicle_local_position_s::evh, uORB::SubscriptionData< T >::get(), hrt_absolute_time(), hrt_elapsed_time(), vehicle_land_detected_s::landed, commander_state_s::main_state, mavlink_log_critical, mavlink_log_emergency, mavlink_log_pub, estimator_status_s::pos_test_ratio, set_health_flags_healthy(), set_health_flags_present_healthy(), vehicle_local_position_s::timestamp, vehicle_global_position_s::timestamp, uORB::SubscriptionData< T >::update(), vehicle_local_position_s::v_xy_valid, vehicle_status_s::vehicle_type, estimator_status_s::vel_test_ratio, vehicle_local_position_s::vx, vehicle_local_position_s::vy, vehicle_local_position_s::xy_valid, and vehicle_local_position_s::z_valid.

Referenced by run().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ get_circuit_breaker_params()

void Commander::get_circuit_breaker_params ( )

◆ handle_command()

bool Commander::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 
)
private

Definition at line 581 of file Commander.cpp.

References _high_latency_datalink_heartbeat, _home_pub, _local_position_sub, _mission_result_sub, answer_command(), arm_disarm(), actuator_armed_s::armed, vehicle_status_s::arming_state, vehicle_command_s::command, commander_boot_timestamp, vehicle_status_s::component_id, vehicle_status_flags_s::condition_auto_mission_available, vehicle_status_flags_s::condition_home_position_valid, vehicle_status_flags_s::condition_system_sensors_initialized, actuator_armed_s::force_failsafe, uORB::PublicationData< T >::get(), uORB::SubscriptionData< T >::get(), handle_command_motor_test(), vehicle_status_s::hil_state, hrt_absolute_time(), INAIR_RESTART_HOLDOFF_INTERVAL, is_ground_rover(), vehicle_land_detected_s::landed, actuator_armed_s::lockdown, commander_state_s::main_state, main_state_transition(), home_position_s::manual_home, map_projection_init(), map_projection_project(), mavlink_and_console_log_info, mavlink_log_critical, mavlink_log_pub, mavlink_log_warning, vehicle_status_s::nav_state, vehicle_status_flags_s::offboard_control_set_by_command, vehicle_command_s::param1, vehicle_command_s::param2, vehicle_command_s::param3, vehicle_command_s::param5, vehicle_command_s::param6, vehicle_command_s::param7, print_reject_mode(), PX4_CUSTOM_MAIN_MODE_ACRO, PX4_CUSTOM_MAIN_MODE_ALTCTL, PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_MAIN_MODE_MANUAL, PX4_CUSTOM_MAIN_MODE_OFFBOARD, PX4_CUSTOM_MAIN_MODE_POSCTL, PX4_CUSTOM_MAIN_MODE_RATTITUDE, PX4_CUSTOM_MAIN_MODE_STABILIZED, PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET, PX4_CUSTOM_SUB_MODE_AUTO_LAND, PX4_CUSTOM_SUB_MODE_AUTO_LOITER, PX4_CUSTOM_SUB_MODE_AUTO_MISSION, PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND, PX4_CUSTOM_SUB_MODE_AUTO_RTL, PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF, vehicle_local_position_s::ref_alt, vehicle_local_position_s::ref_lat, vehicle_local_position_s::ref_lon, reset_posvel_validity(), mission_result_s::seq_total, set_home_position(), vehicle_command_s::source_component, vehicle_command_s::source_system, vehicle_status_s::system_id, vehicle_command_s::target_component, vehicle_command_s::target_system, home_position_s::timestamp, TRANSITION_CHANGED, TRANSITION_DENIED, TRANSITION_NOT_CHANGED, uORB::PublicationData< T >::update(), VEHICLE_MODE_FLAG_AUTO_ENABLED, VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED, VEHICLE_MODE_FLAG_GUIDED_ENABLED, VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED, VEHICLE_MODE_FLAG_STABILIZE_ENABLED, warnx, vehicle_local_position_s::xy_global, manual_control_setpoint_s::z, and vehicle_local_position_s::z_global.

Referenced by run().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ handle_command_motor_test()

unsigned Commander::handle_command_motor_test ( const vehicle_command_s cmd)
private

Definition at line 1123 of file Commander.cpp.

References _test_motor_pub, actuator_armed_s::armed, math::constrain(), FLT_EPSILON, hrt_absolute_time(), vehicle_command_s::param1, vehicle_command_s::param2, vehicle_command_s::param3, vehicle_command_s::param4, vehicle_command_s::param5, uORB::Publication< T >::publish(), safety_s::safety_off, safety_s::safety_switch_available, and test_motor_s::timestamp.

Referenced by handle_command().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ instantiate()

Commander * Commander::instantiate ( int  argc,
char *  argv[] 
)
static
See also
ModuleBase

Definition at line 3728 of file Commander.cpp.

References Commander(), enable_hil(), and ll40ls::instance.

Here is the call graph for this function:

◆ mission_init()

void Commander::mission_init ( )
private

Definition at line 3746 of file Commander.cpp.

References mission_s::count, mission_s::current_seq, mission_s::dataman_id, DM_KEY_MISSION_STATE, DM_KEY_WAYPOINTS_OFFBOARD_0, DM_KEY_WAYPOINTS_OFFBOARD_1, DM_PERSIST_POWER_ON_RESET, dm_read(), dm_write(), hrt_absolute_time(), orb_advert_t, orb_advertise(), ORB_ID, orb_unadvertise(), and mission_s::timestamp.

Referenced by run().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ offboard_control_update()

◆ preflight_check()

bool Commander::preflight_check ( bool  report)
static

Definition at line 3771 of file Commander.cpp.

References PreFlightCheck::ARM_REQ_GPS_BIT, arm_requirements, commander_boot_timestamp, vehicle_status_flags_s::condition_system_sensors_initialized, hrt_elapsed_time(), mavlink_log_pub, and PreFlightCheck::preflightCheck().

Referenced by commander_low_prio_loop(), commander_main(), and run().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ print_usage()

int Commander::print_usage ( const char *  reason = nullptr)
static
See also
ModuleBase

Definition at line 3702 of file Commander.cpp.

Referenced by custom_command().

Here is the caller graph for this function:

◆ reset_posvel_validity()

◆ run()

void Commander::run ( )
override
See also
ModuleBase::run()

< absolute time when engine was healty

Definition at line 1241 of file Commander.cpp.

References _armed_pub, _auto_disarm_killed, _auto_disarm_landed, _battery_current, _battery_warning, _commander_state_pub, _failure_detector, _flight_mode_slots, _flight_termination_triggered, _geofence_loiter_on, _geofence_rtl_on, _geofence_violated_prev, _geofence_warning_action_on, _gpos_probation_time_us, _home_pub, _last_condition_global_position_valid, _last_condition_local_altitude_valid, _last_condition_local_position_valid, _last_gpos_fail_time_us, _last_lpos_fail_time_us, _last_lvel_fail_time_us, _last_sp_man_arm_switch, _local_position_sub, _lpos_probation_time_us, _lvel_probation_time_us, _mission_result_sub, _parameter_update_sub, _status_pub, _vehicle_status_flags_pub, actuator_controls, arm_auth_init(), arm_auth_update(), arm_disarm(), PreFlightCheck::ARM_REQ_ARM_AUTH_BIT, PreFlightCheck::ARM_REQ_ESCS_CHECK_BIT, PreFlightCheck::ARM_REQ_GPS_BIT, PreFlightCheck::ARM_REQ_MISSION_BIT, PreFlightCheck::ARM_REQ_NONE, arm_requirements, manual_control_setpoint_s::arm_switch, actuator_armed_s::armed, vehicle_status_s::arming_state, arming_state_transition(), vehicle_status_flags_s::avoidance_system_required, battery_status_check(), blink_msg_state(), system_power_s::brick_valid, buzzer_deinit(), buzzer_init(), vehicle_status_flags_s::circuit_breaker_engaged_enginefailure_check, vehicle_status_flags_s::circuit_breaker_flight_termination_disabled, commander_boot_timestamp, commander_low_prio_loop(), COMMANDER_MONITORING_INTERVAL, COMMANDER_MONITORING_LOOPSPERMSEC, vehicle_status_s::component_id, vehicle_status_flags_s::condition_auto_mission_available, vehicle_status_flags_s::condition_calibration_enabled, vehicle_status_flags_s::condition_global_position_valid, vehicle_status_flags_s::condition_home_position_valid, vehicle_status_flags_s::condition_local_altitude_valid, vehicle_status_flags_s::condition_local_position_valid, vehicle_status_flags_s::condition_power_input_valid, vehicle_status_flags_s::condition_system_hotplug_timeout, vehicle_status_flags_s::condition_system_sensors_initialized, actuator_controls_s::control, control_status_leds(), uORB::Subscription::copy(), counter, data_link_check(), subsystem_info_s::enabled, vehicle_status_s::engine_failure, vehicle_local_position_s::eph, vehicle_local_position_s::epv, esc_status_check(), estimator_check(), power_button_state_s::event, vehicle_status_s::failsafe, mission_result_s::failure, vehicle_status_s::failure_detector_status, mission_result_s::finished, mission_result_s::flight_termination, actuator_armed_s::force_failsafe, vehicle_land_detected_s::freefall, vtol_vehicle_status_s::fw_permanent_stab, uORB::PublicationData< T >::get(), uORB::SubscriptionData< T >::get(), get_circuit_breaker_params(), systemlib::Hysteresis::get_state(), FailureDetector::getStatus(), handle_command(), vehicle_status_s::hil_state, HOTPLUG_SENS_TIMEOUT, hrt_absolute_time(), hrt_abstime, hrt_elapsed_time(), vehicle_status_s::in_transition_mode, vtol_vehicle_status_s::in_transition_to_fw, vehicle_status_s::in_transition_to_fw, INAIR_RESTART_HOLDOFF_INTERVAL, lps22hb::info(), mission_result_s::instance_count, is_fixed_wing(), is_ground_rover(), is_rotary_wing(), is_vtol(), vehicle_status_s::is_vtol, vehicle_status_s::is_vtol_tailsitter, is_vtol_tailsitter(), FailureDetector::isFailure(), manual_control_setpoint_s::kill_switch, vehicle_land_detected_s::landed, led_deinit(), led_init(), manual_control_setpoint_s::loiter_switch, commander_state_s::main_state, main_state_before_rtl, main_state_transition(), home_position_s::manual_home, actuator_armed_s::manual_lockdown, mavlink_and_console_log_info, mavlink_log_critical, mavlink_log_emergency, mavlink_log_info, mavlink_log_pub, mavlink_wpm_distance_to_point_local(), min_stick_change, vehicle_status_s::mission_failure, mission_init(), vehicle_status_s::nav_state_timestamp, offboard_control_update(), OK, subsystem_info_s::ok, orb_advertise(), ORB_ID, ORB_ID_VEHICLE_ATTITUDE_CONTROLS, param_find(), param_get(), PARAM_INVALID, param_set(), power_button_state_notification_cb(), power_button_state_pub, actuator_armed_s::prearmed, preflight_check(), subsystem_info_s::present, print_reject_arm(), uORB::Publication< T >::publish(), manual_control_setpoint_s::r, vehicle_status_flags_s::rc_calibration_valid, vehicle_status_flags_s::rc_input_blocked, vehicle_status_s::rc_input_mode, vehicle_status_flags_s::rc_signal_found_once, vehicle_status_s::rc_signal_lost, rc_signal_lost_timestamp, manual_control_setpoint_s::return_switch, rgbled_set_color_and_mode(), safety_s::safety_off, safety_s::safety_switch_available, system_power_s::servo_valid, set_health_flags(), set_home_position(), set_home_position_alt_only(), systemlib::Hysteresis::set_hysteresis_time_from(), set_main_state(), set_nav_state(), systemlib::Hysteresis::set_state_and_update(), set_tune(), set_tune_override(), shutdown_if_allowed(), actuator_armed_s::soft_stop, status_flags, mission_result_s::stay_in_failsafe, STICK_ON_OFF_LIMIT, subsystem_info_s::subsystem_type, vehicle_status_s::system_id, vehicle_status_s::system_type, thread_running, thread_should_exit, vehicle_status_flags_s::timestamp, actuator_armed_s::timestamp, mission_result_s::timestamp, system_power_s::timestamp, commander_state_s::timestamp, manual_control_setpoint_s::timestamp, vehicle_status_s::timestamp, TONE_ARMING_WARNING_TUNE, TONE_BATTERY_WARNING_FAST_TUNE, TONE_BATTERY_WARNING_SLOW_TUNE, TONE_GPS_WARNING_TUNE, TONE_NOTIFY_POSITIVE_TUNE, TONE_PARACHUTE_RELEASE_TUNE, TONE_STOP_TUNE, TRANSITION_CHANGED, TRANSITION_DENIED, TRANSITION_NOT_CHANGED, tune_failsafe(), tune_mission_fail(), tune_mission_ok(), tune_negative(), tune_neutral(), tune_positive(), FailureDetector::update(), uORB::SubscriptionData< T >::update(), update_control_mode(), uORB::Subscription::updated(), system_power_s::usb_connected, vehicle_status_flags_s::usb_connected, mission_result_s::valid, vehicle_status_s::vehicle_type, VEHICLE_TYPE_FIXED_WING, vehicle_status_s::vtol_fw_permanent_stab, vtol_vehicle_status_s::vtol_in_rw_mode, vtol_vehicle_status_s::vtol_in_trans_mode, vtol_vehicle_status_s::vtol_transition_failsafe, vehicle_status_flags_s::vtol_transition_failure, warn, mission_result_s::warning, warnx, vehicle_local_position_s::x, home_position_s::x, manual_control_setpoint_s::x, vehicle_local_position_s::xy_valid, vehicle_local_position_s::y, home_position_s::y, manual_control_setpoint_s::y, home_position_s::z, vehicle_local_position_s::z, manual_control_setpoint_s::z, and vehicle_local_position_s::z_valid.

◆ set_home_position()

bool Commander::set_home_position ( )
private

This function initializes the home position an altitude of the vehicle.

This happens first time we get a good GPS fix and each time the vehicle is armed with a good GPS fix.

Definition at line 1174 of file Commander.cpp.

References _global_position_sub, _home_pub, _local_position_sub, vehicle_global_position_s::alt, vehicle_status_flags_s::condition_global_position_valid, vehicle_status_flags_s::condition_home_position_valid, vehicle_status_flags_s::condition_local_position_valid, vehicle_global_position_s::eph, vehicle_global_position_s::epv, uORB::SubscriptionData< T >::get(), hrt_absolute_time(), vehicle_global_position_s::lat, vehicle_global_position_s::lon, home_position_s::timestamp, tune_home_set(), uORB::PublicationData< T >::update(), vehicle_local_position_s::x, vehicle_local_position_s::y, vehicle_local_position_s::yaw, and vehicle_local_position_s::z.

Referenced by handle_command(), and run().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ set_home_position_alt_only()

bool Commander::set_home_position_alt_only ( )
private

Definition at line 1222 of file Commander.cpp.

References _home_pub, _local_position_sub, home_position_s::alt, uORB::PublicationData< T >::get(), uORB::SubscriptionData< T >::get(), hrt_absolute_time(), vehicle_local_position_s::ref_alt, uORB::PublicationData< T >::update(), home_position_s::valid_alt, and vehicle_local_position_s::z_global.

Referenced by run().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ set_main_state()

transition_result_t Commander::set_main_state ( const vehicle_status_s status,
bool *  changed 
)
private

Definition at line 2692 of file Commander.cpp.

References safety_s::override_available, safety_s::override_enabled, set_main_state_override_on(), and set_main_state_rc().

Referenced by run().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ set_main_state_override_on()

transition_result_t Commander::set_main_state_override_on ( const vehicle_status_s status,
bool *  changed 
)
private

Definition at line 2703 of file Commander.cpp.

References main_state_transition(), and TRANSITION_CHANGED.

Referenced by set_main_state().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ set_main_state_rc()

transition_result_t Commander::set_main_state_rc ( const vehicle_status_s status,
bool *  changed 
)
private

Definition at line 2713 of file Commander.cpp.

References _flight_mode_slots, _geofence_warning_action_on, _last_condition_global_position_valid, _last_condition_local_altitude_valid, _last_condition_local_position_valid, manual_control_setpoint_s::acro_switch, vehicle_status_flags_s::condition_global_position_valid, vehicle_status_flags_s::condition_local_altitude_valid, vehicle_status_flags_s::condition_local_position_valid, vehicle_status_s::is_vtol, manual_control_setpoint_s::loiter_switch, commander_state_s::main_state, main_state_transition(), manual_control_setpoint_s::man_switch, manual_control_setpoint_s::mode_slot, manual_control_setpoint_s::mode_switch, manual_control_setpoint_s::offboard_switch, manual_control_setpoint_s::posctl_switch, print_reject_mode(), manual_control_setpoint_s::r, manual_control_setpoint_s::rattitude_switch, reset_posvel_validity(), manual_control_setpoint_s::return_switch, sp_man, manual_control_setpoint_s::stab_switch, manual_control_setpoint_s::timestamp, TRANSITION_DENIED, TRANSITION_NOT_CHANGED, vehicle_status_s::vehicle_type, VEHICLE_TYPE_FIXED_WING, warnx, manual_control_setpoint_s::x, manual_control_setpoint_s::y, and manual_control_setpoint_s::z.

Referenced by set_main_state().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ task_spawn()

int Commander::task_spawn ( int  argc,
char *  argv[] 
)
static
See also
ModuleBase

Definition at line 3711 of file Commander.cpp.

◆ update_control_mode()

Member Data Documentation

◆ _airspeed_sub

uORB::SubscriptionData<airspeed_s> Commander::_airspeed_sub {ORB_ID(airspeed)}
private

Definition at line 264 of file Commander.hpp.

◆ _armed_pub

uORB::Publication<actuator_armed_s> Commander::_armed_pub {ORB_ID(actuator_armed)}
private

Definition at line 274 of file Commander.hpp.

Referenced by run().

◆ _auto_disarm_killed

systemlib::Hysteresis Commander::_auto_disarm_killed {false}
private

Definition at line 256 of file Commander.hpp.

Referenced by run().

◆ _auto_disarm_landed

systemlib::Hysteresis Commander::_auto_disarm_landed {false}
private

Definition at line 255 of file Commander.hpp.

Referenced by Commander(), and run().

◆ _avoidance_system_lost

bool Commander::_avoidance_system_lost {false}
private

Definition at line 239 of file Commander.hpp.

Referenced by data_link_check().

◆ _avoidance_system_status_change

bool Commander::_avoidance_system_status_change {false}
private

Definition at line 241 of file Commander.hpp.

Referenced by data_link_check().

◆ _battery_current

float Commander::_battery_current {0.0f}
private

Definition at line 253 of file Commander.hpp.

Referenced by battery_status_check(), and run().

◆ _battery_sub

uORB::Subscription Commander::_battery_sub {ORB_ID(battery_status)}
private

Definition at line 251 of file Commander.hpp.

Referenced by battery_status_check().

◆ _battery_warning

uint8_t Commander::_battery_warning {battery_status_s::BATTERY_WARNING_NONE}
private

Definition at line 252 of file Commander.hpp.

Referenced by battery_status_check(), and run().

◆ _commander_state_pub

uORB::Publication<commander_state_s> Commander::_commander_state_pub {ORB_ID(commander_state)}
private

Definition at line 275 of file Commander.hpp.

Referenced by run().

◆ _control_mode_pub

uORB::Publication<vehicle_control_mode_s> Commander::_control_mode_pub {ORB_ID(vehicle_control_mode)}
private

Definition at line 272 of file Commander.hpp.

Referenced by update_control_mode().

◆ _datalink_last_heartbeat_avoidance_system

hrt_abstime Commander::_datalink_last_heartbeat_avoidance_system {0}
private

Definition at line 238 of file Commander.hpp.

Referenced by data_link_check().

◆ _datalink_last_heartbeat_gcs

hrt_abstime Commander::_datalink_last_heartbeat_gcs {0}
private

Definition at line 233 of file Commander.hpp.

Referenced by data_link_check().

◆ _datalink_last_heartbeat_onboard_controller

hrt_abstime Commander::_datalink_last_heartbeat_onboard_controller {0}
private

Definition at line 235 of file Commander.hpp.

Referenced by data_link_check().

◆ _datalink_last_status_avoidance_system

uint8_t Commander::_datalink_last_status_avoidance_system {telemetry_status_s::MAV_STATE_UNINIT}
private

Definition at line 242 of file Commander.hpp.

Referenced by data_link_check().

◆ _estimator_status_sub

uORB::SubscriptionData<estimator_status_s> Commander::_estimator_status_sub {ORB_ID(estimator_status)}
private

Definition at line 265 of file Commander.hpp.

Referenced by estimator_check().

◆ _failure_detector

FailureDetector Commander::_failure_detector
private

Definition at line 185 of file Commander.hpp.

Referenced by run().

◆ _flight_termination_triggered

bool Commander::_flight_termination_triggered {false}
private

Definition at line 186 of file Commander.hpp.

Referenced by run().

◆ _geofence_loiter_on

bool Commander::_geofence_loiter_on {false}
private

Definition at line 180 of file Commander.hpp.

Referenced by run().

◆ _geofence_rtl_on

bool Commander::_geofence_rtl_on {false}
private

Definition at line 181 of file Commander.hpp.

Referenced by run().

◆ _geofence_violated_prev

bool Commander::_geofence_violated_prev {false}
private

Definition at line 183 of file Commander.hpp.

Referenced by run().

◆ _geofence_warning_action_on

bool Commander::_geofence_warning_action_on {false}
private

Definition at line 182 of file Commander.hpp.

Referenced by run(), and set_main_state_rc().

◆ _global_position_sub

uORB::SubscriptionData<vehicle_global_position_s> Commander::_global_position_sub {ORB_ID(vehicle_global_position)}
private

Definition at line 268 of file Commander.hpp.

Referenced by estimator_check(), reset_posvel_validity(), and set_home_position().

◆ _gpos_probation_time_us

hrt_abstime Commander::_gpos_probation_time_us = POSVEL_PROBATION_MIN
private

Definition at line 170 of file Commander.hpp.

Referenced by estimator_check(), reset_posvel_validity(), and run().

◆ _high_latency_datalink_heartbeat

hrt_abstime Commander::_high_latency_datalink_heartbeat {0}
private

Definition at line 246 of file Commander.hpp.

Referenced by data_link_check(), and handle_command().

◆ _high_latency_datalink_lost

hrt_abstime Commander::_high_latency_datalink_lost {0}
private

Definition at line 247 of file Commander.hpp.

Referenced by data_link_check().

◆ _home_pub

uORB::PublicationData<home_position_s> Commander::_home_pub {ORB_ID(home_position)}
private

Definition at line 279 of file Commander.hpp.

Referenced by handle_command(), run(), set_home_position(), and set_home_position_alt_only().

◆ _iridiumsbd_status_sub

uORB::Subscription Commander::_iridiumsbd_status_sub {ORB_ID(iridiumsbd_status)}
private

Definition at line 244 of file Commander.hpp.

Referenced by data_link_check().

◆ _last_esc_online_flags

int Commander::_last_esc_online_flags {-1}
private

Definition at line 249 of file Commander.hpp.

Referenced by esc_status_check().

◆ _last_gpos_fail_time_us

hrt_abstime Commander::_last_gpos_fail_time_us {0}
private

Last time that the global position validity recovery check failed (usec)

Definition at line 165 of file Commander.hpp.

Referenced by estimator_check(), reset_posvel_validity(), and run().

◆ _last_lpos_fail_time_us

hrt_abstime Commander::_last_lpos_fail_time_us {0}
private

Last time that the local position validity recovery check failed (usec)

Definition at line 166 of file Commander.hpp.

Referenced by estimator_check(), reset_posvel_validity(), and run().

◆ _last_lvel_fail_time_us

hrt_abstime Commander::_last_lvel_fail_time_us {0}
private

Last time that the local velocity validity recovery check failed (usec)

Definition at line 167 of file Commander.hpp.

Referenced by estimator_check(), reset_posvel_validity(), and run().

◆ _local_position_sub

uORB::SubscriptionData<vehicle_local_position_s> Commander::_local_position_sub {ORB_ID(vehicle_local_position)}
private

◆ _lpos_probation_time_us

hrt_abstime Commander::_lpos_probation_time_us = POSVEL_PROBATION_MIN
private

Definition at line 171 of file Commander.hpp.

Referenced by estimator_check(), reset_posvel_validity(), and run().

◆ _lvel_probation_time_us

hrt_abstime Commander::_lvel_probation_time_us = POSVEL_PROBATION_MIN
private

Definition at line 172 of file Commander.hpp.

Referenced by estimator_check(), reset_posvel_validity(), and run().

◆ _mission_result_sub

uORB::SubscriptionData<mission_result_s> Commander::_mission_result_sub {ORB_ID(mission_result)}
private

Definition at line 266 of file Commander.hpp.

Referenced by handle_command(), and run().

◆ _nav_test_failed

bool Commander::_nav_test_failed {false}
private

true if the post takeoff navigation test has failed

Definition at line 178 of file Commander.hpp.

Referenced by estimator_check().

◆ _nav_test_passed

bool Commander::_nav_test_passed {false}
private

true if the post takeoff navigation test has passed

Definition at line 177 of file Commander.hpp.

Referenced by estimator_check().

◆ _offboard_control_mode_sub

uORB::SubscriptionData<offboard_control_mode_s> Commander::_offboard_control_mode_sub {ORB_ID(offboard_control_mode)}
private

Definition at line 267 of file Commander.hpp.

Referenced by offboard_control_update(), and update_control_mode().

◆ _onboard_controller_lost

bool Commander::_onboard_controller_lost {false}
private

Definition at line 236 of file Commander.hpp.

Referenced by data_link_check().

◆ _parameter_update_sub

uORB::Subscription Commander::_parameter_update_sub {ORB_ID(parameter_update)}
private

Definition at line 261 of file Commander.hpp.

Referenced by run().

◆ _print_avoidance_msg_once

bool Commander::_print_avoidance_msg_once {false}
private

Definition at line 258 of file Commander.hpp.

Referenced by data_link_check().

◆ _status_pub

uORB::Publication<vehicle_status_s> Commander::_status_pub {ORB_ID(vehicle_status)}
private

Definition at line 273 of file Commander.hpp.

Referenced by run().

◆ _telemetry_status_sub

uORB::Subscription Commander::_telemetry_status_sub {ORB_ID(telemetry_status)}
private

Definition at line 231 of file Commander.hpp.

Referenced by data_link_check().

◆ _test_motor_pub

uORB::Publication<test_motor_s> Commander::_test_motor_pub {ORB_ID(test_motor)}
private

Definition at line 277 of file Commander.hpp.

Referenced by handle_command_motor_test().

◆ _time_at_takeoff

hrt_abstime Commander::_time_at_takeoff {0}
private

last time we were on the ground

Definition at line 175 of file Commander.hpp.

Referenced by estimator_check().

◆ _time_last_innov_pass

hrt_abstime Commander::_time_last_innov_pass {0}
private

last time velocity or position innovations passed

Definition at line 176 of file Commander.hpp.

Referenced by estimator_check().

◆ _vehicle_acceleration_sub

uORB::Subscription Commander::_vehicle_acceleration_sub {ORB_ID(vehicle_acceleration)}
private

Definition at line 262 of file Commander.hpp.

◆ _vehicle_status_flags_pub

uORB::Publication<vehicle_status_flags_s> Commander::_vehicle_status_flags_pub {ORB_ID(vehicle_status_flags)}
private

Definition at line 276 of file Commander.hpp.

Referenced by run().

◆ POSVEL_PROBATION_MAX

const int64_t Commander::POSVEL_PROBATION_MAX = 100_s
private

maximum probation duration (usec)

Definition at line 163 of file Commander.hpp.

Referenced by check_posvel_validity().

◆ POSVEL_PROBATION_MIN

const int64_t Commander::POSVEL_PROBATION_MIN = 1_s
private

minimum probation duration (usec)

Definition at line 162 of file Commander.hpp.

Referenced by check_posvel_validity(), and reset_posvel_validity().


The documentation for this class was generated from the following files: