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

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

Browse Source
This commit is contained in:
Youssef Demitri
2015-10-14 11:02:23 +02:00
parent 1ec96d87d7 c599cf1256
commit c22c678b7d
498 changed files with 15244 additions and 11123 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/module.mk → src/modules/fw_pos_control_l1/CMakeLists.txt
+21 -22
View File
@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
# Copyright (c) 2015 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
@@ -30,24 +30,23 @@
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Fixedwing L1 position control application
#
MODULE_COMMAND = fw_pos_control_l1
SRCS = fw_pos_control_l1_main.cpp \
fw_pos_control_l1_params.c \
landingslope.cpp \
mtecs/mTecs.cpp \
mtecs/limitoverride.cpp \
mtecs/mTecs_params.c
# Startup handler, the actual app stack size is
# in the task_spawn command
MODULE_STACKSIZE = 1200
MAXOPTIMIZATION = -Os
EXTRACXXFLAGS = -Wno-float-equal
px4_add_module(
MODULE modules__fw_pos_control_l1
MAIN fw_pos_control_l1
STACK 1200
COMPILE_FLAGS
-Wno-float-equal
-Os
SRCS
fw_pos_control_l1_main.cpp
fw_pos_control_l1_params.c
landingslope.cpp
mtecs/mTecs.cpp
mtecs/limitoverride.cpp
mtecs/mTecs_params.c
DEPENDS
platforms__common
lib__external_lgpl
lib__ecl
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+2 -1
View File
@@ -80,6 +80,7 @@
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/tecs_status.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/pid/pid.h>
@@ -1947,7 +1948,7 @@ FixedwingPositionControl::start()
_control_task = px4_task_spawn_cmd("fw_pos_control_l1",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
1600,
1300,
(px4_main_t)&FixedwingPositionControl::task_main_trampoline,
nullptr);
src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+20 -10
View File
@@ -52,15 +52,17 @@ mTecs::mTecs() :
_mTecsEnabled(this, "ENABLED"),
_airspeedMin(this, "FW_AIRSPD_MIN", false),
/* Publications */
#if defined(__PX4_NUTTX)
_status(ORB_ID(tecs_status), &getPublications()),
#endif // defined(__PX4_NUTTX)
/* control blocks */
_controlTotalEnergy(this, "THR"),
_controlEnergyDistribution(this, "PIT", true),
_controlAltitude(this, "FPA", true),
_controlAirSpeed(this, "ACC"),
_flightPathAngleLowpass(this, "FPA_LP"),
_altitudeLowpass(this, "ALT_LP"),
_airspeedLowpass(this, "A_LP"),
_flightPathAngleLowpass(this, "FPA_LP", 50),
_altitudeLowpass(this, "ALT_LP", 50),
_airspeedLowpass(this, "A_LP", 50),
_airspeedDerivative(this, "AD"),
_throttleSp(0.0f),
_pitchSp(0.0f),
@@ -107,9 +109,11 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti
debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, altitude filtered %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)altitudeFiltered, (double)flightPathAngleSp);
}
#if defined(__PX4_NUTTX)
/* Write part of the status message */
_status.altitudeSp = altitudeSp;
_status.altitude_filtered = altitudeFiltered;
#endif // defined(__PX4_NUTTX)
/* use flightpath angle setpoint for total energy control */
@@ -143,9 +147,11 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
(double)airspeedFiltered, (double)accelerationLongitudinalSp);
}
#if defined(__PX4_NUTTX)
/* Write part of the status message */
_status.airspeedSp = airspeedSp;
_status.airspeed_filtered = airspeedFiltered;
#endif // defined(__PX4_NUTTX)
/* use longitudinal acceleration setpoint for total energy control */
@@ -200,23 +206,23 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
}
/* Check airspeed: if below safe value switch to underspeed mode (if not in land or takeoff mode) */
if (mode != tecs_status_s::TECS_MODE_LAND && mode != tecs_status_s::TECS_MODE_TAKEOFF && airspeedFiltered < _airspeedMin.get()) {
mode = tecs_status_s::TECS_MODE_UNDERSPEED;
if (mode != MTECS_MODE_LAND && mode != MTECS_MODE_TAKEOFF && airspeedFiltered < _airspeedMin.get()) {
mode = MTECS_MODE_UNDERSPEED;
}
/* Set special output limiters if we are not in TECS_MODE_NORMAL */
/* Set special output limiters if we are not in MTECS_MODE_NORMAL */
BlockOutputLimiter *outputLimiterThrottle = &_controlTotalEnergy.getOutputLimiter();
BlockOutputLimiter *outputLimiterPitch = &_controlEnergyDistribution.getOutputLimiter();
if (mode == tecs_status_s::TECS_MODE_TAKEOFF) {
if (mode == MTECS_MODE_TAKEOFF) {
outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle;
outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch;
} else if (mode == tecs_status_s::TECS_MODE_LAND) {
} else if (mode == MTECS_MODE_LAND) {
// only limit pitch but do not limit throttle
outputLimiterPitch = &_BlockOutputLimiterLandPitch;
} else if (mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM) {
} else if (mode == MTECS_MODE_LAND_THROTTLELIM) {
outputLimiterThrottle = &_BlockOutputLimiterLandThrottle;
outputLimiterPitch = &_BlockOutputLimiterLandPitch;
} else if (mode == tecs_status_s::TECS_MODE_UNDERSPEED) {
} else if (mode == MTECS_MODE_UNDERSPEED) {
outputLimiterThrottle = &_BlockOutputLimiterUnderspeedThrottle;
outputLimiterPitch = &_BlockOutputLimiterUnderspeedPitch;
}
@@ -226,6 +232,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
* is running) */
limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch);
// #if defined(__PX4_NUTTX)
// /* Write part of the status message */
// _status.flightPathAngleSp = flightPathAngleSp;
// _status.flightPathAngle = flightPathAngle;
@@ -237,6 +244,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
// _status.energyDistributionRateSp = energyDistributionRateSp;
// _status.energyDistributionRate = energyDistributionRate;
// _status.mode = mode;
// #endif // defined(__PX4_NUTTX)
/** update control blocks **/
/* update total energy rate control block */
@@ -253,8 +261,10 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
(double)accelerationLongitudinalSp, (double)airspeedDerivative);
}
#if defined(__PX4_NUTTX)
/* publish status message */
_status.update();
#endif // defined(__PX4_NUTTX)
/* clean up */
_firstIterationAfterReset = false;
src/modules/fw_pos_control_l1/mtecs/mTecs.h
+23 -3
View File
@@ -49,11 +49,25 @@
#include <controllib/block/BlockParam.hpp>
#include <drivers/drv_hrt.h>
#include <uORB/Publication.hpp>
#if defined(__PX4_NUTTX)
#include <uORB/topics/tecs_status.h>
#endif // defined(__PX4_NUTTX)
namespace fwPosctrl
{
/* corresponds to TECS_MODE in tecs_status.msg */
enum MTECS_MODE {
MTECS_MODE_NORMAL = 0,
MTECS_MODE_UNDERSPEED = 1,
MTECS_MODE_TAKEOFF = 2,
MTECS_MODE_LAND = 3,
MTECS_MODE_LAND_THROTTLELIM = 4,
MTECS_MODE_BAD_DESCENT = 5,
MTECS_MODE_CLIMBOUT = 6
};
/* Main class of the mTecs */
class mTecs : public control::SuperBlock
{
@@ -94,6 +108,10 @@ public:
float getThrottleSetpoint() { return _throttleSp; }
float getPitchSetpoint() { return _pitchSp; }
float airspeedLowpassUpdate(float input) { return _airspeedLowpass.update(input); }
float getFlightPathAngleLowpassState() { return _flightPathAngleLowpass.getState(); }
float getAltitudeLowpassState() { return _altitudeLowpass.getState(); }
float getAirspeedLowpassState() { return _airspeedLowpass.getState(); }
float getAirspeedDerivativeLowpassState() { return _airspeedDerivative.getO(); }
protected:
/* parameters */
@@ -101,7 +119,9 @@ protected:
control::BlockParamFloat _airspeedMin; /**< minimal airspeed */
/* Publications */
#if defined(__PX4_NUTTX)
uORB::Publication<tecs_status_s> _status; /**< publish internal values for logging */
#endif // defined(__PX4_NUTTX)
/* control blocks */
BlockFFPILimitedCustom _controlTotalEnergy; /**< FFPI controller for total energy control: output
@@ -114,9 +134,9 @@ protected:
setpoint */
/* Other calculation Blocks */
control::BlockLowPass _flightPathAngleLowpass; /**< low pass filter for the flight path angle */
control::BlockLowPass _altitudeLowpass; /**< low pass filter for altitude */
control::BlockLowPass _airspeedLowpass; /**< low pass filter for airspeed */
control::BlockLowPass2 _flightPathAngleLowpass; /**< low pass filter for the flight path angle */
control::BlockLowPass2 _altitudeLowpass; /**< low pass filter for altitude */
control::BlockLowPass2 _airspeedLowpass; /**< low pass filter for airspeed */
control::BlockDerivative _airspeedDerivative; /**< airspeed derivative calulation */
/* Output setpoints */