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

Main state machine / business logic. More...

#include "Commander.hpp"
#include "Arming/PreFlightCheck/PreFlightCheck.hpp"
#include "Arming/ArmAuthorization/ArmAuthorization.h"
#include "Arming/HealthFlags/HealthFlags.h"
#include "accelerometer_calibration.h"
#include "airspeed_calibration.h"
#include "baro_calibration.h"
#include "calibration_routines.h"
#include "commander_helper.h"
#include "esc_calibration.h"
#include "gyro_calibration.h"
#include "mag_calibration.h"
#include "px4_custom_mode.h"
#include "rc_calibration.h"
#include "state_machine_helper.h"
#include <dataman/dataman.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_tone_alarm.h>
#include <lib/ecl/geo/geo.h>
#include <mathlib/mathlib.h>
#include <navigator/navigation.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/shutdown.h>
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/time.h>
#include <circuit_breaker/circuit_breaker.h>
#include <systemlib/mavlink_log.h>
#include <math.h>
#include <float.h>
#include <cstring>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/cpuload.h>
#include <uORB/topics/geofence_result.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/mavlink_log.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/power_button_state.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/system_power.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vtol_vehicle_status.h>
#include <uORB/topics/esc_status.h>
Include dependency graph for Commander.cpp:

Go to the source code of this file.

Macros

#define COMMANDER_MONITORING_LOOPSPERMSEC   (1/(COMMANDER_MONITORING_INTERVAL/1000.0f))
 

Typedefs

typedef enum VEHICLE_MODE_FLAG VEHICLE_MODE_FLAG
 

Enumerations

enum  VEHICLE_MODE_FLAG {
  VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, VEHICLE_MODE_FLAG_TEST_ENABLED = 2, VEHICLE_MODE_FLAG_AUTO_ENABLED = 4, VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8,
  VEHICLE_MODE_FLAG_STABILIZE_ENABLED = 16, VEHICLE_MODE_FLAG_HIL_ENABLED = 32, VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, VEHICLE_MODE_FLAG_SAFETY_ARMED = 128,
  VEHICLE_MODE_FLAG_ENUM_END = 129
}
 

Functions

__EXPORT int commander_main (int argc, char *argv[])
 The daemon app only briefly exists to start the background job. More...
 
void usage (const char *reason)
 Print the correct usage. More...
 
void control_status_leds (vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed, const uint8_t battery_warning, const cpuload_s *cpuload_local)
 
bool stabilization_required ()
 
void print_reject_mode (const char *msg)
 
void print_reject_arm (const char *msg)
 
void print_status ()
 
bool shutdown_if_allowed ()
 
transition_result_t arm_disarm (bool arm, bool run_preflight_checks, orb_advert_t *mavlink_log_pub, const char *armedBy)
 
void * commander_low_prio_loop (void *arg)
 Loop that runs at a lower rate and priority for calibration and parameter tasks. More...
 
static void answer_command (const vehicle_command_s &cmd, unsigned result, uORB::PublicationQueued< vehicle_command_ack_s > &command_ack_pub)
 
static int power_button_state_notification_cb (board_power_button_state_notification_e request)
 
static bool send_vehicle_command (uint16_t cmd, float param1=NAN, float param2=NAN)
 

Variables

static constexpr uint64_t COMMANDER_MONITORING_INTERVAL = 10_ms
 
static constexpr float STICK_ON_OFF_LIMIT = 0.9f
 
static constexpr uint64_t OFFBOARD_TIMEOUT = 500_ms
 
static constexpr uint64_t HOTPLUG_SENS_TIMEOUT = 8_s
 wait for hotplug sensors to come online for upto 8 seconds More...
 
static constexpr uint64_t PRINT_MODE_REJECT_INTERVAL = 500_ms
 
static constexpr uint64_t INAIR_RESTART_HOLDOFF_INTERVAL = 500_ms
 
static orb_advert_t mavlink_log_pub = nullptr
 
static orb_advert_t power_button_state_pub = nullptr
 
static volatile bool thread_should_exit = false
 daemon exit flag More...
 
static volatile bool thread_running = false
 daemon status flag More...
 
static hrt_abstime commander_boot_timestamp = 0
 
static unsigned int leds_counter
 
static uint64_t last_print_mode_reject_time = 0
 
static float min_stick_change = 0.25f
 
static struct vehicle_status_s status = {}
 
static struct actuator_armed_s armed = {}
 
static struct safety_s safety = {}
 
static int32_t _flight_mode_slots [manual_control_setpoint_s::MODE_SLOT_NUM]
 
static struct commander_state_s internal_state = {}
 
static uint8_t main_state_before_rtl = commander_state_s::MAIN_STATE_MAX
 
static manual_control_setpoint_s sp_man = {}
 the current manual control setpoint More...
 
static manual_control_setpoint_s _last_sp_man = {}
 the manual control setpoint valid at the last mode switch More...
 
static uint8_t _last_sp_man_arm_switch = 0
 
static struct vtol_vehicle_status_s vtol_status = {}
 
static struct cpuload_s cpuload = {}
 
static bool last_overload = false
 
static struct vehicle_status_flags_s status_flags = {}
 
static uint64_t rc_signal_lost_timestamp
 
static uint8_t arm_requirements = PreFlightCheck::ARM_REQ_NONE
 
static bool _last_condition_local_altitude_valid = false
 
static bool _last_condition_local_position_valid = false
 
static bool _last_condition_global_position_valid = false
 
static struct vehicle_land_detected_s land_detector = {}
 
static float _eph_threshold_adj
 maximum allowable horizontal position uncertainty after adjustment for flight condition More...
 
static bool _skip_pos_accuracy_check = false
 

Detailed Description

Main state machine / business logic.

This application is currently in a rewrite process. Main changes:

  • Calibration routines are moved into the event system
  • Commander is rewritten as class
  • State machines will be model driven

Definition in file Commander.cpp.

Macro Definition Documentation

◆ COMMANDER_MONITORING_LOOPSPERMSEC

#define COMMANDER_MONITORING_LOOPSPERMSEC   (1/(COMMANDER_MONITORING_INTERVAL/1000.0f))

Definition at line 113 of file Commander.cpp.

Referenced by Commander::run().

Typedef Documentation

◆ VEHICLE_MODE_FLAG

Enumeration Type Documentation

◆ VEHICLE_MODE_FLAG

Enumerator
VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED 
VEHICLE_MODE_FLAG_TEST_ENABLED 
VEHICLE_MODE_FLAG_AUTO_ENABLED 
VEHICLE_MODE_FLAG_GUIDED_ENABLED 
VEHICLE_MODE_FLAG_STABILIZE_ENABLED 
VEHICLE_MODE_FLAG_HIL_ENABLED 
VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED 
VEHICLE_MODE_FLAG_SAFETY_ARMED 
VEHICLE_MODE_FLAG_ENUM_END 

Definition at line 99 of file Commander.cpp.

Function Documentation

◆ answer_command()

void answer_command ( const vehicle_command_s cmd,
unsigned  result,
uORB::PublicationQueued< vehicle_command_ack_s > &  command_ack_pub 
)
static

Definition at line 3388 of file Commander.cpp.

References vehicle_command_s::command, hrt_absolute_time(), uORB::PublicationQueued< T >::publish(), vehicle_command_s::source_component, vehicle_command_s::source_system, vehicle_command_ack_s::timestamp, and tune_negative().

Referenced by commander_low_prio_loop(), and Commander::handle_command().

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

◆ arm_disarm()

transition_result_t arm_disarm ( bool  arm,
bool  run_preflight_checks,
orb_advert_t mavlink_log_pub,
const char *  armedBy 
)

Definition at line 529 of file Commander.cpp.

References arm_requirements, arming_state_transition(), commander_boot_timestamp, hrt_elapsed_time(), mavlink_log_info, TRANSITION_CHANGED, TRANSITION_DENIED, TRANSITION_NOT_CHANGED, and tune_negative().

Referenced by commander_main(), Commander::handle_command(), and Commander::run().

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

◆ commander_low_prio_loop()

◆ commander_main()

int commander_main ( int  argc,
char *  argv[] 
)

The daemon app only briefly exists to start the background job.

The stack size assigned in the Makefile does only apply to this management task.

The actual stack size should be set in the call to task_create().

Definition at line 272 of file Commander.cpp.

References arm_disarm(), arm_requirements, vehicle_status_flags_s::condition_local_position_valid, do_accel_calibration(), do_airspeed_calibration(), do_esc_calibration(), do_gyro_calibration(), do_level_calibration(), do_mag_calibration(), f(), main(), main_state_transition(), mavlink_log_pub, OK, PreFlightCheck::preArmCheck(), Commander::preflight_check(), print_status(), send_vehicle_command(), thread_running, thread_should_exit, TRANSITION_CHANGED, TRANSITION_DENIED, usage(), and vehicle_status_s::vehicle_type.

Here is the call graph for this function:

◆ control_status_leds()

void control_status_leds ( vehicle_status_s status_local,
const actuator_armed_s actuator_armed,
bool  changed,
const uint8_t  battery_warning,
const cpuload_s cpuload_local 
)

Definition at line 2557 of file Commander.cpp.

References actuator_armed_s::armed, vehicle_status_s::arming_state, vehicle_status_flags_s::condition_global_position_valid, vehicle_status_flags_s::condition_home_position_valid, vehicle_status_flags_s::condition_system_hotplug_timeout, vehicle_status_flags_s::condition_system_sensors_initialized, vehicle_status_s::failsafe, hrt_absolute_time(), hrt_abstime, hrt_elapsed_time(), last_overload, leds_counter, cpuload_s::load, cpuload_s::ram_usage, actuator_armed_s::ready_to_arm, and rgbled_set_color_and_mode().

Referenced by Commander::run().

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

◆ power_button_state_notification_cb()

static int power_button_state_notification_cb ( board_power_button_state_notification_e  request)
static

Definition at line 211 of file Commander.cpp.

References hrt_absolute_time(), ORB_ID, orb_publish(), power_button_state_pub, and power_button_state_s::timestamp.

Referenced by Commander::run().

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

◆ print_reject_arm()

void print_reject_arm ( const char *  msg)

Definition at line 3377 of file Commander.cpp.

References hrt_absolute_time(), hrt_abstime, last_print_mode_reject_time, mavlink_log_critical, mavlink_log_pub, PRINT_MODE_REJECT_INTERVAL, and tune_negative().

Referenced by Commander::run().

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

◆ print_reject_mode()

void print_reject_mode ( const char *  msg)

Definition at line 3362 of file Commander.cpp.

References actuator_armed_s::armed, hrt_absolute_time(), hrt_abstime, last_print_mode_reject_time, mavlink_log_critical, mavlink_log_pub, PRINT_MODE_REJECT_INTERVAL, and tune_negative().

Referenced by Commander::handle_command(), and Commander::set_main_state_rc().

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

◆ print_status()

void print_status ( )

Definition at line 517 of file Commander.cpp.

References vehicle_status_s::arming_state, and arming_state_names.

Referenced by commander_main(), land_detector::LandDetector::custom_command(), and EstimatorInterface::get_baro_sample_delayed().

Here is the caller graph for this function:

◆ send_vehicle_command()

static bool send_vehicle_command ( uint16_t  cmd,
float  param1 = NAN,
float  param2 = NAN 
)
static

Definition at line 251 of file Commander.cpp.

References vehicle_status_s::component_id, hrt_absolute_time(), ORB_ID, vehicle_status_s::system_id, and vehicle_command_s::timestamp.

Referenced by commander_main().

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

◆ shutdown_if_allowed()

bool shutdown_if_allowed ( )

Definition at line 522 of file Commander.cpp.

References arm_requirements, arming_state_transition(), commander_boot_timestamp, hrt_elapsed_time(), mavlink_log_pub, and TRANSITION_DENIED.

Referenced by Commander::battery_status_check(), and Commander::run().

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

◆ stabilization_required()

bool stabilization_required ( )

Definition at line 3352 of file Commander.cpp.

References vehicle_status_s::vehicle_type, VEHICLE_TYPE_FIXED_WING, vehicle_status_s::vtol_fw_permanent_stab, and vtol_vehicle_status_s::vtol_in_trans_mode.

Referenced by Commander::update_control_mode().

Here is the caller graph for this function:

◆ usage()

static __END_DECLS void usage ( const char *  reason)

Print the correct usage.

Definition at line 4238 of file Commander.cpp.

Referenced by commander_main(), and px4flow::start().

Here is the caller graph for this function:

Variable Documentation

◆ _eph_threshold_adj

float _eph_threshold_adj
static
Initial value:
=
INFINITY

maximum allowable horizontal position uncertainty after adjustment for flight condition

Definition at line 167 of file Commander.cpp.

Referenced by Commander::estimator_check(), and Commander::reset_posvel_validity().

◆ _flight_mode_slots

int32_t _flight_mode_slots[manual_control_setpoint_s::MODE_SLOT_NUM]
static

Definition at line 141 of file Commander.cpp.

Referenced by Commander::run(), and Commander::set_main_state_rc().

◆ _last_condition_global_position_valid

bool _last_condition_global_position_valid = false
static

◆ _last_condition_local_altitude_valid

bool _last_condition_local_altitude_valid = false
static

Definition at line 161 of file Commander.cpp.

Referenced by Commander::run(), and Commander::set_main_state_rc().

◆ _last_condition_local_position_valid

bool _last_condition_local_position_valid = false
static

Definition at line 162 of file Commander.cpp.

Referenced by Commander::run(), and Commander::set_main_state_rc().

◆ _last_sp_man

manual_control_setpoint_s _last_sp_man = {}
static

the manual control setpoint valid at the last mode switch

Definition at line 147 of file Commander.cpp.

◆ _last_sp_man_arm_switch

uint8_t _last_sp_man_arm_switch = 0
static

Definition at line 148 of file Commander.cpp.

Referenced by Commander::run().

◆ _skip_pos_accuracy_check

bool _skip_pos_accuracy_check = false
static

Definition at line 169 of file Commander.cpp.

Referenced by Commander::estimator_check(), and Commander::reset_posvel_validity().

◆ arm_requirements

◆ armed

◆ commander_boot_timestamp

◆ COMMANDER_MONITORING_INTERVAL

constexpr uint64_t COMMANDER_MONITORING_INTERVAL = 10_ms
static

Definition at line 112 of file Commander.cpp.

Referenced by Commander::run().

◆ cpuload

◆ HOTPLUG_SENS_TIMEOUT

constexpr uint64_t HOTPLUG_SENS_TIMEOUT = 8_s
static

wait for hotplug sensors to come online for upto 8 seconds

Definition at line 118 of file Commander.cpp.

Referenced by Commander::run().

◆ INAIR_RESTART_HOLDOFF_INTERVAL

constexpr uint64_t INAIR_RESTART_HOLDOFF_INTERVAL = 500_ms
static

Definition at line 120 of file Commander.cpp.

Referenced by Commander::handle_command(), and Commander::run().

◆ internal_state

struct commander_state_s internal_state = {}
static

Definition at line 142 of file Commander.cpp.

◆ land_detector

struct vehicle_land_detected_s land_detector = {}
static

Definition at line 165 of file Commander.cpp.

◆ last_overload

bool last_overload = false
static

Definition at line 153 of file Commander.cpp.

Referenced by control_status_leds().

◆ last_print_mode_reject_time

uint64_t last_print_mode_reject_time = 0
static

Definition at line 134 of file Commander.cpp.

Referenced by print_reject_arm(), and print_reject_mode().

◆ leds_counter

unsigned int leds_counter
static

Definition at line 132 of file Commander.cpp.

Referenced by control_status_leds().

◆ main_state_before_rtl

uint8_t main_state_before_rtl = commander_state_s::MAIN_STATE_MAX
static

Definition at line 144 of file Commander.cpp.

Referenced by Commander::run().

◆ mavlink_log_pub

◆ min_stick_change

float min_stick_change = 0.25f
static

Definition at line 136 of file Commander.cpp.

Referenced by Commander::run().

◆ OFFBOARD_TIMEOUT

constexpr uint64_t OFFBOARD_TIMEOUT = 500_ms
static

Definition at line 117 of file Commander.cpp.

Referenced by Commander::offboard_control_update().

◆ power_button_state_pub

orb_advert_t power_button_state_pub = nullptr
static

Definition at line 124 of file Commander.cpp.

Referenced by power_button_state_notification_cb(), and Commander::run().

◆ PRINT_MODE_REJECT_INTERVAL

constexpr uint64_t PRINT_MODE_REJECT_INTERVAL = 500_ms
static

Definition at line 119 of file Commander.cpp.

Referenced by print_reject_arm(), and print_reject_mode().

◆ rc_signal_lost_timestamp

uint64_t rc_signal_lost_timestamp
static

Definition at line 157 of file Commander.cpp.

Referenced by Commander::run().

◆ safety

struct safety_s safety = {}
static

Definition at line 140 of file Commander.cpp.

Referenced by PX4IO::io_handle_status(), SafetyButton::Run(), and BlinkM::Run().

◆ sp_man

manual_control_setpoint_s sp_man = {}
static

the current manual control setpoint

Definition at line 146 of file Commander.cpp.

Referenced by Commander::set_main_state_rc().

◆ status

struct vehicle_status_s status = {}
static

Definition at line 138 of file Commander.cpp.

Referenced by RM3100::check_measurement(), MEASAirspeed::collect(), LSM303AGR::collect(), LPS25H::collect(), BMM150::collect(), GPSDriverAshtech::configure(), PreFlightCheck::ekf2Check(), F_write_byte(), free_getprogmeminfo(), frsky_telemetry_thread_main(), Battery::full_cell_voltage(), Ekf::get_quat_reset(), uORB::FastRpcChannel::GetInstance(), MavlinkReceiver::handle_message_cellular_status(), MavlinkReceiver::handle_message_radio_status(), PX4IO::io_handle_status(), EstimatorInterface::isVehicleAtRest(), FXAS21002C::measure(), BMI088_accel::measure(), L3GD20::measure(), BMI160::measure(), LSM303D::measureAccelerometer(), LSM303D::measureMagnetometer(), MK::mk_servo_arm(), GPSDriverUBX::payloadRxDone(), pwmin_tim_isr(), IRLOCK::read_device_block(), IRLOCK::read_device_word(), MicroBenchORB::MicroBenchORB::reset(), Ekf2::Run(), FXOS8701CQ::Run(), ADIS16448::self_test(), MavlinkStreamSysStatus::send(), MavlinkStreamAttitudeQuaternion::send(), MavlinkStreamHILActuatorControls::send(), MavlinkStreamCameraCapture::send(), MavlinkStreamExtendedSysState::send(), uORB::KraitFastRpcChannel::send_message(), GPSDriverAshtech::sendSurveyInStatusUpdate(), RateControl::setFeedForwardGain(), BMP388::soft_reset(), StreamListItem::StreamListItem(), Mavlink::task_main(), TEST_F(), MicroBenchORB::MicroBenchORB::time_px4_uorb_direct(), TemperatureCalibrationCommon< 3, 3 >::update(), MavlinkStreamHighLatency2::update_vehicle_status(), and MicroBenchORB::ut_declare_test_c().

◆ status_flags

struct vehicle_status_flags_s status_flags = {}
static

Definition at line 155 of file Commander.cpp.

Referenced by Commander::run().

◆ STICK_ON_OFF_LIMIT

constexpr float STICK_ON_OFF_LIMIT = 0.9f
static

Definition at line 115 of file Commander.cpp.

Referenced by Commander::run().

◆ thread_running

volatile bool thread_running = false
static

daemon status flag

Definition at line 128 of file Commander.cpp.

Referenced by commander_main(), and Commander::run().

◆ thread_should_exit

volatile bool thread_should_exit = false
static

daemon exit flag

Definition at line 127 of file Commander.cpp.

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

◆ vtol_status

struct vtol_vehicle_status_s vtol_status = {}
static

Definition at line 150 of file Commander.cpp.