Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-06 06:43:21 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

fw_pos_control_l1: added roll setpoint for logging (#4869)

Browse Source 
in altitude control mode for fixed wings the roll setpoint was not
logged because the position controller publishes the attitude setpoint
but the desired roll setpoint is calculated in the attitude control
module. Now the position controller calculates the roll setpoint as well
for the sake of logging.

Signed-off-by: tumbili <roman@px4.io>
This commit is contained in:
Roman Bapst
2016-06-22 15:19:11 +02:00
committed by Lorenz Meier
parent 36103d33d2
commit 37108870e1
1 changed files with 22 additions and 0 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
+22
View File
@@ -291,6 +291,8 @@ private:
float throttle_idle;
float throttle_cruise;
float throttle_slew_max;
float man_roll_max_rad;
float rollsp_offset_rad;
float throttle_land_max;
@@ -342,6 +344,8 @@ private:
param_t throttle_idle;
param_t throttle_cruise;
param_t throttle_slew_max;
param_t man_roll_max_deg;
param_t rollsp_offset_deg;
param_t throttle_land_max;
@@ -611,6 +615,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
_parameter_handles.throttle_slew_max = param_find("FW_THR_SLEW_MAX");
_parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE");
_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.land_slope_angle = param_find("FW_LND_ANG");
_parameter_handles.land_H1_virt = param_find("FW_LND_HVIRT");
@@ -693,6 +699,12 @@ FixedwingPositionControl::parameters_update()
param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max));
param_get(_parameter_handles.man_roll_max_deg, &_parameters.man_roll_max_rad);
_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.time_const, &(_parameters.time_const));
param_get(_parameter_handles.time_const_throt, &(_parameters.time_const_throt));
param_get(_parameter_handles.min_sink_rate, &(_parameters.min_sink_rate));
@@ -1941,6 +1953,16 @@ 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;
} else {
_att_sp.roll_body = (_manual.y * _parameters.man_roll_max_rad)
+ _parameters.rollsp_offset_rad;
}
} else {
_control_mode_current = FW_POSCTRL_MODE_OTHER;