diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2015-01-07 16:00:59 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2015-01-07 16:00:59 +0100 |
commit | fe279d340dccdd40ebfade4c53893e8f7a77b45e (patch) | |
tree | 6ca7e8517507e097097a45cca384c51d343b5fc7 | |
parent | 417a82c699c2512ca3b8998c91c9d77f5d826edb (diff) | |
parent | 6ed2f77ca5c713e537c8fd4a9aacd7adc7887f53 (diff) | |
download | px4-firmware-fe279d340dccdd40ebfade4c53893e8f7a77b45e.tar.gz px4-firmware-fe279d340dccdd40ebfade4c53893e8f7a77b45e.tar.bz2 px4-firmware-fe279d340dccdd40ebfade4c53893e8f7a77b45e.zip |
Merge remote-tracking branch 'upstream/master' into dev_ros
Conflicts:
Makefile
-rw-r--r-- | Makefile | 1 | ||||
-rw-r--r-- | src/drivers/gps/mtk.cpp | 22 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_controller.cpp | 128 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_controller.h | 119 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 80 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_pitch_controller.h | 67 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 68 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_roll_controller.h | 69 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 84 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 68 | ||||
-rw-r--r-- | src/lib/ecl/module.mk | 3 | ||||
-rw-r--r-- | src/modules/fw_att_control/fw_att_control_main.cpp | 47 | ||||
-rw-r--r-- | unittests/CMakeLists.txt | 72 | ||||
-rw-r--r-- | unittests/Makefile | 95 | ||||
-rw-r--r-- | unittests/conversion_test.cpp | 9 | ||||
-rw-r--r-- | unittests/mixer_test.cpp | 16 | ||||
-rw-r--r-- | unittests/sbus2_test.cpp | 39 |
17 files changed, 503 insertions, 484 deletions
@@ -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); } |