fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #1513 from PX4/isvtol

Browse Source 
is_vtol flag in vehicle_status
This commit is contained in:
Lorenz Meier 2014-12-31 14:01:18 +01:00
commit 57ca716402
4 changed files with 49 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/modules/commander/commander.cpp
View File

@ -1098,6 +1098,10 @@ int commander_thread_main(int argc, char *argv[])
status.is_rotary_wing = false;
}
/* set vehicle_status.is_vtol flag */
status.is_vtol = (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR) ||
(status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR);
/* check and update system / component ID */
param_get(_param_system_id, &(status.system_id));
param_get(_param_component_id, &(status.component_id));
@ -1119,6 +1123,8 @@ int commander_thread_main(int argc, char *argv[])
/* navigation parameters */
param_get(_param_takeoff_alt, &takeoff_alt);
/* Safety parameters */
param_get(_param_enable_parachute, &parachute_enabled);
param_get(_param_enable_datalink_loss, &datalink_loss_enabled);
param_get(_param_datalink_loss_timeout, &datalink_loss_timeout);
@ -1127,6 +1133,8 @@ int commander_thread_main(int argc, char *argv[])
param_get(_param_ef_throttle_thres, &ef_throttle_thres);
param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres);
param_get(_param_ef_time_thres, &ef_time_thres);
/* Autostart id */
param_get(_param_autostart_id, &autostart_id);
}
@ -2230,14 +2238,7 @@ set_control_mode()
{
/* set vehicle_control_mode according to set_navigation_state */
control_mode.flag_armed = armed.armed;
/* TODO: check this */
if (autostart_id < 13000 || autostart_id >= 14000) {
control_mode.flag_external_manual_override_ok = !status.is_rotary_wing;
} else {
control_mode.flag_external_manual_override_ok = false;
}
control_mode.flag_external_manual_override_ok = (!status.is_rotary_wing && !status.is_vtol);
control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON;
control_mode.flag_control_offboard_enabled = false;

44
src/modules/fw_att_control/fw_att_control_main.cpp
View File

@ -194,8 +194,6 @@ private:
float man_roll_max; /**< Max Roll in rad */
float man_pitch_max; /**< Max Pitch in rad */
param_t autostart_id; /* indicates which airframe is used */
} _parameters; /**< local copies of interesting parameters */
struct {
@ -236,7 +234,6 @@ private:
param_t man_roll_max;
param_t man_pitch_max;
param_t autostart_id; /* indicates which airframe is used */
} _parameter_handles; /**< handles for interesting parameters */
@ -338,6 +335,9 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_actuators_0_pub(-1),
_actuators_2_pub(-1),
_rates_sp_id(0),
_actuators_id(0),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")),
_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")),
@ -396,20 +396,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_parameter_handles.man_roll_max = param_find("FW_MAN_R_MAX");
_parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX");
_parameter_handles.autostart_id = param_find("SYS_AUTOSTART");
/* fetch initial parameter values */
parameters_update();
/* set correct uORB ID, depending on if vehicle is VTOL or not */
if (_parameters.autostart_id >= 13000 && _parameters.autostart_id <= 13999) { /* VTOL airframe?*/
_rates_sp_id = ORB_ID(fw_virtual_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_virtual_fw);
}
else {
_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_0);
}
}
FixedwingAttitudeControl::~FixedwingAttitudeControl()
@ -484,8 +472,6 @@ FixedwingAttitudeControl::parameters_update()
_parameters.man_roll_max = math::radians(_parameters.man_roll_max);
_parameters.man_pitch_max = math::radians(_parameters.man_pitch_max);
param_get(_parameter_handles.autostart_id, &_parameters.autostart_id);
/* pitch control parameters */
_pitch_ctrl.set_time_constant(_parameters.tconst);
_pitch_ctrl.set_k_p(_parameters.p_p);
@ -601,6 +587,16 @@ FixedwingAttitudeControl::vehicle_status_poll()
if (vehicle_status_updated) {
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
/* set correct uORB ID, depending on if vehicle is VTOL or not */
if (!_rates_sp_id) {
if (_vehicle_status.is_vtol) {
_rates_sp_id = ORB_ID(fw_virtual_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_virtual_fw);
} else {
_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_0);
}
}
}
}
@ -701,11 +697,11 @@ FixedwingAttitudeControl::task_main()
/* load local copies */
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
if (_parameters.autostart_id >= 13000
&& _parameters.autostart_id <= 13999) { //vehicle type is VTOL, need to modify attitude!
/* The following modification to the attitude is vehicle specific and in this case applies
to tail-sitter models !!!
if (_vehicle_status.is_vtol) {
/* vehicle type is VTOL, need to modify attitude!
* The following modification to the attitude is vehicle specific and in this case applies
* to tail-sitter models !!!
*
* Since the VTOL airframe is initialized as a multicopter we need to
* modify the estimated attitude for the fixed wing operation.
* Since the neutral position of the vehicle in fixed wing mode is -90 degrees rotated around
@ -1031,7 +1027,7 @@ FixedwingAttitudeControl::task_main()
if (_rate_sp_pub > 0) {
/* publish the attitude rates setpoint */
orb_publish(_rates_sp_id, _rate_sp_pub, &_rates_sp);
} else {
} else if (_rates_sp_id) {
/* advertise the attitude rates setpoint */
_rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp);
}
@ -1056,7 +1052,7 @@ FixedwingAttitudeControl::task_main()
/* publish the actuator controls */
if (_actuators_0_pub > 0) {
orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
} else {
} else if (_actuators_id) {
_actuators_0_pub= orb_advertise(_actuators_id, &_actuators);
}

39
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -126,8 +126,8 @@ private:
orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */
orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */
orb_id_t _rates_sp_id; // pointer to correct rates setpoint uORB metadata structure
orb_id_t _actuators_id; // pointer to correct actuator controls0 uORB metadata structure
orb_id_t _rates_sp_id; /**< pointer to correct rates setpoint uORB metadata structure */
orb_id_t _actuators_id; /**< pointer to correct actuator controls0 uORB metadata structure */
bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */
@ -175,7 +175,6 @@ private:
param_t acro_pitch_max;
param_t acro_yaw_max;
param_t autostart_id; //what frame are we using?
} _params_handles; /**< handles for interesting parameters */
struct {
@ -191,7 +190,6 @@ private:
float man_yaw_max;
math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */
param_t autostart_id;
} _params;
/**
@ -285,6 +283,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_att_sp_pub(-1),
_v_rates_sp_pub(-1),
_actuators_0_pub(-1),
_rates_sp_id(0),
_actuators_id(0),
_actuators_0_circuit_breaker_enabled(false),
@ -313,8 +313,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_params.man_yaw_max = 0.0f;
_params.acro_rate_max.zero();
_params.autostart_id = 0; //default
_rates_prev.zero();
_rates_sp.zero();
_rates_int.zero();
@ -344,19 +342,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_params_handles.acro_pitch_max = param_find("MC_ACRO_P_MAX");
_params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX");
_params_handles.autostart_id = param_find("SYS_AUTOSTART");
/* fetch initial parameter values */
parameters_update();
// set correct uORB ID, depending on if vehicle is VTOL or not
if (_params.autostart_id >= 13000 && _params.autostart_id <= 13999) { /* VTOL airframe?*/
_rates_sp_id = ORB_ID(mc_virtual_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_virtual_mc);
}
else {
_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_0);
}
}
MulticopterAttitudeControl::~MulticopterAttitudeControl()
@ -440,8 +427,6 @@ MulticopterAttitudeControl::parameters_update()
_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);
param_get(_params_handles.autostart_id, &_params.autostart_id);
return OK;
}
@ -531,6 +516,16 @@ MulticopterAttitudeControl::vehicle_status_poll()
if (vehicle_status_updated) {
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
/* set correct uORB ID, depending on if vehicle is VTOL or not */
if (!_rates_sp_id) {
if (_vehicle_status.is_vtol) {
_rates_sp_id = ORB_ID(mc_virtual_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_virtual_mc);
} else {
_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_0);
}
}
}
}
@ -844,7 +839,7 @@ MulticopterAttitudeControl::task_main()
if (_v_rates_sp_pub > 0) {
orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
} else {
} else if (_rates_sp_id) {
_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
}
@ -868,7 +863,7 @@ MulticopterAttitudeControl::task_main()
if (_v_rates_sp_pub > 0) {
orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
} else {
} else if (_rates_sp_id) {
_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
}
@ -896,7 +891,7 @@ MulticopterAttitudeControl::task_main()
if (_actuators_0_pub > 0) {
orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
} else {
} else if (_actuators_id) {
_actuators_0_pub = orb_advertise(_actuators_id, &_actuators);
}

4
src/modules/uORB/topics/vehicle_status.h
View File

@ -185,7 +185,9 @@ struct vehicle_status_s {
int32_t system_id; /**< system id, inspired by MAVLink's system ID field */
int32_t component_id; /**< subsystem / component id, inspired by MAVLink's component ID field */
bool is_rotary_wing;
bool is_rotary_wing; /**< True if system is in rotary wing configuration, so for a VTOL
this is only true while flying as a multicopter */
bool is_vtol; /**< True if the system is VTOL capable */
bool condition_battery_voltage_valid;
bool condition_system_in_air_restore; /**< true if we can restore in mid air */