PX4 Firmware
PX4 Autopilot Software http://px4.io
|
#include <vtol_att_control_main.h>
Static Public Member Functions | |
static int | task_spawn (int argc, char *argv[]) |
static int | custom_command (int argc, char *argv[]) |
static int | print_usage (const char *reason=nullptr) |
Private Member Functions | |
void | vehicle_cmd_poll () |
Check for command updates. More... | |
int | parameters_update () |
void | handle_command () |
Check received command. More... | |
Definition at line 89 of file vtol_att_control_main.h.
VtolAttitudeControl::VtolAttitudeControl | ( | ) |
Definition at line 55 of file vtol_att_control_main.cpp.
References _params, _params_handles, _vtol_type, _vtol_vehicle_status, Params::idle_pwm_mc, param_find(), parameters_update(), PWM_DEFAULT_MIN, STANDARD, TAILSITTER, TILTROTOR, vtol_vehicle_status_s::vtol_in_rw_mode, Params::vtol_motor_id, and Params::vtol_type.
Referenced by task_spawn().
VtolAttitudeControl::~VtolAttitudeControl | ( | ) |
Definition at line 106 of file vtol_att_control_main.cpp.
References _loop_perf, and perf_free().
void VtolAttitudeControl::abort_front_transition | ( | const char * | reason | ) |
Definition at line 200 of file vtol_att_control_main.cpp.
References _abort_front_transition, _mavlink_log_pub, _vtol_vehicle_status, mavlink_log_critical, and vtol_vehicle_status_s::vtol_transition_failsafe.
Referenced by VtolType::check_quadchute_condition(), and Standard::update_transition_state().
|
static |
Definition at line 460 of file vtol_att_control_main.cpp.
References print_usage().
|
inline |
Definition at line 115 of file vtol_att_control_main.h.
References _actuators_fw_in.
Referenced by VtolType::VtolType().
|
inline |
Definition at line 116 of file vtol_att_control_main.h.
References _actuators_mc_in.
Referenced by VtolType::VtolType().
|
inline |
Definition at line 117 of file vtol_att_control_main.h.
References _actuators_out_0.
Referenced by VtolType::VtolType().
|
inline |
Definition at line 118 of file vtol_att_control_main.h.
References _actuators_out_1.
Referenced by VtolType::VtolType().
|
inline |
Definition at line 119 of file vtol_att_control_main.h.
References _airspeed_validated.
Referenced by VtolType::VtolType().
|
inline |
Definition at line 122 of file vtol_att_control_main.h.
References _v_att.
Referenced by VtolType::VtolType().
|
inline |
Definition at line 123 of file vtol_att_control_main.h.
References _v_att_sp.
Referenced by VtolType::VtolType().
|
inline |
Definition at line 126 of file vtol_att_control_main.h.
References _v_control_mode.
Referenced by VtolType::VtolType().
|
inline |
Definition at line 124 of file vtol_att_control_main.h.
References _fw_virtual_att_sp.
Referenced by VtolType::VtolType().
|
inline |
Definition at line 127 of file vtol_att_control_main.h.
References _land_detected.
Referenced by VtolType::VtolType().
|
inline |
Definition at line 128 of file vtol_att_control_main.h.
References _local_pos.
Referenced by VtolType::VtolType().
|
inline |
Definition at line 129 of file vtol_att_control_main.h.
References _local_pos_sp.
Referenced by VtolType::VtolType().
|
inline |
Definition at line 125 of file vtol_att_control_main.h.
References _mc_virtual_att_sp.
Referenced by VtolType::VtolType().
|
inline |
Definition at line 132 of file vtol_att_control_main.h.
References _params.
Referenced by VtolType::VtolType().
|
inline |
Definition at line 120 of file vtol_att_control_main.h.
References _pos_sp_triplet.
Referenced by Standard::update_mc_state().
|
inline |
Definition at line 121 of file vtol_att_control_main.h.
References _tecs_status.
Referenced by VtolType::VtolType().
|
inline |
Definition at line 130 of file vtol_att_control_main.h.
References _vtol_vehicle_status.
Referenced by Standard::update_mc_state(), and VtolType::VtolType().
|
private |
Check received command.
Definition at line 143 of file vtol_att_control_main.cpp.
References _transition_command, _vehicle_cmd, vehicle_command_s::command, vehicle_command_s::from_external, hrt_absolute_time(), ORB_ID, vehicle_command_s::param1, vehicle_command_s::source_component, vehicle_command_s::source_system, and vehicle_command_ack_s::timestamp.
Referenced by vehicle_cmd_poll().
bool VtolAttitudeControl::init | ( | ) |
Definition at line 112 of file vtol_att_control_main.cpp.
References _actuator_inputs_fw, _actuator_inputs_mc, and uORB::SubscriptionCallback::registerCallback().
Referenced by task_spawn().
bool VtolAttitudeControl::is_fixed_wing_requested | ( | ) |
Definition at line 171 of file vtol_att_control_main.cpp.
References _abort_front_transition, _manual_control_sp, _transition_command, _v_control_mode, _vtol_vehicle_status, vehicle_control_mode_s::flag_control_manual_enabled, manual_control_setpoint_s::transition_switch, and vtol_vehicle_status_s::vtol_transition_failsafe.
Referenced by Tiltrotor::update_vtol_state(), Tailsitter::update_vtol_state(), and Standard::update_vtol_state().
|
private |
Definition at line 210 of file vtol_att_control_main.cpp.
References _params, _params_handles, _vtol_type, _vtol_vehicle_status, Params::airspeed_blend, Params::airspeed_disabled, Params::back_trans_duration, Params::back_trans_throttle, math::constrain(), Params::diff_thrust, Params::diff_thrust_scale, Params::elevons_mc_lock, f(), Params::front_trans_duration, Params::front_trans_throttle, Params::front_trans_time_min, Params::front_trans_time_openloop, Params::front_trans_timeout, Params::fw_alt_err, Params::fw_min_alt, Params::fw_motors_off, vtol_vehicle_status_s::fw_permanent_stab, Params::fw_qc_max_pitch, Params::fw_qc_max_roll, Params::idle_pwm_mc, math::min(), Params::mpc_xy_cruise, OK, param_get(), VtolType::parameters_update(), Params::transition_airspeed, Params::vtol_motor_id, and Params::vtol_type.
Referenced by Run(), and VtolAttitudeControl().
|
override |
Definition at line 466 of file vtol_att_control_main.cpp.
References _loop_perf, and perf_print_counter().
|
static |
Definition at line 474 of file vtol_att_control_main.cpp.
Referenced by custom_command().
|
override |
Definition at line 287 of file vtol_att_control_main.cpp.
References _actuator_inputs_fw, _actuator_inputs_mc, _actuators_0_pub, _actuators_1_pub, _actuators_fw_in, _actuators_mc_in, _actuators_out_0, _actuators_out_1, _airspeed_validated, _airspeed_validated_sub, _fw_virtual_att_sp, _fw_virtual_att_sp_sub, _initialized, _land_detected, _land_detected_sub, _local_pos, _local_pos_sp, _local_pos_sp_sub, _local_pos_sub, _loop_perf, _manual_control_sp, _manual_control_sp_sub, _mc_virtual_att_sp, _mc_virtual_att_sp_sub, _parameter_update_sub, _pos_sp_triplet, _pos_sp_triplet_sub, _tecs_status, _tecs_status_sub, _transition_command, _v_att, _v_att_sp, _v_att_sp_pub, _v_att_sub, _v_control_mode, _v_control_mode_sub, _vtol_type, _vtol_vehicle_status, _vtol_vehicle_status_pub, uORB::Subscription::copy(), VtolType::fill_actuator_outputs(), FIXED_WING, vehicle_control_mode_s::flag_control_manual_enabled, VtolType::get_mode(), hrt_absolute_time(), vtol_vehicle_status_s::in_transition_to_fw, VtolType::init(), parameters_update(), perf_begin(), perf_end(), uORB::Publication< T >::publish(), ROTARY_WING, vtol_vehicle_status_s::timestamp, TRANSITION_TO_FW, TRANSITION_TO_MC, uORB::SubscriptionCallback::unregisterCallback(), uORB::SubscriptionInterval::update(), uORB::Subscription::update(), VtolType::update_fw_state(), VtolType::update_mc_state(), VtolType::update_transition_state(), VtolType::update_vtol_state(), uORB::Subscription::updated(), vehicle_cmd_poll(), vtol_vehicle_status_s::vtol_in_rw_mode, and vtol_vehicle_status_s::vtol_in_trans_mode.
|
static |
Definition at line 436 of file vtol_att_control_main.cpp.
References init(), ll40ls::instance, and VtolAttitudeControl().
|
private |
Check for command updates.
Definition at line 131 of file vtol_att_control_main.cpp.
References _vehicle_cmd, _vehicle_cmd_sub, uORB::Subscription::copy(), handle_command(), and uORB::Subscription::updated().
Referenced by Run().
|
private |
Definition at line 213 of file vtol_att_control_main.h.
Referenced by abort_front_transition(), and is_fixed_wing_requested().
|
private |
Definition at line 136 of file vtol_att_control_main.h.
|
private |
Definition at line 137 of file vtol_att_control_main.h.
|
private |
Definition at line 153 of file vtol_att_control_main.h.
Referenced by Run().
|
private |
Definition at line 154 of file vtol_att_control_main.h.
Referenced by Run().
|
private |
Definition at line 164 of file vtol_att_control_main.h.
Referenced by get_actuators_fw_in(), and Run().
|
private |
Definition at line 165 of file vtol_att_control_main.h.
Referenced by get_actuators_mc_in(), and Run().
|
private |
Definition at line 166 of file vtol_att_control_main.h.
Referenced by get_actuators_out0(), and Run().
|
private |
Definition at line 167 of file vtol_att_control_main.h.
Referenced by get_actuators_out1(), and Run().
|
private |
Definition at line 169 of file vtol_att_control_main.h.
Referenced by get_airspeed(), and Run().
|
private |
Definition at line 139 of file vtol_att_control_main.h.
Referenced by Run().
|
private |
Definition at line 161 of file vtol_att_control_main.h.
Referenced by get_fw_virtual_att_sp(), and Run().
|
private |
Definition at line 140 of file vtol_att_control_main.h.
Referenced by Run().
|
private |
Definition at line 217 of file vtol_att_control_main.h.
Referenced by Run().
|
private |
Definition at line 176 of file vtol_att_control_main.h.
Referenced by get_land_detected(), and Run().
|
private |
Definition at line 141 of file vtol_att_control_main.h.
Referenced by Run().
|
private |
Definition at line 177 of file vtol_att_control_main.h.
Referenced by get_local_pos(), and Run().
|
private |
Definition at line 178 of file vtol_att_control_main.h.
Referenced by get_local_pos_sp(), and Run().
|
private |
Definition at line 142 of file vtol_att_control_main.h.
Referenced by Run().
|
private |
Definition at line 143 of file vtol_att_control_main.h.
Referenced by Run().
|
private |
loop performance counter
Definition at line 219 of file vtol_att_control_main.h.
Referenced by print_status(), Run(), and ~VtolAttitudeControl().
|
private |
Definition at line 170 of file vtol_att_control_main.h.
Referenced by is_fixed_wing_requested(), and Run().
|
private |
Definition at line 144 of file vtol_att_control_main.h.
Referenced by Run().
|
private |
Definition at line 158 of file vtol_att_control_main.h.
Referenced by abort_front_transition().
|
private |
Definition at line 162 of file vtol_att_control_main.h.
Referenced by get_mc_virtual_att_sp(), and Run().
|
private |
Definition at line 145 of file vtol_att_control_main.h.
Referenced by Run().
|
private |
Definition at line 146 of file vtol_att_control_main.h.
Referenced by Run().
|
private |
Definition at line 181 of file vtol_att_control_main.h.
Referenced by get_params(), parameters_update(), and VtolAttitudeControl().
struct { ... } VtolAttitudeControl::_params_handles |
Referenced by parameters_update(), and VtolAttitudeControl().
|
private |
Definition at line 171 of file vtol_att_control_main.h.
Referenced by get_pos_sp_triplet(), and Run().
|
private |
Definition at line 147 of file vtol_att_control_main.h.
Referenced by Run().
|
private |
Definition at line 172 of file vtol_att_control_main.h.
Referenced by get_tecs_status(), and Run().
|
private |
Definition at line 148 of file vtol_att_control_main.h.
Referenced by Run().
|
private |
Definition at line 212 of file vtol_att_control_main.h.
Referenced by handle_command(), is_fixed_wing_requested(), and Run().
|
private |
Definition at line 173 of file vtol_att_control_main.h.
|
private |
Definition at line 160 of file vtol_att_control_main.h.
Referenced by get_att_sp(), and Run().
|
private |
Definition at line 155 of file vtol_att_control_main.h.
Referenced by Run().
|
private |
Definition at line 149 of file vtol_att_control_main.h.
Referenced by Run().
|
private |
Definition at line 175 of file vtol_att_control_main.h.
Referenced by get_control_mode(), is_fixed_wing_requested(), and Run().
|
private |
Definition at line 150 of file vtol_att_control_main.h.
Referenced by Run().
|
private |
Definition at line 174 of file vtol_att_control_main.h.
Referenced by handle_command(), and vehicle_cmd_poll().
|
private |
Definition at line 151 of file vtol_att_control_main.h.
Referenced by vehicle_cmd_poll().
|
private |
Definition at line 215 of file vtol_att_control_main.h.
Referenced by parameters_update(), Run(), and VtolAttitudeControl().
|
private |
Definition at line 179 of file vtol_att_control_main.h.
Referenced by abort_front_transition(), get_vtol_vehicle_status(), is_fixed_wing_requested(), parameters_update(), Run(), and VtolAttitudeControl().
|
private |
Definition at line 156 of file vtol_att_control_main.h.
Referenced by Run().
param_t VtolAttitudeControl::airspeed_blend |
Definition at line 200 of file vtol_att_control_main.h.
param_t VtolAttitudeControl::airspeed_mode |
Definition at line 201 of file vtol_att_control_main.h.
param_t VtolAttitudeControl::back_trans_duration |
Definition at line 196 of file vtol_att_control_main.h.
param_t VtolAttitudeControl::back_trans_throttle |
Definition at line 199 of file vtol_att_control_main.h.
param_t VtolAttitudeControl::diff_thrust |
Definition at line 205 of file vtol_att_control_main.h.
param_t VtolAttitudeControl::diff_thrust_scale |
Definition at line 206 of file vtol_att_control_main.h.
param_t VtolAttitudeControl::elevons_mc_lock |
Definition at line 188 of file vtol_att_control_main.h.
param_t VtolAttitudeControl::front_trans_duration |
Definition at line 195 of file vtol_att_control_main.h.
param_t VtolAttitudeControl::front_trans_throttle |
Definition at line 198 of file vtol_att_control_main.h.
param_t VtolAttitudeControl::front_trans_time_min |
Definition at line 194 of file vtol_att_control_main.h.
param_t VtolAttitudeControl::front_trans_time_openloop |
Definition at line 193 of file vtol_att_control_main.h.
param_t VtolAttitudeControl::front_trans_timeout |
Definition at line 202 of file vtol_att_control_main.h.
param_t VtolAttitudeControl::fw_alt_err |
Definition at line 190 of file vtol_att_control_main.h.
param_t VtolAttitudeControl::fw_min_alt |
Definition at line 189 of file vtol_att_control_main.h.
param_t VtolAttitudeControl::fw_motors_off |
Definition at line 204 of file vtol_att_control_main.h.
param_t VtolAttitudeControl::fw_qc_max_pitch |
Definition at line 191 of file vtol_att_control_main.h.
param_t VtolAttitudeControl::fw_qc_max_roll |
Definition at line 192 of file vtol_att_control_main.h.
param_t VtolAttitudeControl::idle_pwm_mc |
Definition at line 184 of file vtol_att_control_main.h.
param_t VtolAttitudeControl::mpc_xy_cruise |
Definition at line 203 of file vtol_att_control_main.h.
param_t VtolAttitudeControl::transition_airspeed |
Definition at line 197 of file vtol_att_control_main.h.
param_t VtolAttitudeControl::vtol_fw_permanent_stab |
Definition at line 186 of file vtol_att_control_main.h.
param_t VtolAttitudeControl::vtol_motor_id |
Definition at line 185 of file vtol_att_control_main.h.
param_t VtolAttitudeControl::vtol_type |
Definition at line 187 of file vtol_att_control_main.h.