fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-21 18:57:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

fw position controller: fixup attitude setpoint generation

Browse Source 
- generate complete attitude setpoint for position and altitude
control mode
- fix generation of roll setpoint for position control which lead to
wing rock
- add roll and pitch setpoint offsets so that they are logged as well

Signed-off-by: Roman <bapstr@ethz.ch>
This commit is contained in:
Roman
2016-06-27 08:11:22 +02:00
committed by Andreas Antener
parent 880fa47ba2
commit e3f3233ee4
1 changed files with 13 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+13 -7
View File
@@ -291,6 +291,7 @@ private:
float throttle_slew_max;
float man_roll_max_rad;
float rollsp_offset_rad;
float pitchsp_offset_rad;
float throttle_land_max;
@@ -345,6 +346,7 @@ private:
param_t throttle_slew_max;
param_t man_roll_max_deg;
param_t rollsp_offset_deg;
param_t pitchsp_offset_deg;
param_t throttle_land_max;
@@ -623,6 +625,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
_parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX");
_parameter_handles.man_roll_max_deg = param_find("FW_MAN_R_MAX");
_parameter_handles.rollsp_offset_deg = param_find("FW_RSP_OFF");
_parameter_handles.pitchsp_offset_deg = param_find("FW_PSP_OFF");
_parameter_handles.land_slope_angle = param_find("FW_LND_ANG");
_parameter_handles.land_H1_virt = param_find("FW_LND_HVIRT");
@@ -709,6 +712,8 @@ FixedwingPositionControl::parameters_update()
_parameters.man_roll_max_rad = math::radians(_parameters.man_roll_max_rad);
param_get(_parameter_handles.rollsp_offset_deg, &_parameters.rollsp_offset_rad);
_parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_rad);
param_get(_parameter_handles.pitchsp_offset_deg, &_parameters.pitchsp_offset_rad);
_parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_rad);
param_get(_parameter_handles.time_const, &(_parameters.time_const));
@@ -1929,6 +1934,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
} else {
_hdg_hold_enabled = false;
_yaw_lock_engaged = false;
_att_sp.roll_body = _manual.y * _parameters.man_roll_max_rad;
}
} else if (_control_mode.flag_control_altitude_enabled) {
@@ -1980,14 +1986,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
ground_speed,
tecs_status_s::TECS_MODE_NORMAL);
// calculate roll setpoint from user input
// this is already calculated in fw_att_control_main.cpp but we do it here for the sake of logging
if (fabsf(_manual.y) < 0.01f && fabsf(_roll) < 0.2f) {
_att_sp.roll_body = _parameters.rollsp_offset_rad;
if (fabsf(_manual.y) > 0.01f) {
_att_sp.roll_body = _manual.y * _parameters.man_roll_max_rad;
} else {
_att_sp.roll_body = (_manual.y * _parameters.man_roll_max_rad)
+ _parameters.rollsp_offset_rad;
_att_sp.roll_body = 0.0f;
}
} else {
@@ -2204,6 +2206,10 @@ FixedwingPositionControl::task_main()
if (control_position(current_position, ground_speed, _pos_sp_triplet)) {
_att_sp.timestamp = hrt_absolute_time();
// add attitude setpoint offsets
_att_sp.roll_body += _parameters.rollsp_offset_rad;
_att_sp.pitch_body += _parameters.pitchsp_offset_rad;
/* lazily publish the setpoint only once available */
if (_attitude_sp_pub != nullptr) {
/* publish the attitude setpoint */