aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--Makefile1
-rw-r--r--src/drivers/gps/mtk.cpp22
-rw-r--r--src/lib/ecl/attitude_fw/ecl_controller.cpp128
-rw-r--r--src/lib/ecl/attitude_fw/ecl_controller.h119
-rw-r--r--src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp80
-rw-r--r--src/lib/ecl/attitude_fw/ecl_pitch_controller.h67
-rw-r--r--src/lib/ecl/attitude_fw/ecl_roll_controller.cpp68
-rw-r--r--src/lib/ecl/attitude_fw/ecl_roll_controller.h69
-rw-r--r--src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp84
-rw-r--r--src/lib/ecl/attitude_fw/ecl_yaw_controller.h68
-rw-r--r--src/lib/ecl/module.mk3
-rw-r--r--src/modules/fw_att_control/fw_att_control_main.cpp47
-rw-r--r--unittests/CMakeLists.txt72
-rw-r--r--unittests/Makefile95
-rw-r--r--unittests/conversion_test.cpp9
-rw-r--r--unittests/mixer_test.cpp16
-rw-r--r--unittests/sbus2_test.cpp39
17 files changed, 503 insertions, 484 deletions
diff --git a/Makefile b/Makefile
index 859eca0ce..2ba2073eb 100644
--- a/Makefile
+++ b/Makefile
@@ -257,6 +257,7 @@ testbuild:
tests:
$(Q) $(MAKE) generateuorbtopicheaders
$(Q) (cd $(PX4_BASE)/unittests && $(MAKE) unittests)
+ $(Q) (mkdir -p $(PX4_BASE)/unittests/build && cd $(PX4_BASE)/unittests/build && cmake .. && $(MAKE) unittests)
#
# Cleanup targets. 'clean' should remove all built products and force
diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp
index c0c47073b..c112f65a8 100644
--- a/src/drivers/gps/mtk.cpp
+++ b/src/drivers/gps/mtk.cpp
@@ -264,7 +264,7 @@ MTK::handle_message(gps_mtk_packet_t &packet)
_gps_position->satellites_used = packet.satellites;
/* convert time and date information to unix timestamp */
- struct tm timeinfo; //TODO: test this conversion
+ struct tm timeinfo;
uint32_t timeinfo_conversion_temp;
timeinfo.tm_mday = packet.date * 1e-4;
@@ -280,8 +280,24 @@ MTK::handle_message(gps_mtk_packet_t &packet)
timeinfo_conversion_temp -= timeinfo.tm_sec * 1e3;
time_t epoch = mktime(&timeinfo);
- _gps_position->time_utc_usec = epoch * 1e6; //TODO: test this
- _gps_position->time_utc_usec += timeinfo_conversion_temp * 1e3;
+ if (epoch > GPS_EPOCH_SECS) {
+ // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it
+ // and control its drift. Since we rely on the HRT for our monotonic
+ // clock, updating it from time to time is safe.
+
+ timespec ts;
+ ts.tv_sec = epoch;
+ ts.tv_nsec = timeinfo_conversion_temp * 1000000ULL;
+ if (clock_settime(CLOCK_REALTIME, &ts)) {
+ warn("failed setting clock");
+ }
+
+ _gps_position->time_utc_usec = static_cast<uint64_t>(epoch) * 1000000ULL;
+ _gps_position->time_utc_usec += timeinfo_conversion_temp * 1000ULL;
+ } else {
+ _gps_position->time_utc_usec = 0;
+ }
+
_gps_position->timestamp_position = _gps_position->timestamp_time = hrt_absolute_time();
// Position and velocity update always at the same time
diff --git a/src/lib/ecl/attitude_fw/ecl_controller.cpp b/src/lib/ecl/attitude_fw/ecl_controller.cpp
new file mode 100644
index 000000000..46140fbfd
--- /dev/null
+++ b/src/lib/ecl/attitude_fw/ecl_controller.cpp
@@ -0,0 +1,128 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
+ *
+ * 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 ECL 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 ecl_controller.cpp
+ * Definition of base class for other controllers
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ *
+ * Acknowledgements:
+ *
+ * The control design is based on a design
+ * by Paul Riseborough and Andrew Tridgell, 2013,
+ * which in turn is based on initial work of
+ * Jonathan Challinger, 2012.
+ */
+
+#include "ecl_controller.h"
+
+#include <stdio.h>
+
+ECL_Controller::ECL_Controller(const char *name) :
+ _last_run(0),
+ _tc(0.1f),
+ _k_p(0.0f),
+ _k_i(0.0f),
+ _k_ff(0.0f),
+ _integrator_max(0.0f),
+ _max_rate(0.0f),
+ _last_output(0.0f),
+ _integrator(0.0f),
+ _rate_error(0.0f),
+ _rate_setpoint(0.0f),
+ _bodyrate_setpoint(0.0f),
+ _perf_name()
+{
+ /* Init performance counter */
+ snprintf(_perf_name, sizeof(_perf_name), "fw att control %s nonfinite input", name);
+ _nonfinite_input_perf = perf_alloc(PC_COUNT, _perf_name);
+}
+
+ECL_Controller::~ECL_Controller()
+{
+ perf_free(_nonfinite_input_perf);
+}
+
+void ECL_Controller::reset_integrator()
+{
+ _integrator = 0.0f;
+}
+
+void ECL_Controller::set_time_constant(float time_constant)
+{
+ if (time_constant > 0.1f && time_constant < 3.0f) {
+ _tc = time_constant;
+ }
+}
+
+void ECL_Controller::set_k_p(float k_p)
+{
+ _k_p = k_p;
+}
+
+void ECL_Controller::set_k_i(float k_i)
+{
+ _k_i = k_i;
+}
+
+void ECL_Controller::set_k_ff(float k_ff)
+{
+ _k_ff = k_ff;
+}
+
+void ECL_Controller::set_integrator_max(float max)
+{
+ _integrator_max = max;
+}
+
+void ECL_Controller::set_max_rate(float max_rate)
+{
+ _max_rate = max_rate;
+}
+
+float ECL_Controller::get_rate_error()
+{
+ return _rate_error;
+}
+
+float ECL_Controller::get_desired_rate()
+{
+ return _rate_setpoint;
+}
+
+float ECL_Controller::get_desired_bodyrate()
+{
+ return _bodyrate_setpoint;
+}
diff --git a/src/lib/ecl/attitude_fw/ecl_controller.h b/src/lib/ecl/attitude_fw/ecl_controller.h
new file mode 100644
index 000000000..9880d1c17
--- /dev/null
+++ b/src/lib/ecl/attitude_fw/ecl_controller.h
@@ -0,0 +1,119 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
+ *
+ * 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 ECL 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 ecl_controller.h
+ * Definition of base class for other controllers
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ *
+ * Acknowledgements:
+ *
+ * The control design is based on a design
+ * by Paul Riseborough and Andrew Tridgell, 2013,
+ * which in turn is based on initial work of
+ * Jonathan Challinger, 2012.
+ */
+
+#pragma once
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <systemlib/perf_counter.h>
+
+struct ECL_ControlData {
+ float roll;
+ float pitch;
+ float yaw;
+ float roll_rate;
+ float pitch_rate;
+ float yaw_rate;
+ float speed_body_u;
+ float speed_body_v;
+ float speed_body_w;
+ float roll_setpoint;
+ float pitch_setpoint;
+ float yaw_setpoint;
+ float roll_rate_setpoint;
+ float pitch_rate_setpoint;
+ float yaw_rate_setpoint;
+ float airspeed_min;
+ float airspeed_max;
+ float airspeed;
+ float scaler;
+ bool lock_integrator;
+};
+
+class __EXPORT ECL_Controller
+{
+public:
+ ECL_Controller(const char *name);
+
+ ~ECL_Controller();
+
+ virtual float control_attitude(const struct ECL_ControlData &ctl_data) = 0;
+ virtual float control_bodyrate(const struct ECL_ControlData &ctl_data) = 0;
+
+ /* Setters */
+ void set_time_constant(float time_constant);
+ void set_k_p(float k_p);
+ void set_k_i(float k_i);
+ void set_k_ff(float k_ff);
+ void set_integrator_max(float max);
+ void set_max_rate(float max_rate);
+
+ /* Getters */
+ float get_rate_error();
+ float get_desired_rate();
+ float get_desired_bodyrate();
+
+ void reset_integrator();
+
+protected:
+ uint64_t _last_run;
+ float _tc;
+ float _k_p;
+ float _k_i;
+ float _k_ff;
+ float _integrator_max;
+ float _max_rate;
+ float _last_output;
+ float _integrator;
+ float _rate_error;
+ float _rate_setpoint;
+ float _bodyrate_setpoint;
+ perf_counter_t _nonfinite_input_perf;
+ static const uint8_t _perf_name_max = 40;
+ char _perf_name[_perf_name_max];
+};
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
index 926a8db2a..add884146 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
@@ -48,33 +48,24 @@
#include <systemlib/err.h>
ECL_PitchController::ECL_PitchController() :
- _last_run(0),
- _tc(0.1f),
- _k_p(0.0f),
- _k_i(0.0f),
- _k_ff(0.0f),
- _integrator_max(0.0f),
- _max_rate_pos(0.0f),
+ ECL_Controller("pitch"),
_max_rate_neg(0.0f),
- _roll_ff(0.0f),
- _last_output(0.0f),
- _integrator(0.0f),
- _rate_error(0.0f),
- _rate_setpoint(0.0f),
- _bodyrate_setpoint(0.0f),
- _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control pitch nonfinite input"))
+ _roll_ff(0.0f)
{
}
ECL_PitchController::~ECL_PitchController()
{
- perf_free(_nonfinite_input_perf);
}
-float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed)
+float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_data)
{
+ float roll = ctl_data.roll;
/* Do not calculate control signal with bad inputs */
- if (!(isfinite(pitch_setpoint) && isfinite(roll) && isfinite(pitch) && isfinite(airspeed))) {
+ if (!(isfinite(ctl_data.pitch_setpoint) &&
+ isfinite(roll) &&
+ isfinite(ctl_data.pitch) &&
+ isfinite(ctl_data.airspeed))) {
perf_count(_nonfinite_input_perf);
warnx("not controlling pitch");
return _rate_setpoint;
@@ -102,13 +93,13 @@ float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, fl
/* calculate the offset in the rate resulting from rolling */
//xxx needs explanation and conversion to body angular rates or should be removed
- float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) *
+ float turn_offset = fabsf((CONSTANTS_ONE_G / ctl_data.airspeed) *
tanf(roll) * sinf(roll)) * _roll_ff;
if (inverted)
turn_offset = -turn_offset;
/* Calculate the error */
- float pitch_error = pitch_setpoint - pitch;
+ float pitch_error = ctl_data.pitch_setpoint - ctl_data.pitch;
/* Apply P controller: rate setpoint from current error and time constant */
_rate_setpoint = pitch_error / _tc;
@@ -117,9 +108,9 @@ float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, fl
_rate_setpoint += turn_offset;
/* limit the rate */ //XXX: move to body angluar rates
- if (_max_rate_pos > 0.01f && _max_rate_neg > 0.01f) {
+ if (_max_rate > 0.01f && _max_rate_neg > 0.01f) {
if (_rate_setpoint > 0.0f) {
- _rate_setpoint = (_rate_setpoint > _max_rate_pos) ? _max_rate_pos : _rate_setpoint;
+ _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
} else {
_rate_setpoint = (_rate_setpoint < -_max_rate_neg) ? -_max_rate_neg : _rate_setpoint;
}
@@ -129,15 +120,17 @@ float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, fl
return _rate_setpoint;
}
-float ECL_PitchController::control_bodyrate(float roll, float pitch,
- float pitch_rate, float yaw_rate,
- float yaw_rate_setpoint,
- float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
+float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
- if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) &&
- isfinite(yaw_rate_setpoint) && isfinite(airspeed_min) &&
- isfinite(airspeed_max) && isfinite(scaler))) {
+ if (!(isfinite(ctl_data.roll) &&
+ isfinite(ctl_data.pitch) &&
+ isfinite(ctl_data.pitch_rate) &&
+ isfinite(ctl_data.yaw_rate) &&
+ isfinite(ctl_data.yaw_rate_setpoint) &&
+ isfinite(ctl_data.airspeed_min) &&
+ isfinite(ctl_data.airspeed_max) &&
+ isfinite(ctl_data.scaler))) {
perf_count(_nonfinite_input_perf);
return math::constrain(_last_output, -1.0f, 1.0f);
}
@@ -148,28 +141,32 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
float dt = (float)dt_micros * 1e-6f;
/* lock integral for long intervals */
+ bool lock_integrator = ctl_data.lock_integrator;
if (dt_micros > 500000)
lock_integrator = true;
/* input conditioning */
+ float airspeed = ctl_data.airspeed;
if (!isfinite(airspeed)) {
/* airspeed is NaN, +- INF or not available, pick center of band */
- airspeed = 0.5f * (airspeed_min + airspeed_max);
- } else if (airspeed < airspeed_min) {
- airspeed = airspeed_min;
+ airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max);
+ } else if (airspeed < ctl_data.airspeed_min) {
+ airspeed = ctl_data.airspeed_min;
}
- /* Transform setpoint to body angular rates */
- _bodyrate_setpoint = cosf(roll) * _rate_setpoint + cosf(pitch) * sinf(roll) * yaw_rate_setpoint; //jacobian
+ /* Transform setpoint to body angular rates (jacobian) */
+ _bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint +
+ cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint;
- /* Transform estimation to body angular rates */
- float pitch_bodyrate = cosf(roll) * pitch_rate + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian
+ /* Transform estimation to body angular rates (jacobian) */
+ float pitch_bodyrate = cosf(ctl_data.roll) * ctl_data.pitch_rate +
+ cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate;
_rate_error = _bodyrate_setpoint - pitch_bodyrate;
- if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
+ if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) {
- float id = _rate_error * dt * scaler;
+ float id = _rate_error * dt * ctl_data.scaler;
/*
* anti-windup: do not allow integrator to increase if actuator is at limit
@@ -190,15 +187,10 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
/* Apply PI rate controller and store non-limited output */
- _last_output = _bodyrate_setpoint * _k_ff * scaler +
- _rate_error * _k_p * scaler * scaler
+ _last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler +
+ _rate_error * _k_p * ctl_data.scaler * ctl_data.scaler
+ integrator_constrained; //scaler is proportional to 1/airspeed
// warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
// warnx("roll: _last_output %.4f", (double)_last_output);
return math::constrain(_last_output, -1.0f, 1.0f);
}
-
-void ECL_PitchController::reset_integrator()
-{
- _integrator = 0.0f;
-}
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
index 39b9f9d03..47b5605e9 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
@@ -51,46 +51,23 @@
#include <stdbool.h>
#include <stdint.h>
-#include <systemlib/perf_counter.h>
-class __EXPORT ECL_PitchController //XXX: create controller superclass
+#include "ecl_controller.h"
+
+class __EXPORT ECL_PitchController :
+ public ECL_Controller
{
public:
ECL_PitchController();
~ECL_PitchController();
- float control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed);
-
-
- float control_bodyrate(float roll, float pitch,
- float pitch_rate, float yaw_rate,
- float yaw_rate_setpoint,
- float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f), float scaler = 1.0f, bool lock_integrator = false);
-
- void reset_integrator();
-
- void set_time_constant(float time_constant) {
- _tc = time_constant;
- }
- void set_k_p(float k_p) {
- _k_p = k_p;
- }
-
- void set_k_i(float k_i) {
- _k_i = k_i;
- }
-
- void set_k_ff(float k_ff) {
- _k_ff = k_ff;
- }
-
- void set_integrator_max(float max) {
- _integrator_max = max;
- }
+ float control_attitude(const struct ECL_ControlData &ctl_data);
+ float control_bodyrate(const struct ECL_ControlData &ctl_data);
+ /* Additional Setters */
void set_max_rate_pos(float max_rate_pos) {
- _max_rate_pos = max_rate_pos;
+ _max_rate = max_rate_pos;
}
void set_max_rate_neg(float max_rate_neg) {
@@ -101,35 +78,9 @@ public:
_roll_ff = roll_ff;
}
- float get_rate_error() {
- return _rate_error;
- }
-
- float get_desired_rate() {
- return _rate_setpoint;
- }
-
- float get_desired_bodyrate() {
- return _bodyrate_setpoint;
- }
-
-private:
-
- uint64_t _last_run;
- float _tc;
- float _k_p;
- float _k_i;
- float _k_ff;
- float _integrator_max;
- float _max_rate_pos;
+protected:
float _max_rate_neg;
float _roll_ff;
- float _last_output;
- float _integrator;
- float _rate_error;
- float _rate_setpoint;
- float _bodyrate_setpoint;
- perf_counter_t _nonfinite_input_perf;
};
#endif // ECL_PITCH_CONTROLLER_H
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
index 94bd26f03..0606c87cb 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
@@ -48,37 +48,24 @@
#include <systemlib/err.h>
ECL_RollController::ECL_RollController() :
- _last_run(0),
- _tc(0.1f),
- _k_p(0.0f),
- _k_i(0.0f),
- _k_ff(0.0f),
- _integrator_max(0.0f),
- _max_rate(0.0f),
- _last_output(0.0f),
- _integrator(0.0f),
- _rate_error(0.0f),
- _rate_setpoint(0.0f),
- _bodyrate_setpoint(0.0f),
- _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control roll nonfinite input"))
+ ECL_Controller("roll")
{
}
ECL_RollController::~ECL_RollController()
{
- perf_free(_nonfinite_input_perf);
}
-float ECL_RollController::control_attitude(float roll_setpoint, float roll)
+float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
- if (!(isfinite(roll_setpoint) && isfinite(roll))) {
+ if (!(isfinite(ctl_data.roll_setpoint) && isfinite(ctl_data.roll))) {
perf_count(_nonfinite_input_perf);
return _rate_setpoint;
}
/* Calculate error */
- float roll_error = roll_setpoint - roll;
+ float roll_error = ctl_data.roll_setpoint - ctl_data.roll;
/* Apply P controller */
_rate_setpoint = roll_error / _tc;
@@ -92,15 +79,16 @@ float ECL_RollController::control_attitude(float roll_setpoint, float roll)
return _rate_setpoint;
}
-float ECL_RollController::control_bodyrate(float pitch,
- float roll_rate, float yaw_rate,
- float yaw_rate_setpoint,
- float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
+float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
- if (!(isfinite(pitch) && isfinite(roll_rate) && isfinite(yaw_rate) && isfinite(yaw_rate_setpoint) &&
- isfinite(airspeed_min) && isfinite(airspeed_max) &&
- isfinite(scaler))) {
+ if (!(isfinite(ctl_data.pitch) &&
+ isfinite(ctl_data.roll_rate) &&
+ isfinite(ctl_data.yaw_rate) &&
+ isfinite(ctl_data.yaw_rate_setpoint) &&
+ isfinite(ctl_data.airspeed_min) &&
+ isfinite(ctl_data.airspeed_max) &&
+ isfinite(ctl_data.scaler))) {
perf_count(_nonfinite_input_perf);
return math::constrain(_last_output, -1.0f, 1.0f);
}
@@ -111,31 +99,31 @@ float ECL_RollController::control_bodyrate(float pitch,
float dt = (float)dt_micros * 1e-6f;
/* lock integral for long intervals */
+ bool lock_integrator = ctl_data.lock_integrator;
if (dt_micros > 500000)
lock_integrator = true;
/* input conditioning */
-// warnx("airspeed pre %.4f", (double)airspeed);
+ float airspeed = ctl_data.airspeed;
if (!isfinite(airspeed)) {
/* airspeed is NaN, +- INF or not available, pick center of band */
- airspeed = 0.5f * (airspeed_min + airspeed_max);
- } else if (airspeed < airspeed_min) {
- airspeed = airspeed_min;
+ airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max);
+ } else if (airspeed < ctl_data.airspeed_min) {
+ airspeed = ctl_data.airspeed_min;
}
+ /* Transform setpoint to body angular rates (jacobian) */
+ _bodyrate_setpoint = _rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint;
- /* Transform setpoint to body angular rates */
- _bodyrate_setpoint = _rate_setpoint - sinf(pitch) * yaw_rate_setpoint; //jacobian
-
- /* Transform estimation to body angular rates */
- float roll_bodyrate = roll_rate - sinf(pitch) * yaw_rate; //jacobian
+ /* Transform estimation to body angular rates (jacobian) */
+ float roll_bodyrate = ctl_data.roll_rate - sinf(ctl_data.pitch) * ctl_data.yaw_rate;
/* Calculate body angular rate error */
_rate_error = _bodyrate_setpoint - roll_bodyrate; //body angular rate error
- if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
+ if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) {
- float id = _rate_error * dt * scaler;
+ float id = _rate_error * dt * ctl_data.scaler;
/*
* anti-windup: do not allow integrator to increase if actuator is at limit
@@ -157,15 +145,9 @@ float ECL_RollController::control_bodyrate(float pitch,
//warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max);
/* Apply PI rate controller and store non-limited output */
- _last_output = _bodyrate_setpoint * _k_ff * scaler +
- _rate_error * _k_p * scaler * scaler
+ _last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler +
+ _rate_error * _k_p * ctl_data.scaler * ctl_data.scaler
+ integrator_constrained; //scaler is proportional to 1/airspeed
return math::constrain(_last_output, -1.0f, 1.0f);
}
-
-void ECL_RollController::reset_integrator()
-{
- _integrator = 0.0f;
-}
-
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h
index 0799dbe03..6d07bab8a 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h
@@ -51,76 +51,19 @@
#include <stdbool.h>
#include <stdint.h>
-#include <systemlib/perf_counter.h>
-class __EXPORT ECL_RollController //XXX: create controller superclass
+#include "ecl_controller.h"
+
+class __EXPORT ECL_RollController :
+ public ECL_Controller
{
public:
ECL_RollController();
~ECL_RollController();
- float control_attitude(float roll_setpoint, float roll);
-
- float control_bodyrate(float pitch,
- float roll_rate, float yaw_rate,
- float yaw_rate_setpoint,
- float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f), float scaler = 1.0f, bool lock_integrator = false);
-
- void reset_integrator();
-
- void set_time_constant(float time_constant) {
- if (time_constant > 0.1f && time_constant < 3.0f) {
- _tc = time_constant;
- }
- }
-
- void set_k_p(float k_p) {
- _k_p = k_p;
- }
-
- void set_k_i(float k_i) {
- _k_i = k_i;
- }
-
- void set_k_ff(float k_ff) {
- _k_ff = k_ff;
- }
-
- void set_integrator_max(float max) {
- _integrator_max = max;
- }
-
- void set_max_rate(float max_rate) {
- _max_rate = max_rate;
- }
-
- float get_rate_error() {
- return _rate_error;
- }
-
- float get_desired_rate() {
- return _rate_setpoint;
- }
-
- float get_desired_bodyrate() {
- return _bodyrate_setpoint;
- }
-
-private:
- uint64_t _last_run;
- float _tc;
- float _k_p;
- float _k_i;
- float _k_ff;
- float _integrator_max;
- float _max_rate;
- float _last_output;
- float _integrator;
- float _rate_error;
- float _rate_setpoint;
- float _bodyrate_setpoint;
- perf_counter_t _nonfinite_input_perf;
+ float control_attitude(const struct ECL_ControlData &ctl_data);
+ float control_bodyrate(const struct ECL_ControlData &ctl_data);
};
#endif // ECL_ROLL_CONTROLLER_H
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
index fe03b8065..82ba2c6c7 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
@@ -47,45 +47,43 @@
#include <systemlib/err.h>
ECL_YawController::ECL_YawController() :
- _last_run(0),
- _k_p(0.0f),
- _k_i(0.0f),
- _k_ff(0.0f),
- _integrator_max(0.0f),
- _max_rate(0.0f),
- _last_output(0.0f),
- _integrator(0.0f),
- _rate_error(0.0f),
- _rate_setpoint(0.0f),
- _bodyrate_setpoint(0.0f),
- _coordinated_min_speed(1.0f),
- _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control yaw nonfinite input"))
+ ECL_Controller("yaw"),
+ _coordinated_min_speed(1.0f)
{
}
ECL_YawController::~ECL_YawController()
{
- perf_free(_nonfinite_input_perf);
}
-float ECL_YawController::control_attitude(float roll, float pitch,
- float speed_body_u, float speed_body_v, float speed_body_w,
- float roll_rate_setpoint, float pitch_rate_setpoint)
+float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
- if (!(isfinite(roll) && isfinite(pitch) && isfinite(speed_body_u) && isfinite(speed_body_v) &&
- isfinite(speed_body_w) && isfinite(roll_rate_setpoint) &&
- isfinite(pitch_rate_setpoint))) {
+ if (!(isfinite(ctl_data.roll) &&
+ isfinite(ctl_data.pitch) &&
+ isfinite(ctl_data.speed_body_u) &&
+ isfinite(ctl_data.speed_body_v) &&
+ isfinite(ctl_data.speed_body_w) &&
+ isfinite(ctl_data.roll_rate_setpoint) &&
+ isfinite(ctl_data.pitch_rate_setpoint))) {
perf_count(_nonfinite_input_perf);
return _rate_setpoint;
}
+
// static int counter = 0;
/* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */
_rate_setpoint = 0.0f;
- if (sqrtf(speed_body_u * speed_body_u + speed_body_v * speed_body_v + speed_body_w * speed_body_w) > _coordinated_min_speed) {
- float denumerator = (speed_body_u * cosf(roll) * cosf(pitch) + speed_body_w * sinf(pitch));
+ if (sqrtf(ctl_data.speed_body_u * ctl_data.speed_body_u + ctl_data.speed_body_v * ctl_data.speed_body_v +
+ ctl_data.speed_body_w * ctl_data.speed_body_w) > _coordinated_min_speed) {
+ float denumerator = (ctl_data.speed_body_u * cosf(ctl_data.roll) * cosf(ctl_data.pitch) +
+ ctl_data.speed_body_w * sinf(ctl_data.pitch));
+
if(fabsf(denumerator) > FLT_EPSILON) {
- _rate_setpoint = (speed_body_w * roll_rate_setpoint + 9.81f * sinf(roll) * cosf(pitch) + speed_body_u * pitch_rate_setpoint * sinf(roll)) / denumerator;
+ _rate_setpoint = (ctl_data.speed_body_w * ctl_data.roll_rate_setpoint +
+ 9.81f * sinf(ctl_data.roll) * cosf(ctl_data.pitch) +
+ ctl_data.speed_body_u * ctl_data.pitch_rate_setpoint * sinf(ctl_data.roll)) /
+ denumerator;
+
// warnx("yaw: speed_body_u %.f speed_body_w %1.f roll %.1f pitch %.1f denumerator %.1f _rate_setpoint %.1f", speed_body_u, speed_body_w, denumerator, _rate_setpoint);
}
@@ -111,46 +109,49 @@ float ECL_YawController::control_attitude(float roll, float pitch,
return _rate_setpoint;
}
-float ECL_YawController::control_bodyrate(float roll, float pitch,
- float pitch_rate, float yaw_rate,
- float pitch_rate_setpoint,
- float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
+float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
- if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) &&
- isfinite(pitch_rate_setpoint) && isfinite(airspeed_min) &&
- isfinite(airspeed_max) && isfinite(scaler))) {
+ if (!(isfinite(ctl_data.roll) && isfinite(ctl_data.pitch) && isfinite(ctl_data.pitch_rate) &&
+ isfinite(ctl_data.yaw_rate) && isfinite(ctl_data.pitch_rate_setpoint) &&
+ isfinite(ctl_data.airspeed_min) && isfinite(ctl_data.airspeed_max) &&
+ isfinite(ctl_data.scaler))) {
perf_count(_nonfinite_input_perf);
return math::constrain(_last_output, -1.0f, 1.0f);
}
+
/* get the usual dt estimate */
uint64_t dt_micros = ecl_elapsed_time(&_last_run);
_last_run = ecl_absolute_time();
float dt = (float)dt_micros * 1e-6f;
/* lock integral for long intervals */
+ bool lock_integrator = ctl_data.lock_integrator;
if (dt_micros > 500000)
lock_integrator = true;
/* input conditioning */
+ float airspeed = ctl_data.airspeed;
if (!isfinite(airspeed)) {
/* airspeed is NaN, +- INF or not available, pick center of band */
- airspeed = 0.5f * (airspeed_min + airspeed_max);
- } else if (airspeed < airspeed_min) {
- airspeed = airspeed_min;
+ airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max);
+ } else if (airspeed < ctl_data.airspeed_min) {
+ airspeed = ctl_data.airspeed_min;
}
- /* Transform setpoint to body angular rates */
- _bodyrate_setpoint = -sinf(roll) * pitch_rate_setpoint + cosf(roll)*cosf(pitch) * _rate_setpoint; //jacobian
+ /* Transform setpoint to body angular rates (jacobian) */
+ _bodyrate_setpoint = -sinf(ctl_data.roll) * ctl_data.pitch_rate_setpoint +
+ cosf(ctl_data.roll)*cosf(ctl_data.pitch) * _rate_setpoint;
- /* Transform estimation to body angular rates */
- float yaw_bodyrate = -sinf(roll) * pitch_rate + cosf(roll)*cosf(pitch) * yaw_rate; //jacobian
+ /* Transform estimation to body angular rates (jacobian) */
+ float yaw_bodyrate = -sinf(ctl_data.roll) * ctl_data.pitch_rate +
+ cosf(ctl_data.roll)*cosf(ctl_data.pitch) * ctl_data.yaw_rate;
/* Calculate body angular rate error */
_rate_error = _bodyrate_setpoint - yaw_bodyrate; //body angular rate error
- if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
+ if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) {
float id = _rate_error * dt;
@@ -173,14 +174,9 @@ float ECL_YawController::control_bodyrate(float roll, float pitch,
float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
/* Apply PI rate controller and store non-limited output */
- _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed
+ _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * ctl_data.scaler * ctl_data.scaler; //scaler is proportional to 1/airspeed
//warnx("yaw:_last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_last_output, (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
return math::constrain(_last_output, -1.0f, 1.0f);
}
-
-void ECL_YawController::reset_integrator()
-{
- _integrator = 0.0f;
-}
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
index a360c14b8..37c03705e 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
@@ -50,79 +50,27 @@
#include <stdbool.h>
#include <stdint.h>
-#include <systemlib/perf_counter.h>
-class __EXPORT ECL_YawController //XXX: create controller superclass
+#include "ecl_controller.h"
+
+class __EXPORT ECL_YawController :
+ public ECL_Controller
{
public:
ECL_YawController();
~ECL_YawController();
- float control_attitude(float roll, float pitch,
- float speed_body_u, float speed_body_v, float speed_body_w,
- float roll_rate_setpoint, float pitch_rate_setpoint);
-
- float control_bodyrate( float roll, float pitch,
- float pitch_rate, float yaw_rate,
- float pitch_rate_setpoint,
- float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator);
-
- void reset_integrator();
-
- void set_k_p(float k_p) {
- _k_p = k_p;
- }
-
- void set_k_i(float k_i) {
- _k_i = k_i;
- }
-
- void set_k_ff(float k_ff) {
- _k_ff = k_ff;
- }
-
- void set_integrator_max(float max) {
- _integrator_max = max;
- }
-
- void set_max_rate(float max_rate) {
- _max_rate = max_rate;
- }
+ float control_attitude(const struct ECL_ControlData &ctl_data);
+ float control_bodyrate(const struct ECL_ControlData &ctl_data);
+ /* Additional setters */
void set_coordinated_min_speed(float coordinated_min_speed) {
_coordinated_min_speed = coordinated_min_speed;
}
-
- float get_rate_error() {
- return _rate_error;
- }
-
- float get_desired_rate() {
- return _rate_setpoint;
- }
-
- float get_desired_bodyrate() {
- return _bodyrate_setpoint;
- }
-
-private:
- uint64_t _last_run;
- float _k_p;
- float _k_i;
- float _k_ff;
- float _integrator_max;
- float _max_rate;
- float _roll_ff;
- float _last_output;
- float _integrator;
- float _rate_error;
- float _rate_setpoint;
- float _bodyrate_setpoint;
+protected:
float _coordinated_min_speed;
- perf_counter_t _nonfinite_input_perf;
-
};
#endif // ECL_YAW_CONTROLLER_H
diff --git a/src/lib/ecl/module.mk b/src/lib/ecl/module.mk
index 93a5b511f..03974c950 100644
--- a/src/lib/ecl/module.mk
+++ b/src/lib/ecl/module.mk
@@ -35,7 +35,8 @@
# Estimation and Control Library
#
-SRCS = attitude_fw/ecl_pitch_controller.cpp \
+SRCS = attitude_fw/ecl_controller.cpp \
+ attitude_fw/ecl_pitch_controller.cpp \
attitude_fw/ecl_roll_controller.cpp \
attitude_fw/ecl_yaw_controller.cpp \
l1/ecl_l1_pos_controller.cpp
diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index 00e8393bf..d329ba2b0 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -934,19 +934,38 @@ FixedwingAttitudeControl::task_main()
}
}
+ /* Prepare data for attitude controllers */
+ struct ECL_ControlData control_input = {};
+ control_input.roll = _att.roll;
+ control_input.pitch = _att.pitch;
+ control_input.yaw = _att.yaw;
+ control_input.roll_rate = _att.rollspeed;
+ control_input.pitch_rate = _att.pitchspeed;
+ control_input.yaw_rate = _att.yawspeed;
+ control_input.speed_body_u = speed_body_u;
+ control_input.speed_body_v = speed_body_v;
+ control_input.speed_body_w = speed_body_w;
+ control_input.roll_setpoint = roll_sp;
+ control_input.pitch_setpoint = pitch_sp;
+ control_input.airspeed_min = _parameters.airspeed_min;
+ control_input.airspeed_max = _parameters.airspeed_max;
+ control_input.airspeed = airspeed;
+ control_input.scaler = airspeed_scaling;
+ control_input.lock_integrator = lock_integrator;
+
/* Run attitude controllers */
if (isfinite(roll_sp) && isfinite(pitch_sp)) {
- _roll_ctrl.control_attitude(roll_sp, _att.roll);
- _pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed);
- _yaw_ctrl.control_attitude(_att.roll, _att.pitch,
- speed_body_u, speed_body_v, speed_body_w,
- _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude
+ _roll_ctrl.control_attitude(control_input);
+ _pitch_ctrl.control_attitude(control_input);
+ _yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude
+
+ /* Update input data for rate controllers */
+ control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate();
+ control_input.pitch_rate_setpoint = _pitch_ctrl.get_desired_rate();
+ control_input.yaw_rate_setpoint = _yaw_ctrl.get_desired_rate();
/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
- float roll_u = _roll_ctrl.control_bodyrate(_att.pitch,
- _att.rollspeed, _att.yawspeed,
- _yaw_ctrl.get_desired_rate(),
- _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
+ float roll_u = _roll_ctrl.control_bodyrate(control_input);
_actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll;
if (!isfinite(roll_u)) {
_roll_ctrl.reset_integrator();
@@ -957,10 +976,7 @@ FixedwingAttitudeControl::task_main()
}
}
- float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch,
- _att.pitchspeed, _att.yawspeed,
- _yaw_ctrl.get_desired_rate(),
- _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
+ float pitch_u = _pitch_ctrl.control_bodyrate(control_input);
_actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch;
if (!isfinite(pitch_u)) {
_pitch_ctrl.reset_integrator();
@@ -981,10 +997,7 @@ FixedwingAttitudeControl::task_main()
}
}
- float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch,
- _att.pitchspeed, _att.yawspeed,
- _pitch_ctrl.get_desired_rate(),
- _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
+ float yaw_u = _yaw_ctrl.control_bodyrate(control_input);
_actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
/* add in manual rudder control */
diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt
index 47b84f148..b57537aa4 100644
--- a/unittests/CMakeLists.txt
+++ b/unittests/CMakeLists.txt
@@ -2,38 +2,78 @@ cmake_minimum_required(VERSION 2.8)
project(unittests)
enable_testing()
+include(CheckCXXCompilerFlag)
+CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
+CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
+if(COMPILER_SUPPORTS_CXX11)
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+elseif(COMPILER_SUPPORTS_CXX0X)
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+else()
+ message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
+endif()
+set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=c99")
+
set(GTEST_DIR gtest)
add_subdirectory(${GTEST_DIR})
include_directories(${GTEST_DIR}/include)
+
+set(PX_SRC ${CMAKE_SOURCE_DIR}/../src)
include_directories(${CMAKE_SOURCE_DIR})
-include_directories(${CMAKE_SOURCE_DIR}/../src)
-include_directories(${CMAKE_SOURCE_DIR}/../src/modules)
-include_directories(${CMAKE_SOURCE_DIR}/../src/lib)
+include_directories(${PX_SRC})
+include_directories(${PX_SRC}/modules)
+include_directories(${PX_SRC}/lib)
add_definitions(-D__EXPORT=)
add_definitions(-D__PX4_TESTS)
add_definitions(-Dnoreturn_function=)
add_definitions(-Dmain_t=int)
+# check
+add_custom_target(unittests COMMAND ${CMAKE_CTEST_COMMAND} --output-on-failure)
+
function(add_gtest)
foreach(test_name ${ARGN})
target_link_libraries(${test_name} gtest_main)
- add_test(${test_name}Test ${test_name})
+ add_test(NAME ${test_name} COMMAND ${test_name} WORKING_DIRECTORY ${CMAKE_SOURCE_DIR})
+ add_dependencies(unittests ${test_name})
endforeach()
endfunction()
-include(CheckCXXCompilerFlag)
-CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
-CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
-if(COMPILER_SUPPORTS_CXX11)
- set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
-elseif(COMPILER_SUPPORTS_CXX0X)
- set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
-else()
- message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
-endif()
# add each test
-# todo: add mixer_test sbus2_test st24_test sf0x_test
-add_executable(autodeclination_test autodeclination_test.cpp ${CMAKE_SOURCE_DIR}/../src/lib/geo_lookup/geo_mag_declination.c)
+add_executable(autodeclination_test autodeclination_test.cpp ${PX_SRC}/lib/geo_lookup/geo_mag_declination.c)
add_gtest(autodeclination_test)
+
+# mixer_test
+add_custom_command(OUTPUT ${PX_SRC}/modules/systemlib/mixer/mixer_multirotor.generated.h
+ COMMAND ${PX_SRC}/modules/systemlib/mixer/multi_tables.py > ${PX_SRC}/modules/systemlib/mixer/mixer_multirotor.generated.h)
+add_executable(mixer_test mixer_test.cpp hrt.cpp
+ ${PX_SRC}/modules/systemlib/mixer/mixer.cpp
+ ${PX_SRC}/modules/systemlib/mixer/mixer_group.cpp
+ ${PX_SRC}/modules/systemlib/mixer/mixer_load.c
+ ${PX_SRC}/modules/systemlib/mixer/mixer_multirotor.cpp
+ ${PX_SRC}/modules/systemlib/mixer/mixer_multirotor.generated.h
+ ${PX_SRC}/modules/systemlib/mixer/mixer_simple.cpp
+ ${PX_SRC}/modules/systemlib/pwm_limit/pwm_limit.c
+ ${PX_SRC}/systemcmds/tests/test_mixer.cpp)
+add_gtest(mixer_test)
+
+# conversion_test
+add_executable(conversion_test conversion_test.cpp ${PX_SRC}/systemcmds/tests/test_conv.cpp)
+add_gtest(conversion_test)
+
+# sbus2_test
+# TODO: move to gtest
+add_executable(sbus2_test sbus2_test.cpp hrt.cpp)
+add_gtest(sbus2_test)
+
+# st24_test
+# TODO: move to gtest
+add_executable(st24_test st24_test.cpp hrt.cpp ${PX_SRC}/lib/rc/st24.c)
+add_gtest(st24_test)
+
+# sf0x_test
+# TODO: move to gtest
+add_executable(sf0x_test sf0x_test.cpp ${PX_SRC}/drivers/sf0x/sf0x_parser.cpp)
+add_gtest(sf0x_test)
diff --git a/unittests/Makefile b/unittests/Makefile
deleted file mode 100644
index f25a56fc4..000000000
--- a/unittests/Makefile
+++ /dev/null
@@ -1,95 +0,0 @@
-
-CC=$(CXX)
-CFLAGS=-I. -I../src/modules -I ../src/include -I../src/drivers \
- -I../src -I../src/lib -D__EXPORT="" -Dnullptr="0" -lm -std=c++11
-
-# Points to the root of Google Test, relative to where this file is.
-# Remember to tweak this if you move this file.
-GTEST_DIR = gtest
-
-# Flags passed to the preprocessor.
-# Set Google Test's header directory as a system directory, such that
-# the compiler doesn't generate warnings in Google Test headers.
-CFLAGS += -isystem $(GTEST_DIR)/include
-
-# All Google Test headers. Usually you shouldn't change this
-# definition.
-GTEST_HEADERS = $(GTEST_DIR)/include/gtest/*.h \
- $(GTEST_DIR)/include/gtest/internal/*.h
-
-# Usually you shouldn't tweak such internal variables, indicated by a
-# trailing _.
-GTEST_SRCS_ = $(GTEST_DIR)/src/*.cc $(GTEST_DIR)/src/*.h $(GTEST_HEADERS)
-
-# For simplicity and to avoid depending on Google Test's
-# implementation details, the dependencies specified below are
-# conservative and not optimized. This is fine as Google Test
-# compiles fast and for ordinary users its source rarely changes.
-gtest-all.o: $(GTEST_SRCS_)
- $(CC) $(CFLAGS) -I$(GTEST_DIR) -c \
- $(GTEST_DIR)/src/gtest-all.cc
-
-gtest_main.o: $(GTEST_SRCS_)
- $(CC) $(CFLAGS) -I$(GTEST_DIR) -c \
- $(GTEST_DIR)/src/gtest_main.cc
-
-gtest.a: gtest-all.o
- $(AR) $(ARFLAGS) $@ $^
-
-gtest_main.a: gtest-all.o gtest_main.o
- $(AR) $(ARFLAGS) $@ $^
-
-
-all: mixer_test sbus2_test st24_test sf0x_test
-
-MIXER_FILES=../src/systemcmds/tests/test_mixer.cpp \
- ../src/systemcmds/tests/test_conv.cpp \
- ../src/modules/systemlib/mixer/mixer_simple.cpp \
- ../src/modules/systemlib/mixer/mixer_multirotor.cpp \
- ../src/modules/systemlib/mixer/mixer.cpp \
- ../src/modules/systemlib/mixer/mixer_group.cpp \
- ../src/modules/systemlib/mixer/mixer_load.c \
- ../src/modules/systemlib/pwm_limit/pwm_limit.c \
- hrt.cpp \
- mixer_test.cpp
-
-include ../src/modules/systemlib/mixer/multi_tables.mk
-
-SBUS2_FILES=../src/modules/px4iofirmware/sbus.c \
- hrt.cpp \
- sbus2_test.cpp
-
-ST24_FILES=../src/lib/rc/st24.c \
- hrt.cpp \
- st24_test.cpp
-
-SF0X_FILES= \
- hrt.cpp \
- sf0x_test.cpp \
- ../src/drivers/sf0x/sf0x_parser.cpp
-
-mixer_test: $(MIXER_FILES)
- $(CC) -o mixer_test $(MIXER_FILES) $(CFLAGS)
-
-sbus2_test: $(SBUS2_FILES)
- $(CC) -o sbus2_test $(SBUS2_FILES) $(CFLAGS)
-
-sf0x_test: $(SF0X_FILES)
- $(CC) -o sf0x_test $(SF0X_FILES) $(CFLAGS)
-
-st24_test: $(ST24_FILES)
- $(CC) -o st24_test $(ST24_FILES) $(CFLAGS)
-
-cmake_gtests:
- mkdir -p build
- cd build && CC=gcc cmake .. && $(MAKE) && $(MAKE) test
-
-unittests: clean mixer_test sbus2_test sf0x_test st24_test cmake_gtests
- ./mixer_test
- ./sbus2_test
- ./sf0x_test
- ./st24_test
-
-.PHONY: clean
-clean:
- rm -rf gtest.a gtest_main.a *.o $(ODIR)/*.o *~ core $(INCDIR)/*~ sample_unittest mixer_test sbus2_test st24_test sf0x_test build
diff --git a/unittests/conversion_test.cpp b/unittests/conversion_test.cpp
new file mode 100644
index 000000000..99e1c7721
--- /dev/null
+++ b/unittests/conversion_test.cpp
@@ -0,0 +1,9 @@
+#include <systemlib/mixer/mixer.h>
+#include <systemlib/err.h>
+#include "../../src/systemcmds/tests/tests.h"
+
+#include "gtest/gtest.h"
+
+TEST(ConversionTest, FMU_quad_w) {
+ ASSERT_EQ(test_conv(0, NULL), 0) << "Conversion test failed";
+}
diff --git a/unittests/mixer_test.cpp b/unittests/mixer_test.cpp
index 4919e325c..fd3ece7f8 100644
--- a/unittests/mixer_test.cpp
+++ b/unittests/mixer_test.cpp
@@ -2,18 +2,10 @@
#include <systemlib/err.h>
#include "../../src/systemcmds/tests/tests.h"
-int main(int argc, char *argv[]) {
+#include "gtest/gtest.h"
- int ret;
- warnx("Host execution started");
-
- char* args[] = {argv[0], "../ROMFS/px4fmu_common/mixers/IO_pass.mix",
- "../ROMFS/px4fmu_common/mixers/FMU_quad_w.mix"};
-
- if (ret = test_mixer(3, args));
-
- test_conv(1, args);
-
- return 0;
+TEST(MixerTest, Mixer) {
+ char* args[] = {"empty", "../ROMFS/px4fmu_common/mixers/IO_pass.mix", "../ROMFS/px4fmu_common/mixers/FMU_quad_w.mix"};
+ ASSERT_EQ(test_mixer(3, args), 0) << "IO_pass.mix failed";
}
diff --git a/unittests/sbus2_test.cpp b/unittests/sbus2_test.cpp
index ba075f8b3..ee4f3d1d6 100644
--- a/unittests/sbus2_test.cpp
+++ b/unittests/sbus2_test.cpp
@@ -1,33 +1,23 @@
-
#include <stdio.h>
-#include <unistd.h>
#include <string.h>
-#include <systemlib/mixer/mixer.h>
-#include <systemlib/err.h>
+#include <unistd.h>
+
+#include "../../src/systemcmds/tests/tests.h"
#include <drivers/drv_hrt.h>
#include <px4iofirmware/px4io.h>
-#include "../../src/systemcmds/tests/tests.h"
-
-int main(int argc, char *argv[]) {
- warnx("SBUS2 test started");
-
- char *filepath = 0;
+#include <systemlib/err.h>
+#include <systemlib/mixer/mixer.h>
- if (argc < 2) {
- warnx("Using default input file");
- filepath = "testdata/sbus2_r7008SB.txt";
- } else {
- filepath = argv[1];
- }
+#include "gtest/gtest.h"
- warnx("loading data from: %s", filepath);
+TEST(SBUS2Test, SBUS2) {
+ char *filepath = "testdata/sbus2_r7008SB.txt";
FILE *fp;
-
fp = fopen(filepath,"rt");
- if (!fp)
- errx(1, "failed opening file");
+ ASSERT_TRUE(fp);
+ warnx("loading data from: %s", filepath);
float f;
unsigned x;
@@ -73,12 +63,5 @@ int main(int argc, char *argv[]) {
//sbus_parse(now, frame, &partial_frame_count, rc_values, &num_values, &sbus_failsafe, &sbus_frame_drop, max_channels);
}
- if (ret == EOF) {
- warnx("Test finished, reached end of file");
- ret = 0;
- } else {
- warnx("Test aborted, errno: %d", ret);
- }
-
- return ret;
+ ASSERT_EQ(ret, EOF);
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-06-10 13:39:16 +0000