yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

fw_att_control add rattitude mode

Browse Source
This commit is contained in:
Daniel Agar
2017-02-15 23:44:25 -05:00
committed by Andreas Daniel Antener
parent 9120082b0c
commit e81e6a8296
4 changed files with 32 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -3701,7 +3701,7 @@ set_control_mode()
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_rattitude_enabled = true;
control_mode.flag_control_rattitude_enabled = false;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = false;
control_mode.flag_control_position_enabled = false;

8
src/modules/commander/state_machine_helper.cpp
View File

@@ -377,16 +377,8 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
case commander_state_s::MAIN_STATE_MANUAL:
case commander_state_s::MAIN_STATE_STAB:
case commander_state_s::MAIN_STATE_ACRO:
ret = TRANSITION_CHANGED;
break;
case commander_state_s::MAIN_STATE_RATTITUDE:
/* ACRO and RATTITUDE only implemented in MC */
if (status->is_rotary_wing) {
ret = TRANSITION_CHANGED;
}
break;
case commander_state_s::MAIN_STATE_ALTCTL:

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

@@ -207,6 +207,8 @@ private:
float flaps_scale; /**< Scale factor for flaps */
float flaperon_scale; /**< Scale factor for flaperons */
float rattitude_thres;
int vtol_type; /**< VTOL type: 0 = tailsitter, 1 = tiltrotor */
int bat_scale_en; /**< Battery scaling enabled */
@@ -264,6 +266,8 @@ private:
param_t flaps_scale;
param_t flaperon_scale;
param_t rattitude_thres;
param_t vtol_type;
param_t bat_scale_en;
@@ -457,6 +461,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_parameter_handles.flaps_scale = param_find("FW_FLAPS_SCL");
_parameter_handles.flaperon_scale = param_find("FW_FLAPERON_SCL");
_parameter_handles.rattitude_thres = param_find("FW_RATT_TH");
_parameter_handles.vtol_type = param_find("VT_TYPE");
_parameter_handles.bat_scale_en = param_find("FW_BAT_SCALE_EN");
@@ -558,6 +564,8 @@ FixedwingAttitudeControl::parameters_update()
param_get(_parameter_handles.flaps_scale, &_parameters.flaps_scale);
param_get(_parameter_handles.flaperon_scale, &_parameters.flaperon_scale);
param_get(_parameter_handles.rattitude_thres, &_parameters.rattitude_thres);
param_get(_parameter_handles.vtol_type, &_parameters.vtol_type);
param_get(_parameter_handles.bat_scale_en, &_parameters.bat_scale_en);
@@ -930,6 +938,14 @@ FixedwingAttitudeControl::task_main()
_flaperons_applied = flaperon_control;
}
// Check if we are in rattitude mode and the pilot is above the threshold on pitch
if (_vcontrol_mode.flag_control_rattitude_enabled) {
if (fabsf(_manual.y) > _parameters.rattitude_thres ||
fabsf(_manual.x) > _parameters.rattitude_thres) {
_vcontrol_mode.flag_control_attitude_enabled = false;
}
}
/* decide if in stabilized or full manual control */
if (_vcontrol_mode.flag_control_rates_enabled) {
/* scale around tuning airspeed */

14
src/modules/fw_att_control/fw_att_control_params.c
View File

@@ -629,3 +629,17 @@ PARAM_DEFINE_FLOAT(FW_ACRO_Y_MAX, 90);
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_ACRO_Z_MAX, 45);
/**
* Threshold for Rattitude mode
*
* Manual input needed in order to override attitude control rate setpoints
* and instead pass manual stick inputs as rate setpoints
*
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.01
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_RATT_TH, 0.8f);