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

Merge branch 'navigator_rewrite' into navigator_rewrite_estimator

This commit is contained in:
Julian Oes 2014-06-10 14:32:21 +02:00
parent 47b2266b91 784041e0ab
commit f8b4295cfd
2 changed files with 278 additions and 97 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

367
src/modules/fw_att_control/fw_att_control_params.c
View File

@ -50,149 +50,322 @@ f * Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Controller parameters, accessible via MAVLink * Controller parameters, accessible via MAVLink
* *
*/ */
// @DisplayName Attitude Time Constant
// @Description This defines the latency between a step input and the achieved setpoint (inverse to a P gain). Half a second is a good start value and fits for most average systems. Smaller systems may require smaller values, but as this will wear out servos faster, the value should only be decreased as needed. /**
// @Range 0.4 to 1.0 seconds, in tens of seconds * Attitude Time Constant
*
* This defines the latency between a step input and the achieved setpoint
* (inverse to a P gain). Half a second is a good start value and fits for
* most average systems. Smaller systems may require smaller values, but as
* this will wear out servos faster, the value should only be decreased as
* needed.
*
* @unit seconds
* @min 0.4
* @max 1.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.5f); PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.5f);
// @DisplayName Pitch rate proportional gain. /**
// @Description This defines how much the elevator input will be commanded depending on the current body angular rate error. * Pitch rate proportional gain.
// @Range 10 to 200, 1 increments *
* This defines how much the elevator input will be commanded depending on the
* current body angular rate error.
*
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_PR_P, 0.05f); PARAM_DEFINE_FLOAT(FW_PR_P, 0.05f);
// @DisplayName Pitch rate integrator gain. /**
// @Description This gain defines how much control response will result out of a steady state error. It trims any constant error. * Pitch rate integrator gain.
// @Range 0 to 50.0 *
* This gain defines how much control response will result out of a steady
* state error. It trims any constant error.
*
* @min 0.0
* @max 50.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_PR_I, 0.0f); PARAM_DEFINE_FLOAT(FW_PR_I, 0.0f);
// @DisplayName Maximum positive / up pitch rate. /**
// @Description This limits the maximum pitch up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit. * Maximum positive / up pitch rate.
// @Range 0 to 90.0 degrees per seconds, in 1 increments *
* This limits the maximum pitch up angular rate the controller will output (in
* degrees per second). Setting a value of zero disables the limit.
*
* @unit deg/s
* @min 0.0
* @max 90.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 0.0f); PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 0.0f);
// @DisplayName Maximum negative / down pitch rate. /**
// @Description This limits the maximum pitch down up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit. * Maximum negative / down pitch rate.
// @Range 0 to 90.0 degrees per seconds, in 1 increments *
* This limits the maximum pitch down up angular rate the controller will
* output (in degrees per second). Setting a value of zero disables the limit.
*
* @unit deg/s
* @min 0.0
* @max 90.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 0.0f); PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 0.0f);
// @DisplayName Pitch rate integrator limit /**
// @Description The portion of the integrator part in the control surface deflection is limited to this value * Pitch rate integrator limit
// @Range 0.0 to 1 *
// @Increment 0.1 * The portion of the integrator part in the control surface deflection is
* limited to this value
*
* @min 0.0
* @max 1.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.2f); PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.2f);
// @DisplayName Roll to Pitch feedforward gain. /**
// @Description This compensates during turns and ensures the nose stays level. * Roll to Pitch feedforward gain.
// @Range 0.5 2.0 *
// @Increment 0.05 * This compensates during turns and ensures the nose stays level.
// @User User *
* @min 0.0
* @max 2.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 0.0f); //xxx: set to 0 as default, see comment in ECL_PitchController::control_attitude (float turn_offset = ...) PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 0.0f); //xxx: set to 0 as default, see comment in ECL_PitchController::control_attitude (float turn_offset = ...)
// @DisplayName Roll rate proportional Gain. /**
// @Description This defines how much the aileron input will be commanded depending on the current body angular rate error. * Roll rate proportional Gain
// @Range 10.0 200.0 *
// @Increment 10.0 * This defines how much the aileron input will be commanded depending on the
// @User User * current body angular rate error.
*
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f); PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f);
// @DisplayName Roll rate integrator Gain /**
// @Description This gain defines how much control response will result out of a steady state error. It trims any constant error. * Roll rate integrator Gain
// @Range 0.0 100.0 *
// @Increment 5.0 * This gain defines how much control response will result out of a steady
// @User User * state error. It trims any constant error.
*
* @min 0.0
* @max 100.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_RR_I, 0.0f); PARAM_DEFINE_FLOAT(FW_RR_I, 0.0f);
// @DisplayName Roll Integrator Anti-Windup /**
// @Description The portion of the integrator part in the control surface deflection is limited to this value. * Roll Integrator Anti-Windup
// @Range 0.0 to 1.0 *
// @Increment 0.1 * The portion of the integrator part in the control surface deflection is limited to this value.
*
* @min 0.0
* @max 1.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_RR_IMAX, 0.2f); PARAM_DEFINE_FLOAT(FW_RR_IMAX, 0.2f);
// @DisplayName Maximum Roll Rate /**
// @Description This limits the maximum roll rate the controller will output (in degrees per second). Setting a value of zero disables the limit. * Maximum Roll Rate
// @Range 0 to 90.0 degrees per seconds *
// @Increment 1.0 * This limits the maximum roll rate the controller will output (in degrees per
PARAM_DEFINE_FLOAT(FW_R_RMAX, 0); * second). Setting a value of zero disables the limit.
*
* @unit deg/s
* @min 0.0
* @max 90.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_R_RMAX, 0.0f);
// @DisplayName Yaw rate proportional gain. /**
// @Description This defines how much the rudder input will be commanded depending on the current body angular rate error. * Yaw rate proportional gain
// @Range 10 to 200, 1 increments *
PARAM_DEFINE_FLOAT(FW_YR_P, 0.05); * This defines how much the rudder input will be commanded depending on the
* current body angular rate error.
*
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_YR_P, 0.05f);
// @DisplayName Yaw rate integrator gain. /**
// @Description This gain defines how much control response will result out of a steady state error. It trims any constant error. * Yaw rate integrator gain
// @Range 0 to 50.0 *
* This gain defines how much control response will result out of a steady
* state error. It trims any constant error.
*
* @min 0.0
* @max 50.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_YR_I, 0.0f); PARAM_DEFINE_FLOAT(FW_YR_I, 0.0f);
// @DisplayName Yaw rate integrator limit /**
// @Description The portion of the integrator part in the control surface deflection is limited to this value * Yaw rate integrator limit
// @Range 0.0 to 1 *
// @Increment 0.1 * The portion of the integrator part in the control surface deflection is
* limited to this value
*
* @min 0.0
* @max 1.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f); PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f);
// @DisplayName Maximum Yaw Rate /**
// @Description This limits the maximum yaw rate the controller will output (in degrees per second). Setting a value of zero disables the limit. * Maximum Yaw Rate
// @Range 0 to 90.0 degrees per seconds *
// @Increment 1.0 * This limits the maximum yaw rate the controller will output (in degrees per
PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0); * second). Setting a value of zero disables the limit.
*
* @unit deg/s
* @min 0.0
* @max 90.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f);
// @DisplayName Roll rate feed forward /**
// @Description Direct feed forward from rate setpoint to control surface output * Roll rate feed forward
// @Range 0 to 10 *
// @Increment 0.1 * Direct feed forward from rate setpoint to control surface output
*
* @min 0.0
* @max 10.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_RR_FF, 0.3f); PARAM_DEFINE_FLOAT(FW_RR_FF, 0.3f);
// @DisplayName Pitch rate feed forward /**
// @Description Direct feed forward from rate setpoint to control surface output * Pitch rate feed forward
// @Range 0 to 10 *
// @Increment 0.1 * Direct feed forward from rate setpoint to control surface output
*
* @min 0.0
* @max 10.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_PR_FF, 0.4f); PARAM_DEFINE_FLOAT(FW_PR_FF, 0.4f);
// @DisplayName Yaw rate feed forward /**
// @Description Direct feed forward from rate setpoint to control surface output * Yaw rate feed forward
// @Range 0 to 10 *
// @Increment 0.1 * Direct feed forward from rate setpoint to control surface output
*
* @min 0.0
* @max 10.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f); PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f);
// @DisplayName Minimal speed for yaw coordination /**
// @Description For airspeeds above this value the yaw rate is calculated for a coordinated turn. Set to a very high value to disable. * Minimal speed for yaw coordination
// @Range 0 to 90.0 degrees per seconds *
// @Increment 1.0 * For airspeeds above this value, the yaw rate is calculated for a coordinated
* turn. Set to a very high value to disable.
*
* @unit m/s
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_YCO_VMIN, 1000.0f); PARAM_DEFINE_FLOAT(FW_YCO_VMIN, 1000.0f);
/* Airspeed parameters: the following parameters about airspeed are used by the attitude and the positon controller */ /* Airspeed parameters:
* The following parameters about airspeed are used by the attitude and the
* position controller.
* */
// @DisplayName Minimum Airspeed /**
// @Description If the airspeed falls below this value the TECS controller will try to increase airspeed more aggressively * Minimum Airspeed
// @Range 0.0 to 30 *
* If the airspeed falls below this value, the TECS controller will try to
* increase airspeed more aggressively.
*
* @unit m/s
* @min 0.0
* @max 30.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 13.0f); PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 13.0f);
// @DisplayName Trim Airspeed /**
// @Description The TECS controller tries to fly at this airspeed * Trim Airspeed
// @Range 0.0 to 30 *
* The TECS controller tries to fly at this airspeed.
*
* @unit m/s
* @min 0.0
* @max 30.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 20.0f); PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 20.0f);
// @DisplayName Maximum Airspeed /**
// @Description If the airspeed is above this value the TECS controller will try to decrease airspeed more aggressively * Maximum Airspeed
// @Range 0.0 to 30 *
* If the airspeed is above this value, the TECS controller will try to decrease
* airspeed more aggressively.
*
* @unit m/s
* @min 0.0
* @max 30.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 50.0f); PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 50.0f);
// @DisplayName Roll Setpoint Offset /**
// @Description An airframe specific offset of the roll setpoint in degrees, the value is added to the roll setpoint and should correspond to the typical cruise speed of the airframe * Roll Setpoint Offset
// @Range -90.0 to 90.0 *
* An airframe specific offset of the roll setpoint in degrees, the value is
* added to the roll setpoint and should correspond to the typical cruise speed
* of the airframe.
*
* @unit deg
* @min -90.0
* @max 90.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_RSP_OFF, 0.0f); PARAM_DEFINE_FLOAT(FW_RSP_OFF, 0.0f);
// @DisplayName Pitch Setpoint Offset /**
// @Description An airframe specific offset of the pitch setpoint in degrees, the value is added to the pitch setpoint and should correspond to the typical cruise speed of the airframe * Pitch Setpoint Offset
// @Range -90.0 to 90.0 *
* An airframe specific offset of the pitch setpoint in degrees, the value is
* added to the pitch setpoint and should correspond to the typical cruise
* speed of the airframe.
*
* @unit deg
* @min -90.0
* @max 90.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_PSP_OFF, 0.0f); PARAM_DEFINE_FLOAT(FW_PSP_OFF, 0.0f);
// @DisplayName Max Manual Roll /**
// @Description Max roll for manual control in attitude stabilized mode * Max Manual Roll
// @Range 0.0 to 90.0 *
* Max roll for manual control in attitude stabilized mode
*
* @unit deg
* @min 0.0
* @max 90.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f); PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f);
// @DisplayName Max Manual Pitch /**
// @Description Max pitch for manual control in attitude stabilized mode * Max Manual Pitch
// @Range 0.0 to 90.0 *
* Max pitch for manual control in attitude stabilized mode
*
* @unit deg
* @min 0.0
* @max 90.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 45.0f); PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 45.0f);

8
src/modules/navigator/navigator_main.cpp
View File

@ -381,6 +381,14 @@ Navigator::task_main()
_loiter.reset(); _loiter.reset();
} }
/* if nothing is running, set position setpoint triplet invalid */
if (_navigation_mode == nullptr) {
_pos_sp_triplet.previous.valid = false;
_pos_sp_triplet.current.valid = false;
_pos_sp_triplet.next.valid = false;
_update_triplet = true;
}
if (_update_triplet ) { if (_update_triplet ) {
publish_position_setpoint_triplet(); publish_position_setpoint_triplet();
_update_triplet = false; _update_triplet = false;