dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #2395 from tumbili/takeoff_help 

ask for climbout mode when doin takeoff help
This commit is contained in:
Lorenz Meier 2015-06-18 23:56:28 +02:00
parent e08dc0df40 0446efa9a4
commit e23712c47c
1 changed files with 64 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

80
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -389,13 +389,16 @@ private:
float get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos);
/**
* Control position.
* Check if we are in a takeoff situation
*/
bool in_takeoff_situation();
/**
* Do takeoff help when in altitude controlled modes
* @param hold_altitude altitude setpoint for controller
* @param pitch_limit_min minimum pitch allowed
*/
void do_takeoff_help();
void do_takeoff_help(float *hold_altitude, float *pitch_limit_min);
/**
* Update desired altitude base on user pitch stick input
@ -405,6 +408,9 @@ private:
*/
bool update_desired_altitude(float dt);
/**
* Control position.
*/
bool control_position(const math::Vector<2> &global_pos, const math::Vector<3> &ground_speed,
const struct position_setpoint_triplet_s &_pos_sp_triplet);
@ -987,15 +993,25 @@ bool FixedwingPositionControl::update_desired_altitude(float dt)
return climbout_mode;
}
void FixedwingPositionControl::do_takeoff_help()
{
bool FixedwingPositionControl::in_takeoff_situation() {
const hrt_abstime delta_takeoff = 10000000;
const float throttle_threshold = 0.3f;
const float delta_alt_takeoff = 30.0f;
const float throttle_threshold = 0.1f;
/* demand 30 m above ground if user switched into this mode during takeoff */
if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold && _global_pos.alt <= _ground_alt + delta_alt_takeoff) {
_hold_alt = _ground_alt + delta_alt_takeoff;
if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold && _global_pos.alt <= _ground_alt + _parameters.climbout_diff) {
return true;
}
return false;
}
void FixedwingPositionControl::do_takeoff_help(float *hold_altitude, float *pitch_limit_min)
{
/* demand "climbout_diff" m above ground if user switched into this mode during takeoff */
if (in_takeoff_situation()) {
*hold_altitude = _ground_alt + _parameters.climbout_diff;
*pitch_limit_min = math::radians(10.0f);
} else {
*pitch_limit_min = _parameters.pitch_limit_min;
}
}
@ -1020,7 +1036,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
math::Vector<3> accel_earth = _R_nb * accel_body;
if (!_mTecs.getEnabled()) {
if (!_mTecs.getEnabled() && !_vehicle_status.condition_landed) {
_tecs.update_50hz(_global_pos.alt /* XXX might switch to alt err here */, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
}
@ -1118,6 +1134,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
if (in_takeoff_situation()) {
/* limit roll motion to ensure enough lift */
_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f),
math::radians(15.0f));
}
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
@ -1415,8 +1437,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* update desired altitude based on user pitch stick input */
bool climbout_requested = update_desired_altitude(dt);
/* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/
do_takeoff_help();
/* if we assume that user is taking off then help by demanding altitude setpoint well above ground
* and set limit to pitch angle to prevent stearing into ground
*/
float pitch_limit_min;
do_takeoff_help(&_hold_alt, &pitch_limit_min);
/* throttle limiting */
throttle_max = _parameters.throttle_max;
@ -1433,7 +1459,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
throttle_max,
_parameters.throttle_cruise,
climbout_requested,
math::radians(_parameters.pitch_limit_min),
pitch_limit_min,
_global_pos.alt,
ground_speed,
tecs_status_s::TECS_MODE_NORMAL);
@ -1448,6 +1474,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
/* user tries to do a takeoff in heading hold mode, reset the yaw setpoint on every iteration
to make sure the plane does not start rolling
*/
if (in_takeoff_situation()) {
_hdg_hold_enabled = false;
_yaw_lock_engaged = true;
}
if (_yaw_lock_engaged) {
/* just switched back from non heading-hold to heading hold */
@ -1477,6 +1511,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
if (in_takeoff_situation()) {
/* limit roll motion to ensure enough lift */
_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f),
math::radians(15.0f));
}
}
} else {
_hdg_hold_enabled = false;
@ -1508,8 +1548,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* update desired altitude based on user pitch stick input */
bool climbout_requested = update_desired_altitude(dt);
/* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/
do_takeoff_help();
/* if we assume that user is taking off then help by demanding altitude setpoint well above ground
* and set limit to pitch angle to prevent stearing into ground
*/
float pitch_limit_min;
do_takeoff_help(&_hold_alt, &pitch_limit_min);
/* throttle limiting */
throttle_max = _parameters.throttle_max;
@ -1526,7 +1569,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
throttle_max,
_parameters.throttle_cruise,
climbout_requested,
math::radians(_parameters.pitch_limit_min),
pitch_limit_min,
_global_pos.alt,
ground_speed,
tecs_status_s::TECS_MODE_NORMAL);
@ -1753,6 +1796,11 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
const math::Vector<3> &ground_speed,
unsigned mode, bool pitch_max_special)
{
/* do not run tecs if we are not in air */
if (_vehicle_status.condition_landed) {
return;
}
if (_mTecs.getEnabled()) {
/* Using mtecs library: prepare arguments for mtecs call */
float flightPathAngle = 0.0f;