PX4 Firmware
PX4 Autopilot Software http://px4.io
|
#include <vtol_type.h>
Classes | |
struct | _disarmed_pwm_values |
struct | _max_mc_pwm_values |
struct | _min_mc_pwm_values |
Stores the max pwm values given by the system. More... | |
Public Member Functions | |
VtolType (VtolAttitudeControl *att_controller) | |
VtolType (const VtolType &)=delete | |
VtolType & | operator= (const VtolType &)=delete |
virtual | ~VtolType ()=default |
bool | init () |
Initialise. More... | |
virtual void | update_vtol_state ()=0 |
Update vtol state. More... | |
virtual void | update_transition_state ()=0 |
Update transition state. More... | |
virtual void | update_mc_state () |
Update multicopter state. More... | |
virtual void | update_fw_state () |
Update fixed wing state. More... | |
virtual void | fill_actuator_outputs ()=0 |
Write control values to actuator output topics. More... | |
virtual void | waiting_on_tecs () |
Special handling opportunity for the time right after transition to FW before TECS is running. More... | |
void | check_quadchute_condition () |
Checks for fixed-wing failsafe condition and issues abort request if needed. More... | |
bool | can_transition_on_ground () |
Returns true if we're allowed to do a mode transition on the ground. More... | |
mode | get_mode () |
virtual void | parameters_update ()=0 |
Protected Member Functions | |
bool | set_idle_mc () |
Sets mc motor minimum pwm to VT_IDLE_PWM_MC which ensures that they are spinning in mc mode. More... | |
bool | set_idle_fw () |
Sets mc motor minimum pwm to PWM_MIN which ensures that the motors stop spinning on zero throttle in fw mode. More... | |
motor_state | set_motor_state (const motor_state current_state, const motor_state next_state, const int value=0) |
Sets state of a selection of motors, see struct motor_state. More... | |
Static Protected Attributes | |
static constexpr const int | num_outputs_max = 8 |
Private Member Functions | |
bool | apply_pwm_limits (struct pwm_output_values &pwm_values, pwm_limit_type type) |
Adjust minimum/maximum pwm values for the output channels. More... | |
bool | is_channel_set (const int channel, const int target) |
Determines if channel is set in target. More... | |
Definition at line 110 of file vtol_type.h.
VtolType::VtolType | ( | VtolAttitudeControl * | att_controller | ) |
Definition at line 50 of file vtol_type.cpp.
References _actuators_fw_in, _actuators_mc_in, _actuators_out_0, _actuators_out_1, _airspeed_validated, _attc, _fw_virtual_att_sp, _land_detected, _local_pos, _local_pos_sp, _mc_virtual_att_sp, _params, _tecs_status, _v_att, _v_att_sp, _v_control_mode, _vtol_vehicle_status, VtolAttitudeControl::get_actuators_fw_in(), VtolAttitudeControl::get_actuators_mc_in(), VtolAttitudeControl::get_actuators_out0(), VtolAttitudeControl::get_actuators_out1(), VtolAttitudeControl::get_airspeed(), VtolAttitudeControl::get_att(), VtolAttitudeControl::get_att_sp(), VtolAttitudeControl::get_control_mode(), VtolAttitudeControl::get_fw_virtual_att_sp(), VtolAttitudeControl::get_land_detected(), VtolAttitudeControl::get_local_pos(), VtolAttitudeControl::get_local_pos_sp(), VtolAttitudeControl::get_mc_virtual_att_sp(), VtolAttitudeControl::get_params(), VtolAttitudeControl::get_tecs_status(), VtolAttitudeControl::get_vtol_vehicle_status(), PWM_DEFAULT_MAX, PWM_MOTOR_OFF, and ROTARY_WING.
|
delete |
|
virtualdefault |
|
private |
Adjust minimum/maximum pwm values for the output channels.
pwm_output_values | Struct containing the limit values for each channel | |
[in] | type | Specifies if min or max limits are adjusted. |
Definition at line 282 of file vtol_type.cpp.
References fd, OK, PWM_OUTPUT0_DEVICE_PATH, PWM_SERVO_SET_MAX_PWM, PWM_SERVO_SET_MIN_PWM, px4_close(), px4_ioctl(), px4_open(), and TYPE_MINIMUM.
Referenced by set_idle_fw(), set_idle_mc(), and set_motor_state().
bool VtolType::can_transition_on_ground | ( | ) |
Returns true if we're allowed to do a mode transition on the ground.
Definition at line 181 of file vtol_type.cpp.
References _land_detected, _v_control_mode, vehicle_control_mode_s::flag_armed, and vehicle_land_detected_s::landed.
Referenced by Tiltrotor::update_vtol_state(), Tailsitter::update_vtol_state(), and Standard::update_vtol_state().
void VtolType::check_quadchute_condition | ( | ) |
Checks for fixed-wing failsafe condition and issues abort request if needed.
Definition at line 186 of file vtol_type.cpp.
References _attc, _land_detected, _local_pos, _local_pos_sp, _params, _ra_hrate, _ra_hrate_sp, _tecs_running, _tecs_status, _v_att, _v_control_mode, VtolAttitudeControl::abort_front_transition(), tecs_status_s::altitude_filtered, tecs_status_s::altitude_sp, f(), vehicle_control_mode_s::flag_armed, vehicle_control_mode_s::flag_control_altitude_enabled, FLT_EPSILON, Params::fw_alt_err, Params::fw_min_alt, Params::fw_qc_max_pitch, Params::fw_qc_max_roll, tecs_status_s::height_rate, tecs_status_s::height_rate_setpoint, vehicle_land_detected_s::landed, matrix::Euler< Type >::phi(), vehicle_attitude_s::q, math::radians(), matrix::Euler< Type >::theta(), vehicle_local_position_s::v_z_valid, vehicle_local_position_s::vz, vehicle_local_position_setpoint_s::z, vehicle_local_position_s::z, vehicle_local_position_s::z_deriv, and vehicle_local_position_s::z_valid.
Referenced by update_fw_state(), and update_transition_state().
|
pure virtual |
Write control values to actuator output topics.
Implemented in Standard, Tailsitter, and Tiltrotor.
Referenced by VtolAttitudeControl::Run().
|
inline |
Definition at line 168 of file vtol_type.h.
References mpu9x50::parameters_update().
Referenced by VtolAttitudeControl::Run().
bool VtolType::init | ( | ) |
Initialise.
Definition at line 80 of file vtol_type.cpp.
References fd, PWM_OUTPUT0_DEVICE_PATH, PWM_SERVO_GET_DISARMED_PWM, PWM_SERVO_GET_MAX_PWM, PWM_SERVO_GET_MIN_PWM, px4_close(), px4_ioctl(), and px4_open().
Referenced by VtolAttitudeControl::Run().
|
private |
Determines if channel is set in target.
[in] | channel | The channel |
[in] | target | The target to check on. |
Definition at line 363 of file vtol_type.cpp.
References num_outputs_max.
Referenced by set_idle_fw(), set_idle_mc(), and set_motor_state().
|
pure virtual |
Implemented in Standard, Tiltrotor, and Tailsitter.
Referenced by VtolAttitudeControl::parameters_update().
|
protected |
Sets mc motor minimum pwm to PWM_MIN which ensures that the motors stop spinning on zero throttle in fw mode.
Definition at line 264 of file vtol_type.cpp.
References _params, apply_pwm_limits(), pwm_output_values::channel_count, is_channel_set(), num_outputs_max, PWM_MOTOR_OFF, TYPE_MINIMUM, pwm_output_values::values, and Params::vtol_motor_id.
Referenced by update_fw_state().
|
protected |
Sets mc motor minimum pwm to VT_IDLE_PWM_MC which ensures that they are spinning in mc mode.
Definition at line 245 of file vtol_type.cpp.
References _params, apply_pwm_limits(), pwm_output_values::channel_count, Params::idle_pwm_mc, is_channel_set(), num_outputs_max, TYPE_MINIMUM, pwm_output_values::values, and Params::vtol_motor_id.
Referenced by update_mc_state(), Tiltrotor::update_transition_state(), and Tailsitter::update_transition_state().
|
protected |
Sets state of a selection of motors, see struct motor_state.
[in] | current_state | The current motor state |
[in] | next_state | The next state |
[in] | value | Desired pwm value if next_state = motor_state::VALUE |
Definition at line 312 of file vtol_type.cpp.
References _params, apply_pwm_limits(), pwm_output_values::channel_count, DISABLED, ENABLED, Params::fw_motors_off, IDLE, Params::idle_pwm_mc, is_channel_set(), num_outputs_max, TYPE_MAXIMUM, VALUE, pwm_output_values::values, and Params::vtol_motor_id.
Referenced by update_fw_state(), update_mc_state(), Tiltrotor::update_transition_state(), and Standard::update_transition_state().
|
virtual |
Update fixed wing state.
Reimplemented in Standard, Tailsitter, and Tiltrotor.
Definition at line 140 of file vtol_type.cpp.
References _fw_virtual_att_sp, _mc_pitch_weight, _mc_roll_weight, _mc_yaw_weight, _motor_state, _tecs_running, _tecs_running_ts, _tecs_status, _trans_finished_ts, _v_att_sp, _v_control_mode, check_quadchute_condition(), DISABLED, vehicle_control_mode_s::flag_control_altitude_enabled, flag_idle_mc, hrt_absolute_time(), set_idle_fw(), set_motor_state(), vehicle_attitude_setpoint_s::timestamp, tecs_status_s::timestamp, and waiting_on_tecs().
Referenced by VtolAttitudeControl::Run(), Tiltrotor::update_fw_state(), Tailsitter::update_fw_state(), and Standard::update_fw_state().
|
virtual |
Update multicopter state.
Reimplemented in Standard, and Tiltrotor.
Definition at line 121 of file vtol_type.cpp.
References _mc_pitch_weight, _mc_roll_weight, _mc_throttle_weight, _mc_virtual_att_sp, _mc_yaw_weight, _motor_state, _v_att_sp, ENABLED, flag_idle_mc, set_idle_mc(), and set_motor_state().
Referenced by VtolAttitudeControl::Run(), Tiltrotor::update_mc_state(), and Standard::update_mc_state().
|
pure virtual |
Update transition state.
Implemented in Standard, Tailsitter, and Tiltrotor.
Definition at line 176 of file vtol_type.cpp.
References check_quadchute_condition().
Referenced by VtolAttitudeControl::Run(), Tiltrotor::update_transition_state(), and Standard::update_transition_state().
|
pure virtual |
Update vtol state.
Implemented in Standard, Tailsitter, and Tiltrotor.
Referenced by VtolAttitudeControl::Run().
|
inlinevirtual |
Special handling opportunity for the time right after transition to FW before TECS is running.
Reimplemented in Standard, Tailsitter, and Tiltrotor.
Definition at line 154 of file vtol_type.h.
Referenced by update_fw_state().
|
protected |
Definition at line 187 of file vtol_type.h.
Referenced by Tiltrotor::fill_actuator_outputs(), Tailsitter::fill_actuator_outputs(), Standard::fill_actuator_outputs(), and VtolType().
|
protected |
Definition at line 186 of file vtol_type.h.
Referenced by Tiltrotor::fill_actuator_outputs(), Tailsitter::fill_actuator_outputs(), Standard::fill_actuator_outputs(), and VtolType().
|
protected |
Definition at line 184 of file vtol_type.h.
Referenced by Tiltrotor::fill_actuator_outputs(), Tailsitter::fill_actuator_outputs(), Standard::fill_actuator_outputs(), and VtolType().
|
protected |
Definition at line 185 of file vtol_type.h.
Referenced by Tiltrotor::fill_actuator_outputs(), Tailsitter::fill_actuator_outputs(), Standard::fill_actuator_outputs(), and VtolType().
|
protected |
Definition at line 190 of file vtol_type.h.
Referenced by Tiltrotor::update_transition_state(), Standard::update_transition_state(), Tiltrotor::update_vtol_state(), Tailsitter::update_vtol_state(), Standard::update_vtol_state(), and VtolType().
|
protected |
Definition at line 173 of file vtol_type.h.
Referenced by check_quadchute_condition(), Standard::update_mc_state(), Standard::update_transition_state(), Tiltrotor::update_vtol_state(), Tailsitter::update_vtol_state(), Standard::update_vtol_state(), and VtolType().
|
protected |
Definition at line 210 of file vtol_type.h.
Referenced by Tailsitter::Tailsitter(), Tiltrotor::Tiltrotor(), Tiltrotor::update_transition_state(), Tailsitter::update_transition_state(), and Tailsitter::update_vtol_state().
|
protected |
Definition at line 181 of file vtol_type.h.
Referenced by update_fw_state(), Tailsitter::update_transition_state(), and VtolType().
|
protected |
Definition at line 192 of file vtol_type.h.
Referenced by can_transition_on_ground(), check_quadchute_condition(), and VtolType().
|
protected |
Definition at line 188 of file vtol_type.h.
Referenced by check_quadchute_condition(), Standard::update_vtol_state(), and VtolType().
|
protected |
Definition at line 189 of file vtol_type.h.
Referenced by check_quadchute_condition(), and VtolType().
|
protected |
Definition at line 200 of file vtol_type.h.
Referenced by Tiltrotor::fill_actuator_outputs(), Tailsitter::fill_actuator_outputs(), Standard::fill_actuator_outputs(), Standard::Standard(), Tailsitter::Tailsitter(), Tiltrotor::Tiltrotor(), update_fw_state(), update_mc_state(), Tiltrotor::update_transition_state(), Tailsitter::update_transition_state(), Standard::update_transition_state(), and Standard::update_vtol_state().
|
protected |
Definition at line 199 of file vtol_type.h.
Referenced by Tiltrotor::fill_actuator_outputs(), Tailsitter::fill_actuator_outputs(), Standard::fill_actuator_outputs(), Standard::Standard(), Tailsitter::Tailsitter(), Tiltrotor::Tiltrotor(), Tailsitter::update_fw_state(), update_fw_state(), update_mc_state(), Tiltrotor::update_transition_state(), Tailsitter::update_transition_state(), Standard::update_transition_state(), and Standard::update_vtol_state().
|
protected |
Definition at line 202 of file vtol_type.h.
Referenced by Tiltrotor::fill_actuator_outputs(), Standard::fill_actuator_outputs(), Standard::Standard(), update_mc_state(), Tiltrotor::update_transition_state(), Standard::update_transition_state(), and Standard::update_vtol_state().
|
protected |
Definition at line 180 of file vtol_type.h.
Referenced by update_mc_state(), Tiltrotor::update_transition_state(), Tailsitter::update_transition_state(), Standard::update_transition_state(), and VtolType().
|
protected |
Definition at line 201 of file vtol_type.h.
Referenced by Tiltrotor::fill_actuator_outputs(), Tailsitter::fill_actuator_outputs(), Standard::fill_actuator_outputs(), Standard::Standard(), Tailsitter::Tailsitter(), Tiltrotor::Tiltrotor(), update_fw_state(), update_mc_state(), Tiltrotor::update_transition_state(), Tailsitter::update_transition_state(), Standard::update_transition_state(), and Standard::update_vtol_state().
|
protected |
Definition at line 217 of file vtol_type.h.
Referenced by update_fw_state(), update_mc_state(), Tiltrotor::update_transition_state(), and Standard::update_transition_state().
|
protected |
Definition at line 194 of file vtol_type.h.
Referenced by check_quadchute_condition(), Tiltrotor::fill_actuator_outputs(), Tailsitter::fill_actuator_outputs(), Standard::fill_actuator_outputs(), Standard::parameters_update(), set_idle_fw(), set_idle_mc(), set_motor_state(), Tailsitter::update_fw_state(), Tiltrotor::update_transition_state(), Tailsitter::update_transition_state(), Standard::update_transition_state(), Tiltrotor::update_vtol_state(), Tailsitter::update_vtol_state(), Standard::update_vtol_state(), and VtolType().
|
protected |
Definition at line 198 of file vtol_type.h.
Referenced by Standard::Standard().
|
protected |
Definition at line 207 of file vtol_type.h.
Referenced by check_quadchute_condition().
|
protected |
Definition at line 208 of file vtol_type.h.
Referenced by check_quadchute_condition().
|
protected |
Definition at line 214 of file vtol_type.h.
Referenced by check_quadchute_condition(), and update_fw_state().
|
protected |
Definition at line 215 of file vtol_type.h.
Referenced by update_fw_state().
|
protected |
Definition at line 191 of file vtol_type.h.
Referenced by check_quadchute_condition(), update_fw_state(), and VtolType().
|
protected |
Definition at line 205 of file vtol_type.h.
Referenced by Tiltrotor::update_transition_state(), Tiltrotor::waiting_on_tecs(), and Tailsitter::waiting_on_tecs().
|
protected |
Definition at line 212 of file vtol_type.h.
Referenced by update_fw_state(), and Standard::update_vtol_state().
|
protected |
Definition at line 178 of file vtol_type.h.
Referenced by check_quadchute_condition(), Standard::update_mc_state(), Tailsitter::update_transition_state(), Tailsitter::update_vtol_state(), Standard::update_vtol_state(), and VtolType().
|
protected |
Definition at line 179 of file vtol_type.h.
Referenced by update_fw_state(), Standard::update_mc_state(), update_mc_state(), Tiltrotor::update_transition_state(), Tailsitter::update_transition_state(), Standard::update_transition_state(), VtolType(), Tiltrotor::waiting_on_tecs(), Tailsitter::waiting_on_tecs(), and Standard::waiting_on_tecs().
|
protected |
Definition at line 182 of file vtol_type.h.
Referenced by can_transition_on_ground(), check_quadchute_condition(), update_fw_state(), Standard::update_mc_state(), and VtolType().
|
protected |
Definition at line 174 of file vtol_type.h.
Referenced by Tiltrotor::update_vtol_state(), Tailsitter::update_vtol_state(), and Standard::update_vtol_state().
|
protected |
Definition at line 183 of file vtol_type.h.
Referenced by Tailsitter::update_vtol_state(), Standard::update_vtol_state(), and VtolType().
|
protected |
Definition at line 196 of file vtol_type.h.
Referenced by update_fw_state(), update_mc_state(), Tiltrotor::update_transition_state(), and Tailsitter::update_transition_state().
|
staticprotected |
Definition at line 176 of file vtol_type.h.
Referenced by is_channel_set(), set_idle_fw(), set_idle_mc(), and set_motor_state().