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

Merge branch 'master' of github.com:PX4/Firmware into isvtol

Browse Source
This commit is contained in:
Lorenz Meier
2014-12-22 21:50:25 +01:00
parent 6dd98cb311 d54b46355c
commit 46dd95ab34
40 changed files with 20697 additions and 198 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
+69 -57
View File
@@ -200,9 +200,11 @@ private:
ECL_L1_Pos_Controller _l1_control;
TECS _tecs;
fwPosctrl::mTecs _mTecs;
bool _was_pos_control_mode;
bool _was_velocity_control_mode;
bool _was_alt_control_mode;
enum FW_POSCTRL_MODE {
FW_POSCTRL_MODE_AUTO,
FW_POSCTRL_MODE_POSITION,
FW_POSCTRL_MODE_OTHER
} _control_mode_current; ///< used to check the mode in the last control loop iteration. Use to check if the last iteration was in the same mode.
struct {
float l1_period;
@@ -471,7 +473,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
_global_pos_valid(false),
_l1_control(),
_mTecs(),
_was_pos_control_mode(false)
_control_mode_current(FW_POSCTRL_MODE_OTHER)
{
_nav_capabilities.turn_distance = 0.0f;
@@ -908,22 +910,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* no throttle limit as default */
float throttle_max = 1.0f;
/* AUTONOMOUS FLIGHT */
if (_control_mode.flag_control_auto_enabled &&
pos_sp_triplet.current.valid) {
/* AUTONOMOUS FLIGHT */
// XXX this should only execute if auto AND safety off (actuators active),
// else integrators should be constantly reset.
if (pos_sp_triplet.current.valid) {
if (!_was_pos_control_mode) {
/* 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);
}
}
_was_pos_control_mode = true;
_was_velocity_control_mode = false;
_control_mode_current = FW_POSCTRL_MODE_AUTO;
/* reset hold altitude */
_hold_alt = _global_pos.alt;
@@ -1208,15 +1207,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
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);
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
@@ -1248,53 +1247,63 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_att_sp.roll_reset_integral = true;
}
} else if (_control_mode.flag_control_velocity_enabled) {
} 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 */
const float deadBand = (60.0f/1000.0f);
const float factor = 1.0f - deadBand;
if (!_was_velocity_control_mode) {
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;
_was_alt_control_mode = false;
}
_was_velocity_control_mode = true;
_was_pos_control_mode = false;
// Get demanded airspeed
/* 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
float pitch = 0.0f;
/* Get demanded vertical velocity from pitch control */
static bool was_in_deadband = false;
if (_manual.x > deadBand) {
pitch = (_manual.x - deadBand) / factor;
} else if (_manual.x < - deadBand) {
pitch = (_manual.x + deadBand) / factor;
}
if (pitch > 0.0f) {
float pitch = (_manual.x - deadBand) / factor;
_hold_alt -= (_parameters.max_climb_rate * dt) * pitch;
_was_alt_control_mode = false;
} else if (pitch < 0.0f) {
was_in_deadband = false;
} else if (_manual.x < - deadBand) {
float pitch = (_manual.x + deadBand) / factor;
_hold_alt -= (_parameters.max_sink_rate * dt) * pitch;
_was_alt_control_mode = false;
} else if (!_was_alt_control_mode) {
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_alt_control_mode = true;
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);
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);
} else {
_was_velocity_control_mode = false;
_was_pos_control_mode = false;
_control_mode_current = FW_POSCTRL_MODE_OTHER;
/** MANUAL FLIGHT **/
@@ -1311,10 +1320,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
}
/* Copy thrust output for publication */
if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) {
/* Set thrust to 0 to minimize damage */
_att_sp.thrust = 0.0f;
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
} else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto
pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
/* making sure again that the correct thrust is used,
* without depending on library calls for safety reasons */
@@ -1327,8 +1338,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* During a takeoff waypoint while waiting for launch the pitch sp is set
* already (not by tecs) */
if (!(pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
launch_detection_state == LAUNCHDETECTION_RES_NONE)) {
if (!(_control_mode_current == FW_POSCTRL_MODE_AUTO &&
pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
launch_detection_state == LAUNCHDETECTION_RES_NONE)) {
_att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand();
}
src/modules/position_estimator_inav/position_estimator_inav_main.c
-4
View File
@@ -1052,10 +1052,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
landed = false;
landed_time = 0;
}
/* reset xy velocity estimates when landed */
x_est[1] = 0.0f;
y_est[1] = 0.0f;
} else {
if (alt_disp2 < land_disp2 && thrust < params.land_thr) {
if (landed_time == 0) {
src/modules/px4iofirmware/protocol.h
+2
View File
@@ -33,6 +33,8 @@
#pragma once
#include <inttypes.h>
/**
* @file protocol.h
*
src/modules/vtol_att_control/vtol_att_control_main.cpp
+5 -5
View File
@@ -238,11 +238,11 @@ VtolAttitudeControl::VtolAttitudeControl() :
_params.idle_pwm_mc = PWM_LOWEST_MIN;
_params.vtol_motor_count = 0;
_params_handles.idle_pwm_mc = param_find("IDLE_PWM_MC");
_params_handles.vtol_motor_count = param_find("VTOL_MOT_COUNT");
_params_handles.mc_airspeed_min = param_find("VTOL_MC_AIRSPEED_MIN");
_params_handles.mc_airspeed_max = param_find("VTOL_MC_AIRSPEED_MAX");
_params_handles.mc_airspeed_trim = param_find("VTOL_MC_AIRSPEED_TRIM");
_params_handles.idle_pwm_mc = param_find("VT_IDLE_PWM_MC");
_params_handles.vtol_motor_count = param_find("VT_MOT_COUNT");
_params_handles.mc_airspeed_min = param_find("VT_MC_ARSPD_MIN");
_params_handles.mc_airspeed_max = param_find("VT_MC_ARSPD_MAX");
_params_handles.mc_airspeed_trim = param_find("VT_MC_ARSPD_TRIM");
/* fetch initial parameter values */
parameters_update();
src/modules/vtol_att_control/vtol_att_control_params.c
+85 -10
View File
@@ -1,13 +1,88 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file vtol_att_control_params.c
* Parameters for vtol attitude controller.
*
* @author Roman Bapst <bapstr@ethz.ch>
*/
#include <systemlib/param/param.h>
// number of engines
PARAM_DEFINE_INT32(VTOL_MOT_COUNT,0);
// idle pwm in multicopter mode
PARAM_DEFINE_INT32(IDLE_PWM_MC,900);
// min airspeed in multicopter mode
PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MIN,2);
// max airspeed in multicopter mode
PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MAX,30);
// trim airspeed in multicopter mode
PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_TRIM,10);
/**
* VTOL number of engines
*
* @min 1.0
* @group VTOL Attitude Control
*/
PARAM_DEFINE_INT32(VT_MOT_COUNT,0);
/**
* Idle speed of VTOL when in multicopter mode
*
* @min 900
* @group VTOL Attitude Control
*/
PARAM_DEFINE_INT32(VT_IDLE_PWM_MC,900);
/**
* Minimum airspeed in multicopter mode
*
* This is the minimum speed of the air flowing over the control surfaces.
*
* @min 0.0
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MIN,2.0f);
/**
* Maximum airspeed in multicopter mode
*
* This is the maximum speed of the air flowing over the control surfaces.
*
* @min 0.0
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MAX,30.0f);
/**
* Trim airspeed when in multicopter mode
*
* This is the airflow over the control surfaces for which no airspeed scaling is applied in multicopter mode.
*
* @min 0.0
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_MC_ARSPD_TRIM,10.0f);