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

Merge remote-tracking branch 'upstream/master' into dev_ros

Browse Source 
Conflicts:
	Makefile
This commit is contained in:
Thomas Gubler 2015-01-07 16:00:59 +01:00
commit fe279d340d
17 changed files with 503 additions and 484 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
Makefile
View File

@ -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

22
src/drivers/gps/mtk.cpp
View File

@ -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

128
src/lib/ecl/attitude_fw/ecl_controller.cpp Normal file
View File

@ -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;
}

119
src/lib/ecl/attitude_fw/ecl_controller.h Normal file
View File

@ -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];
};

80
src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
View File

@ -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;
}

67
src/lib/ecl/attitude_fw/ecl_pitch_controller.h
View File

@ -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

68
src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
View File

@ -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;
}

69
src/lib/ecl/attitude_fw/ecl_roll_controller.h
View File

@ -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

84
src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
View File

@ -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;
}

68
src/lib/ecl/attitude_fw/ecl_yaw_controller.h
View File

@ -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

3
src/lib/ecl/module.mk
View File

@ -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

47
src/modules/fw_att_control/fw_att_control_main.cpp
View File

@ -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 */

72
unittests/CMakeLists.txt
View File

@ -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)

95
unittests/Makefile
View File

@ -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

9
unittests/conversion_test.cpp Normal file
View File

@ -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";
}

16
unittests/mixer_test.cpp
View File

@ -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";
}

39
unittests/sbus2_test.cpp
View File

@ -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"
#include <systemlib/err.h>
#include <systemlib/mixer/mixer.h>
int main(int argc, char *argv[]) {
warnx("SBUS2 test started");
#include "gtest/gtest.h"
char *filepath = 0;
if (argc < 2) {
warnx("Using default input file");
filepath = "testdata/sbus2_r7008SB.txt";
} else {
filepath = argv[1];
}
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);
}