PX4 Firmware
PX4 Autopilot Software http://px4.io
|
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>
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 |
Main state machine / business logic.
This application is currently in a rewrite process. Main changes:
Definition in file Commander.cpp.
#define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f)) |
Definition at line 113 of file Commander.cpp.
Referenced by Commander::run().
typedef enum VEHICLE_MODE_FLAG VEHICLE_MODE_FLAG |
enum VEHICLE_MODE_FLAG |
Definition at line 99 of file Commander.cpp.
|
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().
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().
void * commander_low_prio_loop | ( | void * | arg | ) |
Loop that runs at a lower rate and priority for calibration and parameter tasks.
Definition at line 3428 of file Commander.cpp.
References answer_command(), arm_requirements, arming_state_transition(), vehicle_command_s::command, commander_boot_timestamp, vehicle_status_flags_s::condition_calibration_enabled, do_accel_calibration(), do_airspeed_calibration(), do_esc_calibration(), do_gyro_calibration(), do_level_calibration(), do_mag_calibration(), do_trim_calibration(), hrt_elapsed_time(), is_safe(), mavlink_log_critical, mavlink_log_info, mavlink_log_pub, OK, orb_copy(), ORB_ID, orb_subscribe(), vehicle_command_s::param1, vehicle_command_s::param2, vehicle_command_s::param3, vehicle_command_s::param4, vehicle_command_s::param5, vehicle_command_s::param6, vehicle_command_s::param7, param_load_default(), param_reset_all(), param_save_default(), Commander::preflight_check(), px4_close(), px4_poll(), vehicle_status_flags_s::rc_input_blocked, thread_should_exit, TRANSITION_DENIED, tune_negative(), tune_positive(), and warn.
Referenced by Commander::run().
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.
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().
|
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().
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().
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().
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().
|
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().
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().
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().
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().
|
static |
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().
|
static |
Definition at line 141 of file Commander.cpp.
Referenced by Commander::run(), and Commander::set_main_state_rc().
|
static |
Definition at line 163 of file Commander.cpp.
Referenced by Commander::estimator_check(), Commander::run(), and Commander::set_main_state_rc().
|
static |
Definition at line 161 of file Commander.cpp.
Referenced by Commander::run(), and Commander::set_main_state_rc().
|
static |
Definition at line 162 of file Commander.cpp.
Referenced by Commander::run(), and Commander::set_main_state_rc().
|
static |
the manual control setpoint valid at the last mode switch
Definition at line 147 of file Commander.cpp.
|
static |
Definition at line 148 of file Commander.cpp.
Referenced by Commander::run().
|
static |
Definition at line 169 of file Commander.cpp.
Referenced by Commander::estimator_check(), and Commander::reset_posvel_validity().
|
static |
Definition at line 159 of file Commander.cpp.
Referenced by arm_disarm(), commander_low_prio_loop(), commander_main(), Commander::preflight_check(), Commander::run(), and shutdown_if_allowed().
|
static |
Definition at line 139 of file Commander.cpp.
Referenced by Simulator::actuator_controls_from_outputs(), StateMachineHelperTest::armingStateTransitionTest(), SafetyButton::FlashButton(), Battery::full_cell_voltage(), Simulator::handle_message_hil_sensor(), PX4IO::io_set_arming_state(), AirspeedModule::Run(), UavcanServers::run(), UavcanNode::Run(), MavlinkStreamVFRHUD::send(), Takeoff::setSpoolupTime(), and px4::logger::Logger::start_stop_logging().
|
static |
Definition at line 130 of file Commander.cpp.
Referenced by arm_disarm(), commander_low_prio_loop(), Commander::handle_command(), Commander::preflight_check(), Commander::run(), and shutdown_if_allowed().
|
static |
Definition at line 112 of file Commander.cpp.
Referenced by Commander::run().
|
static |
Definition at line 151 of file Commander.cpp.
Referenced by load_mon::LoadMon::_cpuload(), events::status::StatusDisplay::check_for_updates(), load_mon::LoadMon::custom_command(), MavlinkStreamSysStatus::send(), and events::SubscriberHandler::subscribe().
|
static |
wait for hotplug sensors to come online for upto 8 seconds
Definition at line 118 of file Commander.cpp.
Referenced by Commander::run().
|
static |
Definition at line 120 of file Commander.cpp.
Referenced by Commander::handle_command(), and Commander::run().
|
static |
Definition at line 142 of file Commander.cpp.
|
static |
Definition at line 165 of file Commander.cpp.
|
static |
Definition at line 153 of file Commander.cpp.
Referenced by control_status_leds().
|
static |
Definition at line 134 of file Commander.cpp.
Referenced by print_reject_arm(), and print_reject_mode().
|
static |
Definition at line 132 of file Commander.cpp.
Referenced by control_status_leds().
|
static |
Definition at line 144 of file Commander.cpp.
Referenced by Commander::run().
|
static |
Definition at line 123 of file Commander.cpp.
Referenced by Commander::battery_status_check(), commander_low_prio_loop(), commander_main(), Commander::data_link_check(), Commander::esc_status_check(), Commander::estimator_check(), Commander::handle_command(), Commander::preflight_check(), print_reject_arm(), print_reject_mode(), Commander::run(), and shutdown_if_allowed().
|
static |
Definition at line 136 of file Commander.cpp.
Referenced by Commander::run().
|
static |
Definition at line 117 of file Commander.cpp.
Referenced by Commander::offboard_control_update().
|
static |
Definition at line 124 of file Commander.cpp.
Referenced by power_button_state_notification_cb(), and Commander::run().
|
static |
Definition at line 119 of file Commander.cpp.
Referenced by print_reject_arm(), and print_reject_mode().
|
static |
Definition at line 157 of file Commander.cpp.
Referenced by Commander::run().
|
static |
Definition at line 140 of file Commander.cpp.
Referenced by PX4IO::io_handle_status(), SafetyButton::Run(), and BlinkM::Run().
|
static |
the current manual control setpoint
Definition at line 146 of file Commander.cpp.
Referenced by Commander::set_main_state_rc().
|
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().
|
static |
Definition at line 155 of file Commander.cpp.
Referenced by Commander::run().
|
static |
Definition at line 115 of file Commander.cpp.
Referenced by Commander::run().
|
static |
daemon status flag
Definition at line 128 of file Commander.cpp.
Referenced by commander_main(), and Commander::run().
|
static |
daemon exit flag
Definition at line 127 of file Commander.cpp.
Referenced by commander_low_prio_loop(), commander_main(), and Commander::run().
|
static |
Definition at line 150 of file Commander.cpp.