fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

mtecs: small cleanup, move subclass to own file

Browse Source
This commit is contained in:
Thomas Gubler 2014-06-11 20:22:37 +02:00
parent 7fa5458bc6
commit a1bcd5d313
6 changed files with 181 additions and 82 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -1456,7 +1456,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
if (ground_speed_length > FLT_EPSILON) {
flightPathAngle = -asinf(ground_speed(2)/ground_speed_length);
}
fwPosctrl::mTecs::LimitOverride limitOverride;
fwPosctrl::LimitOverride limitOverride;
if (climbout_mode) {
limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad);
} else {

1
src/modules/fw_pos_control_l1/module.mk
View File

@ -41,4 +41,5 @@ 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

71
src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp Normal file
View File

@ -0,0 +1,71 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@gmail.com>
*
* 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 limitoverride.cpp
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include "limitoverride.h"
namespace fwPosctrl {
bool LimitOverride::applyOverride(BlockOutputLimiter &outputLimiterThrottle,
BlockOutputLimiter &outputLimiterPitch)
{
bool ret = false;
if (overrideThrottleMinEnabled) {
outputLimiterThrottle.setMin(overrideThrottleMin);
ret = true;
}
if (overrideThrottleMaxEnabled) {
outputLimiterThrottle.setMax(overrideThrottleMax);
ret = true;
}
if (overridePitchMinEnabled) {
outputLimiterPitch.setMin(overridePitchMin);
ret = true;
}
if (overridePitchMaxEnabled) {
outputLimiterPitch.setMax(overridePitchMax);
ret = true;
}
return ret;
}
} /* namespace fwPosctrl */

107
src/modules/fw_pos_control_l1/mtecs/limitoverride.h Normal file
View File

@ -0,0 +1,107 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@gmail.com>
*
* 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 limitoverride.h
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#ifndef LIMITOVERRIDE_H_
#define LIMITOVERRIDE_H_
#include "mTecs_blocks.h"
namespace fwPosctrl
{
/* A small class which provides helper functions to override control output limits which are usually set by
* parameters in special cases
*/
class LimitOverride
{
public:
LimitOverride() :
overrideThrottleMinEnabled(false),
overrideThrottleMaxEnabled(false),
overridePitchMinEnabled(false),
overridePitchMaxEnabled(false)
{};
~LimitOverride() {};
/*
* Override the limits of the outputlimiter instances given by the arguments with the limits saved in
* this class (if enabled)
* @return true if the limit was applied
*/
bool applyOverride(BlockOutputLimiter &outputLimiterThrottle,
BlockOutputLimiter &outputLimiterPitch);
/* Functions to enable or disable the override */
void enableThrottleMinOverride(float value) { enable(&overrideThrottleMinEnabled,
&overrideThrottleMin, value); }
void disableThrottleMinOverride() { disable(&overrideThrottleMinEnabled); }
void enableThrottleMaxOverride(float value) { enable(&overrideThrottleMaxEnabled,
&overrideThrottleMax, value); }
void disableThrottleMaxOverride() { disable(&overrideThrottleMaxEnabled); }
void enablePitchMinOverride(float value) { enable(&overridePitchMinEnabled,
&overridePitchMin, value); }
void disablePitchMinOverride() { disable(&overridePitchMinEnabled); }
void enablePitchMaxOverride(float value) { enable(&overridePitchMaxEnabled,
&overridePitchMax, value); }
void disablePitchMaxOverride() { disable(&overridePitchMaxEnabled); }
protected:
bool overrideThrottleMinEnabled;
float overrideThrottleMin;
bool overrideThrottleMaxEnabled;
float overrideThrottleMax;
bool overridePitchMinEnabled;
float overridePitchMin; //in degrees (replaces param values)
bool overridePitchMaxEnabled;
float overridePitchMax; //in degrees (replaces param values)
/* Enable a specific limit override */
void enable(bool *flag, float *limit, float value) { *flag = true; *limit = value; };
/* Disable a specific limit override */
void disable(bool *flag) { *flag = false; };
};
} /* namespace fwPosctrl */
#endif /* LIMITOVERRIDE_H_ */

25
src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
View File

@ -302,29 +302,4 @@ void mTecs::debug(const char *fmt, ...) {
debug_print(fmt, args);
}
bool mTecs::LimitOverride::applyOverride(BlockOutputLimiter &outputLimiterThrottle,
BlockOutputLimiter &outputLimiterPitch)
{
bool ret = false;
if (overrideThrottleMinEnabled) {
outputLimiterThrottle.setMin(overrideThrottleMin);
ret = true;
}
if (overrideThrottleMaxEnabled) {
outputLimiterThrottle.setMax(overrideThrottleMax);
ret = true;
}
if (overridePitchMinEnabled) {
outputLimiterPitch.setMin(overridePitchMin);
ret = true;
}
if (overridePitchMaxEnabled) {
outputLimiterPitch.setMax(overridePitchMax);
ret = true;
}
return ret;
}
} /* namespace fwPosctrl */

57
src/modules/fw_pos_control_l1/mtecs/mTecs.h
View File

@ -44,6 +44,7 @@
#define MTECS_H_
#include "mTecs_blocks.h"
#include "limitoverride.h"
#include <controllib/block/BlockParam.hpp>
#include <drivers/drv_hrt.h>
@ -60,62 +61,6 @@ public:
mTecs();
virtual ~mTecs();
/* A small class which provides helper fucntions to override control output limits which are usually set by
* parameters in special cases
*/
class LimitOverride
{
public:
LimitOverride() :
overrideThrottleMinEnabled(false),
overrideThrottleMaxEnabled(false),
overridePitchMinEnabled(false),
overridePitchMaxEnabled(false)
{};
~LimitOverride() {};
/*
* Override the limits of the outputlimiter instances given by the arguments with the limits saved in
* this class (if enabled)
* @return true if the limit was applied
*/
bool applyOverride(BlockOutputLimiter &outputLimiterThrottle,
BlockOutputLimiter &outputLimiterPitch);
/* Functions to enable or disable the override */
void enableThrottleMinOverride(float value) { enable(&overrideThrottleMinEnabled,
&overrideThrottleMin, value); }
void disableThrottleMinOverride() { disable(&overrideThrottleMinEnabled); }
void enableThrottleMaxOverride(float value) { enable(&overrideThrottleMaxEnabled,
&overrideThrottleMax, value); }
void disableThrottleMaxOverride() { disable(&overrideThrottleMaxEnabled); }
void enablePitchMinOverride(float value) { enable(&overridePitchMinEnabled,
&overridePitchMin, value); }
void disablePitchMinOverride() { disable(&overridePitchMinEnabled); }
void enablePitchMaxOverride(float value) { enable(&overridePitchMaxEnabled,
&overridePitchMax, value); }
void disablePitchMaxOverride() { disable(&overridePitchMaxEnabled); }
protected:
bool overrideThrottleMinEnabled;
float overrideThrottleMin;
bool overrideThrottleMaxEnabled;
float overrideThrottleMax;
bool overridePitchMinEnabled;
float overridePitchMin; //in degrees (replaces param values)
bool overridePitchMaxEnabled;
float overridePitchMax; //in degrees (replaces param values)
/* Enable a specific limit override */
void enable(bool *flag, float *limit, float value) { *flag = true; *limit = value;
};
/* Disable a specific limit override */
void disable(bool *flag) { *flag = false; };
};
/*
* Control in altitude setpoint and speed mode
*/