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

implemented runway takeoff for fw

Browse Source
This commit is contained in:
tumbili
2015-09-10 14:35:40 +02:00
committed by Roman
parent ee4249f30f
commit f43d50fbc9
3 changed files with 150 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_control/fw_att_control_main.cpp
+8
View File
@@ -811,6 +811,7 @@ FixedwingAttitudeControl::task_main()
float roll_sp = _parameters.rollsp_offset_rad;
float pitch_sp = _parameters.pitchsp_offset_rad;
float yaw_sp = 0.0f;
float yaw_manual = 0.0f;
float throttle_sp = 0.0f;
@@ -824,6 +825,7 @@ FixedwingAttitudeControl::task_main()
/* read in attitude setpoint from attitude setpoint uorb topic */
roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
yaw_sp = _att_sp.yaw_body;
throttle_sp = _att_sp.thrust;
/* reset integrals where needed */
@@ -952,12 +954,18 @@ FixedwingAttitudeControl::task_main()
control_input.acc_body_z = _accel.z;
control_input.roll_setpoint = roll_sp;
control_input.pitch_setpoint = pitch_sp;
control_input.yaw_setpoint = yaw_sp;
control_input.airspeed_min = _parameters.airspeed_min;
control_input.airspeed_max = _parameters.airspeed_max;
control_input.airspeed = airspeed;
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);
}
/* Run attitude controllers */
if (PX4_ISFINITE(roll_sp) && PX4_ISFINITE(pitch_sp)) {
_roll_ctrl.control_attitude(control_input);
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+133 -69
View File
@@ -195,6 +195,8 @@ private:
bool land_useterrain;
bool _was_in_air; /**< indicated wether the plane was in the air in the previous interation*/
bool _takeoff_initialized;
hrt_abstime _takeoff_runway_start; /**< time at which we start takeoff on runway */
hrt_abstime _time_went_in_air; /**< time at which the plane went in the air */
/* takeoff/launch states */
@@ -278,6 +280,8 @@ private:
float land_flare_pitch_max_deg;
int land_use_terrain_estimate;
int do_runway_takeoff;
} _parameters; /**< local copies of interesting parameters */
struct {
@@ -326,6 +330,8 @@ private:
param_t land_flare_pitch_max_deg;
param_t land_use_terrain_estimate;
param_t do_runway_takeoff;
} _parameter_handles; /**< handles for interesting parameters */
@@ -520,6 +526,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
land_onslope(false),
land_useterrain(false),
_was_in_air(false),
_takeoff_initialized(false),
_takeoff_runway_start(0),
_time_went_in_air(0),
launch_detection_state(LAUNCHDETECTION_RES_NONE),
last_manual(false),
@@ -563,6 +571,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
_parameter_handles.land_flare_pitch_max_deg = param_find("FW_FLARE_PMAX");
_parameter_handles.land_use_terrain_estimate= param_find("FW_LND_USETER");
_parameter_handles.do_runway_takeoff = param_find("FW_RUNWAY_TKOFF");
_parameter_handles.time_const = param_find("FW_T_TIME_CONST");
_parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST");
_parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN");
@@ -666,6 +676,8 @@ FixedwingPositionControl::parameters_update()
param_get(_parameter_handles.land_flare_pitch_max_deg, &(_parameters.land_flare_pitch_max_deg));
param_get(_parameter_handles.land_use_terrain_estimate, &(_parameters.land_use_terrain_estimate));
param_get(_parameter_handles.do_runway_takeoff, &(_parameters.do_runway_takeoff));
_l1_control.set_l1_damping(_parameters.l1_damping);
_l1_control.set_l1_period(_parameters.l1_period);
_l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit));
@@ -1054,6 +1066,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
bool setpoint = true;
_att_sp.fw_control_yaw = false; // by default we don't want yaw to be contoller directly with rudder
float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
/* filter speed and altitude for controller */
@@ -1338,94 +1352,144 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
} else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
/* Perform launch detection */
if (launchDetector.launchDetectionEnabled() &&
launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
/* 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");
last_sent = hrt_absolute_time();
if (_parameters.do_runway_takeoff == true) {
if (!_takeoff_initialized) {
_hdg_hold_yaw = _att.yaw;
_takeoff_runway_start = hrt_absolute_time();
_takeoff_initialized = true;
}
/* Detect launch */
launchDetector.update(_sensor_combined.accelerometer_m_s2[0]);
/* update our copy of the launch detection state */
launch_detection_state = launchDetector.getLaunchDetected();
} else {
/* no takeoff detection --> fly */
launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
}
/* Set control values depending on the detection state */
if (launch_detection_state != LAUNCHDETECTION_RES_NONE) {
/* Launch has been detected, hence we have to control the plane. */
_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();
/* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use
* full throttle, otherwise we use the preTakeOff Throttle */
float takeoff_throttle = launch_detection_state !=
LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ?
launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max;
/* select maximum pitch: the launchdetector may impose another limit for the pitch
* depending on the state of the launch */
float takeoff_pitch_max_deg = launchDetector.getPitchMax(_parameters.pitch_limit_max);
float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg);
/* apply minimum pitch and limit roll if target altitude is not within climbout_diff
* meters */
if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) {
if (_airspeed.true_airspeed_m_s < _parameters.airspeed_min * 1.2f) {
_att_sp.pitch_body = math::radians(10.0f); // XXX consider this magic value
// ramp up throttle to max throttle
_att_sp.thrust = _att_sp.thrust < _parameters.throttle_max ? hrt_elapsed_time(&_takeoff_runway_start) / (2000000) : _parameters.throttle_max;
} else {
float takeoff_pitch_max_deg = launchDetector.getPitchMax(_parameters.pitch_limit_max);
float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg);
/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,
calculate_target_airspeed(1.3f * _parameters.airspeed_min),
eas2tas,
math::radians(_parameters.pitch_limit_min),
takeoff_pitch_max_rad,
_parameters.throttle_min, takeoff_throttle,
_parameters.throttle_min, _parameters.throttle_max,
_parameters.throttle_cruise,
true,
math::max(math::radians(pos_sp_triplet.current.pitch_min),
math::max(math::radians(_pos_sp_triplet.current.pitch_min),
math::radians(10.0f)),
_global_pos.alt,
ground_speed,
tecs_status_s::TECS_MODE_TAKEOFF,
takeoff_pitch_max_deg != _parameters.pitch_limit_max);
/* 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 {
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,
takeoff_throttle,
_parameters.throttle_cruise,
false,
math::radians(_parameters.pitch_limit_min),
_global_pos.alt,
ground_speed);
_att_sp.pitch_body = _tecs.get_pitch_demand();
_att_sp.thrust = math::min(_tecs.get_throttle_demand(), _parameters.throttle_max);
}
} else {
/* Tell the attitude controller to stop integrating while we are waiting
* for the launch */
_att_sp.roll_reset_integral = true;
_att_sp.pitch_reset_integral = true;
_att_sp.yaw_reset_integral = true;
/* Set default roll and pitch setpoints during detection phase */
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = math::max(math::radians(pos_sp_triplet.current.pitch_min),
math::radians(10.0f));
if (_global_pos.alt - _global_pos.terrain_alt > 4.0f) { // XXX investigate on this
// start to navigate to takeoff waypoint as soon as we are high enough in the air
_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 {
// for takeoff we want heading control with rudder, roll and pitch stabilization to zero
// throttle should be ramped up to max
_att_sp.roll_body = 0.0f;
_att_sp.yaw_body = _hdg_hold_yaw;
_att_sp.fw_control_yaw = true; // tell attitude controller he should control yaw directly
}
} else {
/* Perform launch detection */
if (launchDetector.launchDetectionEnabled() &&
launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
/* 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");
last_sent = hrt_absolute_time();
}
/* Detect launch */
launchDetector.update(_sensor_combined.accelerometer_m_s2[0]);
/* update our copy of the launch detection state */
launch_detection_state = launchDetector.getLaunchDetected();
} else {
/* no takeoff detection --> fly */
launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
}
/* Set control values depending on the detection state */
if (launch_detection_state != LAUNCHDETECTION_RES_NONE) {
/* Launch has been detected, hence we have to control the plane. */
_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();
/* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use
* full throttle, otherwise we use the preTakeOff Throttle */
float takeoff_throttle = launch_detection_state !=
LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ?
launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max;
/* select maximum pitch: the launchdetector may impose another limit for the pitch
* depending on the state of the launch */
float takeoff_pitch_max_deg = launchDetector.getPitchMax(_parameters.pitch_limit_max);
float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg);
/* apply minimum pitch and limit roll if target altitude is not within climbout_diff
* meters */
if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) {
/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,
calculate_target_airspeed(1.3f * _parameters.airspeed_min),
eas2tas,
math::radians(_parameters.pitch_limit_min),
takeoff_pitch_max_rad,
_parameters.throttle_min, takeoff_throttle,
_parameters.throttle_cruise,
true,
math::max(math::radians(_pos_sp_triplet.current.pitch_min),
math::radians(10.0f)),
_global_pos.alt,
ground_speed,
tecs_status_s::TECS_MODE_TAKEOFF,
takeoff_pitch_max_deg != _parameters.pitch_limit_max);
/* 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 {
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,
takeoff_throttle,
_parameters.throttle_cruise,
false,
math::radians(_parameters.pitch_limit_min),
_global_pos.alt,
ground_speed);
}
} else {
/* Tell the attitude controller to stop integrating while we are waiting
* for the launch */
_att_sp.roll_reset_integral = true;
_att_sp.pitch_reset_integral = true;
_att_sp.yaw_reset_integral = true;
/* Set default roll and pitch setpoints during detection phase */
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = math::max(math::radians(pos_sp_triplet.current.pitch_min),
math::radians(10.0f));
}
}
}
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+9
View File
@@ -426,6 +426,15 @@ PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f);
*/
PARAM_DEFINE_INT32(FW_LND_USETER, 0);
/**
* Enable or disable runway takeoff with landing gear
*
* 0: disabled, 1: enabled
*
* @group L1 Control
*/
PARAM_DEFINE_INT32(FW_RUNWAY_TKOFF, 0);
/**
* Flare, minimum pitch
*