fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 12:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

extracted heading controller

Browse Source
This commit is contained in:
Andreas Antener
2015-11-17 22:28:08 +01:00
committed by Roman
parent 51ef854173
commit 6c31421889
7 changed files with 265 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_control/fw_att_control_main.cpp
+19 -7
View File
@@ -82,6 +82,7 @@
#include <ecl/attitude_fw/ecl_pitch_controller.h>
#include <ecl/attitude_fw/ecl_roll_controller.h>
#include <ecl/attitude_fw/ecl_yaw_controller.h>
#include <ecl/attitude_fw/ecl_heading_controller.h>
#include <platforms/px4_defines.h>
/**
@@ -248,6 +249,7 @@ private:
ECL_RollController _roll_ctrl;
ECL_PitchController _pitch_ctrl;
ECL_YawController _yaw_ctrl;
ECL_HeadingController _heading_ctrl;
/**
@@ -502,6 +504,13 @@ FixedwingAttitudeControl::parameters_update()
_yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method);
_yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax));
/* heading control parameters */
_heading_ctrl.set_k_p(_parameters.y_p);
_heading_ctrl.set_k_i(_parameters.y_i);
_heading_ctrl.set_k_ff(_parameters.y_ff);
_heading_ctrl.set_integrator_max(_parameters.y_integrator_max);
_heading_ctrl.set_max_rate(math::radians(_parameters.y_rmax));
return OK;
}
@@ -961,18 +970,14 @@ FixedwingAttitudeControl::task_main()
control_input.scaler = airspeed_scaling;
control_input.lock_integrator = lock_integrator;
if (_att_sp.fw_control_yaw == true) {
// this method controls heading directly with rudder. Used for auto takeoff on runway
_yaw_ctrl.set_coordinated_method(ECL_YawController::COORD_METHOD_HEADING);
} else {
_yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method);
}
_yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method);
/* Run attitude controllers */
if (PX4_ISFINITE(roll_sp) && PX4_ISFINITE(pitch_sp)) {
_roll_ctrl.control_attitude(control_input);
_pitch_ctrl.control_attitude(control_input);
_yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude
_heading_ctrl.control_attitude(control_input);
/* Update input data for rate controllers */
control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate();
@@ -1012,7 +1017,14 @@ FixedwingAttitudeControl::task_main()
}
}
float yaw_u = _yaw_ctrl.control_bodyrate(control_input);
float yaw_u = 0.0f;
if (_att_sp.fw_control_yaw == true) {
yaw_u = _heading_ctrl.control_bodyrate(control_input);
}
else {
yaw_u = _yaw_ctrl.control_bodyrate(control_input);
}
_actuators.control[2] = (PX4_ISFINITE(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
/* add in manual rudder control */
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+10 -9
View File
@@ -960,7 +960,7 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint
* for the whole landing */
if (_parameters.land_use_terrain_estimate && (global_pos.terrain_alt_valid || land_useterrain)) {
if(!land_useterrain) {
mavlink_log_info(_mavlink_fd, "#audio: Landing, using terrain estimate");
mavlink_log_info(_mavlink_fd, "Landing, using terrain estimate");
land_useterrain = true;
}
return global_pos.terrain_alt;
@@ -1229,7 +1229,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
} else {
target_bearing = _yaw;
}
mavlink_log_info(_mavlink_fd, "#audio: Landing, heading hold");
mavlink_log_info(_mavlink_fd, "#Landing, heading hold");
}
// warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_yaw));
@@ -1290,7 +1290,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
throttle_max = math::min(throttle_max, _parameters.throttle_land_max);
if (!land_motor_lim) {
land_motor_lim = true;
mavlink_log_info(_mavlink_fd, "#audio: Landing, limiting throttle");
mavlink_log_info(_mavlink_fd, "#Landing, limiting throttle");
}
}
@@ -1314,7 +1314,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
land_motor_lim ? tecs_status_s::TECS_MODE_LAND_THROTTLELIM : tecs_status_s::TECS_MODE_LAND);
if (!land_noreturn_vertical) {
mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring");
mavlink_log_info(_mavlink_fd, "#Landing, flaring");
land_noreturn_vertical = true;
}
//warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance);
@@ -1336,7 +1336,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* stay on slope */
altitude_desired_rel = landing_slope_alt_rel_desired;
if (!land_onslope) {
mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope");
mavlink_log_info(_mavlink_fd, "#Landing, on slope");
land_onslope = true;
}
} else {
@@ -1367,7 +1367,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* need this already before takeoff is detected
* doesn't matter if it gets reset when takeoff is detected eventually */
_takeoff_ground_alt = _global_pos.alt;
mavlink_log_info(_mavlink_fd, "#audio: Takeoff on runway");
mavlink_log_info(_mavlink_fd, "#Takeoff on runway");
}
// update navigation
@@ -1417,7 +1417,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* Inform user that launchdetection is running */
static hrt_abstime last_sent = 0;
if(hrt_absolute_time() - last_sent > 4e6) {
mavlink_log_critical(_mavlink_fd, "Launchdetection running");
mavlink_log_critical(_mavlink_fd, "#Launchdetection running");
last_sent = hrt_absolute_time();
}
@@ -1509,7 +1509,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
/* reset takeoff/launch state */
// FIXME: reset on arm/disarm cycle and mode switch
if (pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
reset_takeoff_state();
}
@@ -1750,7 +1749,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
* already (not by tecs) */
if (!(_control_mode_current == FW_POSCTRL_MODE_AUTO &&
pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
launch_detection_state == LAUNCHDETECTION_RES_NONE)) {
(launch_detection_state == LAUNCHDETECTION_RES_NONE ||
_runway_takeoff.runwayTakeoffEnabled())
)) {
_att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand();
}