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

fw pos control: launchdetection logic cleanup

Browse Source
This commit is contained in:
Thomas Gubler
2014-08-23 12:41:48 +02:00
parent 9f5004be2d
commit cfbbe60291
1 changed files with 18 additions and 26 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
+18 -26
View File
@@ -174,7 +174,6 @@ private:
/* takeoff/launch states */
LaunchDetectionResult launch_detection_state;
bool usePreTakeoffThrust;
bool last_manual; ///< true if the last iteration was in manual mode (used to determine when a reset is needed)
@@ -441,7 +440,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
land_motor_lim(false),
land_onslope(false),
launch_detection_state(LAUNCHDETECTION_RES_NONE),
usePreTakeoffThrust(false),
last_manual(false),
landingslope(),
flare_curve_alt_rel_last(0.0f),
@@ -1092,37 +1090,24 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* update our copy of the laucn detection state */
launch_detection_state = launchDetector.getLaunchDetected();
/* Depending on launch detection state set control flags */
if(launch_detection_state == LAUNCHDETECTION_RES_NONE) {
/* 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;
usePreTakeoffThrust = true;
} else if (launch_detection_state == LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL) {
usePreTakeoffThrust = true;
} else {
usePreTakeoffThrust = false;
}
} else {
/* no takeoff detection --> fly */
usePreTakeoffThrust = false;
launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL;
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. The usePreTakeoffThrust
* flag determines how much thrust we apply */
/* 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();
float takeoff_throttle = usePreTakeoffThrust ? launchDetector.getThrottlePreTakeoff() :
_parameters.throttle_max;
/* 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;
/* apply minimum pitch and limit roll if target altitude is not within 10 meters */
if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) {
@@ -1161,6 +1146,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
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),
@@ -1200,8 +1191,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
}
/* making sure again that the correct thrust is used, without depending on library calls */
if (usePreTakeoffThrust) {
/* Copy thrust and pitch values from tecs
* making sure again that the correct thrust is used, without depending on library calls */
if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
_att_sp.thrust = launchDetector.getThrottlePreTakeoff();
}
else {
@@ -1362,7 +1355,6 @@ FixedwingPositionControl::task_main()
void FixedwingPositionControl::reset_takeoff_state()
{
launch_detection_state = LAUNCHDETECTION_RES_NONE;
usePreTakeoffThrust = false;
launchDetector.reset();
}