aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--src/lib/mathlib/math/Matrix.hpp25
-rw-r--r--src/lib/mathlib/math/Vector.hpp21
-rw-r--r--src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp13
-rw-r--r--src/modules/ekf_att_pos_estimator/estimator_utilities.cpp7
-rw-r--r--src/modules/ekf_att_pos_estimator/module.mk2
5 files changed, 49 insertions, 19 deletions
diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp
index ea0cf4ca1..ca931e2da 100644
--- a/src/lib/mathlib/math/Matrix.hpp
+++ b/src/lib/mathlib/math/Matrix.hpp
@@ -69,27 +69,34 @@ public:
/**
* trivial ctor
- * note that this ctor will not initialize elements
+ * Initializes the elements to zero.
*/
- MatrixBase() {
- arm_mat = {M, N, &data[0][0]};
+ MatrixBase() :
+ data{},
+ arm_mat{M, N, &data[0][0]}
+ {
}
+ virtual ~MatrixBase() {};
+
/**
* copyt ctor
*/
- MatrixBase(const MatrixBase<M, N> &m) {
- arm_mat = {M, N, &data[0][0]};
+ MatrixBase(const MatrixBase<M, N> &m) :
+ arm_mat{M, N, &data[0][0]}
+ {
memcpy(data, m.data, sizeof(data));
}
- MatrixBase(const float *d) {
- arm_mat = {M, N, &data[0][0]};
+ MatrixBase(const float *d) :
+ arm_mat{M, N, &data[0][0]}
+ {
memcpy(data, d, sizeof(data));
}
- MatrixBase(const float d[M][N]) {
- arm_mat = {M, N, &data[0][0]};
+ MatrixBase(const float d[M][N]) :
+ arm_mat{M, N, &data[0][0]}
+ {
memcpy(data, d, sizeof(data));
}
diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp
index c7323c215..0ddf77615 100644
--- a/src/lib/mathlib/math/Vector.hpp
+++ b/src/lib/mathlib/math/Vector.hpp
@@ -69,25 +69,32 @@ public:
/**
* trivial ctor
- * note that this ctor will not initialize elements
+ * initializes elements to zero
*/
- VectorBase() {
- arm_col = {N, 1, &data[0]};
+ VectorBase() :
+ data{},
+ arm_col{N, 1, &data[0]}
+ {
+
}
+ virtual ~VectorBase() {};
+
/**
* copy ctor
*/
- VectorBase(const VectorBase<N> &v) {
- arm_col = {N, 1, &data[0]};
+ VectorBase(const VectorBase<N> &v) :
+ arm_col{N, 1, &data[0]}
+ {
memcpy(data, v.data, sizeof(data));
}
/**
* setting ctor
*/
- VectorBase(const float d[N]) {
- arm_col = {N, 1, &data[0]};
+ VectorBase(const float d[N]) :
+ arm_col{N, 1, &data[0]}
+ {
memcpy(data, d, sizeof(data));
}
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 40cb6043f..b3eb816f9 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -112,6 +112,10 @@ public:
*/
FixedwingEstimator();
+ /* we do not want people ever copying this class */
+ FixedwingEstimator(const FixedwingEstimator& that) = delete;
+ FixedwingEstimator operator=(const FixedwingEstimator&) = delete;
+
/**
* Destructor, also kills the sensors task.
*/
@@ -371,9 +375,10 @@ FixedwingEstimator::FixedwingEstimator() :
_mag_offsets({}),
#ifdef SENSOR_COMBINED_SUB
- _sensor_combined({}),
+ _sensor_combined{},
#endif
+ _pos_ref{},
_baro_ref(0.0f),
_baro_ref_offset(0.0f),
_baro_gps_offset(0.0f),
@@ -390,12 +395,18 @@ FixedwingEstimator::FixedwingEstimator() :
/* states */
_baro_init(false),
_gps_initialized(false),
+ _gps_start_time(0),
+ _filter_start_time(0),
+ _last_sensor_timestamp(0),
+ _last_run(0),
_gyro_valid(false),
_accel_valid(false),
_mag_valid(false),
_ekf_logging(true),
_debug(0),
_mavlink_fd(-1),
+ _parameters{},
+ _parameter_handles{},
_ekf(nullptr),
_velocity_xy_filtered(0.0f),
_velocity_z_filtered(0.0f),
diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp
index 29a8c8d1e..77cc1eeeb 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp
@@ -45,8 +45,11 @@ void Vector3f::zero(void)
z = 0.0f;
}
-Mat3f::Mat3f() {
- identity();
+Mat3f::Mat3f() :
+ x{1.0f, 0.0f, 0.0f},
+ y{0.0f, 1.0f, 0.0f},
+ z{0.0f, 0.0f, 1.0f}
+{
}
void Mat3f::identity() {
diff --git a/src/modules/ekf_att_pos_estimator/module.mk b/src/modules/ekf_att_pos_estimator/module.mk
index dc5220bf0..36d854ddd 100644
--- a/src/modules/ekf_att_pos_estimator/module.mk
+++ b/src/modules/ekf_att_pos_estimator/module.mk
@@ -41,3 +41,5 @@ SRCS = ekf_att_pos_estimator_main.cpp \
ekf_att_pos_estimator_params.c \
estimator_23states.cpp \
estimator_utilities.cpp
+
+EXTRACXXFLAGS = -Weffc++