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

Control lib update.

Browse Source
This commit is contained in:
jgoppert
2015-10-20 10:48:10 -04:00
parent a57f8e2ce3
commit b9e461b028
12 changed files with 255 additions and 171 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/controllib/block/Block.hpp
+18 -14
View File
@@ -58,8 +58,8 @@ static const uint16_t maxPublicationsPerBlock = 100;
static const uint8_t blockNameLengthMax = 80;
// forward declaration
class SuperBlock;
class BlockParamBase;
class SuperBlock;
/**
*/
@@ -81,9 +81,9 @@ public:
protected:
// accessors
SuperBlock *getParent() { return _parent; }
List<uORB::SubscriptionNode *> & getSubscriptions() { return _subscriptions; }
List<uORB::PublicationNode *> & getPublications() { return _publications; }
List<BlockParamBase *> & getParams() { return _params; }
List<uORB::SubscriptionNode *> &getSubscriptions() { return _subscriptions; }
List<uORB::PublicationNode *> &getPublications() { return _publications; }
List<BlockParamBase *> &getParams() { return _params; }
// attributes
const char *_name;
SuperBlock *_parent;
@@ -94,8 +94,8 @@ protected:
private:
/* this class has pointer data members and should not be copied (private constructor) */
Block(const control::Block&);
Block operator=(const control::Block&);
Block(const control::Block &);
Block operator=(const control::Block &);
};
class __EXPORT SuperBlock :
@@ -106,28 +106,32 @@ public:
// methods
SuperBlock(SuperBlock *parent, const char *name) :
Block(parent, name),
_children() {
_children()
{
}
virtual ~SuperBlock() {};
virtual void setDt(float dt);
virtual void updateParams() {
virtual void updateParams()
{
Block::updateParams();
if (getChildren().getHead() != NULL) updateChildParams();
if (getChildren().getHead() != NULL) { updateChildParams(); }
}
virtual void updateSubscriptions() {
virtual void updateSubscriptions()
{
Block::updateSubscriptions();
if (getChildren().getHead() != NULL) updateChildSubscriptions();
if (getChildren().getHead() != NULL) { updateChildSubscriptions(); }
}
virtual void updatePublications() {
virtual void updatePublications()
{
Block::updatePublications();
if (getChildren().getHead() != NULL) updateChildPublications();
if (getChildren().getHead() != NULL) { updateChildPublications(); }
}
protected:
// methods
List<Block *> & getChildren() { return _children; }
List<Block *> &getChildren() { return _children; }
void updateChildParams();
void updateChildSubscriptions();
void updateChildPublications();
src/modules/controllib/block/BlockParam.cpp
+12 -7
View File
@@ -65,6 +65,7 @@ BlockParamBase::BlockParamBase(Block *parent, const char *name, bool parent_pref
} else if (parent_prefix) {
snprintf(fullname, blockNameLengthMax, "%s_%s", parentName, name);
} else {
strncpy(fullname, name, blockNameLengthMax);
}
@@ -74,15 +75,17 @@ BlockParamBase::BlockParamBase(Block *parent, const char *name, bool parent_pref
_handle = param_find(fullname);
if (_handle == PARAM_INVALID)
if (_handle == PARAM_INVALID) {
printf("error finding param: %s\n", fullname);
}
};
template <class T>
BlockParam<T>::BlockParam(Block *block, const char *name,
bool parent_prefix) :
bool parent_prefix) :
BlockParamBase(block, name, parent_prefix),
_val() {
_val()
{
update();
}
@@ -93,13 +96,15 @@ template <class T>
void BlockParam<T>::set(T val) { _val = val; }
template <class T>
void BlockParam<T>::update() {
if (_handle != PARAM_INVALID) param_get(_handle, &_val);
void BlockParam<T>::update()
{
if (_handle != PARAM_INVALID) { param_get(_handle, &_val); }
}
template <class T>
void BlockParam<T>::commit() {
if (_handle != PARAM_INVALID) param_set(_handle, &_val);
void BlockParam<T>::commit()
{
if (_handle != PARAM_INVALID) { param_set(_handle, &_val); }
}
template <class T>
src/modules/controllib/blocks.cpp
+5
View File
@@ -126,6 +126,7 @@ float BlockLowPass::update(float input)
if (!isfinite(getState())) {
setState(input);
}
float b = 2 * float(M_PI) * getFCut() * getDt();
float a = b / (1 + b);
setState(a * input + (1 - a)*getState());
@@ -209,6 +210,7 @@ float BlockLowPass2::update(float input)
if (fabsf(_lp.get_cutoff_freq() - getFCutParam()) > FLT_EPSILON) {
_lp.set_cutoff_frequency(_fs, getFCutParam());
}
_state = _lp.apply(input);
return _state;
}
@@ -340,8 +342,10 @@ int blockIntegralTrapTest()
float BlockDerivative::update(float input)
{
float output;
if (_initialized) {
output = _lowPass.update((input - getU()) / getDt());
} else {
// if this is the first call to update
// we have no valid derivative
@@ -351,6 +355,7 @@ float BlockDerivative::update(float input)
output = 0.0f;
_initialized = true;
}
setU(input);
return output;
}
src/modules/controllib/blocks.hpp
+20 -10
View File
@@ -317,7 +317,8 @@ public:
_kP(this, "") // only one param, no need to name
{};
virtual ~BlockP() {};
float update(float input) {
float update(float input)
{
return getKP() * input;
}
// accessors
@@ -343,7 +344,8 @@ public:
_kI(this, "I")
{};
virtual ~BlockPI() {};
float update(float input) {
float update(float input)
{
return getKP() * input +
getKI() * getIntegral().update(input);
}
@@ -374,7 +376,8 @@ public:
_kD(this, "D")
{};
virtual ~BlockPD() {};
float update(float input) {
float update(float input)
{
return getKP() * input +
getKD() * getDerivative().update(input);
}
@@ -407,7 +410,8 @@ public:
_kD(this, "D")
{};
virtual ~BlockPID() {};
float update(float input) {
float update(float input)
{
return getKP() * input +
getKI() * getIntegral().update(input) +
getKD() * getDerivative().update(input);
@@ -440,11 +444,13 @@ public:
SuperBlock(parent, name),
_trim(this, "TRIM"),
_limit(this, ""),
_val(0) {
_val(0)
{
update(0);
};
virtual ~BlockOutput() {};
void update(float input) {
void update(float input)
{
_val = _limit.update(input + getTrim());
}
// accessors
@@ -472,13 +478,15 @@ public:
const char *name) :
Block(parent, name),
_min(this, "MIN"),
_max(this, "MAX") {
_max(this, "MAX")
{
// seed should be initialized somewhere
// in main program for all calls to rand
// XXX currently in nuttx if you seed to 0, rand breaks
};
virtual ~BlockRandUniform() {};
float update() {
float update()
{
static float rand_max = MAX_RAND;
float rand_val = rand();
float bounds = getMax() - getMin();
@@ -503,13 +511,15 @@ public:
const char *name) :
Block(parent, name),
_mean(this, "MEAN"),
_stdDev(this, "DEV") {
_stdDev(this, "DEV")
{
// seed should be initialized somewhere
// in main program for all calls to rand
// XXX currently in nuttx if you seed to 0, rand breaks
};
virtual ~BlockRandGauss() {};
float update() {
float update()
{
static float V1, V2, S;
static int phase = 0;
float X;
src/modules/controllib/uorb/blocks.cpp
+14 -13
View File
@@ -53,10 +53,11 @@ BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *nam
BlockWaypointGuidance::~BlockWaypointGuidance() {};
void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
vehicle_attitude_s &att,
position_setpoint_s &missionCmd,
position_setpoint_s &lastMissionCmd)
void BlockWaypointGuidance::update(
const vehicle_global_position_s &pos,
const vehicle_attitude_s &att,
const position_setpoint_s &missionCmd,
const position_setpoint_s &lastMissionCmd)
{
// heading to waypoint
@@ -83,16 +84,16 @@ void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
// subscriptions
_att(ORB_ID(vehicle_attitude), 20, &getSubscriptions()),
_attCmd(ORB_ID(vehicle_attitude_setpoint), 20, &getSubscriptions()),
_ratesCmd(ORB_ID(vehicle_rates_setpoint), 20, &getSubscriptions()),
_pos(ORB_ID(vehicle_global_position), 20, &getSubscriptions()),
_missionCmd(ORB_ID(position_setpoint_triplet), 20, &getSubscriptions()),
_manual(ORB_ID(manual_control_setpoint), 20, &getSubscriptions()),
_status(ORB_ID(vehicle_status), 20, &getSubscriptions()),
_param_update(ORB_ID(parameter_update), 1000, &getSubscriptions()), // limit to 1 Hz
_att(ORB_ID(vehicle_attitude), 20, 0, &getSubscriptions()),
_attCmd(ORB_ID(vehicle_attitude_setpoint), 20, 0, &getSubscriptions()),
_ratesCmd(ORB_ID(vehicle_rates_setpoint), 20, 0, &getSubscriptions()),
_pos(ORB_ID(vehicle_global_position), 20, 0, &getSubscriptions()),
_missionCmd(ORB_ID(position_setpoint_triplet), 20, 0, &getSubscriptions()),
_manual(ORB_ID(manual_control_setpoint), 20, 0, &getSubscriptions()),
_status(ORB_ID(vehicle_status), 20, 0, &getSubscriptions()),
_param_update(ORB_ID(parameter_update), 1000, 0, &getSubscriptions()), // limit to 1 Hz
// publications
_actuators(ORB_ID(actuator_controls_0), &getPublications())
_actuators(ORB_ID(actuator_controls_0), -1, &getPublications())
{
}
src/modules/controllib/uorb/blocks.hpp
+4 -4
View File
@@ -80,10 +80,10 @@ private:
public:
BlockWaypointGuidance(SuperBlock *parent, const char *name);
virtual ~BlockWaypointGuidance();
void update(vehicle_global_position_s &pos,
vehicle_attitude_s &att,
position_setpoint_s &missionCmd,
position_setpoint_s &lastMissionCmd);
void update(const vehicle_global_position_s &pos,
const vehicle_attitude_s &att,
const position_setpoint_s &missionCmd,
const position_setpoint_s &lastMissionCmd);
float getPsiCmd() { return _psiCmd; }
};