dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

weathervane: get rid of passive strategy 

Signed-off-by: Roman <bapstroman@gmail.com>
This commit is contained in:
Roman 2018-09-18 20:06:55 +02:00 committed by Lorenz Meier
parent 0e835cb498
commit 96f3feb088
9 changed files with 1 additions and 95 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/vehicle_attitude_setpoint.msg
View File

@ -19,7 +19,6 @@ bool pitch_reset_integral # Reset pitch integral part (navigation logic change)
bool yaw_reset_integral # Reset yaw integral part (navigation logic change)
bool fw_control_yaw # control heading with rudder (used for auto takeoff on runway)
bool disable_mc_yaw_control # control yaw for mc (used for vtol weather-vane mode)
uint8 apply_flaps # flap config specifier
uint8 FLAPS_OFF = 0 # no flaps

4
src/modules/mc_att_control/mc_att_control.hpp
View File

@ -234,9 +234,7 @@ private:
(ParamFloat<px4::params::SENS_BOARD_X_OFF>) _board_offset_x,
(ParamFloat<px4::params::SENS_BOARD_Y_OFF>) _board_offset_y,
(ParamFloat<px4::params::SENS_BOARD_Z_OFF>) _board_offset_z,
(ParamFloat<px4::params::VT_WV_YAWR_SCL>) _vtol_wv_yaw_rate_scale /**< Scale value [0, 1] for yaw rate setpoint */
(ParamFloat<px4::params::SENS_BOARD_Z_OFF>) _board_offset_z
)
matrix::Vector3f _attitude_p; /**< P gain for attitude control */

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

@ -450,18 +450,6 @@ MulticopterAttitudeControl::control_attitude(float dt)
_rates_sp(i) = math::constrain(_rates_sp(i), -_mc_rate_max(i), _mc_rate_max(i));
}
}
/* VTOL weather-vane mode, dampen yaw rate */
if (_vehicle_status.is_vtol && _v_att_sp.disable_mc_yaw_control) {
if (_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) {
const float wv_yaw_rate_max = _auto_rate_max(2) * _vtol_wv_yaw_rate_scale.get();
_rates_sp(2) = math::constrain(_rates_sp(2), -wv_yaw_rate_max, wv_yaw_rate_max);
// prevent integrator winding up in weathervane mode
_rates_int(2) = 0.0f;
}
}
}
/*

2
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -753,7 +753,6 @@ MulticopterPositionControl::task_main()
_att_sp = ControlMath::thrustToAttitude(thr_sp, _control.getYawSetpoint());
_att_sp.yaw_sp_move_rate = _control.getYawspeedSetpoint();
_att_sp.fw_control_yaw = false;
_att_sp.disable_mc_yaw_control = false;
_att_sp.apply_flaps = false;
if (!constraints.landing_gear) {
@ -779,7 +778,6 @@ MulticopterPositionControl::task_main()
_att_sp.yaw_body = _local_pos.yaw;
_att_sp.yaw_sp_move_rate = 0.0f;
_att_sp.fw_control_yaw = false;
_att_sp.disable_mc_yaw_control = false;
_att_sp.apply_flaps = false;
matrix::Quatf q_sp = matrix::Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body);
q_sp.copyTo(_att_sp.q_d);

15
src/modules/vtol_att_control/vtol_att_control_main.cpp
View File

@ -87,11 +87,6 @@ VtolAttitudeControl::VtolAttitudeControl()
_params_handles.mpc_xy_cruise = param_find("MPC_XY_CRUISE");
_params_handles.fw_motors_off = param_find("VT_FW_MOT_OFFID");
_params_handles.wv_takeoff = param_find("VT_WV_TKO_EN");
_params_handles.wv_land = param_find("VT_WV_LND_EN");
_params_handles.wv_loiter = param_find("VT_WV_LTR_EN");
/* fetch initial parameter values */
parameters_update();
@ -479,16 +474,6 @@ VtolAttitudeControl::parameters_update()
_params.front_trans_time_min = math::min(_params.front_trans_time_openloop * 0.9f,
_params.front_trans_time_min);
/* weathervane */
param_get(_params_handles.wv_takeoff, &l);
_params.wv_takeoff = (l == 1);
param_get(_params_handles.wv_loiter, &l);
_params.wv_loiter = (l == 1);
param_get(_params_handles.wv_land, &l);
_params.wv_land = (l == 1);
param_get(_params_handles.front_trans_duration, &_params.front_trans_duration);
param_get(_params_handles.back_trans_duration, &_params.back_trans_duration);

3
src/modules/vtol_att_control/vtol_att_control_main.h
View File

@ -189,9 +189,6 @@ private:
param_t fw_qc_max_roll;
param_t front_trans_time_openloop;
param_t front_trans_time_min;
param_t wv_takeoff;
param_t wv_loiter;
param_t wv_land;
param_t front_trans_duration;
param_t back_trans_duration;
param_t transition_airspeed;

38
src/modules/vtol_att_control/vtol_att_control_params.c
View File

@ -279,44 +279,6 @@ PARAM_DEFINE_INT32(VT_FW_QC_R, 0);
*/
PARAM_DEFINE_FLOAT(VT_F_TR_OL_TM, 6.0f);
/**
* Weather-vane yaw rate scale.
*
* The desired yawrate from the controller will be scaled in order to avoid
* yaw fighting against the wind.
*
* @min 0.0
* @max 1.0
* @increment 0.01
* @decimal 3
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_WV_YAWR_SCL, 0.15f);
/**
* Enable weather-vane mode takeoff for missions
*
* @boolean
* @group Mission
*/
PARAM_DEFINE_INT32(VT_WV_TKO_EN, 0);
/**
* Weather-vane mode for loiter
*
* @boolean
* @group Mission
*/
PARAM_DEFINE_INT32(VT_WV_LTR_EN, 0);
/**
* Weather-vane mode landings for missions
*
* @boolean
* @group Mission
*/
PARAM_DEFINE_INT32(VT_WV_LND_EN, 0);
/**
* The channel number of motors that must be turned off in fixed wing mode.
*

18
src/modules/vtol_att_control/vtol_type.cpp
View File

@ -124,24 +124,6 @@ void VtolType::update_mc_state()
_mc_roll_weight = 1.0f;
_mc_pitch_weight = 1.0f;
_mc_yaw_weight = 1.0f;
// VTOL weathervane
_v_att_sp->disable_mc_yaw_control = false;
if (_attc->get_pos_sp_triplet()->current.valid &&
!_v_control_mode->flag_control_manual_enabled) {
if (_params->wv_takeoff && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
_v_att_sp->disable_mc_yaw_control = true;
} else if (_params->wv_loiter
&& _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
_v_att_sp->disable_mc_yaw_control = true;
} else if (_params->wv_land && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
_v_att_sp->disable_mc_yaw_control = true;
}
}
}
void VtolType::update_fw_state()

3
src/modules/vtol_att_control/vtol_type.h
View File

@ -58,9 +58,6 @@ struct Params {
float fw_qc_max_roll; // maximum roll angle FW mode (QuadChute)
float front_trans_time_openloop;
float front_trans_time_min;
bool wv_takeoff;
bool wv_loiter;
bool wv_land;
float front_trans_duration;
float back_trans_duration;
float transition_airspeed;