diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-10-13 03:12:17 -0700 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-10-13 03:12:17 -0700 |
commit | 062ba0acab52f486661e95ef38d6f55cbec5fa6e (patch) | |
tree | e709d7b3955bc6d918007352643e274bf68fcdd1 | |
parent | db4ff7f080dab08e2558b006ac8c657466c144c6 (diff) | |
parent | 42c412f6ddf3963c8a9bbef5ba37896a24e1f0fa (diff) | |
download | px4-firmware-062ba0acab52f486661e95ef38d6f55cbec5fa6e.tar.gz px4-firmware-062ba0acab52f486661e95ef38d6f55cbec5fa6e.tar.bz2 px4-firmware-062ba0acab52f486661e95ef38d6f55cbec5fa6e.zip |
Merge pull request #453 from PX4/gcc_47
GCC 4.7.4 compatibility
-rw-r--r-- | makefiles/config_px4fmu-v1_default.mk | 5 | ||||
-rw-r--r-- | makefiles/config_px4fmu-v2_default.mk | 6 | ||||
-rw-r--r-- | src/modules/att_pos_estimator_ekf/KalmanNav.hpp | 26 | ||||
-rw-r--r-- | src/modules/controllib/block/BlockParam.hpp | 29 | ||||
-rw-r--r-- | src/modules/controllib/blocks.hpp | 36 |
5 files changed, 63 insertions, 39 deletions
diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 1dd949076..46640f3c5 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -134,7 +134,10 @@ MODULES += lib/geo # Tutorial code from # https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control -MODULES += examples/fixedwing_control +#MODULES += examples/fixedwing_control + +# Hardware test +#MODULES += examples/hwtest # # Transitional support - add commands from the NuttX export archive. diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 66216f8fe..bb589cb9f 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -128,8 +128,12 @@ MODULES += lib/geo # https://pixhawk.ethz.ch/px4/dev/debug_values #MODULES += examples/px4_mavlink_debug +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control +#MODULES += examples/fixedwing_control + # Hardware test -MODULES += examples/hwtest +#MODULES += examples/hwtest # # Transitional support - add commands from the NuttX export archive. diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp index f01ee0355..799fc2fb9 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp @@ -166,19 +166,19 @@ protected: double lat, lon; /**< lat, lon radians */ float alt; /**< altitude, meters */ // parameters - control::BlockParam<float> _vGyro; /**< gyro process noise */ - control::BlockParam<float> _vAccel; /**< accelerometer process noise */ - control::BlockParam<float> _rMag; /**< magnetometer measurement noise */ - control::BlockParam<float> _rGpsVel; /**< gps velocity measurement noise */ - control::BlockParam<float> _rGpsPos; /**< gps position measurement noise */ - control::BlockParam<float> _rGpsAlt; /**< gps altitude measurement noise */ - control::BlockParam<float> _rPressAlt; /**< press altitude measurement noise */ - control::BlockParam<float> _rAccel; /**< accelerometer measurement noise */ - control::BlockParam<float> _magDip; /**< magnetic inclination with level */ - control::BlockParam<float> _magDec; /**< magnetic declination, clockwise rotation */ - control::BlockParam<float> _g; /**< gravitational constant */ - control::BlockParam<float> _faultPos; /**< fault detection threshold for position */ - control::BlockParam<float> _faultAtt; /**< fault detection threshold for attitude */ + control::BlockParamFloat _vGyro; /**< gyro process noise */ + control::BlockParamFloat _vAccel; /**< accelerometer process noise */ + control::BlockParamFloat _rMag; /**< magnetometer measurement noise */ + control::BlockParamFloat _rGpsVel; /**< gps velocity measurement noise */ + control::BlockParamFloat _rGpsPos; /**< gps position measurement noise */ + control::BlockParamFloat _rGpsAlt; /**< gps altitude measurement noise */ + control::BlockParamFloat _rPressAlt; /**< press altitude measurement noise */ + control::BlockParamFloat _rAccel; /**< accelerometer measurement noise */ + control::BlockParamFloat _magDip; /**< magnetic inclination with level */ + control::BlockParamFloat _magDec; /**< magnetic declination, clockwise rotation */ + control::BlockParamFloat _g; /**< gravitational constant */ + control::BlockParamFloat _faultPos; /**< fault detection threshold for position */ + control::BlockParamFloat _faultAtt; /**< fault detection threshold for attitude */ // status bool _attitudeInitialized; bool _positionInitialized; diff --git a/src/modules/controllib/block/BlockParam.hpp b/src/modules/controllib/block/BlockParam.hpp index 58a9bfc0d..36bc8c24b 100644 --- a/src/modules/controllib/block/BlockParam.hpp +++ b/src/modules/controllib/block/BlockParam.hpp @@ -69,22 +69,39 @@ protected: /** * Parameters that are tied to blocks for updating and nameing. */ -template<class T> -class __EXPORT BlockParam : public BlockParamBase + +class __EXPORT BlockParamFloat : public BlockParamBase +{ +public: + BlockParamFloat(Block *block, const char *name, bool parent_prefix=true) : + BlockParamBase(block, name, parent_prefix), + _val() { + update(); + } + float get() { return _val; } + void set(float val) { _val = val; } + void update() { + if (_handle != PARAM_INVALID) param_get(_handle, &_val); + } +protected: + float _val; +}; + +class __EXPORT BlockParamInt : public BlockParamBase { public: - BlockParam(Block *block, const char *name, bool parent_prefix=true) : + BlockParamInt(Block *block, const char *name, bool parent_prefix=true) : BlockParamBase(block, name, parent_prefix), _val() { update(); } - T get() { return _val; } - void set(T val) { _val = val; } + int get() { return _val; } + void set(int val) { _val = val; } void update() { if (_handle != PARAM_INVALID) param_get(_handle, &_val); } protected: - T _val; + int _val; }; } // namespace control diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp index fefe99702..66e929038 100644 --- a/src/modules/controllib/blocks.hpp +++ b/src/modules/controllib/blocks.hpp @@ -74,8 +74,8 @@ public: float getMax() { return _max.get(); } protected: // attributes - BlockParam<float> _min; - BlockParam<float> _max; + control::BlockParamFloat _min; + control::BlockParamFloat _max; }; int __EXPORT blockLimitTest(); @@ -99,7 +99,7 @@ public: float getMax() { return _max.get(); } protected: // attributes - BlockParam<float> _max; + control::BlockParamFloat _max; }; int __EXPORT blockLimitSymTest(); @@ -126,7 +126,7 @@ public: protected: // attributes float _state; - BlockParam<float> _fCut; + control::BlockParamFloat _fCut; }; int __EXPORT blockLowPassTest(); @@ -157,7 +157,7 @@ protected: // attributes float _u; /**< previous input */ float _y; /**< previous output */ - BlockParam<float> _fCut; /**< cut-off frequency, Hz */ + control::BlockParamFloat _fCut; /**< cut-off frequency, Hz */ }; int __EXPORT blockHighPassTest(); @@ -273,7 +273,7 @@ public: // accessors float getKP() { return _kP.get(); } protected: - BlockParam<float> _kP; + control::BlockParamFloat _kP; }; int __EXPORT blockPTest(); @@ -303,8 +303,8 @@ public: BlockIntegral &getIntegral() { return _integral; } private: BlockIntegral _integral; - BlockParam<float> _kP; - BlockParam<float> _kI; + control::BlockParamFloat _kP; + control::BlockParamFloat _kI; }; int __EXPORT blockPITest(); @@ -334,8 +334,8 @@ public: BlockDerivative &getDerivative() { return _derivative; } private: BlockDerivative _derivative; - BlockParam<float> _kP; - BlockParam<float> _kD; + control::BlockParamFloat _kP; + control::BlockParamFloat _kD; }; int __EXPORT blockPDTest(); @@ -372,9 +372,9 @@ private: // attributes BlockIntegral _integral; BlockDerivative _derivative; - BlockParam<float> _kP; - BlockParam<float> _kI; - BlockParam<float> _kD; + control::BlockParamFloat _kP; + control::BlockParamFloat _kI; + control::BlockParamFloat _kD; }; int __EXPORT blockPIDTest(); @@ -404,7 +404,7 @@ public: float get() { return _val; } private: // attributes - BlockParam<float> _trim; + control::BlockParamFloat _trim; BlockLimit _limit; float _val; }; @@ -439,8 +439,8 @@ public: float getMax() { return _max.get(); } private: // attributes - BlockParam<float> _min; - BlockParam<float> _max; + control::BlockParamFloat _min; + control::BlockParamFloat _max; }; int __EXPORT blockRandUniformTest(); @@ -486,8 +486,8 @@ public: float getStdDev() { return _stdDev.get(); } private: // attributes - BlockParam<float> _mean; - BlockParam<float> _stdDev; + control::BlockParamFloat _mean; + control::BlockParamFloat _stdDev; }; int __EXPORT blockRandGaussTest(); |