fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-29 13:40:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

FW control: Add skeleton for distinct altitude control and position control flight modes

Browse Source 
Conflicts:
	src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
This commit is contained in:
Lorenz Meier
2015-05-17 13:08:09 +02:00
parent 8e935e6fa6
commit 5197be67a7
2 changed files with 167 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_control/fw_att_control_main.cpp
+26
View File
@@ -854,6 +854,32 @@ FixedwingAttitudeControl::task_main()
_yaw_ctrl.reset_integrator();
}
} else if (_vcontrol_mode.flag_control_velocity_enabled) {
/* the pilot does not want to change direction,
* take straight attitude setpoint from position controller
*/
if (fabsf(_manual.y) < 0.01f) {
roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
} else {
roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll)
+ _parameters.rollsp_offset_rad;
}
pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
throttle_sp = _att_sp.thrust;
/* reset integrals where needed */
if (_att_sp.roll_reset_integral) {
_roll_ctrl.reset_integrator();
}
if (_att_sp.pitch_reset_integral) {
_pitch_ctrl.reset_integrator();
}
if (_att_sp.yaw_reset_integral) {
_yaw_ctrl.reset_integrator();
}
} else if (_vcontrol_mode.flag_control_altitude_enabled) {
/*
* Velocity should be controlled and manual is enabled.
*/
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+141 -2
View File
@@ -165,7 +165,11 @@ private:
perf_counter_t _loop_perf; /**< loop performance counter */
float _hold_alt; /**< hold altitude for velocity mode */
float _hold_alt; /**< hold altitude for altitude mode */
float _hdg_hold_yaw; /**< hold heading for velocity mode */
bool _hdg_hold_enabled; /**< heading hold enabled */
struct position_setpoint_s _hdg_hold_prev_wp; /**< position where heading hold started */
struct position_setpoint_s _hdg_hold_curr_wp; /**< position to which heading hold flies */
hrt_abstime _control_position_last_called; /**<last call of control_position */
/* land states */
@@ -355,6 +359,17 @@ private:
*/
void navigation_capabilities_publish();
/**
* Get a new waypoint based on heading and distance from current position
*
* @param heading the heading to fly to
* @param distance the distance of the generated waypoint
* @param waypoint_prev the waypoint at the current position
* @param waypoint_next the waypoint in the heading direction
*/
void get_waypoint_heading_distance(float heading, float distance,
struct position_setpoint_s &waypoint_prev, struct position_setpoint_s &waypoint_next, bool flag_init);
/**
* Return the terrain estimate during landing: uses the wp altitude value or the terrain estimate if available
*/
@@ -454,6 +469,10 @@ FixedwingPositionControl::FixedwingPositionControl() :
_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
_hold_alt(0.0f),
_hdg_hold_yaw(0.0f),
_hdg_hold_enabled(false),
_hdg_hold_prev_wp{},
_hdg_hold_curr_wp{},
_control_position_last_called(0),
land_noreturn_horizontal(false),
@@ -862,6 +881,28 @@ void FixedwingPositionControl::navigation_capabilities_publish()
}
}
void FixedwingPositionControl::get_waypoint_heading_distance(float heading, float distance,
struct position_setpoint_s &waypoint_prev, struct position_setpoint_s &waypoint_next, bool flag_init)
{
waypoint_prev.valid = true;
waypoint_prev.alt = _hold_alt;
if (flag_init) {
// on init set first waypoint to momentary position
waypoint_prev.lat = _global_pos.lat;
waypoint_prev.lon = _global_pos.lon;
} else {
// use waypoint which is still in front of us
waypoint_prev.lat = waypoint_next.lat;
waypoint_prev.lon = waypoint_next.lon;
}
waypoint_next.valid = true;
waypoint_next.lat = waypoint_prev.lat + cos(heading) * (double)distance / 1e6;
waypoint_next.lon = waypoint_prev.lon + sin(heading) * (double)distance / 1e6;
waypoint_next.alt = _hold_alt;
}
float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos)
{
if (!isfinite(global_pos.terrain_alt)) {
@@ -928,6 +969,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* reset hold altitude */
_hold_alt = _global_pos.alt;
/* reset hold yaw */
_hdg_hold_yaw = _att.yaw;
/* get circle mode */
bool was_circle_mode = _l1_control.circle_mode();
@@ -1251,7 +1294,103 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
} else if (_control_mode.flag_control_velocity_enabled &&
_control_mode.flag_control_altitude_enabled) {
/* POSITION CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */
/* POSITION CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed,
heading is set to a distant waypoint */
const float deadBand = (60.0f/1000.0f);
const float factor = 1.0f - deadBand;
if (_control_mode_current != FW_POSCTRL_MODE_POSITION) {
/* Need to init because last loop iteration was in a different mode */
_hold_alt = _global_pos.alt;
_hdg_hold_yaw = _att.yaw;
}
/* Reset integrators if switching to this mode from a other mode in which posctl was not active */
if (_control_mode_current == FW_POSCTRL_MODE_OTHER) {
/* reset integrators */
if (_mTecs.getEnabled()) {
_mTecs.resetIntegrators();
_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
}
}
_control_mode_current = FW_POSCTRL_MODE_POSITION;
/* Get demanded airspeed */
float altctrl_airspeed = _parameters.airspeed_min +
(_parameters.airspeed_max - _parameters.airspeed_min) *
_manual.z;
/* Get demanded vertical velocity from pitch control */
static bool was_in_deadband = false;
if (_manual.x > deadBand) {
float pitch = (_manual.x - deadBand) / factor;
_hold_alt -= (_parameters.max_climb_rate * dt) * pitch;
was_in_deadband = false;
} else if (_manual.x < - deadBand) {
float pitch = (_manual.x + deadBand) / factor;
_hold_alt -= (_parameters.max_sink_rate * dt) * pitch;
was_in_deadband = false;
} else if (!was_in_deadband) {
/* store altitude at which manual.x was inside deadBand
* The aircraft should immediately try to fly at this altitude
* as this is what the pilot expects when he moves the stick to the center */
_hold_alt = _global_pos.alt;
was_in_deadband = true;
}
tecs_update_pitch_throttle(_hold_alt,
altctrl_airspeed,
eas2tas,
math::radians(_parameters.pitch_limit_min),
math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min,
_parameters.throttle_max,
_parameters.throttle_cruise,
false,
math::radians(_parameters.pitch_limit_min),
_global_pos.alt,
ground_speed,
TECS_MODE_NORMAL);
/* heading control */
if (fabsf(_manual.y) < 0.01f) {
/* heading / roll is zero, lock onto current heading */
// XXX calculate a waypoint in some distance
// and lock on to it
/* just switched back from non heading-hold to heading hold */
if (!_hdg_hold_enabled) {
_hdg_hold_enabled = true;
_hdg_hold_yaw = _att.yaw;
get_waypoint_heading_distance(_hdg_hold_yaw, 3000, _hdg_hold_prev_wp, _hdg_hold_curr_wp, true);
}
/* we have a valid heading hold position, are we too close? */
if (get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon,
_hdg_hold_curr_wp.lat, _hdg_hold_curr_wp.lon) < 1000) {
get_waypoint_heading_distance(_hdg_hold_yaw, 3000, _hdg_hold_prev_wp, _hdg_hold_curr_wp, false);
}
math::Vector<2> prev_wp;
prev_wp(0) = (float)_hdg_hold_prev_wp.lat;
prev_wp(1) = (float)_hdg_hold_prev_wp.lon;
math::Vector<2> curr_wp;
curr_wp(0) = (float)_hdg_hold_curr_wp.lat;
curr_wp(1) = (float)_hdg_hold_curr_wp.lon;
/* populate l1 control setpoint */
_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
} else {
_hdg_hold_enabled = false;
}
} else if (_control_mode.flag_control_altitude_enabled) {
/* ALTITUDE CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */
const float deadBand = (60.0f/1000.0f);
const float factor = 1.0f - deadBand;