From 1781e3caf8a18f259c139fa0d1a8dda3385030bc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Oct 2015 15:54:20 +0100 Subject: [PATCH 001/201] ECL: Remove in preparation for an external library --- src/lib/ecl/CMakeLists.txt | 49 --- src/lib/ecl/attitude_fw/ecl_controller.cpp | 140 ------- src/lib/ecl/attitude_fw/ecl_controller.h | 125 ------ .../ecl/attitude_fw/ecl_pitch_controller.cpp | 198 ---------- .../ecl/attitude_fw/ecl_pitch_controller.h | 89 ----- .../ecl/attitude_fw/ecl_roll_controller.cpp | 146 ------- src/lib/ecl/attitude_fw/ecl_roll_controller.h | 69 ---- .../ecl/attitude_fw/ecl_wheel_controller.cpp | 153 ------- .../ecl/attitude_fw/ecl_wheel_controller.h | 70 ---- .../ecl/attitude_fw/ecl_yaw_controller.cpp | 240 ----------- src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 98 ----- src/lib/ecl/ecl.h | 46 --- src/lib/ecl/l1/ecl_l1_pos_controller.cpp | 372 ------------------ src/lib/ecl/l1/ecl_l1_pos_controller.h | 268 ------------- src/lib/ecl/validation/data_validator.cpp | 169 -------- src/lib/ecl/validation/data_validator.h | 159 -------- .../ecl/validation/data_validator_group.cpp | 266 ------------- src/lib/ecl/validation/data_validator_group.h | 122 ------ 18 files changed, 2779 deletions(-) delete mode 100644 src/lib/ecl/CMakeLists.txt delete mode 100644 src/lib/ecl/attitude_fw/ecl_controller.cpp delete mode 100644 src/lib/ecl/attitude_fw/ecl_controller.h delete mode 100644 src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp delete mode 100644 src/lib/ecl/attitude_fw/ecl_pitch_controller.h delete mode 100644 src/lib/ecl/attitude_fw/ecl_roll_controller.cpp delete mode 100644 src/lib/ecl/attitude_fw/ecl_roll_controller.h delete mode 100644 src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp delete mode 100644 src/lib/ecl/attitude_fw/ecl_wheel_controller.h delete mode 100644 src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp delete mode 100644 src/lib/ecl/attitude_fw/ecl_yaw_controller.h delete mode 100644 src/lib/ecl/ecl.h delete mode 100644 src/lib/ecl/l1/ecl_l1_pos_controller.cpp delete mode 100644 src/lib/ecl/l1/ecl_l1_pos_controller.h delete mode 100644 src/lib/ecl/validation/data_validator.cpp delete mode 100644 src/lib/ecl/validation/data_validator.h delete mode 100644 src/lib/ecl/validation/data_validator_group.cpp delete mode 100644 src/lib/ecl/validation/data_validator_group.h diff --git a/src/lib/ecl/CMakeLists.txt b/src/lib/ecl/CMakeLists.txt deleted file mode 100644 index 9f8e02ba2e..0000000000 --- a/src/lib/ecl/CMakeLists.txt +++ /dev/null @@ -1,49 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015 PX4 Development Team. 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 PX4 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. -# -############################################################################ -px4_add_module( - MODULE lib__ecl - COMPILE_FLAGS - -Os - SRCS - attitude_fw/ecl_controller.cpp - attitude_fw/ecl_pitch_controller.cpp - attitude_fw/ecl_roll_controller.cpp - attitude_fw/ecl_yaw_controller.cpp - attitude_fw/ecl_wheel_controller.cpp - l1/ecl_l1_pos_controller.cpp - validation/data_validator.cpp - validation/data_validator_group.cpp - DEPENDS - platforms__common - ) -# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/lib/ecl/attitude_fw/ecl_controller.cpp b/src/lib/ecl/attitude_fw/ecl_controller.cpp deleted file mode 100644 index 7bf4129e48..0000000000 --- a/src/lib/ecl/attitude_fw/ecl_controller.cpp +++ /dev/null @@ -1,140 +0,0 @@ -/**************************************************************************** - * - * 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 - * @author Thomas Gubler - * - * 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 -#include - -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; -} - -float ECL_Controller::constrain_airspeed(float airspeed, float minspeed, float maxspeed) { - float airspeed_result = airspeed; - if (!PX4_ISFINITE(airspeed)) { - /* airspeed is NaN, +- INF or not available, pick center of band */ - airspeed_result = 0.5f * (minspeed + maxspeed); - } else if (airspeed < minspeed) { - airspeed_result = minspeed; - } - return airspeed_result; -} diff --git a/src/lib/ecl/attitude_fw/ecl_controller.h b/src/lib/ecl/attitude_fw/ecl_controller.h deleted file mode 100644 index fed7d1d8fb..0000000000 --- a/src/lib/ecl/attitude_fw/ecl_controller.h +++ /dev/null @@ -1,125 +0,0 @@ -/**************************************************************************** - * - * 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 - * @author Thomas Gubler - * - * 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 -#include -#include - -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 acc_body_x; - float acc_body_y; - float acc_body_z; - 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; - float groundspeed; - float groundspeed_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]; - float constrain_airspeed(float airspeed, float minspeed, float maxspeed); -}; diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp deleted file mode 100644 index d04fe38ac3..0000000000 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ /dev/null @@ -1,198 +0,0 @@ -/**************************************************************************** - * - * 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_pitch_controller.cpp - * Implementation of a simple orthogonal pitch PID controller. - * - * Authors and acknowledgements in header. - */ - -#include "ecl_pitch_controller.h" -#include -#include -#include -#include -#include -#include -#include - -ECL_PitchController::ECL_PitchController() : - ECL_Controller("pitch"), - _max_rate_neg(0.0f), - _roll_ff(0.0f) -{ -} - -ECL_PitchController::~ECL_PitchController() -{ -} - -float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_data) -{ - - /* Do not calculate control signal with bad inputs */ - if (!(PX4_ISFINITE(ctl_data.pitch_setpoint) && - PX4_ISFINITE(ctl_data.roll) && - PX4_ISFINITE(ctl_data.pitch) && - PX4_ISFINITE(ctl_data.airspeed))) { - perf_count(_nonfinite_input_perf); - warnx("not controlling pitch"); - return _rate_setpoint; - } - - /* Calculate the error */ - 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; - - /* limit the rate */ - if (_max_rate > 0.01f && _max_rate_neg > 0.01f) { - if (_rate_setpoint > 0.0f) { - _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; - - } else { - _rate_setpoint = (_rate_setpoint < -_max_rate_neg) ? -_max_rate_neg : _rate_setpoint; - } - - } - - return _rate_setpoint; -} - -float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_data) -{ - /* Do not calculate control signal with bad inputs */ - if (!(PX4_ISFINITE(ctl_data.roll) && - PX4_ISFINITE(ctl_data.pitch) && - PX4_ISFINITE(ctl_data.pitch_rate) && - PX4_ISFINITE(ctl_data.yaw_rate) && - PX4_ISFINITE(ctl_data.yaw_rate_setpoint) && - PX4_ISFINITE(ctl_data.airspeed_min) && - PX4_ISFINITE(ctl_data.airspeed_max) && - PX4_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; - } - - /* 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; - - /* apply turning offset to desired bodyrate setpoint*/ - /* flying inverted (wings upside down)*/ - bool inverted = false; - float constrained_roll; - /* roll is used as feedforward term and inverted flight needs to be considered */ - if (fabsf(ctl_data.roll) < math::radians(90.0f)) { - /* not inverted, but numerically still potentially close to infinity */ - constrained_roll = math::constrain(ctl_data.roll, math::radians(-80.0f), math::radians(80.0f)); - - } else { - /* inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity */ - inverted = true; - /* note: the ranges are extended by 10 deg here to avoid numeric resolution effects */ - if (ctl_data.roll > 0.0f) { - /* right hemisphere */ - constrained_roll = math::constrain(ctl_data.roll, math::radians(100.0f), math::radians(180.0f)); - - } else { - /* left hemisphere */ - constrained_roll = math::constrain(ctl_data.roll, math::radians(-100.0f), math::radians(-180.0f)); - } - } - - /* input conditioning */ - float airspeed = constrain_airspeed(ctl_data.airspeed, ctl_data.airspeed_min, ctl_data.airspeed_max); - - /* Calculate desired body fixed y-axis angular rate needed to compensate for roll angle. - For reference see Automatic Control of Aircraft and Missiles by John H. Blakelock, pg. 175 - Availible on google books 8/11/2015: - https://books.google.com/books?id=ubcczZUDCsMC&pg=PA175#v=onepage&q&f=false*/ - float body_fixed_turn_offset = (fabsf((CONSTANTS_ONE_G / airspeed) * - tanf(constrained_roll) * sinf(constrained_roll))); - - if (inverted) { - body_fixed_turn_offset = -body_fixed_turn_offset; - } - - /* Finally add the turn offset to your bodyrate setpoint*/ - _bodyrate_setpoint += body_fixed_turn_offset; - - - _rate_error = _bodyrate_setpoint - ctl_data.pitch_rate; - - if (!lock_integrator && _k_i > 0.0f) { - - float id = _rate_error * dt * ctl_data.scaler; - - /* - * anti-windup: do not allow integrator to increase if actuator is at limit - */ - if (_last_output < -1.0f) { - /* only allow motion to center: increase value */ - id = math::max(id, 0.0f); - - } else if (_last_output > 1.0f) { - /* only allow motion to center: decrease value */ - id = math::min(id, 0.0f); - } - - _integrator += id; - } - - /* integrator limit */ - //xxx: until start detection is available: integral part in control signal is limited here - 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 * 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); -} diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h deleted file mode 100644 index fa7612cb99..0000000000 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h +++ /dev/null @@ -1,89 +0,0 @@ -/**************************************************************************** - * - * 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_pitch_controller.h - * Definition of a simple orthogonal pitch PID controller. - * - * @author Lorenz Meier - * @author Thomas Gubler - * - * 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. - */ - -#ifndef ECL_PITCH_CONTROLLER_H -#define ECL_PITCH_CONTROLLER_H - -#include -#include - -#include "ecl_controller.h" - -class __EXPORT ECL_PitchController : - public ECL_Controller -{ -public: - ECL_PitchController(); - - ~ECL_PitchController(); - - 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 = max_rate_pos; - } - - void set_max_rate_neg(float max_rate_neg) - { - _max_rate_neg = max_rate_neg; - } - - void set_roll_ff(float roll_ff) - { - _roll_ff = roll_ff; - } - -protected: - float _max_rate_neg; - float _roll_ff; -}; - -#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 deleted file mode 100644 index 9d40c73f3b..0000000000 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ /dev/null @@ -1,146 +0,0 @@ -/**************************************************************************** - * - * 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_roll_controller.cpp - * Implementation of a simple orthogonal roll PID controller. - * - * Authors and acknowledgements in header. - */ - -#include -#include "ecl_roll_controller.h" -#include -#include -#include -#include -#include -#include - -ECL_RollController::ECL_RollController() : - ECL_Controller("roll") -{ -} - -ECL_RollController::~ECL_RollController() -{ -} - -float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_data) -{ - /* Do not calculate control signal with bad inputs */ - if (!(PX4_ISFINITE(ctl_data.roll_setpoint) && PX4_ISFINITE(ctl_data.roll))) { - perf_count(_nonfinite_input_perf); - return _rate_setpoint; - } - - /* Calculate error */ - float roll_error = ctl_data.roll_setpoint - ctl_data.roll; - - /* Apply P controller */ - _rate_setpoint = roll_error / _tc; - - /* limit the rate */ //XXX: move to body angluar rates - - if (_max_rate > 0.01f) { - _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; - _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; - } - - return _rate_setpoint; -} - -float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_data) -{ - /* Do not calculate control signal with bad inputs */ - if (!(PX4_ISFINITE(ctl_data.pitch) && - PX4_ISFINITE(ctl_data.roll_rate) && - PX4_ISFINITE(ctl_data.yaw_rate) && - PX4_ISFINITE(ctl_data.yaw_rate_setpoint) && - PX4_ISFINITE(ctl_data.airspeed_min) && - PX4_ISFINITE(ctl_data.airspeed_max) && - PX4_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; - } - - /* Transform setpoint to body angular rates (jacobian) */ - _bodyrate_setpoint = _rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint; - - /* Calculate body angular rate error */ - _rate_error = _bodyrate_setpoint - ctl_data.roll_rate; //body angular rate error - - if (!lock_integrator && _k_i > 0.0f) { - - float id = _rate_error * dt * ctl_data.scaler; - - /* - * anti-windup: do not allow integrator to increase if actuator is at limit - */ - if (_last_output < -1.0f) { - /* only allow motion to center: increase value */ - id = math::max(id, 0.0f); - - } else if (_last_output > 1.0f) { - /* only allow motion to center: decrease value */ - id = math::min(id, 0.0f); - } - - _integrator += id; - } - - /* integrator limit */ - //xxx: until start detection is available: integral part in control signal is limited here - float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); - //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 * 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); -} - diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h deleted file mode 100644 index 6d07bab8ab..0000000000 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h +++ /dev/null @@ -1,69 +0,0 @@ -/**************************************************************************** - * - * 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_roll_controller.h - * Definition of a simple orthogonal roll PID controller. - * - * @author Lorenz Meier - * @author Thomas Gubler - * - * 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. - */ - -#ifndef ECL_ROLL_CONTROLLER_H -#define ECL_ROLL_CONTROLLER_H - -#include -#include - -#include "ecl_controller.h" - -class __EXPORT ECL_RollController : - public ECL_Controller -{ -public: - ECL_RollController(); - - ~ECL_RollController(); - - 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_wheel_controller.cpp b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp deleted file mode 100644 index 7e34ff2a61..0000000000 --- a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp +++ /dev/null @@ -1,153 +0,0 @@ -/**************************************************************************** - * - * 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_wheel_controller.cpp - * Implementation of a simple PID wheel controller for heading tracking. - * - * Authors and acknowledgements in header. - */ - -#include "ecl_wheel_controller.h" -#include -#include -#include -#include -#include -#include -#include - -ECL_WheelController::ECL_WheelController() : - ECL_Controller("wheel") -{ -} - -ECL_WheelController::~ECL_WheelController() -{ -} - -float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_data) -{ - /* Do not calculate control signal with bad inputs */ - if (!(PX4_ISFINITE(ctl_data.yaw_rate) && - PX4_ISFINITE(ctl_data.groundspeed) && - PX4_ISFINITE(ctl_data.groundspeed_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 min_speed = 1.0f; - - /* Calculate body angular rate error */ - _rate_error = _rate_setpoint - ctl_data.yaw_rate; //body angular rate error - - if (!lock_integrator && _k_i > 0.0f && ctl_data.groundspeed > min_speed) { - - float id = _rate_error * dt * ctl_data.groundspeed_scaler; - - /* - * anti-windup: do not allow integrator to increase if actuator is at limit - */ - if (_last_output < -1.0f) { - /* only allow motion to center: increase value */ - id = math::max(id, 0.0f); - - } else if (_last_output > 1.0f) { - /* only allow motion to center: decrease value */ - id = math::min(id, 0.0f); - } - - _integrator += id; - } - - /* integrator limit */ - //xxx: until start detection is available: integral part in control signal is limited here - float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); - - /* Apply PI rate controller and store non-limited output */ - _last_output = _rate_setpoint * _k_ff * ctl_data.groundspeed_scaler + - _rate_error * _k_p * ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler + - integrator_constrained; - /*warnx("wheel: _last_output: %.4f, _integrator: %.4f, scaler %.4f", - (double)_last_output, (double)_integrator, (double)ctl_data.groundspeed_scaler);*/ - - - return math::constrain(_last_output, -1.0f, 1.0f); -} - - -float ECL_WheelController::control_attitude(const struct ECL_ControlData &ctl_data) -{ - /* Do not calculate control signal with bad inputs */ - if (!(PX4_ISFINITE(ctl_data.yaw_setpoint) && - PX4_ISFINITE(ctl_data.yaw))) { - perf_count(_nonfinite_input_perf); - warnx("not controlling wheel"); - return _rate_setpoint; - } - - /* Calculate the error */ - float yaw_error = ctl_data.yaw_setpoint - ctl_data.yaw; - /* shortest angle (wrap around) */ - yaw_error = (float)fmod((float)fmod((yaw_error + M_PI_F), M_TWOPI_F) + M_TWOPI_F, M_TWOPI_F) - M_PI_F; - /*warnx("yaw_error: %.4f", (double)yaw_error);*/ - - /* Apply P controller: rate setpoint from current error and time constant */ - _rate_setpoint = yaw_error / _tc; - - /* limit the rate */ - if (_max_rate > 0.01f) { - if (_rate_setpoint > 0.0f) { - _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; - - } else { - _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; - } - - } - - return _rate_setpoint; -} diff --git a/src/lib/ecl/attitude_fw/ecl_wheel_controller.h b/src/lib/ecl/attitude_fw/ecl_wheel_controller.h deleted file mode 100644 index f153a8f8f1..0000000000 --- a/src/lib/ecl/attitude_fw/ecl_wheel_controller.h +++ /dev/null @@ -1,70 +0,0 @@ -/**************************************************************************** - * - * 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_wheel_controller.h - * Definition of a simple orthogonal coordinated turn yaw PID controller. - * - * @author Lorenz Meier - * @author Thomas Gubler - * @author Andreas Antener - * - * 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. - */ -#ifndef ECL_HEADING_CONTROLLER_H -#define ECL_HEADING_CONTROLLER_H - -#include -#include - -#include "ecl_controller.h" - -class __EXPORT ECL_WheelController : - public ECL_Controller -{ -public: - ECL_WheelController(); - - ~ECL_WheelController(); - - float control_attitude(const struct ECL_ControlData &ctl_data); - - float control_bodyrate(const struct ECL_ControlData &ctl_data); -}; - -#endif // ECL_HEADING_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp deleted file mode 100644 index 4849bdad79..0000000000 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ /dev/null @@ -1,240 +0,0 @@ -/**************************************************************************** - * - * 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_yaw_controller.cpp - * Implementation of a simple orthogonal coordinated turn yaw PID controller. - * - * Authors and acknowledgements in header. - */ - -#include "ecl_yaw_controller.h" -#include -#include -#include -#include -#include -#include -#include - -ECL_YawController::ECL_YawController() : - ECL_Controller("yaw"), - _coordinated_min_speed(1.0f), - _coordinated_method(0) -{ -} - -ECL_YawController::~ECL_YawController() -{ -} - -float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data) -{ - switch (_coordinated_method) { - case COORD_METHOD_OPEN: - return control_attitude_impl_openloop(ctl_data); - - case COORD_METHOD_CLOSEACC: - return control_attitude_impl_accclosedloop(ctl_data); - - default: - static hrt_abstime last_print = 0; - - if (ecl_elapsed_time(&last_print) > 5e6) { - warnx("invalid param setting FW_YCO_METHOD"); - last_print = ecl_absolute_time(); - } - } - - return _rate_setpoint; -} - -float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data) -{ - switch (_coordinated_method) { - case COORD_METHOD_OPEN: - case COORD_METHOD_CLOSEACC: - return control_bodyrate_impl(ctl_data); - - default: - static hrt_abstime last_print = 0; - - if (ecl_elapsed_time(&last_print) > 5e6) { - warnx("invalid param setting FW_YCO_METHOD"); - last_print = ecl_absolute_time(); - } - } - - return math::constrain(_last_output, -1.0f, 1.0f); -} - -float ECL_YawController::control_attitude_impl_openloop(const struct ECL_ControlData &ctl_data) -{ - /* Do not calculate control signal with bad inputs */ - if (!(PX4_ISFINITE(ctl_data.roll) && - PX4_ISFINITE(ctl_data.pitch) && - PX4_ISFINITE(ctl_data.speed_body_u) && - PX4_ISFINITE(ctl_data.speed_body_v) && - PX4_ISFINITE(ctl_data.speed_body_w) && - PX4_ISFINITE(ctl_data.roll_rate_setpoint) && - PX4_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(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 = (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); - } - -// if(counter % 20 == 0) { -// warnx("denumerator: %.4f, speed_body_u: %.4f, speed_body_w: %.4f, cosf(roll): %.4f, cosf(pitch): %.4f, sinf(pitch): %.4f", (double)denumerator, (double)speed_body_u, (double)speed_body_w, (double)cosf(roll), (double)cosf(pitch), (double)sinf(pitch)); -// } - } - - /* limit the rate */ //XXX: move to body angluar rates - - if (_max_rate > 0.01f) { - _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; - _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; - } - - -// counter++; - - if (!PX4_ISFINITE(_rate_setpoint)) { - warnx("yaw rate sepoint not finite"); - _rate_setpoint = 0.0f; - } - - return _rate_setpoint; -} - -float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl_data) -{ - /* Do not calculate control signal with bad inputs */ - if (!(PX4_ISFINITE(ctl_data.roll) && PX4_ISFINITE(ctl_data.pitch) && PX4_ISFINITE(ctl_data.pitch_rate) && - PX4_ISFINITE(ctl_data.yaw_rate) && PX4_ISFINITE(ctl_data.pitch_rate_setpoint) && - PX4_ISFINITE(ctl_data.airspeed_min) && PX4_ISFINITE(ctl_data.airspeed_max) && - PX4_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 (!PX4_ISFINITE(airspeed)) { - /* airspeed is NaN, +- INF or not available, pick center of band */ - 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 = -sinf(ctl_data.roll) * ctl_data.pitch_rate_setpoint + - cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _rate_setpoint; - - /* Close the acceleration loop if _coordinated_method wants this: change body_rate setpoint */ - if (_coordinated_method == COORD_METHOD_CLOSEACC) { - //XXX: filtering of acceleration? - _bodyrate_setpoint -= (ctl_data.acc_body_y / (airspeed * cosf(ctl_data.pitch))); - } - - /* Calculate body angular rate error */ - _rate_error = _bodyrate_setpoint - ctl_data.yaw_rate; //body angular rate error - - if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) { - - float id = _rate_error * dt; - - /* - * anti-windup: do not allow integrator to increase if actuator is at limit - */ - if (_last_output < -1.0f) { - /* only allow motion to center: increase value */ - id = math::max(id, 0.0f); - - } else if (_last_output > 1.0f) { - /* only allow motion to center: decrease value */ - id = math::min(id, 0.0f); - } - - _integrator += id; - } - - /* integrator limit */ - //xxx: until start detection is available: integral part in control signal is limited here - 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) * 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); -} - -float ECL_YawController::control_attitude_impl_accclosedloop(const struct ECL_ControlData &ctl_data) -{ - /* dont set a rate setpoint */ - return 0.0f; -} diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h deleted file mode 100644 index 511b5fd36f..0000000000 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ /dev/null @@ -1,98 +0,0 @@ -/**************************************************************************** - * - * 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_yaw_controller.h - * Definition of a simple orthogonal coordinated turn yaw PID controller. - * - * @author Lorenz Meier - * @author Thomas Gubler - * - * 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. - */ -#ifndef ECL_YAW_CONTROLLER_H -#define ECL_YAW_CONTROLLER_H - -#include -#include - -#include "ecl_controller.h" - -class __EXPORT ECL_YawController : - public ECL_Controller -{ -public: - ECL_YawController(); - - ~ECL_YawController(); - - 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; - } - - void set_coordinated_method(int32_t coordinated_method) - { - _coordinated_method = coordinated_method; - } - - enum { - COORD_METHOD_OPEN = 0, - COORD_METHOD_CLOSEACC = 1 - }; - -protected: - float _coordinated_min_speed; - float _max_rate; - - int32_t _coordinated_method; - - float control_bodyrate_impl(const struct ECL_ControlData &ctl_data); - - float control_attitude_impl_openloop(const struct ECL_ControlData &ctl_data); - - float control_attitude_impl_accclosedloop(const struct ECL_ControlData &ctl_data); - -}; - -#endif // ECL_YAW_CONTROLLER_H diff --git a/src/lib/ecl/ecl.h b/src/lib/ecl/ecl.h deleted file mode 100644 index 39e1f50809..0000000000 --- a/src/lib/ecl/ecl.h +++ /dev/null @@ -1,46 +0,0 @@ -/**************************************************************************** - * - * 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 APL 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.h - * Adapter / shim layer for system calls needed by ECL - * - */ - -#include -#include - -#define ecl_absolute_time hrt_absolute_time -#define ecl_elapsed_time hrt_elapsed_time -#define ECL_WARN PX4_WARN -#define ECL_INFO PX4_INFO diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp deleted file mode 100644 index d1c864d782..0000000000 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp +++ /dev/null @@ -1,372 +0,0 @@ -/**************************************************************************** - * - * 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_l1_pos_controller.h - * Implementation of L1 position control. - * Authors and acknowledgements in header. - * - */ - -#include - -#include "ecl_l1_pos_controller.h" - -float ECL_L1_Pos_Controller::nav_roll() -{ - float ret = atanf(_lateral_accel * 1.0f / CONSTANTS_ONE_G); - ret = math::constrain(ret, -_roll_lim_rad, _roll_lim_rad); - return ret; -} - -float ECL_L1_Pos_Controller::nav_lateral_acceleration_demand() -{ - return _lateral_accel; -} - -float ECL_L1_Pos_Controller::nav_bearing() -{ - return _wrap_pi(_nav_bearing); -} - -float ECL_L1_Pos_Controller::bearing_error() -{ - return _bearing_error; -} - -float ECL_L1_Pos_Controller::target_bearing() -{ - return _target_bearing; -} - -float ECL_L1_Pos_Controller::switch_distance(float wp_radius) -{ - /* following [2], switching on L1 distance */ - return math::min(wp_radius, _L1_distance); -} - -bool ECL_L1_Pos_Controller::reached_loiter_target(void) -{ - return _circle_mode; -} - -float ECL_L1_Pos_Controller::crosstrack_error(void) -{ - return _crosstrack_error; -} - -void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector<2> &vector_A, const math::Vector<2> &vector_B, const math::Vector<2> &vector_curr_position, - const math::Vector<2> &ground_speed_vector) -{ - - /* this follows the logic presented in [1] */ - - float eta; - float xtrack_vel; - float ltrack_vel; - - /* get the direction between the last (visited) and next waypoint */ - _target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_B(0), vector_B(1)); - - /* enforce a minimum ground speed of 0.1 m/s to avoid singularities */ - float ground_speed = math::max(ground_speed_vector.length(), 0.1f); - - /* calculate the L1 length required for the desired period */ - _L1_distance = _L1_ratio * ground_speed; - - /* calculate vector from A to B */ - math::Vector<2> vector_AB = get_local_planar_vector(vector_A, vector_B); - - /* - * check if waypoints are on top of each other. If yes, - * skip A and directly continue to B - */ - if (vector_AB.length() < 1.0e-6f) { - vector_AB = get_local_planar_vector(vector_curr_position, vector_B); - } - - vector_AB.normalize(); - - /* calculate the vector from waypoint A to the aircraft */ - math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); - - /* calculate crosstrack error (output only) */ - _crosstrack_error = vector_AB % vector_A_to_airplane; - - /* - * If the current position is in a +-135 degree angle behind waypoint A - * and further away from A than the L1 distance, then A becomes the L1 point. - * If the aircraft is already between A and B normal L1 logic is applied. - */ - float distance_A_to_airplane = vector_A_to_airplane.length(); - float alongTrackDist = vector_A_to_airplane * vector_AB; - - /* estimate airplane position WRT to B */ - math::Vector<2> vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized(); - - /* calculate angle of airplane position vector relative to line) */ - - // XXX this could probably also be based solely on the dot product - float AB_to_BP_bearing = atan2f(vector_B_to_P_unit % vector_AB, vector_B_to_P_unit * vector_AB); - - /* extension from [2], fly directly to A */ - if (distance_A_to_airplane > _L1_distance && alongTrackDist / math::max(distance_A_to_airplane , 1.0f) < -0.7071f) { - - /* calculate eta to fly to waypoint A */ - - /* unit vector from waypoint A to current position */ - math::Vector<2> vector_A_to_airplane_unit = vector_A_to_airplane.normalized(); - /* velocity across / orthogonal to line */ - xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit); - /* velocity along line */ - ltrack_vel = ground_speed_vector * (-vector_A_to_airplane_unit); - eta = atan2f(xtrack_vel, ltrack_vel); - /* bearing from current position to L1 point */ - _nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0)); - - /* - * If the AB vector and the vector from B to airplane point in the same - * direction, we have missed the waypoint. At +- 90 degrees we are just passing it. - */ - } else if (fabsf(AB_to_BP_bearing) < math::radians(100.0f)) { - /* - * Extension, fly back to waypoint. - * - * This corner case is possible if the system was following - * the AB line from waypoint A to waypoint B, then is - * switched to manual mode (or otherwise misses the waypoint) - * and behind the waypoint continues to follow the AB line. - */ - - /* calculate eta to fly to waypoint B */ - - /* velocity across / orthogonal to line */ - xtrack_vel = ground_speed_vector % (-vector_B_to_P_unit); - /* velocity along line */ - ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit); - eta = atan2f(xtrack_vel, ltrack_vel); - /* bearing from current position to L1 point */ - _nav_bearing = atan2f(-vector_B_to_P_unit(1) , -vector_B_to_P_unit(0)); - - } else { - - /* calculate eta to fly along the line between A and B */ - - /* velocity across / orthogonal to line */ - xtrack_vel = ground_speed_vector % vector_AB; - /* velocity along line */ - ltrack_vel = ground_speed_vector * vector_AB; - /* calculate eta2 (angle of velocity vector relative to line) */ - float eta2 = atan2f(xtrack_vel, ltrack_vel); - /* calculate eta1 (angle to L1 point) */ - float xtrackErr = vector_A_to_airplane % vector_AB; - float sine_eta1 = xtrackErr / math::max(_L1_distance , 0.1f); - /* limit output to 45 degrees */ - sine_eta1 = math::constrain(sine_eta1, -0.7071f, 0.7071f); //sin(pi/4) = 0.7071 - float eta1 = asinf(sine_eta1); - eta = eta1 + eta2; - /* bearing from current position to L1 point */ - _nav_bearing = atan2f(vector_AB(1), vector_AB(0)) + eta1; - - } - - /* limit angle to +-90 degrees */ - eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f); - _lateral_accel = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta); - - /* flying to waypoints, not circling them */ - _circle_mode = false; - - /* the bearing angle, in NED frame */ - _bearing_error = eta; -} - -void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector<2> &vector_A, const math::Vector<2> &vector_curr_position, float radius, int8_t loiter_direction, - const math::Vector<2> &ground_speed_vector) -{ - /* the complete guidance logic in this section was proposed by [2] */ - - /* calculate the gains for the PD loop (circle tracking) */ - float omega = (2.0f * M_PI_F / _L1_period); - float K_crosstrack = omega * omega; - float K_velocity = 2.0f * _L1_damping * omega; - - /* update bearing to next waypoint */ - _target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_A(0), vector_A(1)); - - /* ground speed, enforce minimum of 0.1 m/s to avoid singularities */ - float ground_speed = math::max(ground_speed_vector.length() , 0.1f); - - /* calculate the L1 length required for the desired period */ - _L1_distance = _L1_ratio * ground_speed; - - /* calculate the vector from waypoint A to current position */ - math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); - - math::Vector<2> vector_A_to_airplane_unit; - - /* prevent NaN when normalizing */ - if (vector_A_to_airplane.length() > FLT_EPSILON) { - /* store the normalized vector from waypoint A to current position */ - vector_A_to_airplane_unit = vector_A_to_airplane.normalized(); - } else { - vector_A_to_airplane_unit = vector_A_to_airplane; - } - - /* calculate eta angle towards the loiter center */ - - /* velocity across / orthogonal to line from waypoint to current position */ - float xtrack_vel_center = vector_A_to_airplane_unit % ground_speed_vector; - /* velocity along line from waypoint to current position */ - float ltrack_vel_center = - (ground_speed_vector * vector_A_to_airplane_unit); - float eta = atan2f(xtrack_vel_center, ltrack_vel_center); - /* limit eta to 90 degrees */ - eta = math::constrain(eta, -M_PI_F / 2.0f, +M_PI_F / 2.0f); - - /* calculate the lateral acceleration to capture the center point */ - float lateral_accel_sp_center = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta); - - /* for PD control: Calculate radial position and velocity errors */ - - /* radial velocity error */ - float xtrack_vel_circle = -ltrack_vel_center; - /* radial distance from the loiter circle (not center) */ - float xtrack_err_circle = vector_A_to_airplane.length() - radius; - - /* cross track error for feedback */ - _crosstrack_error = xtrack_err_circle; - - /* calculate PD update to circle waypoint */ - float lateral_accel_sp_circle_pd = (xtrack_err_circle * K_crosstrack + xtrack_vel_circle * K_velocity); - - /* calculate velocity on circle / along tangent */ - float tangent_vel = xtrack_vel_center * loiter_direction; - - /* prevent PD output from turning the wrong way */ - if (tangent_vel < 0.0f) { - lateral_accel_sp_circle_pd = math::max(lateral_accel_sp_circle_pd , 0.0f); - } - - /* calculate centripetal acceleration setpoint */ - float lateral_accel_sp_circle_centripetal = tangent_vel * tangent_vel / math::max((0.5f * radius) , (radius + xtrack_err_circle)); - - /* add PD control on circle and centripetal acceleration for total circle command */ - float lateral_accel_sp_circle = loiter_direction * (lateral_accel_sp_circle_pd + lateral_accel_sp_circle_centripetal); - - /* - * Switch between circle (loiter) and capture (towards waypoint center) mode when - * the commands switch over. Only fly towards waypoint if outside the circle. - */ - - // XXX check switch over - if ((lateral_accel_sp_center < lateral_accel_sp_circle && loiter_direction > 0 && xtrack_err_circle > 0.0f) || - (lateral_accel_sp_center > lateral_accel_sp_circle && loiter_direction < 0 && xtrack_err_circle > 0.0f)) { - _lateral_accel = lateral_accel_sp_center; - _circle_mode = false; - /* angle between requested and current velocity vector */ - _bearing_error = eta; - /* bearing from current position to L1 point */ - _nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0)); - - } else { - _lateral_accel = lateral_accel_sp_circle; - _circle_mode = true; - _bearing_error = 0.0f; - /* bearing from current position to L1 point */ - _nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0)); - } -} - - -void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const math::Vector<2> &ground_speed_vector) -{ - /* the complete guidance logic in this section was proposed by [2] */ - - float eta; - - /* - * As the commanded heading is the only reference - * (and no crosstrack correction occurs), - * target and navigation bearing become the same - */ - _target_bearing = _nav_bearing = _wrap_pi(navigation_heading); - eta = _target_bearing - _wrap_pi(current_heading); - eta = _wrap_pi(eta); - - /* consequently the bearing error is exactly eta: */ - _bearing_error = eta; - - /* ground speed is the length of the ground speed vector */ - float ground_speed = ground_speed_vector.length(); - - /* adjust L1 distance to keep constant frequency */ - _L1_distance = ground_speed / _heading_omega; - float omega_vel = ground_speed * _heading_omega; - - /* not circling a waypoint */ - _circle_mode = false; - - /* navigating heading means by definition no crosstrack error */ - _crosstrack_error = 0; - - /* limit eta to 90 degrees */ - eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f); - _lateral_accel = 2.0f * sinf(eta) * omega_vel; -} - -void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading) -{ - /* the logic in this section is trivial, but originally proposed by [2] */ - - /* reset all heading / error measures resulting in zero roll */ - _target_bearing = current_heading; - _nav_bearing = current_heading; - _bearing_error = 0; - _crosstrack_error = 0; - _lateral_accel = 0; - - /* not circling a waypoint when flying level */ - _circle_mode = false; - -} - - -math::Vector<2> ECL_L1_Pos_Controller::get_local_planar_vector(const math::Vector<2> &origin, const math::Vector<2> &target) const -{ - /* this is an approximation for small angles, proposed by [2] */ - - math::Vector<2> out(math::radians((target(0) - origin(0))), math::radians((target(1) - origin(1))*cosf(math::radians(origin(0))))); - - return out * static_cast(CONSTANTS_RADIUS_OF_EARTH); -} - diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.h b/src/lib/ecl/l1/ecl_l1_pos_controller.h deleted file mode 100644 index 5c0804a392..0000000000 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.h +++ /dev/null @@ -1,268 +0,0 @@ -/**************************************************************************** - * - * 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_l1_pos_control.h - * Implementation of L1 position control. - * - * - * Acknowledgements and References: - * - * This implementation has been built for PX4 based on the original - * publication from [1] and does include a lot of the ideas (not code) - * from [2]. - * - * - * [1] S. Park, J. Deyst, and J. P. How, "A New Nonlinear Guidance Logic for Trajectory Tracking," - * Proceedings of the AIAA Guidance, Navigation and Control - * Conference, Aug 2004. AIAA-2004-4900. - * - * [2] Paul Riseborough, Brandon Jones and Andrew Tridgell, L1 control for APM. Aug 2013. - * - Explicit control over frequency and damping - * - Explicit control over track capture angle - * - Ability to use loiter radius smaller than L1 length - * - Modified to use PD control for circle tracking to enable loiter radius less than L1 length - * - Modified to enable period and damping of guidance loop to be set explicitly - * - Modified to provide explicit control over capture angle - * - */ - -#ifndef ECL_L1_POS_CONTROLLER_H -#define ECL_L1_POS_CONTROLLER_H - -#include -#include -#include - -/** - * L1 Nonlinear Guidance Logic - */ -class __EXPORT ECL_L1_Pos_Controller -{ -public: - ECL_L1_Pos_Controller() { - _L1_period = 25; - _L1_damping = 0.75f; - } - - /** - * The current target bearing - * - * @return bearing angle (-pi..pi, in NED frame) - */ - float nav_bearing(); - - - /** - * Get lateral acceleration demand. - * - * @return Lateral acceleration in m/s^2 - */ - float nav_lateral_acceleration_demand(); - - - /** - * Heading error. - * - * The heading error is either compared to the current track - * or to the tangent of the current loiter radius. - */ - float bearing_error(); - - - /** - * Bearing from aircraft to current target. - * - * @return bearing angle (-pi..pi, in NED frame) - */ - float target_bearing(); - - - /** - * Get roll angle setpoint for fixed wing. - * - * @return Roll angle (in NED frame) - */ - float nav_roll(); - - - /** - * Get the current crosstrack error. - * - * @return Crosstrack error in meters. - */ - float crosstrack_error(); - - - /** - * Returns true if the loiter waypoint has been reached - */ - bool reached_loiter_target(); - - - /** - * Returns true if following a circle (loiter) - */ - bool circle_mode() { - return _circle_mode; - } - - - /** - * Get the switch distance - * - * This is the distance at which the system will - * switch to the next waypoint. This depends on the - * period and damping - * - * @param waypoint_switch_radius The switching radius the waypoint has set. - */ - float switch_distance(float waypoint_switch_radius); - - - /** - * Navigate between two waypoints - * - * Calling this function with two waypoints results in the - * control outputs to fly to the line segment defined by - * the points and once captured following the line segment. - * This follows the logic in [1]. - * - * @return sets _lateral_accel setpoint - */ - void navigate_waypoints(const math::Vector<2> &vector_A, const math::Vector<2> &vector_B, const math::Vector<2> &vector_curr_position, - const math::Vector<2> &ground_speed); - - - /** - * Navigate on an orbit around a loiter waypoint. - * - * This allow orbits smaller than the L1 length, - * this modification was introduced in [2]. - * - * @return sets _lateral_accel setpoint - */ - void navigate_loiter(const math::Vector<2> &vector_A, const math::Vector<2> &vector_curr_position, float radius, int8_t loiter_direction, - const math::Vector<2> &ground_speed_vector); - - - /** - * Navigate on a fixed bearing. - * - * This only holds a certain direction and does not perform cross - * track correction. Helpful for semi-autonomous modes. Introduced - * by [2]. - * - * @return sets _lateral_accel setpoint - */ - void navigate_heading(float navigation_heading, float current_heading, const math::Vector<2> &ground_speed); - - - /** - * Keep the wings level. - * - * This is typically needed for maximum-lift-demand situations, - * such as takeoff or near stall. Introduced in [2]. - */ - void navigate_level_flight(float current_heading); - - - /** - * Set the L1 period. - */ - void set_l1_period(float period) { - _L1_period = period; - /* calculate the ratio introduced in [2] */ - _L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period; - /* calculate normalized frequency for heading tracking */ - _heading_omega = sqrtf(2.0f) * M_PI_F / _L1_period; - } - - - /** - * Set the L1 damping factor. - * - * The original publication recommends a default of sqrt(2) / 2 = 0.707 - */ - void set_l1_damping(float damping) { - _L1_damping = damping; - /* calculate the ratio introduced in [2] */ - _L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period; - /* calculate the L1 gain (following [2]) */ - _K_L1 = 4.0f * _L1_damping * _L1_damping; - } - - - /** - * Set the maximum roll angle output in radians - * - */ - void set_l1_roll_limit(float roll_lim_rad) { - _roll_lim_rad = roll_lim_rad; - } - -private: - - float _lateral_accel; ///< Lateral acceleration setpoint in m/s^2 - float _L1_distance; ///< L1 lead distance, defined by period and damping - bool _circle_mode; ///< flag for loiter mode - float _nav_bearing; ///< bearing to L1 reference point - float _bearing_error; ///< bearing error - float _crosstrack_error; ///< crosstrack error in meters - float _target_bearing; ///< the heading setpoint - - float _L1_period; ///< L1 tracking period in seconds - float _L1_damping; ///< L1 damping ratio - float _L1_ratio; ///< L1 ratio for navigation - float _K_L1; ///< L1 control gain for _L1_damping - float _heading_omega; ///< Normalized frequency - - float _roll_lim_rad; /// get_local_planar_vector(const math::Vector<2> &origin, const math::Vector<2> &target) const; - -}; - - -#endif /* ECL_L1_POS_CONTROLLER_H */ diff --git a/src/lib/ecl/validation/data_validator.cpp b/src/lib/ecl/validation/data_validator.cpp deleted file mode 100644 index 4e55849df0..0000000000 --- a/src/lib/ecl/validation/data_validator.cpp +++ /dev/null @@ -1,169 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015 PX4 Development Team. 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 PX4 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 data_validator.c - * - * A data validation class to identify anomalies in data streams - * - * @author Lorenz Meier - */ - -#include "data_validator.h" -#include - -DataValidator::DataValidator(DataValidator *prev_sibling) : - _error_mask(ERROR_FLAG_NO_ERROR), - _time_last(0), - _timeout_interval(20000), - _event_count(0), - _error_count(0), - _error_density(0), - _priority(0), - _mean{0.0f}, - _lp{0.0f}, - _M2{0.0f}, - _rms{0.0f}, - _value{0.0f}, - _value_equal_count(0), - _sibling(prev_sibling) -{ - -} - -DataValidator::~DataValidator() -{ - -} - -void -DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in, int priority_in) -{ - _event_count++; - - if (error_count_in > _error_count) { - _error_density += (error_count_in - _error_count); - } else if (_error_density > 0) { - _error_density--; - } - - _error_count = error_count_in; - _priority = priority_in; - - for (unsigned i = 0; i < _dimensions; i++) { - if (_time_last == 0) { - _mean[i] = 0; - _lp[i] = val[i]; - _M2[i] = 0; - } else { - float lp_val = val[i] - _lp[i]; - - float delta_val = lp_val - _mean[i]; - _mean[i] += delta_val / _event_count; - _M2[i] += delta_val * (lp_val - _mean[i]); - _rms[i] = sqrtf(_M2[i] / (_event_count - 1)); - - if (fabsf(_value[i] - val[i]) < 0.000001f) { - _value_equal_count++; - } else { - _value_equal_count = 0; - } - } - - // XXX replace with better filter, make it auto-tune to update rate - _lp[i] = _lp[i] * 0.5f + val[i] * 0.5f; - - _value[i] = val[i]; - } - - _time_last = timestamp; -} - -float -DataValidator::confidence(uint64_t timestamp) -{ - float ret = 1.0f; - - /* check if we have any data */ - if (_time_last == 0) { - _error_mask |= ERROR_FLAG_NO_DATA; - ret = 0.0f; - - /* timed out - that's it */ - } else if (timestamp - _time_last > _timeout_interval) { - _error_mask |= ERROR_FLAG_TIMEOUT; - ret = 0.0f; - - /* we got the exact same sensor value N times in a row */ - } else if (_value_equal_count > VALUE_EQUAL_COUNT_MAX) { - _error_mask |= ERROR_FLAG_STALE_DATA; - ret = 0.0f; - - /* check error count limit */ - } else if (_error_count > NORETURN_ERRCOUNT) { - _error_mask |= ERROR_FLAG_HIGH_ERRCOUNT; - ret = 0.0f; - - /* cap error density counter at window size */ - } else if (_error_density > ERROR_DENSITY_WINDOW) { - _error_mask |= ERROR_FLAG_HIGH_ERRDENSITY; - _error_density = ERROR_DENSITY_WINDOW; - - /* no error */ - } else { - _error_mask = ERROR_FLAG_NO_ERROR; - } - - /* no critical errors */ - if (ret > 0.0f) { - /* return local error density for last N measurements */ - ret = 1.0f - (_error_density / ERROR_DENSITY_WINDOW); - } - - return ret; -} - -void -DataValidator::print() -{ - if (_time_last == 0) { - ECL_INFO("\tno data"); - return; - } - - for (unsigned i = 0; i < _dimensions; i++) { - ECL_INFO("\tval: %8.4f, lp: %8.4f mean dev: %8.4f RMS: %8.4f conf: %8.4f", - (double) _value[i], (double)_lp[i], (double)_mean[i], - (double)_rms[i], (double)confidence(hrt_absolute_time())); - } -} diff --git a/src/lib/ecl/validation/data_validator.h b/src/lib/ecl/validation/data_validator.h deleted file mode 100644 index c313b1cb04..0000000000 --- a/src/lib/ecl/validation/data_validator.h +++ /dev/null @@ -1,159 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015 PX4 Development Team. 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 PX4 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 data_validator.h - * - * A data validation class to identify anomalies in data streams - * - * @author Lorenz Meier - */ - -#pragma once - -#include -#include - -class __EXPORT DataValidator { -public: - DataValidator(DataValidator *prev_sibling = nullptr); - virtual ~DataValidator(); - - /** - * Put an item into the validator. - * - * @param val Item to put - */ - void put(uint64_t timestamp, float val[3], uint64_t error_count, int priority); - - /** - * Get the next sibling in the group - * - * @return the next sibling - */ - DataValidator* sibling() { return _sibling; } - - /** - * Get the confidence of this validator - * @return the confidence between 0 and 1 - */ - float confidence(uint64_t timestamp); - - /** - * Get the error count of this validator - * @return the error count - */ - uint64_t error_count() { return _error_count; } - - /** - * Get the values of this validator - * @return the stored value - */ - float* value() { return _value; } - - /** - * Get the used status of this validator - * @return true if this validator ever saw data - */ - bool used() { return (_time_last > 0); } - - /** - * Get the priority of this validator - * @return the stored priority - */ - int priority() { return (_priority); } - - /** - * Get the error state of this validator - * @return the bitmask with the error status - */ - uint32_t state() { return _error_mask; } - - /** - * Reset the error state of this validator - */ - void reset_state() { _error_mask = ERROR_FLAG_NO_ERROR; } - - /** - * Get the RMS values of this validator - * @return the stored RMS - */ - float* rms() { return _rms; } - - /** - * Print the validator value - * - */ - void print(); - - /** - * Set the timeout value - * - * @param timeout_interval_us The timeout interval in microseconds - */ - void set_timeout(uint64_t timeout_interval_us) { _timeout_interval = timeout_interval_us; } - - /** - * Data validator error states - */ - static constexpr uint32_t ERROR_FLAG_NO_ERROR = (0x00000000U); - static constexpr uint32_t ERROR_FLAG_NO_DATA = (0x00000001U); - static constexpr uint32_t ERROR_FLAG_STALE_DATA = (0x00000001U << 1); - static constexpr uint32_t ERROR_FLAG_TIMEOUT = (0x00000001U << 2); - static constexpr uint32_t ERROR_FLAG_HIGH_ERRCOUNT = (0x00000001U << 3); - static constexpr uint32_t ERROR_FLAG_HIGH_ERRDENSITY = (0x00000001U << 4); - -private: - static const unsigned _dimensions = 3; - uint32_t _error_mask; /**< sensor error state */ - uint64_t _time_last; /**< last timestamp */ - uint64_t _timeout_interval; /**< interval in which the datastream times out in us */ - uint64_t _event_count; /**< total data counter */ - uint64_t _error_count; /**< error count */ - int _error_density; /**< ratio between successful reads and errors */ - int _priority; /**< sensor nominal priority */ - float _mean[_dimensions]; /**< mean of value */ - float _lp[3]; /**< low pass value */ - float _M2[3]; /**< RMS component value */ - float _rms[3]; /**< root mean square error */ - float _value[3]; /**< last value */ - float _value_equal_count; /**< equal values in a row */ - DataValidator *_sibling; /**< sibling in the group */ - const unsigned NORETURN_ERRCOUNT = 10000; /**< if the error count reaches this value, return sensor as invalid */ - const float ERROR_DENSITY_WINDOW = 100.0f; /**< window in measurement counts for errors */ - const unsigned VALUE_EQUAL_COUNT_MAX = 100; /**< if the sensor value is the same (accumulated also between axes) this many times, flag it */ - - /* we don't want this class to be copied */ - DataValidator(const DataValidator&); - DataValidator operator=(const DataValidator&); -}; diff --git a/src/lib/ecl/validation/data_validator_group.cpp b/src/lib/ecl/validation/data_validator_group.cpp deleted file mode 100644 index 8bf060cf65..0000000000 --- a/src/lib/ecl/validation/data_validator_group.cpp +++ /dev/null @@ -1,266 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015 PX4 Development Team. 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 PX4 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 data_validator_group.cpp - * - * A data validation group to identify anomalies in data streams - * - * @author Lorenz Meier - */ - -#include "data_validator_group.h" -#include - -DataValidatorGroup::DataValidatorGroup(unsigned siblings) : - _first(nullptr), - _curr_best(-1), - _prev_best(-1), - _first_failover_time(0), - _toggle_count(0) -{ - DataValidator *next = _first; - - for (unsigned i = 0; i < siblings; i++) { - next = new DataValidator(next); - } - - _first = next; -} - -DataValidatorGroup::~DataValidatorGroup() -{ - -} - -void -DataValidatorGroup::set_timeout(uint64_t timeout_interval_us) -{ - DataValidator *next = _first; - - while (next != nullptr) { - next->set_timeout(timeout_interval_us); - next = next->sibling(); - } -} - -void -DataValidatorGroup::put(unsigned index, uint64_t timestamp, float val[3], uint64_t error_count, int priority) -{ - DataValidator *next = _first; - unsigned i = 0; - - while (next != nullptr) { - if (i == index) { - next->put(timestamp, val, error_count, priority); - break; - } - next = next->sibling(); - i++; - } -} - -float* -DataValidatorGroup::get_best(uint64_t timestamp, int *index) -{ - DataValidator *next = _first; - - // XXX This should eventually also include voting - int pre_check_best = _curr_best; - float pre_check_confidence = 1.0f; - int pre_check_prio = -1; - float max_confidence = -1.0f; - int max_priority = -1000; - int max_index = -1; - DataValidator *best = nullptr; - - unsigned i = 0; - - while (next != nullptr) { - float confidence = next->confidence(timestamp); - - if (static_cast(i) == pre_check_best) { - pre_check_prio = next->priority(); - pre_check_confidence = confidence; - } - - /* - * Switch if: - * 1) the confidence is higher and priority is equal or higher - * 2) the confidence is no less than 1% different and the priority is higher - */ - if (((max_confidence < MIN_REGULAR_CONFIDENCE) && (confidence >= MIN_REGULAR_CONFIDENCE)) || - (confidence > max_confidence && (next->priority() >= max_priority)) || - (fabsf(confidence - max_confidence) < 0.01f && (next->priority() > max_priority)) - ) { - max_index = i; - max_confidence = confidence; - max_priority = next->priority(); - best = next; - } - - next = next->sibling(); - i++; - } - - /* the current best sensor is not matching the previous best sensor */ - if (max_index != _curr_best) { - - bool true_failsafe = true; - - /* check whether the switch was a failsafe or preferring a higher priority sensor */ - if (pre_check_prio != -1 && pre_check_prio < max_priority && - fabsf(pre_check_confidence - max_confidence) < 0.1f) { - /* this is not a failover */ - true_failsafe = false; - /* reset error flags, this is likely a hotplug sensor coming online late */ - best->reset_state(); - } - - /* if we're no initialized, initialize the bookkeeping but do not count a failsafe */ - if (_curr_best < 0) { - _prev_best = max_index; - } else { - /* we were initialized before, this is a real failsafe */ - _prev_best = pre_check_best; - - if (true_failsafe) { - _toggle_count++; - - /* if this is the first time, log when we failed */ - if (_first_failover_time == 0) { - _first_failover_time = timestamp; - } - } - } - - /* for all cases we want to keep a record of the best index */ - _curr_best = max_index; - } - *index = max_index; - return (best) ? best->value() : nullptr; -} - -float -DataValidatorGroup::get_vibration_factor(uint64_t timestamp) -{ - DataValidator *next = _first; - - float vibe = 0.0f; - - /* find the best RMS value of a non-timed out sensor */ - while (next != nullptr) { - - if (next->confidence(timestamp) > 0.5f) { - float* rms = next->rms(); - - for (unsigned j = 0; j < 3; j++) { - if (rms[j] > vibe) { - vibe = rms[j]; - } - } - } - - next = next->sibling(); - } - - return vibe; -} - -void -DataValidatorGroup::print() -{ - /* print the group's state */ - ECL_INFO("validator: best: %d, prev best: %d, failsafe: %s (%u events)", - _curr_best, _prev_best, (_toggle_count > 0) ? "YES" : "NO", - _toggle_count); - - DataValidator *next = _first; - unsigned i = 0; - - while (next != nullptr) { - if (next->used()) { - uint32_t flags = next->state(); - - ECL_INFO("sensor #%u, prio: %d, state:%s%s%s%s%s%s", i, next->priority(), - ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " NO_DATA" : ""), - ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE_DATA" : ""), - ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " DATA_TIMEOUT" : ""), - ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " HIGH_ERRCOUNT" : ""), - ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " HIGH_ERRDENSITY" : ""), - ((flags == DataValidator::ERROR_FLAG_NO_ERROR) ? " OK" : "")); - - next->print(); - } - next = next->sibling(); - i++; - } -} - -unsigned -DataValidatorGroup::failover_count() -{ - return _toggle_count; -} - -int -DataValidatorGroup::failover_index() -{ - DataValidator *next = _first; - unsigned i = 0; - - while (next != nullptr) { - if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == (unsigned)_prev_best)) { - return i; - } - next = next->sibling(); - i++; - } - return -1; -} - -uint32_t -DataValidatorGroup::failover_state() -{ - DataValidator *next = _first; - unsigned i = 0; - - while (next != nullptr) { - if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == (unsigned)_prev_best)) { - return next->state(); - } - next = next->sibling(); - i++; - } - return DataValidator::ERROR_FLAG_NO_ERROR; -} diff --git a/src/lib/ecl/validation/data_validator_group.h b/src/lib/ecl/validation/data_validator_group.h deleted file mode 100644 index 855c3ed4ae..0000000000 --- a/src/lib/ecl/validation/data_validator_group.h +++ /dev/null @@ -1,122 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015 PX4 Development Team. 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 PX4 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 data_validator_group.h - * - * A data validation group to identify anomalies in data streams - * - * @author Lorenz Meier - */ - -#pragma once - -#include "data_validator.h" - -class __EXPORT DataValidatorGroup { -public: - DataValidatorGroup(unsigned siblings); - virtual ~DataValidatorGroup(); - - /** - * Put an item into the validator group. - * - * @param index Sensor index - * @param timestamp The timestamp of the measurement - * @param val The 3D vector - * @param error_count The current error count of the sensor - * @param priority The priority of the sensor - */ - void put(unsigned index, uint64_t timestamp, - float val[3], uint64_t error_count, int priority); - - /** - * Get the best data triplet of the group - * - * @return pointer to the array of best values - */ - float* get_best(uint64_t timestamp, int *index); - - /** - * Get the RMS / vibration factor - * - * @return float value representing the RMS, which a valid indicator for vibration - */ - float get_vibration_factor(uint64_t timestamp); - - /** - * Get the number of failover events - * - * @return the number of failovers - */ - unsigned failover_count(); - - /** - * Get the index of the failed sensor in the group - * - * @return index of the failed sensor - */ - int failover_index(); - - /** - * Get the error state of the failed sensor in the group - * - * @return bitmask with erro states of the failed sensor - */ - uint32_t failover_state(); - - /** - * Print the validator value - * - */ - void print(); - - /** - * Set the timeout value - * - * @param timeout_interval_us The timeout interval in microseconds - */ - void set_timeout(uint64_t timeout_interval_us); - -private: - DataValidator *_first; /**< sibling in the group */ - int _curr_best; /**< currently best index */ - int _prev_best; /**< the previous best index */ - uint64_t _first_failover_time; /**< timestamp where the first failover occured or zero if none occured */ - unsigned _toggle_count; /**< number of back and forth switches between two sensors */ - static constexpr float MIN_REGULAR_CONFIDENCE = 0.9f; - - /* we don't want this class to be copied */ - DataValidatorGroup(const DataValidatorGroup&); - DataValidatorGroup operator=(const DataValidatorGroup&); -}; From 32d062836e85bd2edb94674478edf51ad8a7313c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Oct 2015 16:08:32 +0100 Subject: [PATCH 002/201] Moved ECL into its own submodule --- .gitmodules | 3 +++ CMakeLists.txt | 1 + src/lib/ecl | 1 + src/modules/fw_att_control/CMakeLists.txt | 2 ++ src/modules/fw_pos_control_l1/CMakeLists.txt | 1 + src/platforms/px4_defines.h | 1 + 6 files changed, 9 insertions(+) create mode 160000 src/lib/ecl diff --git a/.gitmodules b/.gitmodules index b84b78548e..374befd536 100644 --- a/.gitmodules +++ b/.gitmodules @@ -31,3 +31,6 @@ [submodule "src/lib/DriverFramework"] path = src/lib/DriverFramework url = https://github.com/PX4/DriverFramework.git +[submodule "src/lib/ecl"] + path = src/lib/ecl + url = https://github.com/LorenzMeier/ecl.git diff --git a/CMakeLists.txt b/CMakeLists.txt index 607fafb9ee..9bd26e839c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -245,6 +245,7 @@ px4_add_git_submodule(TARGET git_uavcan PATH "src/modules/uavcan/libuavcan") px4_add_git_submodule(TARGET git_nuttx PATH "NuttX") px4_add_git_submodule(TARGET git_dspal PATH "src/lib/dspal") px4_add_git_submodule(TARGET git_driverframework PATH "src/lib/DriverFramework") +px4_add_git_submodule(TARGET git_ecl PATH "src/lib/ecl") px4_add_git_submodule(TARGET git_jmavsim PATH "Tools/jMAVSim") px4_add_git_submodule(TARGET git_gazebo PATH "Tools/sitl_gazebo") px4_add_git_submodule(TARGET git_matrix PATH "src/lib/matrix") diff --git a/src/lib/ecl b/src/lib/ecl new file mode 160000 index 0000000000..e5743d503c --- /dev/null +++ b/src/lib/ecl @@ -0,0 +1 @@ +Subproject commit e5743d503c416693f11be3af13fdcd442e49cbce diff --git a/src/modules/fw_att_control/CMakeLists.txt b/src/modules/fw_att_control/CMakeLists.txt index b093c71cee..5d0e6648c2 100644 --- a/src/modules/fw_att_control/CMakeLists.txt +++ b/src/modules/fw_att_control/CMakeLists.txt @@ -40,5 +40,7 @@ px4_add_module( fw_att_control_main.cpp DEPENDS platforms__common + git_ecl + lib__ecl ) # vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/fw_pos_control_l1/CMakeLists.txt b/src/modules/fw_pos_control_l1/CMakeLists.txt index e74b6e4b37..720d97fac7 100644 --- a/src/modules/fw_pos_control_l1/CMakeLists.txt +++ b/src/modules/fw_pos_control_l1/CMakeLists.txt @@ -44,6 +44,7 @@ px4_add_module( mtecs/limitoverride.cpp DEPENDS platforms__common + git_ecl lib__external_lgpl lib__ecl ) diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 4d453e5983..6f46bb2421 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -40,6 +40,7 @@ #pragma once #include +#include /* Get the name of the default value fiven the param name */ #define PX4_PARAM_DEFAULT_VALUE_NAME(_name) PARAM_##_name##_DEFAULT From 19a5f9e73d9b37d9c0b11a8221b2c9469b3bc7c9 Mon Sep 17 00:00:00 2001 From: Roman Date: Thu, 26 Nov 2015 23:53:29 +0100 Subject: [PATCH 003/201] updated ecl lib --- cmake/configs/posix_sitl_default.cmake | 15 +- posix-configs/SITL/init/rcS_jmavsim_iris | 1 + src/lib/ecl | 2 +- src/modules/ekf2/CMakeLists.txt | 47 ++++ src/modules/ekf2/ekf2_main.cpp | 320 +++++++++++++++++++++++ 5 files changed, 373 insertions(+), 12 deletions(-) create mode 100644 src/modules/ekf2/CMakeLists.txt create mode 100644 src/modules/ekf2/ekf2_main.cpp diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake index b3c9205586..5aef3ffc7f 100644 --- a/cmake/configs/posix_sitl_default.cmake +++ b/cmake/configs/posix_sitl_default.cmake @@ -32,23 +32,15 @@ set(config_module_list modules/sensors modules/simulator modules/mavlink + modules/dataman modules/attitude_estimator_ekf - modules/attitude_estimator_q + #modules/attitude_estimator_q modules/ekf_att_pos_estimator modules/position_estimator_inav modules/navigator modules/vtol_att_control - modules/mc_pos_control - modules/mc_att_control - modules/mc_pos_control_multiplatform - modules/mc_att_control_multiplatform - modules/land_detector - modules/fw_att_control - modules/fw_pos_control_l1 - modules/dataman - modules/sdlog2 - modules/commander modules/controllib + modules/ekf2 lib/mathlib lib/mathlib/math/filter lib/conversion @@ -59,6 +51,7 @@ set(config_module_list lib/launchdetection lib/terrain_estimation lib/runway_takeoff + lib/ecl/EKF/tests/base ) set(config_extra_builtin_cmds diff --git a/posix-configs/SITL/init/rcS_jmavsim_iris b/posix-configs/SITL/init/rcS_jmavsim_iris index 88a3f3978e..71d53bec34 100644 --- a/posix-configs/SITL/init/rcS_jmavsim_iris +++ b/posix-configs/SITL/init/rcS_jmavsim_iris @@ -61,3 +61,4 @@ mavlink stream -r 250 -s HIGHRES_IMU -u 14556 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 mavlink boot_complete sdlog2 start -r 100 -e -t -a +ekf2 start diff --git a/src/lib/ecl b/src/lib/ecl index e5743d503c..8d6864304f 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit e5743d503c416693f11be3af13fdcd442e49cbce +Subproject commit 8d6864304fa3a18f728730f9e49fa07640a87e72 diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt new file mode 100644 index 0000000000..75db628be7 --- /dev/null +++ b/src/modules/ekf2/CMakeLists.txt @@ -0,0 +1,47 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. 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 PX4 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. +# +############################################################################# +set(MODULE_CFLAGS) +if (${OS} STREQUAL "nuttx") + list(APPEND MODULE_CFLAGS -Wframe-larger-than=30000) +endif() +px4_add_module( + MODULE modules__ekf2 + MAIN ekf2 + COMPILE_FLAGS ${MODULE_CFLAGS} + STACK 30000 + SRCS + ekf2_main.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp new file mode 100644 index 0000000000..d89aebcb0e --- /dev/null +++ b/src/modules/ekf2/ekf2_main.cpp @@ -0,0 +1,320 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. 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 PX4 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 ekf2_main.cpp + * Implementation of the attitude and position estimator. + * + * @author Roman Bapst + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + + +extern "C" __EXPORT int ekf2_main(int argc, char *argv[]); + + +class Ekf2; + +namespace ekf2 +{ +Ekf2 *instance; +} + + +class Ekf2 +{ +public: + /** + * Constructor + */ + Ekf2(); + + /** + * Destructor, also kills task. + */ + ~Ekf2(); + + /** + * Start task. + * + * @return OK on success. + */ + int start(); + + static void task_main_trampoline(int argc, char *argv[]); + + void task_main(); + + void print(); + +private: + static constexpr float _dt_max = 0.02; + bool _task_should_exit = false; /**< if true, task should exit */ + int _control_task = -1; /**< task handle for task */ + + int _sensors_sub = -1; + int _gps_sub = -1; + int _airspeed_sub = -1; + + EstimatorBase *_ekf; + + + void update_parameters(bool force); + + int update_subscriptions(); + +}; + +Ekf2::Ekf2() +{ + _ekf = new EstimatorBase(); +} + +Ekf2::~Ekf2() +{ + +} + +void Ekf2::print() +{ + _ekf->printStoredGps(); + _ekf->printStoredBaro(); + _ekf->printStoredMag(); + _ekf->printStoredIMU(); +} + +void Ekf2::task_main() +{ + // subscribe to relevant topics + _sensors_sub = orb_subscribe(ORB_ID(sensor_combined)); + _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + + px4_pollfd_struct_t fds[1]; + fds[0].fd = _sensors_sub; + fds[0].events = POLLIN; + + while (!_task_should_exit) { + int ret = px4_poll(fds, 1, 1000); + + if (ret < 0) { + // Poll error, sleep and try again + usleep(10000); + continue; + + } else if (ret == 0 || fds[0].revents != POLLIN) { + // Poll timeout or no new data, do nothing + continue; + } + + bool gps_updated = false; + bool airspeed_updated = false; + + sensor_combined_s sensors = {}; + vehicle_gps_position_s gps = {}; + airspeed_s airspeed = {}; + + orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors); + + // update all other topics if they have new data + orb_check(_gps_sub, &gps_updated); + + if (gps_updated) { + orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &gps); + } + + orb_check(_airspeed_sub, &airspeed_updated); + + if (airspeed_updated) { + orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed); + } + + hrt_abstime now = hrt_absolute_time(); + // push imu data into estimator + _ekf->setIMUData(now, sensors.gyro_integral_dt[0], sensors.accelerometer_integral_dt[0], + &sensors.gyro_integral_rad[0], &sensors.accelerometer_integral_m_s[0]); + + // read mag data + _ekf->setMagData(sensors.magnetometer_timestamp[0], &sensors.magnetometer_ga[0]); + + // read baro data + _ekf->setBaroData(sensors.baro_timestamp[0], &sensors.baro_alt_meter[0]); + + // read gps data if available + if (gps_updated) { + struct gps_message gps_msg = {}; + gps_msg.time_usec = gps.timestamp_position; + gps_msg.lat = gps.lat; + gps_msg.lon = gps.lon; + gps_msg.alt = gps.alt; + gps_msg.fix_type = gps.fix_type; + gps_msg.eph = gps.eph; + gps_msg.epv = gps.epv; + gps_msg.time_usec_vel = gps.timestamp_velocity; + gps_msg.vel_m_s = gps.vel_m_s; + gps_msg.vel_ned[0] = gps.vel_n_m_s; + gps_msg.vel_ned[1] = gps.vel_e_m_s; + gps_msg.vel_ned[2] = gps.vel_d_m_s; + gps_msg.vel_ned_valid = gps.vel_ned_valid; + + _ekf->setGpsData(gps.timestamp_position, &gps_msg); + } + + // read airspeed data if available + if (airspeed_updated) { + _ekf->setAirspeedData(airspeed.timestamp, &airspeed.indicated_airspeed_m_s); + } + } +} + +void Ekf2::task_main_trampoline(int argc, char *argv[]) +{ + ekf2::instance->task_main(); +} + +int Ekf2::start() +{ + ASSERT(_control_task == -1); + + /* start the task */ + _control_task = px4_task_spawn_cmd("ekf2", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 30000, + (px4_main_t)&Ekf2::task_main_trampoline, + nullptr); + + if (_control_task < 0) { + PX4_WARN("task start failed"); + return -errno; + } + + return OK; +} + +int ekf2_main(int argc, char *argv[]) +{ + if (argc < 1) { + PX4_WARN("usage: ekf2 {start|stop|status}"); + return 1; + } + + if (!strcmp(argv[1], "start")) { + + if (ekf2::instance != nullptr) { + PX4_WARN("already running"); + return 1; + } + + ekf2::instance = new Ekf2(); + + if (ekf2::instance == nullptr) { + PX4_WARN("alloc failed"); + return 1; + } + + if (OK != ekf2::instance->start()) { + delete ekf2::instance; + ekf2::instance = nullptr; + PX4_WARN("start failed"); + return 1; + } + + return 0; + } + + if (!strcmp(argv[1], "stop")) { + if (ekf2::instance == nullptr) { + PX4_WARN("not running"); + return 1; + } + + delete ekf2::instance; + ekf2::instance = nullptr; + return 0; + } + + if (!strcmp(argv[1], "print")) { + if (ekf2::instance != nullptr) { + ekf2::instance->print(); + return 0; + } + + return 1; + } + + if (!strcmp(argv[1], "status")) { + if (ekf2::instance) { + PX4_WARN("running"); + return 0; + + } else { + PX4_WARN("not running"); + return 1; + } + } + + PX4_WARN("unrecognized command"); + return 1; +} \ No newline at end of file From 5ded6884edbfe202925308f44192db7478099488 Mon Sep 17 00:00:00 2001 From: Roman Date: Fri, 27 Nov 2015 08:53:21 +0100 Subject: [PATCH 004/201] updated matrix lib --- src/lib/matrix | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/matrix b/src/lib/matrix index 9cd6ac3dd9..4122c1d36d 160000 --- a/src/lib/matrix +++ b/src/lib/matrix @@ -1 +1 @@ -Subproject commit 9cd6ac3dd9a255a1a2ef299da413c38a9040b8c7 +Subproject commit 4122c1d36d01954b604b284f2ef61359526530f6 From 63ac712eab14004e2edbb7b960c427714d71707a Mon Sep 17 00:00:00 2001 From: Roman Date: Sun, 6 Dec 2015 13:24:42 +0100 Subject: [PATCH 005/201] allow testing ekf2 in simulation with jmavsim --- cmake/configs/posix_sitl_default.cmake | 4 +++- posix-configs/SITL/init/rcS_jmavsim_iris | 3 +-- src/modules/ekf2/CMakeLists.txt | 4 ++-- src/modules/ekf2/ekf2_main.cpp | 6 ++++-- 4 files changed, 10 insertions(+), 7 deletions(-) diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake index 5aef3ffc7f..5d4b370e27 100644 --- a/cmake/configs/posix_sitl_default.cmake +++ b/cmake/configs/posix_sitl_default.cmake @@ -37,7 +37,10 @@ set(config_module_list #modules/attitude_estimator_q modules/ekf_att_pos_estimator modules/position_estimator_inav + modules/mc_pos_control + modules/mc_att_control modules/navigator + modules/commander modules/vtol_att_control modules/controllib modules/ekf2 @@ -51,7 +54,6 @@ set(config_module_list lib/launchdetection lib/terrain_estimation lib/runway_takeoff - lib/ecl/EKF/tests/base ) set(config_extra_builtin_cmds diff --git a/posix-configs/SITL/init/rcS_jmavsim_iris b/posix-configs/SITL/init/rcS_jmavsim_iris index 71d53bec34..2f240d7fed 100644 --- a/posix-configs/SITL/init/rcS_jmavsim_iris +++ b/posix-configs/SITL/init/rcS_jmavsim_iris @@ -45,8 +45,7 @@ sensors start commander start land_detector start multicopter navigator start -attitude_estimator_q start -position_estimator_inav start +ekf_att_pos_estimator start mc_pos_control start mc_att_control start mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 75db628be7..ebdc09c46c 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -32,13 +32,13 @@ ############################################################################# set(MODULE_CFLAGS) if (${OS} STREQUAL "nuttx") - list(APPEND MODULE_CFLAGS -Wframe-larger-than=30000) + list(APPEND MODULE_CFLAGS -Wframe-larger-than=60000) endif() px4_add_module( MODULE modules__ekf2 MAIN ekf2 COMPILE_FLAGS ${MODULE_CFLAGS} - STACK 30000 + STACK 60000 SRCS ekf2_main.cpp DEPENDS diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index d89aebcb0e..0e0390d3a0 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -68,7 +68,7 @@ #include #include -#include +#include extern "C" __EXPORT int ekf2_main(int argc, char *argv[]); @@ -128,7 +128,7 @@ private: Ekf2::Ekf2() { - _ekf = new EstimatorBase(); + _ekf = new Ekf(); } Ekf2::~Ekf2() @@ -225,6 +225,8 @@ void Ekf2::task_main() if (airspeed_updated) { _ekf->setAirspeedData(airspeed.timestamp, &airspeed.indicated_airspeed_m_s); } + + _ekf->update(); } } From b1035fe115ebb9d2e61191417cb16edd20881007 Mon Sep 17 00:00:00 2001 From: Roman Date: Mon, 7 Dec 2015 22:47:46 +0100 Subject: [PATCH 006/201] updated ecl --- src/lib/ecl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/ecl b/src/lib/ecl index 8d6864304f..d2972e94e8 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit 8d6864304fa3a18f728730f9e49fa07640a87e72 +Subproject commit d2972e94e8828c18024138716acb0bccbaa5a461 From 08871e77c2dcab20bd5333e4f1fc35436506d694 Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 9 Dec 2015 18:08:44 +0100 Subject: [PATCH 007/201] updated ecl EKF --- src/lib/ecl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/ecl b/src/lib/ecl index d2972e94e8..8de8b0eb76 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit d2972e94e8828c18024138716acb0bccbaa5a461 +Subproject commit 8de8b0eb76d38297b8a461d5ee3bbd21a76c546a From 07abac3ea433f87618116f92b742ef32c8174072 Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 10 Dec 2015 16:38:51 +0100 Subject: [PATCH 008/201] ekf2 publish attitude position and control state --- src/modules/ekf2/ekf2_main.cpp | 77 +++++++++++++++++++++++++++++++++- 1 file changed, 76 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 0e0390d3a0..3c61d231dd 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -67,6 +67,9 @@ #include #include #include +#include +#include +#include #include @@ -117,6 +120,15 @@ private: int _gps_sub = -1; int _airspeed_sub = -1; + orb_advert_t _att_pub; + orb_advert_t _lpos_pub; + orb_advert_t _control_state_pub; + + /* Low pass filter for attitude rates */ + math::LowPassFilter2p _lp_roll_rate; + math::LowPassFilter2p _lp_pitch_rate; + math::LowPassFilter2p _lp_yaw_rate; + EstimatorBase *_ekf; @@ -126,9 +138,15 @@ private: }; -Ekf2::Ekf2() +Ekf2::Ekf2(): +_lp_roll_rate(250.0f, 30.0f), +_lp_pitch_rate(250.0f, 30.0f), +_lp_yaw_rate(250.0f, 20.0f) { _ekf = new Ekf(); + _att_pub = nullptr; + _lpos_pub = nullptr; + _control_state_pub = nullptr; } Ekf2::~Ekf2() @@ -226,7 +244,64 @@ void Ekf2::task_main() _ekf->setAirspeedData(airspeed.timestamp, &airspeed.indicated_airspeed_m_s); } + struct vehicle_attitude_s att; + struct vehicle_local_position_s lpos; + att.timestamp = hrt_absolute_time(); + lpos.timestamp = hrt_absolute_time(); _ekf->update(); + + _ekf->copy_quaternion(att.q); + matrix::Quaternion q(att.q[0], att.q[1], att.q[2], att.q[3]); + matrix::Euler euler(q); + att.roll = euler(0); + att.pitch = euler(1); + att.yaw = euler(2); + + float pos[3] = {}; + float vel[3] = {}; + + _ekf->copy_position(pos); + lpos.x = pos[0]; + lpos.y = pos[1]; + lpos.z = pos[2]; + + _ekf->copy_velocity(vel); + lpos.vx = vel[0]; + lpos.vy = vel[1]; + lpos.vz = vel[2]; + + control_state_s ctrl_state = {}; + ctrl_state.timestamp = hrt_absolute_time(); + ctrl_state.roll_rate = _lp_roll_rate.apply(sensors.gyro_rad_s[0]); + + ctrl_state.pitch_rate = _lp_pitch_rate.apply(sensors.gyro_rad_s[1]); + + ctrl_state.yaw_rate = _lp_yaw_rate.apply(sensors.gyro_rad_s[2]); + + ctrl_state.q[0] = q(0); + ctrl_state.q[1] = q(1); + ctrl_state.q[2] = q(2); + ctrl_state.q[3] = q(3); + + if (_control_state_pub == nullptr) { + _control_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state); + } else { + orb_publish(ORB_ID(control_state), _control_state_pub, &ctrl_state); + } + + if (_att_pub == nullptr) { + _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); + } else { + orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att); + } + + if (_lpos_pub == nullptr) { + _lpos_pub = orb_advertise(ORB_ID(vehicle_local_position), &lpos); + } else { + orb_publish(ORB_ID(vehicle_local_position), _lpos_pub, &lpos); + } + + } } From ddad6d2019de07f0119c5ae0148fe1e97752c345 Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 10 Dec 2015 16:39:10 +0100 Subject: [PATCH 009/201] remove old estimator from startup file --- posix-configs/SITL/init/rcS_jmavsim_iris | 1 - 1 file changed, 1 deletion(-) diff --git a/posix-configs/SITL/init/rcS_jmavsim_iris b/posix-configs/SITL/init/rcS_jmavsim_iris index 2f240d7fed..58237da408 100644 --- a/posix-configs/SITL/init/rcS_jmavsim_iris +++ b/posix-configs/SITL/init/rcS_jmavsim_iris @@ -45,7 +45,6 @@ sensors start commander start land_detector start multicopter navigator start -ekf_att_pos_estimator start mc_pos_control start mc_att_control start mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix From 763e2bb144a480e9e8dcb3a84b1e7dc0c07c17a3 Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 10 Dec 2015 16:41:02 +0100 Subject: [PATCH 010/201] update matrix lib --- src/lib/matrix | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/matrix b/src/lib/matrix index 4122c1d36d..aedd56990e 160000 --- a/src/lib/matrix +++ b/src/lib/matrix @@ -1 +1 @@ -Subproject commit 4122c1d36d01954b604b284f2ef61359526530f6 +Subproject commit aedd56990e772090465f72fd6d187c1c327af591 From 50aeb3924d0561c5cb792e251c94daf32d68d989 Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 10 Dec 2015 16:41:14 +0100 Subject: [PATCH 011/201] updated ecl EKF --- src/lib/ecl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/ecl b/src/lib/ecl index 8de8b0eb76..c58d1df7bc 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit 8de8b0eb76d38297b8a461d5ee3bbd21a76c546a +Subproject commit c58d1df7bc233399e99de6c45974f3329e7d79ba From 33fdfbf77a44e17fff4c46e9fecf927d0f9ebe0f Mon Sep 17 00:00:00 2001 From: Roman Date: Sat, 19 Dec 2015 08:54:12 +0100 Subject: [PATCH 012/201] ecl: use data validation code from master --- src/lib/ecl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/ecl b/src/lib/ecl index c58d1df7bc..dd171670ca 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit c58d1df7bc233399e99de6c45974f3329e7d79ba +Subproject commit dd171670cac7d019f9e454ab7b48f042f1e71fef From c919969ebd79e2733b996f99c4a5ab58741c184a Mon Sep 17 00:00:00 2001 From: Roman Date: Sat, 19 Dec 2015 09:23:32 +0100 Subject: [PATCH 013/201] ecl:fix cmake file --- src/lib/ecl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/ecl b/src/lib/ecl index dd171670ca..d009118437 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit dd171670cac7d019f9e454ab7b48f042f1e71fef +Subproject commit d00911843708996883faeb7ad79c163d51c78d61 From d291441468692efc29d7fc2a88a6ff223b8e6a24 Mon Sep 17 00:00:00 2001 From: Roman Date: Sat, 19 Dec 2015 09:41:31 +0100 Subject: [PATCH 014/201] updated ecl --- src/lib/ecl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/ecl b/src/lib/ecl index d009118437..f153a7cb44 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit d00911843708996883faeb7ad79c163d51c78d61 +Subproject commit f153a7cb44ec1eef32faab36052ee2d17771b6df From 19bc791fe9c1a0c225184035a8117ef80ead0407 Mon Sep 17 00:00:00 2001 From: Roman Date: Sat, 19 Dec 2015 09:50:01 +0100 Subject: [PATCH 015/201] updated matrix lib --- src/lib/matrix | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/matrix b/src/lib/matrix index aedd56990e..c3c6a0a9df 160000 --- a/src/lib/matrix +++ b/src/lib/matrix @@ -1 +1 @@ -Subproject commit aedd56990e772090465f72fd6d187c1c327af591 +Subproject commit c3c6a0a9df19ffa782e095e4ec3993bac738d935 From 5228e5c8c7f8c7b963164a3f01734051669d6e2c Mon Sep 17 00:00:00 2001 From: Roman Date: Tue, 22 Dec 2015 11:26:42 +0100 Subject: [PATCH 016/201] updated ecl EKF --- src/lib/ecl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/ecl b/src/lib/ecl index f153a7cb44..6b49e2495c 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit f153a7cb44ec1eef32faab36052ee2d17771b6df +Subproject commit 6b49e2495cd6bad27dafe749d141a2f34f6f17b3 From 87982ca73f9b68f5c46bdb3808b58b2a89a73b17 Mon Sep 17 00:00:00 2001 From: Roman Date: Tue, 22 Dec 2015 11:26:55 +0100 Subject: [PATCH 017/201] updated matrix lib --- src/lib/matrix | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/matrix b/src/lib/matrix index c3c6a0a9df..cc1658db15 160000 --- a/src/lib/matrix +++ b/src/lib/matrix @@ -1 +1 @@ -Subproject commit c3c6a0a9df19ffa782e095e4ec3993bac738d935 +Subproject commit cc1658db15d720e05d577787363823be0f2bd161 From e72c672350b7c83bee7840eaba92f0b842438935 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Dec 2015 11:40:46 +0100 Subject: [PATCH 018/201] POSIX configs: Default normal config back to normal operation --- posix-configs/SITL/init/rcS_jmavsim_iris | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/posix-configs/SITL/init/rcS_jmavsim_iris b/posix-configs/SITL/init/rcS_jmavsim_iris index 58237da408..88a3f3978e 100644 --- a/posix-configs/SITL/init/rcS_jmavsim_iris +++ b/posix-configs/SITL/init/rcS_jmavsim_iris @@ -45,6 +45,8 @@ sensors start commander start land_detector start multicopter navigator start +attitude_estimator_q start +position_estimator_inav start mc_pos_control start mc_att_control start mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix @@ -59,4 +61,3 @@ mavlink stream -r 250 -s HIGHRES_IMU -u 14556 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 mavlink boot_complete sdlog2 start -r 100 -e -t -a -ekf2 start From ed8645a83b9a1abdbca64fe8645e78dd41134fca Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Dec 2015 11:41:06 +0100 Subject: [PATCH 019/201] Add EKF2 config and startup option for jMAVSim --- Makefile | 3 + cmake/configs/posix_sitl_ekf2.cmake | 91 +++++++++++++++++++ posix-configs/SITL/init/rcS_ekf2_jmavsim_iris | 62 +++++++++++++ 3 files changed, 156 insertions(+) create mode 100644 cmake/configs/posix_sitl_ekf2.cmake create mode 100644 posix-configs/SITL/init/rcS_ekf2_jmavsim_iris diff --git a/Makefile b/Makefile index 1c269a8b56..ee2670244d 100644 --- a/Makefile +++ b/Makefile @@ -148,6 +148,9 @@ posix_sitl_default: posix_sitl_lpe: $(call cmake-build,$@) +posix_sitl_ekf2: + $(call cmake-build,$@) + ros_sitl_default: $(call cmake-build,$@) diff --git a/cmake/configs/posix_sitl_ekf2.cmake b/cmake/configs/posix_sitl_ekf2.cmake new file mode 100644 index 0000000000..aefe3255d9 --- /dev/null +++ b/cmake/configs/posix_sitl_ekf2.cmake @@ -0,0 +1,91 @@ +include(posix/px4_impl_posix) + +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake) + +set(config_module_list + drivers/device + drivers/boards/sitl + drivers/pwm_out_sim + platforms/common + platforms/posix/px4_layer + platforms/posix/work_queue + platforms/posix/drivers/adcsim + platforms/posix/drivers/gpssim + platforms/posix/drivers/tonealrmsim + platforms/posix/drivers/accelsim + platforms/posix/drivers/airspeedsim + platforms/posix/drivers/barosim + platforms/posix/drivers/gyrosim + platforms/posix/drivers/rgbledsim + platforms/posix/drivers/ledsim + systemcmds/param + systemcmds/mixer + systemcmds/ver + systemcmds/esc_calib + systemcmds/reboot + systemcmds/topic_listener + systemcmds/perf + modules/uORB + modules/param + modules/systemlib + modules/systemlib/mixer + modules/sensors + modules/simulator + modules/mavlink + modules/dataman + modules/attitude_estimator_ekf + #modules/attitude_estimator_q + modules/ekf_att_pos_estimator + modules/position_estimator_inav + modules/mc_pos_control + modules/mc_att_control + modules/navigator + modules/commander + modules/vtol_att_control + modules/controllib + modules/ekf2 + lib/mathlib + lib/mathlib/math/filter + lib/conversion + lib/ecl + lib/external_lgpl + lib/geo + lib/geo_lookup + lib/launchdetection + lib/terrain_estimation + lib/runway_takeoff + ) + +set(config_extra_builtin_cmds + serdis + sercon + ) + +set(config_sitl_rcS + posix-configs/SITL/init/rcS_ekf2 + CACHE FILEPATH "init script for sitl" + ) + +set(config_sitl_viewer + jmavsim + CACHE STRING "viewer for sitl" + ) +set_property(CACHE config_sitl_viewer + PROPERTY STRINGS "jmavsim;none") + +set(config_sitl_debugger + disable + CACHE STRING "debugger for sitl" + ) +set_property(CACHE config_sitl_debugger + PROPERTY STRINGS "disable;gdb;lldb") + + + +add_custom_target(sercon) +set_target_properties(sercon PROPERTIES + MAIN "sercon" STACK "2048") + +add_custom_target(serdis) +set_target_properties(serdis PROPERTIES + MAIN "serdis" STACK "2048") diff --git a/posix-configs/SITL/init/rcS_ekf2_jmavsim_iris b/posix-configs/SITL/init/rcS_ekf2_jmavsim_iris new file mode 100644 index 0000000000..58237da408 --- /dev/null +++ b/posix-configs/SITL/init/rcS_ekf2_jmavsim_iris @@ -0,0 +1,62 @@ +uorb start +simulator start -s +param load +param set MAV_TYPE 2 +param set MC_PITCHRATE_P 0.15 +param set MC_PITCH_P 7 +param set MC_ROLL_P 7 +param set MC_ROLLRATE_P 0.15 +param set MC_YAW_P 2.8 +param set MC_YAWRATE_P 0.35 +param set SYS_AUTOSTART 4010 +param set SYS_RESTART_TYPE 2 +dataman start +param set CAL_GYRO0_ID 2293768 +param set CAL_ACC0_ID 1376264 +param set CAL_ACC1_ID 1310728 +param set CAL_MAG0_ID 196616 +param set CAL_GYRO0_XOFF 0.01 +param set CAL_ACC0_XOFF 0.01 +param set CAL_ACC0_YOFF -0.01 +param set CAL_ACC0_ZOFF 0.01 +param set CAL_ACC0_XSCALE 1.01 +param set CAL_ACC0_YSCALE 1.01 +param set CAL_ACC0_ZSCALE 1.01 +param set CAL_ACC1_XOFF 0.01 +param set CAL_MAG0_XOFF 0.01 +param set MPC_XY_P 0.4 +param set MPC_XY_VEL_P 0.2 +param set MPC_XY_VEL_D 0.005 +param set SENS_BOARD_ROT 0 +param set COM_RC_IN_MODE 1 +param set NAV_ACC_RAD 2.0 +param set RTL_RETURN_ALT 30.0 +param set RTL_DESCEND_ALT 10.0 +rgbledsim start +tone_alarm start +gyrosim start +accelsim start +barosim start +adcsim start +gpssim start +pwm_out_sim mode_pwm +sleep 1 +sensors start +commander start +land_detector start multicopter +navigator start +mc_pos_control start +mc_att_control start +mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix +mavlink start -u 14556 -r 2000000 +mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556 +mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556 +mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556 +mavlink stream -r 80 -s ATTITUDE -u 14556 +mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556 +mavlink stream -r 20 -s RC_CHANNELS -u 14556 +mavlink stream -r 250 -s HIGHRES_IMU -u 14556 +mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 +mavlink boot_complete +sdlog2 start -r 100 -e -t -a +ekf2 start From f50af4308861680bb03a173208af7b9f749f3426 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Dec 2015 11:55:13 +0100 Subject: [PATCH 020/201] Restore normal SITL startup script --- cmake/configs/posix_sitl_default.cmake | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake index 5d4b370e27..b3c9205586 100644 --- a/cmake/configs/posix_sitl_default.cmake +++ b/cmake/configs/posix_sitl_default.cmake @@ -32,18 +32,23 @@ set(config_module_list modules/sensors modules/simulator modules/mavlink - modules/dataman modules/attitude_estimator_ekf - #modules/attitude_estimator_q + modules/attitude_estimator_q modules/ekf_att_pos_estimator modules/position_estimator_inav + modules/navigator + modules/vtol_att_control modules/mc_pos_control modules/mc_att_control - modules/navigator + modules/mc_pos_control_multiplatform + modules/mc_att_control_multiplatform + modules/land_detector + modules/fw_att_control + modules/fw_pos_control_l1 + modules/dataman + modules/sdlog2 modules/commander - modules/vtol_att_control modules/controllib - modules/ekf2 lib/mathlib lib/mathlib/math/filter lib/conversion From 49cef930af53600d5309cc4a2a0c96db00066ab9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Dec 2015 12:00:55 +0100 Subject: [PATCH 021/201] Update location of ECL repo --- .gitmodules | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.gitmodules b/.gitmodules index 374befd536..98a23841b5 100644 --- a/.gitmodules +++ b/.gitmodules @@ -33,4 +33,4 @@ url = https://github.com/PX4/DriverFramework.git [submodule "src/lib/ecl"] path = src/lib/ecl - url = https://github.com/LorenzMeier/ecl.git + url = https://github.com/PX4/ecl.git From 979cda1b10adb8c78cd09f458e5bb64016e03f8e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Dec 2015 12:01:09 +0100 Subject: [PATCH 022/201] Add dependency to ECL to CMake --- src/modules/ekf2/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index ebdc09c46c..82d13d5fe3 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -43,5 +43,6 @@ px4_add_module( ekf2_main.cpp DEPENDS platforms__common + git_ecl ) # vim: set noet ft=cmake fenc=utf-8 ff=unix : From 02652af758be4dbb0601e7483cb4e35ce985717e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Dec 2015 12:41:38 +0100 Subject: [PATCH 023/201] Enable EKF2 if LPE and INAV are both off --- ROMFS/px4fmu_common/init.d/rc.mc_apps | 2 +- cmake/configs/nuttx_px4fmu-v2_default.cmake | 1 + cmake/configs/nuttx_px4fmu-v4_default.cmake | 1 + 3 files changed, 3 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 4c6cdbd7cd..9c782d7bb0 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -17,7 +17,7 @@ else attitude_estimator_q start local_position_estimator start else - ekf_att_pos_estimator start + ekf2 start fi fi diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake index 9fafe851be..0523f8b67c 100644 --- a/cmake/configs/nuttx_px4fmu-v2_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake @@ -83,6 +83,7 @@ set(config_module_list modules/attitude_estimator_q modules/ekf_att_pos_estimator modules/position_estimator_inav + modules/ekf2 # # Vehicle Control diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake index a6dd256a34..98582ede2d 100644 --- a/cmake/configs/nuttx_px4fmu-v4_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake @@ -80,6 +80,7 @@ set(config_module_list modules/attitude_estimator_q modules/ekf_att_pos_estimator modules/position_estimator_inav + modules/ekf2 # # Vehicle Control From 2d50002973df6ea21495118a8e7213a1dbb14a38 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Dec 2015 12:41:48 +0100 Subject: [PATCH 024/201] Fix stack settings of EKF2 --- src/modules/ekf2/CMakeLists.txt | 4 ++-- src/modules/ekf2/ekf2_main.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 82d13d5fe3..37e4f78d0b 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -32,13 +32,13 @@ ############################################################################# set(MODULE_CFLAGS) if (${OS} STREQUAL "nuttx") - list(APPEND MODULE_CFLAGS -Wframe-larger-than=60000) + list(APPEND MODULE_CFLAGS -Wframe-larger-than=4000) endif() px4_add_module( MODULE modules__ekf2 MAIN ekf2 COMPILE_FLAGS ${MODULE_CFLAGS} - STACK 60000 + STACK 1000 SRCS ekf2_main.cpp DEPENDS diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 3c61d231dd..7b02c1dadf 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -318,7 +318,7 @@ int Ekf2::start() _control_task = px4_task_spawn_cmd("ekf2", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 30000, + 9000, (px4_main_t)&Ekf2::task_main_trampoline, nullptr); @@ -374,7 +374,7 @@ int ekf2_main(int argc, char *argv[]) if (!strcmp(argv[1], "print")) { if (ekf2::instance != nullptr) { - ekf2::instance->print(); + return 0; } From 366eaf4146feda15ecb220c4e0644bdb8cc829d9 Mon Sep 17 00:00:00 2001 From: Roman Date: Tue, 22 Dec 2015 18:01:29 +0100 Subject: [PATCH 025/201] ecl:ekf use simple heading fusion --- src/lib/ecl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/ecl b/src/lib/ecl index 6b49e2495c..457a57c6ff 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit 6b49e2495cd6bad27dafe749d141a2f34f6f17b3 +Subproject commit 457a57c6ff224a3bfd5fc304e75995db02e1952a From b958928095cbd65a8de2eff9fb454694315af73e Mon Sep 17 00:00:00 2001 From: Roman Date: Wed, 23 Dec 2015 11:40:25 +0100 Subject: [PATCH 026/201] publish rates and quaternion on attitude topic --- src/modules/ekf2/ekf2_main.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 7b02c1dadf..13f615c4e2 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -283,6 +283,16 @@ void Ekf2::task_main() ctrl_state.q[2] = q(2); ctrl_state.q[3] = q(3); + att.q[0] = q(0); + att.q[1] = q(1); + att.q[2] = q(2); + att.q[3] = q(3); + att.q_valid = true; + + att.rollspeed = sensors.gyro_rad_s[0]; + att.pitchspeed = sensors.gyro_rad_s[1]; + att.yawspeed = sensors.gyro_rad_s[2]; + if (_control_state_pub == nullptr) { _control_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state); } else { From ee2e3811d73c18158d0ca0dfe2c99a21f636b3a2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Nov 2015 11:12:17 +0100 Subject: [PATCH 027/201] RC lib: Support separate config from init, allow DSM bind from all boards --- src/lib/rc/dsm.c | 84 +++++++++++++++++++++-------------------------- src/lib/rc/dsm.h | 9 +++++ src/lib/rc/sbus.c | 22 +++++++++++-- src/lib/rc/sbus.h | 1 + 4 files changed, 67 insertions(+), 49 deletions(-) diff --git a/src/lib/rc/dsm.c b/src/lib/rc/dsm.c index 5d7186ec1b..a024b5c044 100644 --- a/src/lib/rc/dsm.c +++ b/src/lib/rc/dsm.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -40,6 +40,8 @@ */ #include +#include +#include #include #include @@ -49,12 +51,8 @@ #include "dsm.h" #include -#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 -#include -#endif - -#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 -#include +#ifndef GPIO_SPEKTRUM_PWR_EN + #error DSM input driver not supported by this board config #endif #define DSM_FRAME_SIZE 16 /**= 0) { @@ -247,15 +233,34 @@ dsm_init(const char *device) /* reset the format detector */ dsm_guess_format(true); - //debug("DSM: ready"); - - } else { - - //debug("DSM: open failed"); - + ret = 0; } - return dsm_fd; + return ret; +} + +/** + * Initialize the DSM receive functionality + * + * Open the UART for receiving DSM frames and configure it appropriately + * + * @param[in] device Device name of DSM UART + */ +int +dsm_init(const char *device) +{ + + if (dsm_fd < 0) { + dsm_fd = open(device, O_RDONLY | O_NONBLOCK); + } + + int ret = dsm_config(dsm_fd); + + if (!ret) { + return dsm_fd; + } else { + return -1; + } } /** @@ -267,43 +272,32 @@ dsm_init(const char *device) void dsm_bind(uint16_t cmd, int pulses) { -#if !defined(GPIO_USART1_RX_SPEKTRUM) -#else - if (dsm_fd < 0) { return; } switch (cmd) { - case dsm_bind_power_down: + case DSM_CMD_BIND_POWER_DOWN: /*power down DSM satellite*/ -#if defined(CONFIG_ARCH_BOARD_PX4IO_V1) - POWER_RELAY1(0); -#elif defined(CONFIG_ARCH_BOARD_PX4IO_V2) POWER_SPEKTRUM(0); -#endif break; - case dsm_bind_power_up: + case DSM_CMD_BIND_POWER_UP: /*power up DSM satellite*/ -#if defined(CONFIG_ARCH_BOARD_PX4IO_V1) - POWER_RELAY1(1); -#elif defined(CONFIG_ARCH_BOARD_PX4IO_V2) POWER_SPEKTRUM(1); -#endif dsm_guess_format(true); break; - case dsm_bind_set_rx_out: + case DSM_CMD_BIND_SET_RX_OUT: /*Set UART RX pin to active output mode*/ stm32_configgpio(GPIO_USART1_RX_SPEKTRUM); break; - case dsm_bind_send_pulses: + case DSM_CMD_BIND_SEND_PULSES: /*Pulse RX pin a number of times*/ for (int i = 0; i < pulses; i++) { @@ -315,15 +309,13 @@ dsm_bind(uint16_t cmd, int pulses) break; - case dsm_bind_reinit_uart: + case DSM_CMD_BIND_REINIT_UART: /*Restore USART RX pin to RS232 receive mode*/ stm32_configgpio(GPIO_USART1_RX); break; } - -#endif } /** diff --git a/src/lib/rc/dsm.h b/src/lib/rc/dsm.h index 80614d066e..acef60705d 100644 --- a/src/lib/rc/dsm.h +++ b/src/lib/rc/dsm.h @@ -46,7 +46,16 @@ __BEGIN_DECLS __EXPORT int dsm_init(const char *device); +__EXPORT int dsm_config(int dsm_fd); __EXPORT bool dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes, unsigned max_values); __EXPORT void dsm_bind(uint16_t cmd, int pulses); +enum { /* DSM bind states */ + DSM_CMD_BIND_POWER_DOWN = 0, + DSM_CMD_BIND_POWER_UP, + DSM_CMD_BIND_SET_RX_OUT, + DSM_CMD_BIND_SEND_PULSES, + DSM_CMD_BIND_REINIT_UART +} DSM_CMD; + __END_DECLS diff --git a/src/lib/rc/sbus.c b/src/lib/rc/sbus.c index f911648416..e89e8bfe1f 100644 --- a/src/lib/rc/sbus.c +++ b/src/lib/rc/sbus.c @@ -122,6 +122,20 @@ sbus_init(const char *device, bool singlewire) { int sbus_fd = open(device, O_RDWR | O_NONBLOCK); + int ret = sbus_config(sbus_fd, singlewire); + + if (!ret) { + return sbus_fd; + } else { + return -1; + } +} + +int +sbus_config(int sbus_fd, bool singlewire) +{ + int ret = -1; + if (sbus_fd >= 0) { struct termios t; @@ -133,8 +147,8 @@ sbus_init(const char *device, bool singlewire) if (singlewire) { /* only defined in configs capable of IOCTL */ -#ifdef SBUS_SERIAL_PORT - //ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); +#ifdef TIOCSSINGLEWIRE + ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); #endif } @@ -143,9 +157,11 @@ sbus_init(const char *device, bool singlewire) last_rx_time = hrt_absolute_time(); last_frame_time = last_rx_time; sbus_frame_drops = 0; + + ret = 0; } - return sbus_fd; + return ret; } void diff --git a/src/lib/rc/sbus.h b/src/lib/rc/sbus.h index 0da5ea2935..2c8f389c86 100644 --- a/src/lib/rc/sbus.h +++ b/src/lib/rc/sbus.h @@ -68,6 +68,7 @@ __EXPORT int sbus_init(const char *device, bool singlewire); * provides a degree of protection. Of course, it would be better * if we didn't drop bytes... */ +__EXPORT int sbus_config(int sbus_fd, bool singlewire); __EXPORT bool sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels); From e175ef5626510d15c1e501c27d7aa55e82071e4a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Nov 2015 11:12:32 +0100 Subject: [PATCH 028/201] Aerocore: Generalize PWM out config --- src/drivers/boards/aerocore/board_config.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/boards/aerocore/board_config.h b/src/drivers/boards/aerocore/board_config.h index 155173db5f..d6932c713d 100644 --- a/src/drivers/boards/aerocore/board_config.h +++ b/src/drivers/boards/aerocore/board_config.h @@ -150,6 +150,7 @@ __BEGIN_DECLS #define GPIO_TIM3_CH2OUT GPIO_TIM3_CH2OUT_3 #define GPIO_TIM3_CH3OUT GPIO_TIM3_CH3OUT_2 #define GPIO_TIM3_CH4OUT GPIO_TIM3_CH4OUT_2 +#define DIRECT_PWM_OUTPUT_CHANNELS 8 /* High-resolution timer */ #define HRT_TIMER 8 /* use timer 8 for the HRT */ From 1426779572740e150c84a46df7ebe1f30492da67 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Nov 2015 11:12:45 +0100 Subject: [PATCH 029/201] FMUv1: Generalize board config --- src/drivers/boards/px4fmu-v1/board_config.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h index 5957ce6fc5..2c14788ee6 100644 --- a/src/drivers/boards/px4fmu-v1/board_config.h +++ b/src/drivers/boards/px4fmu-v1/board_config.h @@ -182,6 +182,7 @@ __BEGIN_DECLS #define GPIO_TIM2_CH2OUT GPIO_TIM2_CH2OUT_1 #define GPIO_TIM2_CH3OUT GPIO_TIM2_CH3OUT_1 #define GPIO_TIM2_CH4OUT GPIO_TIM2_CH4OUT_1 +#define DIRECT_PWM_OUTPUT_CHANNELS 4 /* USB OTG FS * From 9a1301dd96fb7eeb35f2a7bbe6d3858eadea2540 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Nov 2015 11:12:57 +0100 Subject: [PATCH 030/201] FMUv2: Generalize board config --- src/drivers/boards/px4fmu-v2/board_config.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index ea60df9100..12593937f7 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -210,6 +210,7 @@ __BEGIN_DECLS #define GPIO_TIM1_CH4OUT GPIO_TIM1_CH4OUT_2 #define GPIO_TIM4_CH2OUT GPIO_TIM4_CH2OUT_2 #define GPIO_TIM4_CH3OUT GPIO_TIM4_CH3OUT_2 +#define DIRECT_PWM_OUTPUT_CHANNELS 6 /* USB OTG FS * From 4637cc1f078daa9de0f880200c6917eb7e6a4be0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Nov 2015 11:13:11 +0100 Subject: [PATCH 031/201] FMUv4: Generalize board config --- src/drivers/boards/px4fmu-v4/board_config.h | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index a29c961f5a..de9998d928 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -200,6 +200,7 @@ __BEGIN_DECLS #define GPIO_TIM1_CH4OUT GPIO_TIM1_CH4OUT_2 #define GPIO_TIM4_CH2OUT GPIO_TIM4_CH2OUT_2 #define GPIO_TIM4_CH3OUT GPIO_TIM4_CH3OUT_2 +#define DIRECT_PWM_OUTPUT_CHANNELS 6 /* USB OTG FS * @@ -223,15 +224,20 @@ __BEGIN_DECLS #define GPIO_RSSI_IN (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1) #define GPIO_LED_SAFETY (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN3) -#define GPIO_SAFETY_SWITCH_IN (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4) +#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4) #define GPIO_PERIPH_3V3_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN5) #define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) #define GPIO_8266_GPIO0 (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN2) -#define GPIO_SPEKTRUM_POWER (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) +#define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) #define GPIO_8266_PD (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN5) #define GPIO_8266_RST (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN6) +/* Power switch controls ******************************************************/ + +#define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s)) +#define GPIO_USART1_RX_SPEKTRUM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN7) + /**************************************************************************************************** * Public Types ****************************************************************************************************/ From 32d298c4b6c99906a0814f4639e149376226467f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Nov 2015 11:13:25 +0100 Subject: [PATCH 032/201] IOv1: Generalize board config --- src/drivers/boards/px4io-v1/board_config.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/boards/px4io-v1/board_config.h b/src/drivers/boards/px4io-v1/board_config.h index f83f18e503..dfa9d2af74 100644 --- a/src/drivers/boards/px4io-v1/board_config.h +++ b/src/drivers/boards/px4io-v1/board_config.h @@ -83,6 +83,8 @@ #define GPIO_RELAY1_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN12) #define GPIO_RELAY2_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN11) +#define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s)) + /* Analog inputs ********************************************************************/ #define GPIO_ADC_VBATT (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4) From a5835bfb4fa2d3c559c59b20d6337f63f6f15643 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Nov 2015 11:13:38 +0100 Subject: [PATCH 033/201] IOv2: Generalize board config --- src/drivers/boards/px4io-v2/board_config.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h index d97f3b203c..5c6f08827d 100644 --- a/src/drivers/boards/px4io-v2/board_config.h +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -79,7 +79,7 @@ #define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13) #define GPIO_LED4 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN11) -#define GPIO_USART1_RX_SPEKTRUM (GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10) +#define GPIO_USART1_RX_SPEKTRUM (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN10) /* Safety switch button *******************************************************/ @@ -88,7 +88,7 @@ /* Power switch controls ******************************************************/ #define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) - +#define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s)) #define GPIO_SERVO_FAULT_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN15) /* Analog inputs **************************************************************/ From 9e45768ec29ab238ce4cc0da3ea0b232fc1f2fed Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Nov 2015 11:14:32 +0100 Subject: [PATCH 034/201] FMU driver: Use generic configs and less defines --- src/drivers/px4fmu/fmu.cpp | 34 ++++++++++------------------------ 1 file changed, 10 insertions(+), 24 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 8892cfedeb..00b9f81f25 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -90,12 +90,8 @@ # include #endif -/* - * This is the analog to FMU_INPUT_DROP_LIMIT_US on the IO side - */ - -#define CONTROL_INPUT_DROP_LIMIT_US 2000 -#define NAN_VALUE (0.0f/0.0f) +#define SCHEDULE_INTERVAL 2000 /**< The schedule interval in usec (500 Hz) */ +#define NAN_VALUE (0.0f/0.0f) /**< NaN value for throttle lock mode */ class PX4FMU : public device::CDev { @@ -123,15 +119,7 @@ public: int set_i2c_bus_clock(unsigned bus, unsigned clock_hz); private: -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) - static const unsigned _max_actuators = 4; -#endif -#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) - static const unsigned _max_actuators = 6; -#endif -#if defined(CONFIG_ARCH_BOARD_AEROCORE) - static const unsigned _max_actuators = 8; -#endif + static const unsigned _max_actuators = DIRECT_PWM_OUTPUT_CHANNELS; Mode _mode; unsigned _pwm_default_rate; @@ -189,7 +177,7 @@ private: int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate); int pwm_ioctl(file *filp, int cmd, unsigned long arg); void update_pwm_rev_mask(); - void publish_pwm_outputs(uint16_t *values, size_t numvalues); + void publish_pwm_outputs(uint16_t *values, size_t numvalues); struct GPIOConfig { uint32_t input; @@ -321,11 +309,9 @@ PX4FMU::PX4FMU() : memset(_controls, 0, sizeof(_controls)); memset(_poll_fds, 0, sizeof(_poll_fds)); -#ifdef HRT_PPM_CHANNEL // rc input, published to ORB memset(&_rc_in, 0, sizeof(_rc_in)); _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM; -#endif #ifdef GPIO_SBUS_INV // this board has a GPIO to control SBUS inversion @@ -728,8 +714,8 @@ PX4FMU::cycle() //main_out_latency = hrt_absolute_time() - _controls[i].timestamp - 250; /* do only correct within the current phase */ - if (abs(main_out_latency) > CONTROL_INPUT_DROP_LIMIT_US) { - main_out_latency = CONTROL_INPUT_DROP_LIMIT_US; + if (abs(main_out_latency) > SCHEDULE_INTERVAL) { + main_out_latency = SCHEDULE_INTERVAL; } if (main_out_latency < 250) { @@ -905,7 +891,7 @@ PX4FMU::cycle() } work_queue(HPWORK, &_work, (worker_t)&PX4FMU::cycle_trampoline, this, - USEC2TICK(CONTROL_INPUT_DROP_LIMIT_US - main_out_latency)); + USEC2TICK(SCHEDULE_INTERVAL - main_out_latency)); } void PX4FMU::work_stop() @@ -1683,9 +1669,9 @@ PX4FMU::peripheral_reset(int ms) stm32_gpiowrite(GPIO_PERIPH_3V3_EN, 0); - bool last = stm32_gpioread(GPIO_SPEKTRUM_POWER); + bool last = stm32_gpioread(GPIO_SPEKTRUM_PWR_EN); /* Keep Spektum on to discharge rail*/ - stm32_gpiowrite(GPIO_SPEKTRUM_POWER, 1); + stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, 1); /* wait for the peripheral rail to reach GND */ usleep(ms * 1000); @@ -1694,7 +1680,7 @@ PX4FMU::peripheral_reset(int ms) /* re-enable power */ /* switch the peripheral rail back on */ - stm32_gpiowrite(GPIO_SPEKTRUM_POWER, last); + stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, last); stm32_gpiowrite(GPIO_PERIPH_3V3_EN, 1); #endif } From 494bccb6184625d4c0bc5f6358ff9218880aa5f9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Nov 2015 11:14:59 +0100 Subject: [PATCH 035/201] IO: Remove define now part of board config --- src/modules/px4iofirmware/px4io.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 2197b7b776..3f35c471a5 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -170,7 +170,6 @@ extern pwm_limit_t pwm_limit; #ifdef CONFIG_ARCH_BOARD_PX4IO_V2 # define PX4IO_RELAY_CHANNELS 0 -# define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s)) # define ENABLE_SBUS_OUT(_s) stm32_gpiowrite(GPIO_SBUS_OENABLE, !(_s)) # define VDD_SERVO_FAULT (!stm32_gpioread(GPIO_SERVO_FAULT_DETECT)) From d71c62997066db026c95ac8fef02ae46fc03dd3d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 28 Nov 2015 11:15:52 +0100 Subject: [PATCH 036/201] SBUS: Fix code style --- src/lib/rc/dsm.c | 3 ++- src/lib/rc/sbus.c | 1 + 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/lib/rc/dsm.c b/src/lib/rc/dsm.c index a024b5c044..fc5b69686f 100644 --- a/src/lib/rc/dsm.c +++ b/src/lib/rc/dsm.c @@ -52,7 +52,7 @@ #include #ifndef GPIO_SPEKTRUM_PWR_EN - #error DSM input driver not supported by this board config +#error DSM input driver not supported by this board config #endif #define DSM_FRAME_SIZE 16 /** Date: Thu, 10 Dec 2015 21:32:08 +0100 Subject: [PATCH 037/201] Add DSM X test data --- unittests/testdata/dsm_x_data.txt | 1119 +++++++++++++++++++++++++++++ 1 file changed, 1119 insertions(+) create mode 100644 unittests/testdata/dsm_x_data.txt diff --git a/unittests/testdata/dsm_x_data.txt b/unittests/testdata/dsm_x_data.txt new file mode 100644 index 0000000000..bdf28d92bf --- /dev/null +++ b/unittests/testdata/dsm_x_data.txt @@ -0,0 +1,1119 @@ +Time [s],Value,Parity Error,Framing Error +9.260826000000000,0x00,, +9.260914000000000,0x00,, +9.260999999999999,0x8C,, +9.261088000000001,0x00,, +9.261174000000000,0x2E,, +9.261262000000000,0xAA,, +9.261350000000000,0x14,, +9.261436000000000,0x00,, +9.261524000000000,0x1C,, +9.261609999999999,0x00,, +9.261698000000001,0x01,, +9.261786000000001,0x60,, +9.261872000000000,0x3C,, +9.261960000000000,0x00,, +9.262046000000000,0x36,, +9.262134000000000,0xAA,, +9.271818000000000,0x00,, +9.271906000000000,0x00,, +9.271991999999999,0x0C,, +9.272080000000001,0x00,, +9.272168000000001,0x2E,, +9.272254000000000,0xAA,, +9.272342000000000,0x14,, +9.272428000000000,0x00,, +9.272516000000000,0x1C,, +9.272603999999999,0x00,, +9.272690000000001,0x26,, +9.272778000000001,0xAA,, +9.272864000000000,0x44,, +9.272952000000000,0x00,, +9.273040000000000,0x4C,, +9.273126000000000,0x00,, +9.282828000000000,0x00,, +9.282916000000000,0x00,, +9.283002000000000,0x8C,, +9.283090000000000,0x00,, +9.283177999999999,0x2E,, +9.283264000000001,0xAA,, +9.283352000000001,0x14,, +9.283438000000000,0x00,, +9.283526000000000,0x1C,, +9.283614000000000,0x00,, +9.283700000000000,0x01,, +9.283787999999999,0x61,, +9.283874000000001,0x3C,, +9.283962000000001,0x00,, +9.284050000000001,0x36,, +9.284136000000000,0xAA,, +9.293830000000000,0x00,, +9.293915999999999,0x00,, +9.294003999999999,0x0C,, +9.294090000000001,0x00,, +9.294178000000000,0x2E,, +9.294266000000000,0xAA,, +9.294352000000000,0x14,, +9.294440000000000,0x00,, +9.294525999999999,0x1C,, +9.294613999999999,0x00,, +9.294701999999999,0x26,, +9.294788000000000,0xAA,, +9.294876000000000,0x44,, +9.294962000000000,0x00,, +9.295050000000000,0x4C,, +9.295138000000000,0x00,, +9.304830000000001,0x00,, +9.304918000000001,0x00,, +9.305004000000000,0x8C,, +9.305092000000000,0x00,, +9.305178000000000,0x2E,, +9.305266000000000,0xAA,, +9.305353999999999,0x14,, +9.305440000000001,0x00,, +9.305528000000001,0x1C,, +9.305614000000000,0x00,, +9.305702000000000,0x01,, +9.305790000000000,0x60,, +9.305876000000000,0x3C,, +9.305963999999999,0x00,, +9.306050000000001,0x36,, +9.306138000000001,0xAA,, +9.315832000000000,0x00,, +9.315918000000000,0x00,, +9.316006000000000,0x0C,, +9.316091999999999,0x00,, +9.316179999999999,0x2E,, +9.316268000000001,0xAA,, +9.316354000000000,0x14,, +9.316442000000000,0x00,, +9.316528000000000,0x1C,, +9.316616000000000,0x00,, +9.316704000000000,0x26,, +9.316789999999999,0xAA,, +9.316878000000001,0x44,, +9.316964000000000,0x00,, +9.317052000000000,0x4C,, +9.317140000000000,0x00,, +9.326824000000000,0x00,, +9.326910000000000,0x00,, +9.326998000000000,0x8C,, +9.327083999999999,0x00,, +9.327171999999999,0x2E,, +9.327260000000001,0xAA,, +9.327346000000000,0x14,, +9.327434000000000,0x00,, +9.327520000000000,0x1C,, +9.327608000000000,0x00,, +9.327696000000000,0x01,, +9.327781999999999,0x61,, +9.327870000000001,0x3C,, +9.327956000000000,0x00,, +9.328044000000000,0x36,, +9.328132000000000,0xAA,, +9.337834000000001,0x00,, +9.337920000000000,0x00,, +9.338008000000000,0x0C,, +9.338094000000000,0x00,, +9.338182000000000,0x2E,, +9.338270000000000,0xAA,, +9.338355999999999,0x14,, +9.338444000000001,0x00,, +9.338530000000000,0x1C,, +9.338618000000000,0x00,, +9.338706000000000,0x26,, +9.338792000000000,0xAA,, +9.338880000000000,0x44,, +9.338965999999999,0x00,, +9.339054000000001,0x4C,, +9.339142000000001,0x00,, +9.348834000000000,0x00,, +9.348922000000000,0x00,, +9.349008000000000,0x8C,, +9.349095999999999,0x00,, +9.349183999999999,0x2E,, +9.349270000000001,0xAA,, +9.349358000000001,0x14,, +9.349444000000000,0x00,, +9.349532000000000,0x1C,, +9.349620000000000,0x00,, +9.349705999999999,0x01,, +9.349793999999999,0x61,, +9.349880000000001,0x3C,, +9.349968000000001,0x00,, +9.350056000000000,0x36,, +9.350142000000000,0xAA,, +9.359836000000000,0x00,, +9.359921999999999,0x00,, +9.360010000000001,0x0C,, +9.360098000000001,0x00,, +9.360184000000000,0x2E,, +9.360272000000000,0xAA,, +9.360358000000000,0x14,, +9.360446000000000,0x00,, +9.360533999999999,0x1C,, +9.360620000000001,0x00,, +9.360708000000001,0x26,, +9.360794000000000,0xAA,, +9.360882000000000,0x44,, +9.360970000000000,0x00,, +9.361056000000000,0x4C,, +9.361143999999999,0x00,, +9.370837999999999,0x00,, +9.370924000000000,0x00,, +9.371012000000000,0x8C,, +9.371098000000000,0x00,, +9.371186000000000,0x2E,, +9.371274000000000,0xAA,, +9.371359999999999,0x14,, +9.371447999999999,0x00,, +9.371534000000000,0x1C,, +9.371622000000000,0x00,, +9.371710000000000,0x01,, +9.371796000000000,0x61,, +9.371884000000000,0x3C,, +9.371969999999999,0x00,, +9.372058000000001,0x36,, +9.372146000000001,0xAA,, +9.381840000000000,0x00,, +9.381926000000000,0x00,, +9.382014000000000,0x0C,, +9.382099999999999,0x00,, +9.382187999999999,0x2E,, +9.382275999999999,0xAA,, +9.382362000000001,0x14,, +9.382450000000000,0x00,, +9.382536000000000,0x1C,, +9.382624000000000,0x00,, +9.382712000000000,0x26,, +9.382797999999999,0xAA,, +9.382885999999999,0x44,, +9.382972000000001,0x00,, +9.383060000000000,0x4C,, +9.383148000000000,0x00,, +9.392842000000000,0x00,, +9.392927999999999,0x00,, +9.393015999999999,0x8C,, +9.393102000000001,0x00,, +9.393190000000001,0x2E,, +9.393278000000000,0xAA,, +9.393364000000000,0x14,, +9.393452000000000,0x00,, +9.393537999999999,0x1C,, +9.393625999999999,0x00,, +9.393713999999999,0x01,, +9.393800000000001,0x61,, +9.393888000000000,0x3C,, +9.393974000000000,0x00,, +9.394062000000000,0x36,, +9.394150000000000,0xAA,, +9.403834000000000,0x00,, +9.403922000000000,0x00,, +9.404007999999999,0x0C,, +9.404095999999999,0x00,, +9.404184000000001,0x2E,, +9.404270000000000,0xAA,, +9.404358000000000,0x14,, +9.404444000000000,0x00,, +9.404532000000000,0x1C,, +9.404620000000000,0x00,, +9.404705999999999,0x26,, +9.404794000000001,0xAA,, +9.404880000000000,0x44,, +9.404968000000000,0x00,, +9.405056000000000,0x4C,, +9.405142000000000,0x00,, +9.414835999999999,0x00,, +9.414923999999999,0x00,, +9.415010000000001,0x8C,, +9.415098000000000,0x00,, +9.415184000000000,0x2E,, +9.415272000000000,0xAA,, +9.415360000000000,0x14,, +9.415445999999999,0x00,, +9.415533999999999,0x1C,, +9.415620000000001,0x00,, +9.415708000000000,0x01,, +9.415796000000000,0x60,, +9.415882000000000,0x3C,, +9.415970000000000,0x00,, +9.416055999999999,0x36,, +9.416143999999999,0xAA,, +9.425838000000001,0x00,, +9.425926000000000,0x00,, +9.426012000000000,0x0C,, +9.426100000000000,0x00,, +9.426186000000000,0x2E,, +9.426273999999999,0xAA,, +9.426361999999999,0x14,, +9.426448000000001,0x00,, +9.426536000000000,0x1C,, +9.426622000000000,0x00,, +9.426710000000000,0x26,, +9.426798000000000,0xAA,, +9.426883999999999,0x44,, +9.426971999999999,0x00,, +9.427058000000001,0x4C,, +9.427146000000000,0x00,, +9.436840000000000,0x00,, +9.436926000000000,0x00,, +9.437014000000000,0x8C,, +9.437101999999999,0x00,, +9.437188000000001,0x2E,, +9.437276000000001,0xAA,, +9.437362000000000,0x14,, +9.437450000000000,0x00,, +9.437538000000000,0x1C,, +9.437624000000000,0x00,, +9.437711999999999,0x01,, +9.437798000000001,0x61,, +9.437886000000001,0x3C,, +9.437974000000001,0x00,, +9.438060000000000,0x36,, +9.438148000000000,0xAA,, +9.447842000000000,0x00,, +9.447927999999999,0x00,, +9.448016000000001,0x0C,, +9.448102000000000,0x00,, +9.448190000000000,0x2E,, +9.448278000000000,0xAA,, +9.448364000000000,0x14,, +9.448452000000000,0x00,, +9.448537999999999,0x1C,, +9.448626000000001,0x00,, +9.448714000000001,0x26,, +9.448800000000000,0xAA,, +9.448888000000000,0x44,, +9.448974000000000,0x00,, +9.449062000000000,0x4C,, +9.449149999999999,0x00,, +9.458842000000001,0x00,, +9.458930000000001,0x00,, +9.459018000000000,0x8C,, +9.459104000000000,0x00,, +9.459192000000000,0x2E,, +9.459277999999999,0xAA,, +9.459365999999999,0x14,, +9.459453999999999,0x00,, +9.459540000000001,0x1C,, +9.459628000000000,0x00,, +9.459714000000000,0x01,, +9.459802000000000,0x60,, +9.459890000000000,0x3C,, +9.459975999999999,0x00,, +9.460063999999999,0x36,, +9.460150000000001,0xAA,, +9.469844000000000,0x00,, +9.469932000000000,0x00,, +9.470018000000000,0x0C,, +9.470105999999999,0x00,, +9.470193999999999,0x2E,, +9.470280000000001,0xAA,, +9.470368000000001,0x14,, +9.470454000000000,0x00,, +9.470542000000000,0x1C,, +9.470630000000000,0x00,, +9.470715999999999,0x26,, +9.470803999999999,0xAA,, +9.470890000000001,0x44,, +9.470978000000001,0x00,, +9.471066000000000,0x4C,, +9.471152000000000,0x00,, +9.480846000000000,0x00,, +9.480931999999999,0x00,, +9.481019999999999,0x8C,, +9.481108000000001,0x00,, +9.481194000000000,0x2E,, +9.481282000000000,0xAA,, +9.481368000000000,0x14,, +9.481456000000000,0x00,, +9.481544000000000,0x1C,, +9.481629999999999,0x00,, +9.481718000000001,0x01,, +9.481804000000000,0x60,, +9.481892000000000,0x3C,, +9.481980000000000,0x00,, +9.482066000000000,0x36,, +9.482154000000000,0xAA,, +9.491838000000000,0x00,, +9.491925999999999,0x00,, +9.492012000000001,0x0C,, +9.492100000000001,0x00,, +9.492186000000000,0x2E,, +9.492274000000000,0xAA,, +9.492362000000000,0x14,, +9.492448000000000,0x00,, +9.492535999999999,0x1C,, +9.492622000000001,0x00,, +9.492710000000001,0x26,, +9.492796000000000,0xAA,, +9.492884000000000,0x44,, +9.492972000000000,0x00,, +9.493058000000000,0x4C,, +9.493145999999999,0x00,, +9.502848000000000,0x00,, +9.502934000000000,0x00,, +9.503022000000000,0x8C,, +9.503110000000000,0x00,, +9.503196000000001,0x2E,, +9.503284000000001,0xAA,, +9.503370000000000,0x14,, +9.503458000000000,0x00,, +9.503546000000000,0x1C,, +9.503632000000000,0x00,, +9.503720000000000,0x01,, +9.503806000000001,0x61,, +9.503894000000001,0x3C,, +9.503982000000001,0x00,, +9.504068000000000,0x36,, +9.504156000000000,0xAA,, +9.513847999999999,0x00,, +9.513935999999999,0x00,, +9.514023999999999,0x0C,, +9.514110000000001,0x00,, +9.514198000000000,0x2E,, +9.514284000000000,0xAA,, +9.514372000000000,0x14,, +9.514460000000000,0x00,, +9.514545999999999,0x1C,, +9.514633999999999,0x00,, +9.514720000000001,0x26,, +9.514808000000000,0xAA,, +9.514896000000000,0x44,, +9.514982000000000,0x00,, +9.515070000000000,0x4C,, +9.515155999999999,0x00,, +9.524842000000000,0x00,, +9.524927999999999,0x00,, +9.525016000000001,0x8C,, +9.525102000000000,0x00,, +9.525190000000000,0x2E,, +9.525278000000000,0xAA,, +9.525364000000000,0x14,, +9.525452000000000,0x00,, +9.525537999999999,0x1C,, +9.525626000000001,0x00,, +9.525714000000001,0x01,, +9.525800000000000,0x60,, +9.525888000000000,0x3C,, +9.525974000000000,0x00,, +9.526062000000000,0x36,, +9.526149999999999,0xAA,, +9.535852000000000,0x00,, +9.535938000000000,0x00,, +9.536026000000000,0x0C,, +9.536111999999999,0x00,, +9.536199999999999,0x2E,, +9.536288000000001,0xAA,, +9.536374000000000,0x14,, +9.536462000000000,0x00,, +9.536548000000000,0x1C,, +9.536636000000000,0x00,, +9.536724000000000,0x26,, +9.536809999999999,0xAA,, +9.536898000000001,0x44,, +9.536984000000000,0x00,, +9.537072000000000,0x4C,, +9.537160000000000,0x00,, +9.546851999999999,0x00,, +9.546939999999999,0x00,, +9.547026000000001,0x8C,, +9.547114000000001,0x00,, +9.547202000000000,0x2E,, +9.547288000000000,0xAA,, +9.547376000000000,0x14,, +9.547461999999999,0x00,, +9.547549999999999,0x1C,, +9.547637999999999,0x00,, +9.547724000000001,0x01,, +9.547812000000000,0x60,, +9.547898000000000,0x3C,, +9.547986000000000,0x00,, +9.548074000000000,0x36,, +9.548159999999999,0xAA,, +9.557846000000000,0x00,, +9.557931999999999,0x00,, +9.558020000000001,0x0C,, +9.558106000000000,0x00,, +9.558194000000000,0x2E,, +9.558282000000000,0xAA,, +9.558368000000000,0x14,, +9.558456000000000,0x00,, +9.558541999999999,0x1C,, +9.558630000000001,0x00,, +9.558718000000001,0x26,, +9.558804000000000,0xAA,, +9.558892000000000,0x44,, +9.558978000000000,0x00,, +9.559066000000000,0x4C,, +9.559153999999999,0x00,, +9.568856000000000,0x00,, +9.568944000000000,0x00,, +9.569030000000000,0x8C,, +9.569118000000000,0x00,, +9.569203999999999,0x2E,, +9.569292000000001,0xAA,, +9.569380000000001,0x14,, +9.569466000000000,0x00,, +9.569554000000000,0x1C,, +9.569640000000000,0x00,, +9.569728000000000,0x01,, +9.569815999999999,0x60,, +9.569902000000001,0x3C,, +9.569990000000001,0x00,, +9.570076000000000,0x36,, +9.570164000000000,0xAA,, +9.579848000000000,0x00,, +9.579936000000000,0x00,, +9.580024000000000,0x0C,, +9.580109999999999,0x00,, +9.580197999999999,0x2E,, +9.580284000000001,0xAA,, +9.580372000000001,0x14,, +9.580460000000000,0x00,, +9.580546000000000,0x1C,, +9.580634000000000,0x00,, +9.580719999999999,0x26,, +9.580807999999999,0xAA,, +9.580895999999999,0x44,, +9.580982000000001,0x00,, +9.581070000000000,0x4C,, +9.581156000000000,0x00,, +9.590850000000000,0x00,, +9.590938000000000,0x00,, +9.591024000000001,0x8C,, +9.591112000000001,0x00,, +9.591200000000001,0x2E,, +9.591286000000000,0xAA,, +9.591374000000000,0x14,, +9.591460000000000,0x00,, +9.591548000000000,0x1C,, +9.591635999999999,0x00,, +9.591722000000001,0x01,, +9.591810000000001,0x60,, +9.591896000000000,0x3C,, +9.591984000000000,0x00,, +9.592072000000000,0x36,, +9.592158000000000,0xAA,, +9.601851999999999,0x00,, +9.601938000000001,0x00,, +9.602026000000000,0x0C,, +9.602114000000000,0x00,, +9.602200000000000,0x2E,, +9.602288000000000,0xAA,, +9.602373999999999,0x14,, +9.602461999999999,0x00,, +9.602550000000001,0x1C,, +9.602636000000000,0x00,, +9.602724000000000,0x26,, +9.602810000000000,0xAA,, +9.602898000000000,0x44,, +9.602986000000000,0x00,, +9.603071999999999,0x4C,, +9.603160000000001,0x00,, +9.612852000000000,0x00,, +9.612940000000000,0x00,, +9.613028000000000,0x8C,, +9.613113999999999,0x00,, +9.613201999999999,0x2E,, +9.613288000000001,0xAA,, +9.613376000000001,0x14,, +9.613464000000000,0x00,, +9.613550000000000,0x1C,, +9.613638000000000,0x00,, +9.613723999999999,0x01,, +9.613811999999999,0x61,, +9.613898000000001,0x3C,, +9.613986000000001,0x00,, +9.614074000000000,0x36,, +9.614160000000000,0xAA,, +9.623854000000000,0x00,, +9.623939999999999,0x00,, +9.624027999999999,0x0C,, +9.624116000000001,0x00,, +9.624202000000000,0x2E,, +9.624290000000000,0xAA,, +9.624376000000000,0x14,, +9.624464000000000,0x00,, +9.624549999999999,0x1C,, +9.624637999999999,0x00,, +9.624726000000001,0x26,, +9.624812000000000,0xAA,, +9.624900000000000,0x44,, +9.624986000000000,0x00,, +9.625074000000000,0x4C,, +9.625162000000000,0x00,, +9.634854000000001,0x00,, +9.634942000000001,0x00,, +9.635028000000000,0x8C,, +9.635116000000000,0x00,, +9.635204000000000,0x2E,, +9.635289999999999,0xAA,, +9.635377999999999,0x14,, +9.635464000000001,0x00,, +9.635552000000001,0x1C,, +9.635640000000000,0x00,, +9.635726000000000,0x01,, +9.635814000000000,0x61,, +9.635899999999999,0x3C,, +9.635987999999999,0x00,, +9.636075999999999,0x36,, +9.636162000000001,0xAA,, +9.645856000000000,0x00,, +9.645944000000000,0x00,, +9.646030000000000,0x0C,, +9.646118000000000,0x00,, +9.646203999999999,0x2E,, +9.646292000000001,0xAA,, +9.646380000000001,0x14,, +9.646466000000000,0x00,, +9.646554000000000,0x1C,, +9.646640000000000,0x00,, +9.646728000000000,0x26,, +9.646815999999999,0xAA,, +9.646902000000001,0x44,, +9.646990000000001,0x00,, +9.647076000000000,0x4C,, +9.647164000000000,0x00,, +9.656858000000000,0x00,, +9.656943999999999,0x00,, +9.657031999999999,0x8C,, +9.657118000000001,0x00,, +9.657206000000000,0x2E,, +9.657294000000000,0xAA,, +9.657380000000000,0x14,, +9.657468000000000,0x00,, +9.657553999999999,0x1C,, +9.657641999999999,0x00,, +9.657730000000001,0x01,, +9.657816000000000,0x61,, +9.657904000000000,0x3C,, +9.657990000000000,0x00,, +9.658078000000000,0x36,, +9.658166000000000,0xAA,, +9.667858000000001,0x00,, +9.667946000000001,0x00,, +9.668032000000000,0x0C,, +9.668120000000000,0x00,, +9.668208000000000,0x2E,, +9.668293999999999,0xAA,, +9.668381999999999,0x14,, +9.668468000000001,0x00,, +9.668556000000001,0x1C,, +9.668644000000000,0x00,, +9.668730000000000,0x26,, +9.668818000000000,0xAA,, +9.668903999999999,0x44,, +9.668991999999999,0x00,, +9.669079999999999,0x4C,, +9.669166000000001,0x00,, +9.678860000000000,0x00,, +9.678946000000000,0x00,, +9.679034000000000,0x8C,, +9.679122000000000,0x00,, +9.679207999999999,0x2E,, +9.679296000000001,0xAA,, +9.679382000000000,0x14,, +9.679470000000000,0x00,, +9.679556000000000,0x1C,, +9.679644000000000,0x00,, +9.679732000000000,0x01,, +9.679817999999999,0x60,, +9.679906000000001,0x3C,, +9.679992000000000,0x00,, +9.680080000000000,0x36,, +9.680168000000000,0xAA,, +9.689859999999999,0x00,, +9.689947999999999,0x00,, +9.690034000000001,0x0C,, +9.690122000000001,0x00,, +9.690210000000000,0x2E,, +9.690296000000000,0xAA,, +9.690384000000000,0x14,, +9.690469999999999,0x00,, +9.690557999999999,0x1C,, +9.690644000000001,0x00,, +9.690732000000001,0x26,, +9.690820000000000,0xAA,, +9.690906000000000,0x44,, +9.690994000000000,0x00,, +9.691079999999999,0x4C,, +9.691167999999999,0x00,, +9.700862000000001,0x00,, +9.700948000000000,0x00,, +9.701036000000000,0x8C,, +9.701122000000000,0x00,, +9.701210000000000,0x2E,, +9.701298000000000,0xAA,, +9.701383999999999,0x14,, +9.701472000000001,0x00,, +9.701558000000000,0x1C,, +9.701646000000000,0x00,, +9.701734000000000,0x01,, +9.701820000000000,0x61,, +9.701908000000000,0x3C,, +9.701993999999999,0x00,, +9.702082000000001,0x36,, +9.702170000000001,0xAA,, +9.711862000000000,0x00,, +9.711950000000000,0x00,, +9.712038000000000,0x0C,, +9.712123999999999,0x00,, +9.712211999999999,0x2E,, +9.712298000000001,0xAA,, +9.712386000000000,0x14,, +9.712474000000000,0x00,, +9.712560000000000,0x1C,, +9.712648000000000,0x00,, +9.712733999999999,0x26,, +9.712821999999999,0xAA,, +9.712910000000001,0x44,, +9.712996000000000,0x00,, +9.713084000000000,0x4C,, +9.713170000000000,0x00,, +9.722864000000000,0x00,, +9.722951999999999,0x00,, +9.723038000000001,0x8C,, +9.723126000000001,0x00,, +9.723212000000000,0x2E,, +9.723300000000000,0xAA,, +9.723388000000000,0x14,, +9.723474000000000,0x00,, +9.723561999999999,0x1C,, +9.723648000000001,0x00,, +9.723736000000001,0x01,, +9.723824000000000,0x60,, +9.723910000000000,0x3C,, +9.723998000000000,0x00,, +9.724084000000000,0x36,, +9.724171999999999,0xAA,, +9.733866000000001,0x00,, +9.733952000000000,0x00,, +9.734040000000000,0x0C,, +9.734126000000000,0x00,, +9.734214000000000,0x2E,, +9.734302000000000,0xAA,, +9.734387999999999,0x14,, +9.734476000000001,0x00,, +9.734562000000000,0x1C,, +9.734650000000000,0x00,, +9.734738000000000,0x26,, +9.734824000000000,0xAA,, +9.734912000000000,0x44,, +9.734997999999999,0x00,, +9.735086000000001,0x4C,, +9.735174000000001,0x00,, +9.744866000000000,0x00,, +9.744954000000000,0x00,, +9.745039999999999,0x8C,, +9.745127999999999,0x00,, +9.745215999999999,0x2E,, +9.745302000000001,0xAA,, +9.745390000000000,0x14,, +9.745476000000000,0x00,, +9.745564000000000,0x1C,, +9.745652000000000,0x00,, +9.745737999999999,0x01,, +9.745825999999999,0x60,, +9.745912000000001,0x3C,, +9.746000000000000,0x00,, +9.746088000000000,0x36,, +9.746174000000000,0xAA,, +9.755858000000000,0x00,, +9.755946000000000,0x00,, +9.756034000000000,0x0C,, +9.756119999999999,0x00,, +9.756208000000001,0x2E,, +9.756294000000000,0xAA,, +9.756382000000000,0x14,, +9.756470000000000,0x00,, +9.756556000000000,0x1C,, +9.756644000000000,0x00,, +9.756729999999999,0x26,, +9.756818000000001,0xAA,, +9.756906000000001,0x44,, +9.756992000000000,0x00,, +9.757080000000000,0x4C,, +9.757166000000000,0x00,, +9.766868000000001,0x00,, +9.766956000000000,0x00,, +9.767042000000000,0x8C,, +9.767130000000000,0x00,, +9.767218000000000,0x2E,, +9.767303999999999,0xAA,, +9.767391999999999,0x14,, +9.767478000000001,0x00,, +9.767566000000000,0x1C,, +9.767654000000000,0x00,, +9.767740000000000,0x01,, +9.767828000000000,0x61,, +9.767913999999999,0x3C,, +9.768001999999999,0x00,, +9.768090000000001,0x36,, +9.768176000000000,0xAA,, +9.777870000000000,0x00,, +9.777956000000000,0x00,, +9.778044000000000,0x0C,, +9.778131999999999,0x00,, +9.778218000000001,0x2E,, +9.778306000000001,0xAA,, +9.778392000000000,0x14,, +9.778480000000000,0x00,, +9.778568000000000,0x1C,, +9.778654000000000,0x00,, +9.778741999999999,0x26,, +9.778828000000001,0xAA,, +9.778916000000001,0x44,, +9.779004000000000,0x00,, +9.779090000000000,0x4C,, +9.779178000000000,0x00,, +9.788869999999999,0x00,, +9.788957999999999,0x00,, +9.789046000000001,0x8C,, +9.789132000000000,0x00,, +9.789220000000000,0x2E,, +9.789306000000000,0xAA,, +9.789394000000000,0x14,, +9.789482000000000,0x00,, +9.789567999999999,0x1C,, +9.789656000000001,0x00,, +9.789742000000000,0x01,, +9.789830000000000,0x61,, +9.789918000000000,0x3C,, +9.790004000000000,0x00,, +9.790092000000000,0x36,, +9.790177999999999,0xAA,, +9.799863999999999,0x00,, +9.799950000000001,0x00,, +9.800038000000001,0x0C,, +9.800126000000001,0x00,, +9.800212000000000,0x2E,, +9.800300000000000,0xAA,, +9.800386000000000,0x14,, +9.800473999999999,0x00,, +9.800561999999999,0x1C,, +9.800648000000001,0x00,, +9.800736000000001,0x26,, +9.800822000000000,0xAA,, +9.800910000000000,0x44,, +9.800998000000000,0x00,, +9.801083999999999,0x4C,, +9.801171999999999,0x00,, +9.810874000000000,0x00,, +9.810962000000000,0x00,, +9.811048000000000,0x8C,, +9.811135999999999,0x00,, +9.811223999999999,0x2E,, +9.811310000000001,0xAA,, +9.811398000000001,0x14,, +9.811484000000000,0x00,, +9.811572000000000,0x1C,, +9.811660000000000,0x00,, +9.811745999999999,0x01,, +9.811833999999999,0x61,, +9.811920000000001,0x3C,, +9.812008000000001,0x00,, +9.812096000000000,0x36,, +9.812182000000000,0xAA,, +9.821876000000000,0x00,, +9.821963999999999,0x00,, +9.822050000000001,0x0C,, +9.822138000000001,0x00,, +9.822224000000000,0x2E,, +9.822312000000000,0xAA,, +9.822400000000000,0x14,, +9.822486000000000,0x00,, +9.822573999999999,0x1C,, +9.822660000000001,0x00,, +9.822748000000001,0x26,, +9.822836000000001,0xAA,, +9.822922000000000,0x44,, +9.823010000000000,0x00,, +9.823096000000000,0x4C,, +9.823183999999999,0x00,, +9.832870000000000,0x00,, +9.832955999999999,0x00,, +9.833043999999999,0x8C,, +9.833130000000001,0x00,, +9.833218000000000,0x2E,, +9.833306000000000,0xAA,, +9.833392000000000,0x14,, +9.833480000000000,0x00,, +9.833565999999999,0x1C,, +9.833653999999999,0x00,, +9.833742000000001,0x01,, +9.833828000000000,0x61,, +9.833916000000000,0x3C,, +9.834002000000000,0x00,, +9.834090000000000,0x36,, +9.834178000000000,0xAA,, +9.843870000000001,0x00,, +9.843958000000001,0x00,, +9.844044000000000,0x0C,, +9.844132000000000,0x00,, +9.844220000000000,0x2E,, +9.844306000000000,0xAA,, +9.844393999999999,0x14,, +9.844480000000001,0x00,, +9.844568000000001,0x1C,, +9.844656000000001,0x00,, +9.844742000000000,0x26,, +9.844830000000000,0xAA,, +9.844916000000000,0x44,, +9.845003999999999,0x00,, +9.845091999999999,0x4C,, +9.845178000000001,0x00,, +9.854872000000000,0x00,, +9.854958000000000,0x00,, +9.855046000000000,0x8C,, +9.855134000000000,0x00,, +9.855219999999999,0x2E,, +9.855308000000001,0xAA,, +9.855394000000000,0x14,, +9.855482000000000,0x00,, +9.855570000000000,0x1C,, +9.855656000000000,0x00,, +9.855744000000000,0x01,, +9.855829999999999,0x61,, +9.855918000000001,0x3C,, +9.856006000000001,0x00,, +9.856092000000000,0x36,, +9.856180000000000,0xAA,, +9.865881999999999,0x00,, +9.865968000000001,0x00,, +9.866056000000000,0x0C,, +9.866142000000000,0x00,, +9.866230000000000,0x2E,, +9.866318000000000,0xAA,, +9.866403999999999,0x14,, +9.866491999999999,0x00,, +9.866578000000001,0x1C,, +9.866666000000000,0x00,, +9.866754000000000,0x26,, +9.866840000000000,0xAA,, +9.866928000000000,0x44,, +9.867013999999999,0x00,, +9.867101999999999,0x4C,, +9.867190000000001,0x00,, +9.876882000000000,0x00,, +9.876970000000000,0x00,, +9.877056000000000,0x8C,, +9.877143999999999,0x00,, +9.877231999999999,0x2E,, +9.877318000000001,0xAA,, +9.877406000000001,0x14,, +9.877492000000000,0x00,, +9.877580000000000,0x1C,, +9.877668000000000,0x00,, +9.877753999999999,0x01,, +9.877841999999999,0x61,, +9.877928000000001,0x3C,, +9.878016000000001,0x00,, +9.878104000000000,0x36,, +9.878190000000000,0xAA,, +9.887874000000000,0x00,, +9.887962000000000,0x00,, +9.888050000000000,0x0C,, +9.888135999999999,0x00,, +9.888223999999999,0x2E,, +9.888310000000001,0xAA,, +9.888398000000000,0x14,, +9.888486000000000,0x00,, +9.888572000000000,0x1C,, +9.888660000000000,0x00,, +9.888745999999999,0x26,, +9.888833999999999,0xAA,, +9.888922000000001,0x44,, +9.889008000000000,0x00,, +9.889096000000000,0x4C,, +9.889182000000000,0x00,, +9.898876000000000,0x00,, +9.898963999999999,0x00,, +9.899050000000001,0x8C,, +9.899138000000001,0x00,, +9.899224000000000,0x2E,, +9.899312000000000,0xAA,, +9.899400000000000,0x14,, +9.899486000000000,0x00,, +9.899573999999999,0x1C,, +9.899660000000001,0x00,, +9.899748000000001,0x01,, +9.899836000000001,0x61,, +9.899922000000000,0x3C,, +9.900010000000000,0x00,, +9.900096000000000,0x36,, +9.900183999999999,0xAA,, +9.909878000000001,0x00,, +9.909964000000000,0x00,, +9.910052000000000,0x0C,, +9.910138000000000,0x00,, +9.910226000000000,0x2E,, +9.910314000000000,0xAA,, +9.910399999999999,0x14,, +9.910488000000001,0x00,, +9.910574000000000,0x1C,, +9.910662000000000,0x00,, +9.910750000000000,0x26,, +9.910836000000000,0xAA,, +9.910924000000000,0x44,, +9.911009999999999,0x00,, +9.911098000000001,0x4C,, +9.911186000000001,0x00,, +9.920878000000000,0x00,, +9.920966000000000,0x00,, +9.921052000000000,0x8C,, +9.921139999999999,0x00,, +9.921227999999999,0x2E,, +9.921314000000001,0xAA,, +9.921402000000000,0x14,, +9.921488000000000,0x00,, +9.921576000000000,0x1C,, +9.921664000000000,0x00,, +9.921749999999999,0x01,, +9.921837999999999,0x60,, +9.921924000000001,0x3C,, +9.922012000000000,0x00,, +9.922100000000000,0x36,, +9.922186000000000,0xAA,, +9.931880000000000,0x00,, +9.931965999999999,0x00,, +9.932054000000001,0x0C,, +9.932140000000000,0x00,, +9.932228000000000,0x2E,, +9.932316000000000,0xAA,, +9.932402000000000,0x14,, +9.932490000000000,0x00,, +9.932575999999999,0x1C,, +9.932664000000001,0x00,, +9.932752000000001,0x26,, +9.932838000000000,0xAA,, +9.932926000000000,0x44,, +9.933012000000000,0x00,, +9.933100000000000,0x4C,, +9.933187999999999,0x00,, +9.942888000000000,0x00,, +9.942976000000000,0x00,, +9.943064000000000,0x8C,, +9.943149999999999,0x00,, +9.943237999999999,0x2E,, +9.943324000000000,0xAA,, +9.943412000000000,0x14,, +9.943500000000000,0x00,, +9.943586000000000,0x1C,, +9.943674000000000,0x00,, +9.943759999999999,0x01,, +9.943847999999999,0x61,, +9.943936000000001,0x3C,, +9.944022000000000,0x00,, +9.944110000000000,0x36,, +9.944196000000000,0xAA,, +9.953882000000000,0x00,, +9.953968000000000,0x00,, +9.954056000000000,0x0C,, +9.954141999999999,0x00,, +9.954230000000001,0x2E,, +9.954318000000001,0xAA,, +9.954404000000000,0x14,, +9.954492000000000,0x00,, +9.954578000000000,0x1C,, +9.954666000000000,0x00,, +9.954751999999999,0x26,, +9.954840000000001,0xAA,, +9.954928000000001,0x44,, +9.955014000000000,0x00,, +9.955102000000000,0x4C,, +9.955188000000000,0x00,, +9.964890000000000,0x00,, +9.964978000000000,0x00,, +9.965066000000000,0x8C,, +9.965152000000000,0x00,, +9.965240000000000,0x2E,, +9.965325999999999,0xAA,, +9.965414000000001,0x14,, +9.965502000000001,0x00,, +9.965588000000000,0x1C,, +9.965676000000000,0x00,, +9.965762000000000,0x01,, +9.965850000000000,0x60,, +9.965938000000000,0x3C,, +9.966024000000001,0x00,, +9.966112000000001,0x36,, +9.966198000000000,0xAA,, +9.975882000000000,0x00,, +9.975970000000000,0x00,, +9.976058000000000,0x0C,, +9.976144000000000,0x00,, +9.976232000000000,0x2E,, +9.976317999999999,0xAA,, +9.976406000000001,0x14,, +9.976494000000001,0x00,, +9.976580000000000,0x1C,, +9.976668000000000,0x00,, +9.976754000000000,0x26,, +9.976842000000000,0xAA,, +9.976929999999999,0x44,, +9.977016000000001,0x00,, +9.977104000000001,0x4C,, +9.977190000000000,0x00,, +9.986884000000000,0x00,, +9.986969999999999,0x00,, +9.987057999999999,0x8C,, +9.987144000000001,0x00,, +9.987232000000001,0x2E,, +9.987320000000000,0xAA,, +9.987406000000000,0x14,, +9.987494000000000,0x00,, +9.987579999999999,0x1C,, +9.987667999999999,0x00,, +9.987755999999999,0x01,, +9.987842000000001,0x61,, +9.987930000000000,0x3C,, +9.988016000000000,0x00,, +9.988104000000000,0x36,, +9.988192000000000,0xAA,, +9.997884000000001,0x00,, +9.997972000000001,0x00,, +9.998058000000000,0x0C,, +9.998146000000000,0x00,, +9.998232000000000,0x2E,, +9.998320000000000,0xAA,, +9.998408000000000,0x14,, +9.998494000000001,0x00,, +9.998582000000001,0x1C,, +9.998668000000000,0x00,, +9.998756000000000,0x26,, +9.998844000000000,0xAA,, +9.998930000000000,0x44,, +9.999018000000000,0x00,, +9.999104000000001,0x4C,, +9.999192000000001,0x00,, +10.008894000000000,0x00,, +10.008979999999999,0x00,, +10.009067999999999,0x8C,, +10.009156000000001,0x00,, +10.009242000000000,0x2E,, +10.009330000000000,0xAA,, +10.009416000000000,0x14,, +10.009504000000000,0x00,, +10.009592000000000,0x1C,, +10.009677999999999,0x00,, +10.009766000000001,0x01,, +10.009852000000000,0x60,, +10.009940000000000,0x3C,, +10.010028000000000,0x00,, +10.010114000000000,0x36,, +10.010202000000000,0xAA,, +10.019886000000000,0x00,, +10.019971999999999,0x00,, +10.020060000000001,0x0C,, +10.020148000000001,0x00,, +10.020234000000000,0x2E,, +10.020322000000000,0xAA,, +10.020408000000000,0x14,, +10.020496000000000,0x00,, +10.020583999999999,0x1C,, +10.020670000000001,0x00,, +10.020758000000001,0x26,, +10.020844000000000,0xAA,, +10.020932000000000,0x44,, +10.021020000000000,0x00,, From 64b7072eaada046a325cece1ed6ed4a008743793 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Dec 2015 09:26:53 +0100 Subject: [PATCH 038/201] Add DSM decoder test --- unittests/CMakeLists.txt | 6 ++++ unittests/dsm_test.cpp | 75 ++++++++++++++++++++++++++++++++++++++++ 2 files changed, 81 insertions(+) create mode 100644 unittests/dsm_test.cpp diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt index fcfcee3702..09c9b767f5 100644 --- a/unittests/CMakeLists.txt +++ b/unittests/CMakeLists.txt @@ -153,6 +153,12 @@ add_executable(sbus2_test sbus2_test.cpp hrt.cpp target_link_libraries( sbus2_test px4_platform ) add_gtest(sbus2_test) +# DSM test +add_executable(dsm_test dsm_test.cpp hrt.cpp + ${PX_SRC}/lib/rc/dsm.c) +target_link_libraries( dsm_test px4_platform ) +add_gtest(dsm_test) + # st24_test add_executable(rc_input_test st24_test.cpp hrt.cpp ${PX_SRC}/lib/rc/st24.c sumd_test.cpp ${PX_SRC}/lib/rc/sumd.c) target_link_libraries(rc_input_test px4_platform) diff --git a/unittests/dsm_test.cpp b/unittests/dsm_test.cpp new file mode 100644 index 0000000000..d6b6faa021 --- /dev/null +++ b/unittests/dsm_test.cpp @@ -0,0 +1,75 @@ +#include +#include +#include + +#include "../../src/systemcmds/tests/tests.h" +#include +// Enable DSM parser output +#define DSM_DEBUG +#include +#include +#include + +#include "gtest/gtest.h" + +TEST(DSMTest, DSM) +{ + const char *filepath = "testdata/dsm_x_data.txt"; + + FILE *fp; + fp = fopen(filepath, "rt"); + + ASSERT_TRUE(fp); + warnx("loading data from: %s", filepath); + + float f; + unsigned x; + int ret; + + // Trash the first 20 lines + for (unsigned i = 0; i < 20; i++) { + char buf[200]; + (void)fgets(buf, sizeof(buf), fp); + } + + // Init the parser + uint8_t frame[20]; + uint16_t rc_values[18]; + uint16_t num_values; + unsigned sbus_frame_drops = 0; + unsigned sbus_frame_resets = 0; + bool sbus_failsafe; + bool sbus_frame_drop; + uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]); + + int rate_limiter = 0; + unsigned last_drop = 0; + + while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) { + + ASSERT_GT(ret, 0); + + frame[0] = x; + unsigned len = 1; + + // Pipe the data into the parser + hrt_abstime now = hrt_absolute_time(); + + // if (rate_limiter % byte_offset == 0) { + bool result = false;//sbus_parse(now, &frame[0], len, rc_values, &num_values, + // &sbus_failsafe, &sbus_frame_drop, &sbus_frame_drops, max_channels); + + if (result) { + //warnx("decoded packet"); + } + + if (last_drop != (sbus_frame_drops + sbus_frame_resets)) { + warnx("frame dropped, now #%d", (sbus_frame_drops + sbus_frame_resets)); + last_drop = sbus_frame_drops + sbus_frame_resets; + } + + rate_limiter++; + } + + ASSERT_EQ(ret, EOF); +} From f7784bc7ac1ca154bafd69d6f171ba8cf0811c8f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Dec 2015 09:28:06 +0100 Subject: [PATCH 039/201] DSM decoder: Fixes for non-IO environment. --- src/lib/rc/dsm.c | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/lib/rc/dsm.c b/src/lib/rc/dsm.c index fc5b69686f..19e41e0ab2 100644 --- a/src/lib/rc/dsm.c +++ b/src/lib/rc/dsm.c @@ -42,7 +42,6 @@ #include #include #include -#include #include #include @@ -52,7 +51,12 @@ #include #ifndef GPIO_SPEKTRUM_PWR_EN -#error DSM input driver not supported by this board config +#warning DSM input driver not supporting binding by this board config + #define POWER_SPEKTRUM(arg) +#endif + +#if defined (__PX4_LINUX) || defined (__PX4_DARWIN) + #define up_udelay(arg) #endif #define DSM_FRAME_SIZE 16 /** Date: Mon, 14 Dec 2015 13:07:42 +0000 Subject: [PATCH 040/201] FMUv4: Add DSM controls --- src/drivers/boards/px4fmu-v4/board_config.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index de9998d928..4131666546 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -237,6 +237,9 @@ __BEGIN_DECLS #define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s)) #define GPIO_USART1_RX_SPEKTRUM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN7) +#define SPEKTRUM_RX_HIGH(_s) stm32_gpiowrite(GPIO_USART1_RX_SPEKTRUM, (_s)) +#define SPEKTRUM_RX_AS_UART() stm32_configgpio(GPIO_USART1_RX) +#define SPEKTRUM_RX_AS_GPIO() stm32_configgpio(GPIO_USART1_RX_SPEKTRUM) /**************************************************************************************************** * Public Types From 8520db884e1b1758079d8689a1fa9937c75a6a12 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Dec 2015 13:07:57 +0000 Subject: [PATCH 041/201] IOv1: Add DSM controls --- src/drivers/boards/px4io-v1/board_config.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/drivers/boards/px4io-v1/board_config.h b/src/drivers/boards/px4io-v1/board_config.h index dfa9d2af74..30cfa9b660 100644 --- a/src/drivers/boards/px4io-v1/board_config.h +++ b/src/drivers/boards/px4io-v1/board_config.h @@ -85,6 +85,10 @@ #define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s)) +#define SPEKTRUM_RX_HIGH(_s) stm32_gpiowrite(GPIO_USART1_RX_SPEKTRUM, (_s)) +#define SPEKTRUM_RX_AS_UART() stm32_configgpio(GPIO_USART1_RX) +#define SPEKTRUM_RX_AS_GPIO() stm32_configgpio(GPIO_USART1_RX_SPEKTRUM) + /* Analog inputs ********************************************************************/ #define GPIO_ADC_VBATT (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4) From 72eda2d7c9e7df7937c250a0d33fbba55311a949 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Dec 2015 13:08:06 +0000 Subject: [PATCH 042/201] IOv2: Add DSM controls --- src/drivers/boards/px4io-v2/board_config.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h index 5c6f08827d..0d572552c2 100644 --- a/src/drivers/boards/px4io-v2/board_config.h +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -89,6 +89,11 @@ #define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) #define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s)) + +#define SPEKTRUM_RX_HIGH(_s) stm32_gpiowrite(GPIO_USART1_RX_SPEKTRUM, (_s)) +#define SPEKTRUM_RX_AS_UART() stm32_configgpio(GPIO_USART1_RX) +#define SPEKTRUM_RX_AS_GPIO() stm32_configgpio(GPIO_USART1_RX_SPEKTRUM) + #define GPIO_SERVO_FAULT_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN15) /* Analog inputs **************************************************************/ From 87add556205726dd0696a8be853895d625708340 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Dec 2015 13:08:23 +0000 Subject: [PATCH 043/201] DSM: Generalize parsing --- src/lib/rc/dsm.c | 313 ++++++++++++++++++++++++++++++++++------------- src/lib/rc/dsm.h | 22 +++- 2 files changed, 244 insertions(+), 91 deletions(-) diff --git a/src/lib/rc/dsm.c b/src/lib/rc/dsm.c index 19e41e0ab2..8747a45f4e 100644 --- a/src/lib/rc/dsm.c +++ b/src/lib/rc/dsm.c @@ -50,28 +50,32 @@ #include "dsm.h" #include -#ifndef GPIO_SPEKTRUM_PWR_EN -#warning DSM input driver not supporting binding by this board config - #define POWER_SPEKTRUM(arg) -#endif - #if defined (__PX4_LINUX) || defined (__PX4_DARWIN) - #define up_udelay(arg) + #define dsm_udelay(arg) usleep(arg) +#else + #include + #define dsm_udelay(arg) up_udelay(arg) #endif -#define DSM_FRAME_SIZE 16 /** 1000000) && (dsm_channel_shift != 0)) { + if (((frame_time - dsm_last_frame_time) > 1000000) && (dsm_channel_shift != 0)) { dsm_guess_format(true); } - /* we have received something we think is a dsm_frame */ - dsm_last_frame_time = frame_time; - /* if we don't know the dsm_frame format, update the guessing state machine */ if (dsm_channel_shift == 0) { - dsm_guess_format(false); - return false; + if (!dsm_guess_format(false)) { + return false; + } } /* @@ -377,6 +402,12 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, unsig continue; } + /* reset bit guessing state machine if the channel index is out of bounds */ + if (channel > DSM_MAX_CHANNEL_COUNT) { + dsm_guess_format(true); + return false; + } + /* ignore channels out of range */ if (channel >= max_values) { continue; @@ -408,6 +439,15 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, unsig /* scaled integer for decent accuracy while staying efficient */ value = ((((int)value - 1024) * 1000) / 1700) + 1500; + /* if the value is unrealistic, fail the parsing entirely */ + if (value < 600 || value > 2400) { +#ifdef DSM_DEBUG + printf("DSM: VALUE RANGE FAIL\n"); +#endif + dsm_chan_count = 0; + return false; + } + /* * Store the decoded channel into the R/C input buffer, taking into * account the different ideas about channel assignement that we have. @@ -444,14 +484,20 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, unsig *num_values = 12; } - if (dsm_channel_shift == 11) { - /* Set the 11-bit data indicator */ - *num_values |= 0x8000; - } + /* Set the 11-bit data indicator */ + *dsm_11_bit = (dsm_channel_shift == 11); + + /* we have received something we think is a dsm_frame */ + dsm_last_frame_time = frame_time; /* * XXX Note that we may be in failsafe here; we need to work out how to detect that. */ + +#ifdef DSM_DEBUG + printf("PARSED PACKET\n"); +#endif + return true; } @@ -477,55 +523,148 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, unsig * @return true=decoded raw channel values updated, false=no update */ bool -dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes, unsigned max_values) +dsm_input(int dsm_fd, uint16_t *values, uint16_t *num_values, bool *dsm_11_bit, uint8_t *n_bytes, uint8_t **bytes, + unsigned max_values) { - ssize_t ret; - hrt_abstime now; + int ret = 1; + hrt_abstime now; - /* - */ - now = hrt_absolute_time(); + /* + * The S.BUS protocol doesn't provide reliable framing, + * so we detect frame boundaries by the inter-frame delay. + * + * The minimum frame spacing is 7ms; with 25 bytes at 100000bps + * frame transmission time is ~2ms. + * + * We expect to only be called when bytes arrive for processing, + * and if an interval of more than 3ms passes between calls, + * the first byte we read will be the first byte of a frame. + * + * In the case where byte(s) are dropped from a frame, this also + * provides a degree of protection. Of course, it would be better + * if we didn't drop bytes... + */ + now = hrt_absolute_time(); - if ((now - dsm_last_rx_time) > 5000) { - if (dsm_partial_frame_count > 0) { - dsm_frame_drops++; - dsm_partial_frame_count = 0; - } - } + /* + * Fetch bytes, but no more than we would need to complete + * a complete frame. + */ - /* - * Fetch bytes, but no more than we would need to complete - * the current dsm frame. - */ - ret = read(dsm_fd, &dsm_frame[dsm_partial_frame_count], DSM_FRAME_SIZE - dsm_partial_frame_count); + ret = read(dsm_fd, &dsm_buf[0], sizeof(dsm_buf) / sizeof(dsm_buf[0])); - /* if the read failed for any reason, just give up here */ - if (ret < 1) { - return false; + /* if the read failed for any reason, just give up here */ + if (ret < 1) { + return false; - } else { - *n_bytes = ret; - *bytes = &dsm_frame[dsm_partial_frame_count]; - } + } else { + *n_bytes = ret; + *bytes = &dsm_buf[0]; + } - dsm_last_rx_time = now; - - /* - * Add bytes to the current dsm frame - */ - dsm_partial_frame_count += ret; - - /* - * If we don't have a full dsm frame, return - */ - if (dsm_partial_frame_count < DSM_FRAME_SIZE) { - return false; - } - - /* - * Great, it looks like we might have a dsm frame. Go ahead and - * decode it. - */ - dsm_partial_frame_count = 0; - return dsm_decode(now, values, num_values, max_values); + /* + * Try to decode something with what we got + */ + return dsm_parse(now, &dsm_buf[0], ret, values, num_values, dsm_11_bit, &dsm_frame_drops, max_values); +} + +bool +dsm_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values, + uint16_t *num_values, bool *dsm_11_bit, unsigned *frame_drops, uint16_t max_channels) +{ + + /* this is set by the decoding state machine and will default to false + * once everything that was decodable has been decoded. + */ + bool decode_ret = false; + + /* keep decoding until we have consumed the buffer */ + for (unsigned d = 0; d < len; d++) { + + /* overflow check */ + if (dsm_partial_frame_count == sizeof(dsm_frame) / sizeof(dsm_frame[0])) { + dsm_partial_frame_count = 0; + dsm_decode_state = DSM_DECODE_STATE_DESYNC; +#ifdef DSM_DEBUG + printf("DSM: RESET (BUF LIM)\n"); +#endif + } + + if (dsm_partial_frame_count == DSM_FRAME_SIZE) { + dsm_partial_frame_count = 0; + dsm_decode_state = DSM_DECODE_STATE_DESYNC; +#ifdef DSM_DEBUG + printf("DSM: RESET (PACKET LIM)\n"); +#endif + } + +#ifdef DSM_DEBUG +#if 1 + printf("dsm state: %s%s, count: %d, val: %02x\n", + (dsm_decode_state == DSM_DECODE_STATE_DESYNC) ? "DSM_DECODE_STATE_DESYNC" : "", + (dsm_decode_state == DSM_DECODE_STATE_SYNC) ? "DSM_DECODE_STATE_SYNC" : "", + dsm_partial_frame_count, + (unsigned)frame[d]); +#endif +#endif + + switch (dsm_decode_state) { + case DSM_DECODE_STATE_DESYNC: + + /* we are de-synced and only interested in the frame marker */ + if ((now - dsm_last_rx_time) > 5000) { + dsm_decode_state = DSM_DECODE_STATE_SYNC; + dsm_partial_frame_count = 0; + dsm_chan_count = 0; + dsm_frame[dsm_partial_frame_count++] = frame[d]; + } + + break; + + case DSM_DECODE_STATE_SYNC: { + dsm_frame[dsm_partial_frame_count++] = frame[d]; + + /* decode whatever we got and expect */ + if (dsm_partial_frame_count < DSM_FRAME_SIZE) { + break; + } + + /* + * Great, it looks like we might have a frame. Go ahead and + * decode it. + */ + decode_ret = dsm_decode(now, values, &dsm_chan_count, dsm_11_bit, max_channels); + + /* we consumed the partial frame, reset */ + dsm_partial_frame_count = 0; + + /* if decoding failed, set proto to desync */ + if (decode_ret == false) { + dsm_decode_state = DSM_DECODE_STATE_DESYNC; + } + } + break; + + default: +#ifdef DSM_DEBUG + printf("UNKNOWN PROTO STATE"); +#endif + decode_ret = false; + } + + + } + + if (frame_drops) { + *frame_drops = dsm_frame_drops; + } + + if (decode_ret) { + *num_values = dsm_chan_count; + } + + dsm_last_rx_time = now; + + /* return false as default */ + return decode_ret; } diff --git a/src/lib/rc/dsm.h b/src/lib/rc/dsm.h index acef60705d..f75fda9251 100644 --- a/src/lib/rc/dsm.h +++ b/src/lib/rc/dsm.h @@ -42,20 +42,34 @@ #pragma once #include +#include +#include +#include __BEGIN_DECLS +#define DSM_FRAME_SIZE 16 /**< DSM frame size in bytes */ +#define DSM_FRAME_CHANNELS 7 /**< Max supported DSM channels per frame */ +#define DSM_MAX_CHANNEL_COUNT 18 /**< Max channel count of any DSM RC */ +#define DSM_BUFFER_SIZE (DSM_FRAME_SIZE + DSM_FRAME_SIZE / 2) + __EXPORT int dsm_init(const char *device); __EXPORT int dsm_config(int dsm_fd); -__EXPORT bool dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes, unsigned max_values); -__EXPORT void dsm_bind(uint16_t cmd, int pulses); +__EXPORT bool dsm_input(int dsm_fd, uint16_t *values, uint16_t *num_values, bool *dsm_11_bit, uint8_t *n_bytes, uint8_t **bytes, unsigned max_values); -enum { /* DSM bind states */ +__EXPORT bool dsm_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values, + uint16_t *num_values, bool *dsm_11_bit, unsigned *frame_drops, uint16_t max_channels); + +#ifdef GPIO_SPEKTRUM_PWR_EN +__EXPORT void dsm_bind(uint16_t cmd, int pulses); +#endif + +enum DSM_CMD { /* DSM bind states */ DSM_CMD_BIND_POWER_DOWN = 0, DSM_CMD_BIND_POWER_UP, DSM_CMD_BIND_SET_RX_OUT, DSM_CMD_BIND_SEND_PULSES, DSM_CMD_BIND_REINIT_UART -} DSM_CMD; +}; __END_DECLS From 1871a616761e8d0e0aaf4c79958c2abecdcd28d2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Dec 2015 13:08:51 +0000 Subject: [PATCH 044/201] S.BUS: Fix compile error for single wire IOCTL --- src/lib/rc/sbus.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/lib/rc/sbus.c b/src/lib/rc/sbus.c index de2d9277bf..c34b7b1f2d 100644 --- a/src/lib/rc/sbus.c +++ b/src/lib/rc/sbus.c @@ -44,6 +44,10 @@ #include #include +#ifdef TIOCSSINGLEWIRE +#include +#endif + #include "sbus.h" #include @@ -149,7 +153,7 @@ sbus_config(int sbus_fd, bool singlewire) if (singlewire) { /* only defined in configs capable of IOCTL */ #ifdef TIOCSSINGLEWIRE - ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); + ioctl(sbus_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); #endif } From 7005b18a66d1b75193403f6f658b57d2190efabb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Dec 2015 13:09:15 +0000 Subject: [PATCH 045/201] IO: Cleanup DSM input routine --- src/modules/px4iofirmware/controls.c | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index f7321a966d..068e54653d 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -73,16 +73,15 @@ static unsigned _rssi_adc_counts = 0; bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated) { - perf_begin(c_gather_dsm); - uint16_t temp_count = r_raw_rc_count; + perf_begin(c_gather_dsm); uint8_t n_bytes = 0; uint8_t *bytes; - *dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes, PX4IO_RC_INPUT_CHANNELS); + bool dsm_11_bit; + *dsm_updated = dsm_input(_dsm_fd, r_raw_rc_values, &r_raw_rc_count, &dsm_11_bit, &n_bytes, &bytes, PX4IO_RC_INPUT_CHANNELS); - if (*dsm_updated) { - r_raw_rc_count = temp_count & 0x7fff; + if (*dsm_updated) { - if (temp_count & 0x8000) { + if (dsm_11_bit) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11; } else { From 13606173ee68f4dfc7847dccc7d55d3635d4902b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Dec 2015 13:09:34 +0000 Subject: [PATCH 046/201] Write DSM unit test --- unittests/dsm_test.cpp | 25 ++++++++++++------------- 1 file changed, 12 insertions(+), 13 deletions(-) diff --git a/unittests/dsm_test.cpp b/unittests/dsm_test.cpp index d6b6faa021..7bd6558139 100644 --- a/unittests/dsm_test.cpp +++ b/unittests/dsm_test.cpp @@ -36,10 +36,8 @@ TEST(DSMTest, DSM) uint8_t frame[20]; uint16_t rc_values[18]; uint16_t num_values; - unsigned sbus_frame_drops = 0; - unsigned sbus_frame_resets = 0; - bool sbus_failsafe; - bool sbus_frame_drop; + bool dsm_11_bit; + unsigned dsm_frame_drops = 0; uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]); int rate_limiter = 0; @@ -53,19 +51,20 @@ TEST(DSMTest, DSM) unsigned len = 1; // Pipe the data into the parser - hrt_abstime now = hrt_absolute_time(); - - // if (rate_limiter % byte_offset == 0) { - bool result = false;//sbus_parse(now, &frame[0], len, rc_values, &num_values, - // &sbus_failsafe, &sbus_frame_drop, &sbus_frame_drops, max_channels); + bool result = dsm_parse(f*1e6, &frame[0], len, rc_values, &num_values, + &dsm_11_bit, &dsm_frame_drops, max_channels); if (result) { - //warnx("decoded packet"); + warnx("decoded packet with %d channels and %s encoding:", num_values, (dsm_11_bit) ? "11 bit" : "10 bit"); + + for (unsigned i = 0; i < num_values; i++) { + printf("chan #%u:\t%d\n", i, (int)rc_values[i]); + } } - if (last_drop != (sbus_frame_drops + sbus_frame_resets)) { - warnx("frame dropped, now #%d", (sbus_frame_drops + sbus_frame_resets)); - last_drop = sbus_frame_drops + sbus_frame_resets; + if (last_drop != (dsm_frame_drops)) { + warnx("frame dropped, now #%d", (dsm_frame_drops)); + last_drop = dsm_frame_drops; } rate_limiter++; From 5b4b5de34c43f2ef3db1c4f2d497459a7e775fc9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Dec 2015 13:09:56 +0000 Subject: [PATCH 047/201] Disable DSM debug output --- src/lib/rc/dsm.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/rc/dsm.c b/src/lib/rc/dsm.c index 8747a45f4e..37a3b7a7ba 100644 --- a/src/lib/rc/dsm.c +++ b/src/lib/rc/dsm.c @@ -57,7 +57,7 @@ #define dsm_udelay(arg) up_udelay(arg) #endif -#define DSM_DEBUG +//#define DSM_DEBUG static enum DSM_DECODE_STATE { DSM_DECODE_STATE_DESYNC = 0, From d84fcfffaf0264fef9ea585386870d6941114295 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Dec 2015 16:10:14 +0000 Subject: [PATCH 048/201] DSM decoder: Guard against invalid values --- src/lib/rc/dsm.c | 22 +++++++++++++--------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/src/lib/rc/dsm.c b/src/lib/rc/dsm.c index 37a3b7a7ba..3271c148e5 100644 --- a/src/lib/rc/dsm.c +++ b/src/lib/rc/dsm.c @@ -439,15 +439,6 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool /* scaled integer for decent accuracy while staying efficient */ value = ((((int)value - 1024) * 1000) / 1700) + 1500; - /* if the value is unrealistic, fail the parsing entirely */ - if (value < 600 || value > 2400) { -#ifdef DSM_DEBUG - printf("DSM: VALUE RANGE FAIL\n"); -#endif - dsm_chan_count = 0; - return false; - } - /* * Store the decoded channel into the R/C input buffer, taking into * account the different ideas about channel assignement that we have. @@ -498,6 +489,18 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool printf("PARSED PACKET\n"); #endif + /* check all values */ + for (unsigned i = 0; i < *num_values; i++) { + /* if the value is unrealistic, fail the parsing entirely */ + if (values[i] < 600 || values[i] > 2400) { +#ifdef DSM_DEBUG + printf("DSM: VALUE RANGE FAIL\n"); +#endif + dsm_chan_count = 0; + return false; + } + } + return true; } @@ -641,6 +644,7 @@ dsm_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values, /* if decoding failed, set proto to desync */ if (decode_ret == false) { dsm_decode_state = DSM_DECODE_STATE_DESYNC; + dsm_frame_drops++; } } break; From 5a4472b41881130c28bd68b6e6dac9d7c4c6246d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Dec 2015 16:10:43 +0000 Subject: [PATCH 049/201] DSM: Destroy test data to trigger decoding fails and test recovery --- unittests/testdata/dsm_x_data.txt | 13 ------------- 1 file changed, 13 deletions(-) diff --git a/unittests/testdata/dsm_x_data.txt b/unittests/testdata/dsm_x_data.txt index bdf28d92bf..2a4cf782bc 100644 --- a/unittests/testdata/dsm_x_data.txt +++ b/unittests/testdata/dsm_x_data.txt @@ -946,14 +946,12 @@ Time [s],Value,Parity Error,Framing Error 9.909878000000001,0x00,, 9.909964000000000,0x00,, 9.910052000000000,0x0C,, -9.910138000000000,0x00,, 9.910226000000000,0x2E,, 9.910314000000000,0xAA,, 9.910399999999999,0x14,, 9.910488000000001,0x00,, 9.910574000000000,0x1C,, 9.910662000000000,0x00,, -9.910750000000000,0x26,, 9.910836000000000,0xAA,, 9.910924000000000,0x44,, 9.911009999999999,0x00,, @@ -975,8 +973,6 @@ Time [s],Value,Parity Error,Framing Error 9.922012000000000,0x00,, 9.922100000000000,0x36,, 9.922186000000000,0xAA,, -9.931880000000000,0x00,, -9.931965999999999,0x00,, 9.932054000000001,0x0C,, 9.932140000000000,0x00,, 9.932228000000000,0x2E,, @@ -1008,15 +1004,6 @@ Time [s],Value,Parity Error,Framing Error 9.944110000000000,0x36,, 9.944196000000000,0xAA,, 9.953882000000000,0x00,, -9.953968000000000,0x00,, -9.954056000000000,0x0C,, -9.954141999999999,0x00,, -9.954230000000001,0x2E,, -9.954318000000001,0xAA,, -9.954404000000000,0x14,, -9.954492000000000,0x00,, -9.954578000000000,0x1C,, -9.954666000000000,0x00,, 9.954751999999999,0x26,, 9.954840000000001,0xAA,, 9.954928000000001,0x44,, From 2fb604e9514d91f8358123c1d3f37cf5cbd094c1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 17 Dec 2015 11:32:42 +0000 Subject: [PATCH 050/201] Formatted FMUv4 board config --- src/drivers/boards/px4fmu-v4/board_config.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index 4131666546..93f96277ce 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -238,7 +238,7 @@ __BEGIN_DECLS #define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s)) #define GPIO_USART1_RX_SPEKTRUM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN7) #define SPEKTRUM_RX_HIGH(_s) stm32_gpiowrite(GPIO_USART1_RX_SPEKTRUM, (_s)) -#define SPEKTRUM_RX_AS_UART() stm32_configgpio(GPIO_USART1_RX) +#define SPEKTRUM_RX_AS_UART() stm32_configgpio(GPIO_USART1_RX) #define SPEKTRUM_RX_AS_GPIO() stm32_configgpio(GPIO_USART1_RX_SPEKTRUM) /**************************************************************************************************** From 40f9ed05078134eb9a33ab7a41c68621fe9e8eac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 17 Dec 2015 11:32:59 +0000 Subject: [PATCH 051/201] Formatted IOv1 board config --- src/drivers/boards/px4io-v1/board_config.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/boards/px4io-v1/board_config.h b/src/drivers/boards/px4io-v1/board_config.h index 30cfa9b660..7456f716f0 100644 --- a/src/drivers/boards/px4io-v1/board_config.h +++ b/src/drivers/boards/px4io-v1/board_config.h @@ -86,7 +86,7 @@ #define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s)) #define SPEKTRUM_RX_HIGH(_s) stm32_gpiowrite(GPIO_USART1_RX_SPEKTRUM, (_s)) -#define SPEKTRUM_RX_AS_UART() stm32_configgpio(GPIO_USART1_RX) +#define SPEKTRUM_RX_AS_UART() stm32_configgpio(GPIO_USART1_RX) #define SPEKTRUM_RX_AS_GPIO() stm32_configgpio(GPIO_USART1_RX_SPEKTRUM) /* Analog inputs ********************************************************************/ From 7ed3fbb1bcf9170b374421761fbe5a34406cb60d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 17 Dec 2015 11:33:17 +0000 Subject: [PATCH 052/201] Formatted IOv2 board config --- src/drivers/boards/px4io-v2/board_config.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h index 0d572552c2..c54cf7b6f6 100644 --- a/src/drivers/boards/px4io-v2/board_config.h +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -91,7 +91,7 @@ #define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s)) #define SPEKTRUM_RX_HIGH(_s) stm32_gpiowrite(GPIO_USART1_RX_SPEKTRUM, (_s)) -#define SPEKTRUM_RX_AS_UART() stm32_configgpio(GPIO_USART1_RX) +#define SPEKTRUM_RX_AS_UART() stm32_configgpio(GPIO_USART1_RX) #define SPEKTRUM_RX_AS_GPIO() stm32_configgpio(GPIO_USART1_RX_SPEKTRUM) #define GPIO_SERVO_FAULT_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN15) From 6e3749f904a329f4866e106e6d23ff298c6a732f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 17 Dec 2015 11:34:15 +0000 Subject: [PATCH 053/201] Code style for DSM decoder --- src/lib/rc/dsm.c | 310 +++++++++++++++++++++++------------------------ src/lib/rc/dsm.h | 5 +- 2 files changed, 158 insertions(+), 157 deletions(-) diff --git a/src/lib/rc/dsm.c b/src/lib/rc/dsm.c index 3271c148e5..22aeeddb63 100644 --- a/src/lib/rc/dsm.c +++ b/src/lib/rc/dsm.c @@ -51,17 +51,17 @@ #include #if defined (__PX4_LINUX) || defined (__PX4_DARWIN) - #define dsm_udelay(arg) usleep(arg) +#define dsm_udelay(arg) usleep(arg) #else - #include - #define dsm_udelay(arg) up_udelay(arg) +#include +#define dsm_udelay(arg) up_udelay(arg) #endif //#define DSM_DEBUG static enum DSM_DECODE_STATE { - DSM_DECODE_STATE_DESYNC = 0, - DSM_DECODE_STATE_SYNC + DSM_DECODE_STATE_DESYNC = 0, + DSM_DECODE_STATE_SYNC } dsm_decode_state = DSM_DECODE_STATE_DESYNC; static int dsm_fd = -1; /**< File handle to the DSM UART */ @@ -128,9 +128,9 @@ dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *va static bool dsm_guess_format(bool reset) { - static uint32_t cs10 = 0; - static uint32_t cs11 = 0; - static unsigned samples = 0; + static uint32_t cs10 = 0; + static uint32_t cs11 = 0; + static unsigned samples = 0; /* reset the 10/11 bit sniffed channel masks */ if (reset) { @@ -138,7 +138,7 @@ dsm_guess_format(bool reset) cs11 = 0; samples = 0; dsm_channel_shift = 0; - return false; + return false; } /* scan the channels in the current dsm_frame in both 10- and 11-bit mode */ @@ -160,16 +160,16 @@ dsm_guess_format(bool reset) /* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-dsm_frame format */ } - samples++; + samples++; #ifdef DSM_DEBUG - printf("dsm guess format: samples: %d %s\n", samples, - (reset) ? "RESET" : ""); + printf("dsm guess format: samples: %d %s\n", samples, + (reset) ? "RESET" : ""); #endif /* wait until we have seen plenty of frames - 5 should normally be enough */ - if (samples < 5) { - return false; + if (samples < 5) { + return false; } /* @@ -209,25 +209,25 @@ dsm_guess_format(bool reset) if ((votes11 == 1) && (votes10 == 0)) { dsm_channel_shift = 11; #ifdef DSM_DEBUG - printf("DSM: 11-bit format\n"); + printf("DSM: 11-bit format\n"); #endif - return true; + return true; } if ((votes10 == 1) && (votes11 == 0)) { dsm_channel_shift = 10; #ifdef DSM_DEBUG - printf("DSM: 10-bit format\n"); + printf("DSM: 10-bit format\n"); #endif - return true; + return true; } /* call ourselves to reset our state ... we have to try again */ #ifdef DSM_DEBUG - printf("DSM: format detect fail, 10: 0x%08x %d 11: 0x%08x %d\n", cs10, votes10, cs11, votes11); + printf("DSM: format detect fail, 10: 0x%08x %d 11: 0x%08x %d\n", cs10, votes10, cs11, votes11); #endif dsm_guess_format(true); - return false; + return false; } int @@ -278,10 +278,10 @@ dsm_init(const char *device) dsm_fd = open(device, O_RDONLY | O_NONBLOCK); } - dsm_channel_shift = 0; - dsm_frame_drops = 0; - dsm_chan_count = 0; - dsm_decode_state = DSM_DECODE_STATE_DESYNC; + dsm_channel_shift = 0; + dsm_frame_drops = 0; + dsm_chan_count = 0; + dsm_decode_state = DSM_DECODE_STATE_DESYNC; int ret = dsm_config(dsm_fd); @@ -325,17 +325,17 @@ dsm_bind(uint16_t cmd, int pulses) case DSM_CMD_BIND_SET_RX_OUT: /*Set UART RX pin to active output mode*/ - SPEKTRUM_RX_AS_GPIO(); + SPEKTRUM_RX_AS_GPIO(); break; case DSM_CMD_BIND_SEND_PULSES: /*Pulse RX pin a number of times*/ for (int i = 0; i < pulses; i++) { - dsm_udelay(120); - SPEKTRUM_RX_HIGH(false); - dsm_udelay(120); - SPEKTRUM_RX_HIGH(true); + dsm_udelay(120); + SPEKTRUM_RX_HIGH(false); + dsm_udelay(120); + SPEKTRUM_RX_HIGH(true); } break; @@ -343,7 +343,7 @@ dsm_bind(uint16_t cmd, int pulses) case DSM_CMD_BIND_REINIT_UART: /*Restore USART RX pin to RS232 receive mode*/ - SPEKTRUM_RX_AS_UART(); + SPEKTRUM_RX_AS_UART(); break; } @@ -370,15 +370,15 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool * If we have lost signal for at least a second, reset the * format guessing heuristic. */ - if (((frame_time - dsm_last_frame_time) > 1000000) && (dsm_channel_shift != 0)) { + if (((frame_time - dsm_last_frame_time) > 1000000) && (dsm_channel_shift != 0)) { dsm_guess_format(true); } /* if we don't know the dsm_frame format, update the guessing state machine */ if (dsm_channel_shift == 0) { - if (!dsm_guess_format(false)) { - return false; - } + if (!dsm_guess_format(false)) { + return false; + } } /* @@ -402,11 +402,11 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool continue; } - /* reset bit guessing state machine if the channel index is out of bounds */ - if (channel > DSM_MAX_CHANNEL_COUNT) { - dsm_guess_format(true); - return false; - } + /* reset bit guessing state machine if the channel index is out of bounds */ + if (channel > DSM_MAX_CHANNEL_COUNT) { + dsm_guess_format(true); + return false; + } /* ignore channels out of range */ if (channel >= max_values) { @@ -475,31 +475,31 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *num_values = 12; } - /* Set the 11-bit data indicator */ - *dsm_11_bit = (dsm_channel_shift == 11); + /* Set the 11-bit data indicator */ + *dsm_11_bit = (dsm_channel_shift == 11); - /* we have received something we think is a dsm_frame */ - dsm_last_frame_time = frame_time; + /* we have received something we think is a dsm_frame */ + dsm_last_frame_time = frame_time; /* * XXX Note that we may be in failsafe here; we need to work out how to detect that. */ #ifdef DSM_DEBUG - printf("PARSED PACKET\n"); + printf("PARSED PACKET\n"); #endif - /* check all values */ - for (unsigned i = 0; i < *num_values; i++) { - /* if the value is unrealistic, fail the parsing entirely */ - if (values[i] < 600 || values[i] > 2400) { + /* check all values */ + for (unsigned i = 0; i < *num_values; i++) { + /* if the value is unrealistic, fail the parsing entirely */ + if (values[i] < 600 || values[i] > 2400) { #ifdef DSM_DEBUG - printf("DSM: VALUE RANGE FAIL\n"); + printf("DSM: VALUE RANGE FAIL\n"); #endif - dsm_chan_count = 0; - return false; - } - } + dsm_chan_count = 0; + return false; + } + } return true; } @@ -527,148 +527,148 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool */ bool dsm_input(int dsm_fd, uint16_t *values, uint16_t *num_values, bool *dsm_11_bit, uint8_t *n_bytes, uint8_t **bytes, - unsigned max_values) + unsigned max_values) { - int ret = 1; - hrt_abstime now; + int ret = 1; + hrt_abstime now; - /* - * The S.BUS protocol doesn't provide reliable framing, - * so we detect frame boundaries by the inter-frame delay. - * - * The minimum frame spacing is 7ms; with 25 bytes at 100000bps - * frame transmission time is ~2ms. - * - * We expect to only be called when bytes arrive for processing, - * and if an interval of more than 3ms passes between calls, - * the first byte we read will be the first byte of a frame. - * - * In the case where byte(s) are dropped from a frame, this also - * provides a degree of protection. Of course, it would be better - * if we didn't drop bytes... - */ - now = hrt_absolute_time(); + /* + * The S.BUS protocol doesn't provide reliable framing, + * so we detect frame boundaries by the inter-frame delay. + * + * The minimum frame spacing is 7ms; with 25 bytes at 100000bps + * frame transmission time is ~2ms. + * + * We expect to only be called when bytes arrive for processing, + * and if an interval of more than 3ms passes between calls, + * the first byte we read will be the first byte of a frame. + * + * In the case where byte(s) are dropped from a frame, this also + * provides a degree of protection. Of course, it would be better + * if we didn't drop bytes... + */ + now = hrt_absolute_time(); - /* - * Fetch bytes, but no more than we would need to complete - * a complete frame. - */ + /* + * Fetch bytes, but no more than we would need to complete + * a complete frame. + */ - ret = read(dsm_fd, &dsm_buf[0], sizeof(dsm_buf) / sizeof(dsm_buf[0])); + ret = read(dsm_fd, &dsm_buf[0], sizeof(dsm_buf) / sizeof(dsm_buf[0])); - /* if the read failed for any reason, just give up here */ - if (ret < 1) { - return false; + /* if the read failed for any reason, just give up here */ + if (ret < 1) { + return false; - } else { - *n_bytes = ret; - *bytes = &dsm_buf[0]; - } + } else { + *n_bytes = ret; + *bytes = &dsm_buf[0]; + } - /* - * Try to decode something with what we got - */ - return dsm_parse(now, &dsm_buf[0], ret, values, num_values, dsm_11_bit, &dsm_frame_drops, max_values); + /* + * Try to decode something with what we got + */ + return dsm_parse(now, &dsm_buf[0], ret, values, num_values, dsm_11_bit, &dsm_frame_drops, max_values); } bool dsm_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values, - uint16_t *num_values, bool *dsm_11_bit, unsigned *frame_drops, uint16_t max_channels) + uint16_t *num_values, bool *dsm_11_bit, unsigned *frame_drops, uint16_t max_channels) { - /* this is set by the decoding state machine and will default to false - * once everything that was decodable has been decoded. - */ - bool decode_ret = false; + /* this is set by the decoding state machine and will default to false + * once everything that was decodable has been decoded. + */ + bool decode_ret = false; - /* keep decoding until we have consumed the buffer */ - for (unsigned d = 0; d < len; d++) { + /* keep decoding until we have consumed the buffer */ + for (unsigned d = 0; d < len; d++) { - /* overflow check */ - if (dsm_partial_frame_count == sizeof(dsm_frame) / sizeof(dsm_frame[0])) { - dsm_partial_frame_count = 0; - dsm_decode_state = DSM_DECODE_STATE_DESYNC; + /* overflow check */ + if (dsm_partial_frame_count == sizeof(dsm_frame) / sizeof(dsm_frame[0])) { + dsm_partial_frame_count = 0; + dsm_decode_state = DSM_DECODE_STATE_DESYNC; #ifdef DSM_DEBUG - printf("DSM: RESET (BUF LIM)\n"); + printf("DSM: RESET (BUF LIM)\n"); #endif - } + } - if (dsm_partial_frame_count == DSM_FRAME_SIZE) { - dsm_partial_frame_count = 0; - dsm_decode_state = DSM_DECODE_STATE_DESYNC; + if (dsm_partial_frame_count == DSM_FRAME_SIZE) { + dsm_partial_frame_count = 0; + dsm_decode_state = DSM_DECODE_STATE_DESYNC; #ifdef DSM_DEBUG - printf("DSM: RESET (PACKET LIM)\n"); + printf("DSM: RESET (PACKET LIM)\n"); #endif - } + } #ifdef DSM_DEBUG #if 1 - printf("dsm state: %s%s, count: %d, val: %02x\n", - (dsm_decode_state == DSM_DECODE_STATE_DESYNC) ? "DSM_DECODE_STATE_DESYNC" : "", - (dsm_decode_state == DSM_DECODE_STATE_SYNC) ? "DSM_DECODE_STATE_SYNC" : "", - dsm_partial_frame_count, - (unsigned)frame[d]); + printf("dsm state: %s%s, count: %d, val: %02x\n", + (dsm_decode_state == DSM_DECODE_STATE_DESYNC) ? "DSM_DECODE_STATE_DESYNC" : "", + (dsm_decode_state == DSM_DECODE_STATE_SYNC) ? "DSM_DECODE_STATE_SYNC" : "", + dsm_partial_frame_count, + (unsigned)frame[d]); #endif #endif - switch (dsm_decode_state) { - case DSM_DECODE_STATE_DESYNC: + switch (dsm_decode_state) { + case DSM_DECODE_STATE_DESYNC: - /* we are de-synced and only interested in the frame marker */ - if ((now - dsm_last_rx_time) > 5000) { - dsm_decode_state = DSM_DECODE_STATE_SYNC; - dsm_partial_frame_count = 0; - dsm_chan_count = 0; - dsm_frame[dsm_partial_frame_count++] = frame[d]; - } + /* we are de-synced and only interested in the frame marker */ + if ((now - dsm_last_rx_time) > 5000) { + dsm_decode_state = DSM_DECODE_STATE_SYNC; + dsm_partial_frame_count = 0; + dsm_chan_count = 0; + dsm_frame[dsm_partial_frame_count++] = frame[d]; + } - break; + break; - case DSM_DECODE_STATE_SYNC: { - dsm_frame[dsm_partial_frame_count++] = frame[d]; + case DSM_DECODE_STATE_SYNC: { + dsm_frame[dsm_partial_frame_count++] = frame[d]; - /* decode whatever we got and expect */ - if (dsm_partial_frame_count < DSM_FRAME_SIZE) { - break; - } + /* decode whatever we got and expect */ + if (dsm_partial_frame_count < DSM_FRAME_SIZE) { + break; + } - /* - * Great, it looks like we might have a frame. Go ahead and - * decode it. - */ - decode_ret = dsm_decode(now, values, &dsm_chan_count, dsm_11_bit, max_channels); + /* + * Great, it looks like we might have a frame. Go ahead and + * decode it. + */ + decode_ret = dsm_decode(now, values, &dsm_chan_count, dsm_11_bit, max_channels); - /* we consumed the partial frame, reset */ - dsm_partial_frame_count = 0; + /* we consumed the partial frame, reset */ + dsm_partial_frame_count = 0; - /* if decoding failed, set proto to desync */ - if (decode_ret == false) { - dsm_decode_state = DSM_DECODE_STATE_DESYNC; - dsm_frame_drops++; - } - } - break; + /* if decoding failed, set proto to desync */ + if (decode_ret == false) { + dsm_decode_state = DSM_DECODE_STATE_DESYNC; + dsm_frame_drops++; + } + } + break; - default: + default: #ifdef DSM_DEBUG - printf("UNKNOWN PROTO STATE"); + printf("UNKNOWN PROTO STATE"); #endif - decode_ret = false; - } + decode_ret = false; + } - } + } - if (frame_drops) { - *frame_drops = dsm_frame_drops; - } + if (frame_drops) { + *frame_drops = dsm_frame_drops; + } - if (decode_ret) { - *num_values = dsm_chan_count; - } + if (decode_ret) { + *num_values = dsm_chan_count; + } - dsm_last_rx_time = now; + dsm_last_rx_time = now; - /* return false as default */ - return decode_ret; + /* return false as default */ + return decode_ret; } diff --git a/src/lib/rc/dsm.h b/src/lib/rc/dsm.h index f75fda9251..669b319017 100644 --- a/src/lib/rc/dsm.h +++ b/src/lib/rc/dsm.h @@ -55,10 +55,11 @@ __BEGIN_DECLS __EXPORT int dsm_init(const char *device); __EXPORT int dsm_config(int dsm_fd); -__EXPORT bool dsm_input(int dsm_fd, uint16_t *values, uint16_t *num_values, bool *dsm_11_bit, uint8_t *n_bytes, uint8_t **bytes, unsigned max_values); +__EXPORT bool dsm_input(int dsm_fd, uint16_t *values, uint16_t *num_values, bool *dsm_11_bit, uint8_t *n_bytes, + uint8_t **bytes, unsigned max_values); __EXPORT bool dsm_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values, - uint16_t *num_values, bool *dsm_11_bit, unsigned *frame_drops, uint16_t max_channels); + uint16_t *num_values, bool *dsm_11_bit, unsigned *frame_drops, uint16_t max_channels); #ifdef GPIO_SPEKTRUM_PWR_EN __EXPORT void dsm_bind(uint16_t cmd, int pulses); From 74543b2cd602866958f388207bf8dd7f140c0e61 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 17 Dec 2015 11:34:41 +0000 Subject: [PATCH 054/201] S.BUS code style --- src/lib/rc/sbus.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/rc/sbus.c b/src/lib/rc/sbus.c index c34b7b1f2d..fe72bfb959 100644 --- a/src/lib/rc/sbus.c +++ b/src/lib/rc/sbus.c @@ -153,7 +153,7 @@ sbus_config(int sbus_fd, bool singlewire) if (singlewire) { /* only defined in configs capable of IOCTL */ #ifdef TIOCSSINGLEWIRE - ioctl(sbus_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); + ioctl(sbus_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); #endif } From 49f7df572473f22767f21486452d7596fd56e430 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 17 Dec 2015 11:34:57 +0000 Subject: [PATCH 055/201] IO firmware decode style --- src/modules/px4iofirmware/controls.c | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 068e54653d..b857aa41fa 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -73,15 +73,16 @@ static unsigned _rssi_adc_counts = 0; bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated) { - perf_begin(c_gather_dsm); + perf_begin(c_gather_dsm); uint8_t n_bytes = 0; uint8_t *bytes; - bool dsm_11_bit; - *dsm_updated = dsm_input(_dsm_fd, r_raw_rc_values, &r_raw_rc_count, &dsm_11_bit, &n_bytes, &bytes, PX4IO_RC_INPUT_CHANNELS); + bool dsm_11_bit; + *dsm_updated = dsm_input(_dsm_fd, r_raw_rc_values, &r_raw_rc_count, &dsm_11_bit, &n_bytes, &bytes, + PX4IO_RC_INPUT_CHANNELS); - if (*dsm_updated) { + if (*dsm_updated) { - if (dsm_11_bit) { + if (dsm_11_bit) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11; } else { From 0af727192e332e1961135f8f861d560000870d36 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 17 Dec 2015 12:58:16 +0000 Subject: [PATCH 056/201] FMUv1 board config: Enable DSM binding --- src/drivers/boards/px4io-v1/board_config.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/boards/px4io-v1/board_config.h b/src/drivers/boards/px4io-v1/board_config.h index 7456f716f0..6475d717d8 100644 --- a/src/drivers/boards/px4io-v1/board_config.h +++ b/src/drivers/boards/px4io-v1/board_config.h @@ -83,6 +83,7 @@ #define GPIO_RELAY1_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN12) #define GPIO_RELAY2_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN11) +#define GPIO_SPEKTRUM_PWR_EN GPIO_RELAY1_EN #define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s)) #define SPEKTRUM_RX_HIGH(_s) stm32_gpiowrite(GPIO_USART1_RX_SPEKTRUM, (_s)) From 45e4def3eb0daebd7af43adb85a7d48a37c02f70 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Dec 2015 11:48:40 +0100 Subject: [PATCH 057/201] FMUv4: Rename Spektrum power correctly --- src/drivers/boards/px4fmu-v4/board_config.h | 2 +- src/drivers/boards/px4fmu-v4/px4fmu_init.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index 93f96277ce..d57396a8fd 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -65,7 +65,7 @@ __BEGIN_DECLS //{0, GPIO_PERIPH_3V3_EN, 0}, Owned by the 8266 driver //{0, GPIO_SBUS_INV, 0}, https://github.com/PX4/Firmware/blob/master/src/modules/px4iofirmware/sbus.c //{GPIO_8266_GPIO0, 0, 0}, Owned by the 8266 driver -//{0, GPIO_SPEKTRUM_POWER, 0}, Owned Spektum driver input to auto pilot +//{0, GPIO_SPEKTRUM_PWR_EN, 0}, Owned Spektum driver input to auto pilot //{0, GPIO_8266_PD, 0}, Owned by the 8266 driver //{0, GPIO_8266_RST, 0}, Owned by the 8266 driver diff --git a/src/drivers/boards/px4fmu-v4/px4fmu_init.c b/src/drivers/boards/px4fmu-v4/px4fmu_init.c index 7966f57165..d093e47bf4 100644 --- a/src/drivers/boards/px4fmu-v4/px4fmu_init.c +++ b/src/drivers/boards/px4fmu-v4/px4fmu_init.c @@ -230,7 +230,7 @@ __EXPORT int nsh_archinitialize(void) stm32_configgpio(GPIO_SBUS_INV); stm32_configgpio(GPIO_8266_GPIO0); - stm32_configgpio(GPIO_SPEKTRUM_POWER); + stm32_configgpio(GPIO_SPEKTRUM_PWR_EN); stm32_configgpio(GPIO_8266_PD); stm32_configgpio(GPIO_8266_RST); From f36669723d4fc29aca32e78abd52e85429937978 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sat, 28 Nov 2015 17:44:37 -0700 Subject: [PATCH 058/201] add rotation for stryker vtol --- src/lib/conversion/rotation.cpp | 5 +++++ src/lib/conversion/rotation.h | 4 +++- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index f56513e209..16bf874303 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -233,5 +233,10 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z) y = -y; return; } + case ROTATION_PITCH_90_ROLL_90: { + tmp = x; x = y; + y = -z; z = -tmp; + return; + } } } diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index 280b9cad0c..ed8e4fda38 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -77,6 +77,7 @@ enum Rotation { ROTATION_ROLL_270_YAW_270 = 26, ROTATION_ROLL_180_PITCH_270 = 27, ROTATION_PITCH_90_YAW_180 = 28, + ROTATION_PITCH_90_ROLL_90 = 29, ROTATION_MAX }; @@ -115,7 +116,8 @@ const rot_lookup_t rot_lookup[] = { { 0, 270, 0 }, {270, 0, 270 }, {180, 270, 0 }, - { 0, 90, 180 } + { 0, 90, 180 }, + { 90, 90, 0 } }; /** From 4e5fdebb13ee7b2b1f4639183bfb296ed2e8379b Mon Sep 17 00:00:00 2001 From: Roman Date: Fri, 11 Dec 2015 10:11:39 +0100 Subject: [PATCH 059/201] added tailsitter recovery library --- src/lib/tailsitter_recovery/CMakeLists.txt | 42 ++++ .../tailsitter_recovery.cpp | 182 ++++++++++++++++++ .../tailsitter_recovery/tailsitter_recovery.h | 74 +++++++ 3 files changed, 298 insertions(+) create mode 100644 src/lib/tailsitter_recovery/CMakeLists.txt create mode 100644 src/lib/tailsitter_recovery/tailsitter_recovery.cpp create mode 100644 src/lib/tailsitter_recovery/tailsitter_recovery.h diff --git a/src/lib/tailsitter_recovery/CMakeLists.txt b/src/lib/tailsitter_recovery/CMakeLists.txt new file mode 100644 index 0000000000..c7bd713592 --- /dev/null +++ b/src/lib/tailsitter_recovery/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. 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 PX4 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. +# +############################################################################ +px4_add_module( + MODULE lib__tailsitter_recovery + COMPILE_FLAGS + -Os + SRCS + tailsitter_recovery.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/lib/tailsitter_recovery/tailsitter_recovery.cpp b/src/lib/tailsitter_recovery/tailsitter_recovery.cpp new file mode 100644 index 0000000000..f18081b6f5 --- /dev/null +++ b/src/lib/tailsitter_recovery/tailsitter_recovery.cpp @@ -0,0 +1,182 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. 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 PX4 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 tailsitter_recovery.cpp +*/ + +#include "tailsitter_recovery.h" +#include + +TailsitterRecovery::TailsitterRecovery(): + _in_recovery_mode(false) +{ + _att_p(0) = _att_p(1) = _att_p(2) = 4.0f; +} + +TailsitterRecovery::~TailsitterRecovery() +{ + +} + +void TailsitterRecovery::setAttGains(math::Vector<3> &att_p, float yaw_ff) +{ + _att_p = att_p; + _yaw_ff = yaw_ff; +} + +void TailsitterRecovery::calcOptimalRates(math::Quaternion &q, math::Quaternion &q_sp, float yaw_move_rate, + math::Vector<3> &rates_opt) +{ + math::Matrix<3, 3> R = q.to_dcm(); + + // compute error quaternion + math::Quaternion q_sp_inv = {q_sp(0), -q_sp(1), -q_sp(2), -q_sp(3)}; + math::Quaternion q_error = q * q_sp_inv; + + // compute tilt angle and corresponding tilt axis + math::Vector<3> zB = {0, 0, -1.0f}; + + math::Vector<3> zI = q_error.conjugate(zB); + + float tilt_angle; + float inner_prod = zI * zB; + + if (inner_prod >= 1.0f) { + tilt_angle = 0.0f; + + } else if (inner_prod <= -1.0f) { + tilt_angle = M_PI_F; + + } else { + tilt_angle = acosf(inner_prod); + } + + math::Vector<3> tilt_axis = {0, 0, -1}; + + if (math::min(fabsf(tilt_angle), fabsf(tilt_angle - M_PI_F)) > 0.00001f) { + tilt_axis = zI % zB; + } + + tilt_axis = R.transposed() * tilt_axis; + + // compute desired rates based on tilt angle and tilt axis + float tilt_dir = atan2f(tilt_axis(0), tilt_axis(1)); + float sign_x; + float sign_y; + float sign_z; + + if (tilt_dir < -M_PI_2_F) { + tilt_dir += M_PI_F; + sign_x = -1; + sign_y = -1; + sign_z = 1; + + } else if (tilt_dir < 0.0f) { + tilt_dir = 0.0f - tilt_dir; + sign_x = -1; + sign_y = 1; + sign_z = -1; + + } else if (tilt_dir < M_PI_2_F) { + tilt_dir += 0; + sign_x = 1; + sign_y = 1; + sign_z = 1; + + } else { + tilt_dir = M_PI_F - tilt_dir; + sign_x = 1; + sign_y = -1; + sign_z = -1; + } + + // optimal coefficients + float pwx_kx1 = -1.676f; + float pwx_ky1 = 1.38f; + float pwx_ky2 = 0.8725f; + float pwx_sx0 = 0.3586f; + float pwx_sx1 = 2.642f; + + float pwy_kx1 = -3.997f; + float pwy_sx0 = 2.133f; + float pwy_sx1 = 4.013f; + + float pwz_kx1 = 2.726f; + float pwz_ky1 = 3.168f; + float pwz_ky2 = -0.3913f; + float pwz_sx0 = 1.75f; + float pwz_sx1 = 2.298f; + + float x = tilt_angle; + float y = tilt_dir; + + rates_opt(1) = (pwx_ky1 * (M_PI_2_F - y) + pwx_ky2 * powf(M_PI_2_F - y, 2)) * + (pwx_kx1 * x * SigmoidFunction(pwx_sx1 * (pwx_sx0 - x)) - pwx_kx1 * x * expf(pwx_sx1 * + (pwx_sx0 - M_PI_F)) * SigmoidFunction(pwx_sx1 * (M_PI_F - pwx_sx0))); + rates_opt(0) = pwy_kx1 * x + powf((M_PI_2_F - y) / (M_PI_2_F), + 2) * (-pwy_kx1 * x * SigmoidFunction(pwy_sx1 * (pwy_sx0 - x))); + rates_opt(2) = (pwz_ky1 * (M_PI_2_F - y) + pwz_ky2 * powf(M_PI_2_F - y, 2)) * + (pwz_kx1 * (x - M_PI_F) * SigmoidFunction(pwz_sx1 * (x - pwz_sx0)) - pwz_kx1 * (x - M_PI_F) * SigmoidFunction( + -pwz_sx0 * pwz_sx1)); + rates_opt(0) *= -sign_x; + rates_opt(1) *= -sign_y * 1.5f; + rates_opt(2) *= -sign_z; + + float yaw_w = R(2, 2) > 0.0f ? R(2, 2) : 0.0f; + yaw_w = math::constrain(yaw_w, 0.0f, 1.0f); + + // we activate recovery mode if our tilt error is more that 30 degrees or + if (!_in_recovery_mode) { + if (fabsf(tilt_angle) > math::radians(30.0f)) { + _in_recovery_mode = true; + } + + } else { + if (fabsf(tilt_angle) < math::radians(5.0f)) { + _in_recovery_mode = false; + } + } + + // do normal attitude control, the other case has already been handled + if (!_in_recovery_mode) { + q_error = q_sp_inv * q; + rates_opt = q_error(0) > 0.0f ? _att_p.emult(q_error.imag()) * (-2.0f) : _att_p.emult(q_error.imag()) * (2.0f); + // don't want too strong yaw control. after recovery vehicle might have + // very large yaw error and shouldn't turn too fast + rates_opt(2) = math::constrain(rates_opt(2), -1.0f, 1.0f); + } + + rates_opt(2) += yaw_move_rate * yaw_w * _yaw_ff; + +} diff --git a/src/lib/tailsitter_recovery/tailsitter_recovery.h b/src/lib/tailsitter_recovery/tailsitter_recovery.h new file mode 100644 index 0000000000..5de16d25e8 --- /dev/null +++ b/src/lib/tailsitter_recovery/tailsitter_recovery.h @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. 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 PX4 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 tailsitter_recovery.h + * + * Tailsitter optimal rate controller (underactuated pitch axis) + * + * This function can be used to compute desired body rates of a tailsitter + * suffering from an underactuated pitch axis. Tailsitters which produce a pitching moment + * from airflow over control surfaces mostly suffer from an underactuated pitch axis. + * This functions captures the solution of an optimal control problem which minimises + * the vehicle's tilt error and which penalises the desired rates of the underactuated + * pitch axis. + * Publication: + * Robin Ritz and Raffaello D'Andrea. A Global Strategy for Tailsitter Hover Control. + * + * @author Roman Bapst +*/ +#include + +#define SigmoidFunction(val) 1/(1 + expf(-val)) + +class TailsitterRecovery +{ +public: + TailsitterRecovery(); + ~TailsitterRecovery(); + + // Calculate the optimal rates: + // If the vehicle is not in need of a recovery, this function will do normal + // attitude control based on attitude error. If a recovery situation is detected + // then the rates are computed in an optimal way as described above. + void calcOptimalRates(math::Quaternion &q, math::Quaternion &q_sp, float yaw_move_rate, math::Vector<3> &rates_opt); + + // Set the gains of the controller attitude loop. + void setAttGains(math::Vector<3> &att_p, float yaw_ff); + +private: + bool _in_recovery_mode; // indicates that the tailsitter is performing a recovery to hover + + math::Vector<3> _att_p; // gains for attitude loop + float _yaw_ff; // yaw feed forward gain +}; From ac4e95df058e481bce20ed377f2daeee77343ce6 Mon Sep 17 00:00:00 2001 From: Roman Date: Fri, 11 Dec 2015 10:57:41 +0100 Subject: [PATCH 060/201] use optimal recovery strategy for tailsitters --- cmake/configs/nuttx_px4fmu-v1_default.cmake | 1 + cmake/configs/nuttx_px4fmu-v2_default.cmake | 1 + cmake/configs/posix_sitl_default.cmake | 1 + .../mc_att_control/mc_att_control_main.cpp | 108 ++++++++++++------ 4 files changed, 78 insertions(+), 33 deletions(-) diff --git a/cmake/configs/nuttx_px4fmu-v1_default.cmake b/cmake/configs/nuttx_px4fmu-v1_default.cmake index 902447c282..ef4540094a 100644 --- a/cmake/configs/nuttx_px4fmu-v1_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v1_default.cmake @@ -113,6 +113,7 @@ set(config_module_list lib/launchdetection lib/terrain_estimation lib/runway_takeoff + lib/tailsitter_recovery platforms/nuttx # had to add for cmake, not sure why wasn't in original config diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake index 0523f8b67c..1d6498ac59 100644 --- a/cmake/configs/nuttx_px4fmu-v2_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake @@ -124,6 +124,7 @@ set(config_module_list lib/launchdetection lib/terrain_estimation lib/runway_takeoff + lib/tailsitter_recovery platforms/nuttx # had to add for cmake, not sure why wasn't in original config diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake index 4f4be42ab2..66eca5205d 100644 --- a/cmake/configs/posix_sitl_default.cmake +++ b/cmake/configs/posix_sitl_default.cmake @@ -59,6 +59,7 @@ set(config_module_list lib/launchdetection lib/terrain_estimation lib/runway_takeoff + lib/tailsitter_recovery examples/px4_simple_app ) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 477e364a5b..3764c95a39 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -88,6 +88,7 @@ #include #include #include +#include /** * Multicopter attitude control app start / stop handling function @@ -194,6 +195,8 @@ private: param_t acro_yaw_max; param_t rattitude_thres; + param_t vtol_type; + } _params_handles; /**< handles for interesting parameters */ struct { @@ -211,8 +214,11 @@ private: math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */ float rattitude_thres; + int vtol_type; /**< 0 = Tailsitter, 1 = Tiltrotor, 2 = Standard airframe */ } _params; + TailsitterRecovery *_ts_opt_recovery; /**< Computes optimal rates for tailsitter recovery */ + /** * Update our local parameter cache. */ @@ -296,7 +302,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _task_should_exit(false), _control_task(-1), -/* subscriptions */ + /* subscriptions */ _ctrl_state_sub(-1), _v_att_sp_sub(-1), _v_control_mode_sub(-1), @@ -305,7 +311,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _armed_sub(-1), _vehicle_status_sub(-1), -/* publications */ + /* publications */ _v_rates_sp_pub(nullptr), _actuators_0_pub(nullptr), _controller_status_pub(nullptr), @@ -314,9 +320,10 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _actuators_0_circuit_breaker_enabled(false), -/* performance counters */ + /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")), - _controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")) + _controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")), + _ts_opt_recovery(nullptr) { memset(&_ctrl_state, 0, sizeof(_ctrl_state)); @@ -328,7 +335,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : memset(&_armed, 0, sizeof(_armed)); memset(&_vehicle_status, 0, sizeof(_vehicle_status)); memset(&_motor_limits, 0, sizeof(_motor_limits)); - memset(&_controller_status,0,sizeof(_controller_status)); + memset(&_controller_status, 0, sizeof(_controller_status)); _vehicle_status.is_rotary_wing = true; _params.att_p.zero(); @@ -376,9 +383,18 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.acro_pitch_max = param_find("MC_ACRO_P_MAX"); _params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX"); _params_handles.rattitude_thres = param_find("MC_RATT_TH"); + _params_handles.vtol_type = param_find("VT_TYPE"); /* fetch initial parameter values */ parameters_update(); + vehicle_status_poll(); + + if (_vehicle_status.is_vtol && _params.vtol_type == 0) { + // the vehicle is a tailsitter, use optimal recovery control strategy + _ts_opt_recovery = new TailsitterRecovery(); + } + + } MulticopterAttitudeControl::~MulticopterAttitudeControl() @@ -402,6 +418,7 @@ MulticopterAttitudeControl::~MulticopterAttitudeControl() } while (_control_task != -1); } + delete _ts_opt_recovery; mc_att_control::g_control = nullptr; } @@ -467,6 +484,8 @@ MulticopterAttitudeControl::parameters_update() /* stick deflection needed in rattitude mode to control rates not angles */ param_get(_params_handles.rattitude_thres, &_params.rattitude_thres); + param_get(_params_handles.vtol_type, &_params.vtol_type); + _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY); return OK; @@ -558,11 +577,13 @@ MulticopterAttitudeControl::vehicle_status_poll() if (vehicle_status_updated) { orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + /* set correct uORB ID, depending on if vehicle is VTOL or not */ if (!_rates_sp_id) { if (_vehicle_status.is_vtol) { _rates_sp_id = ORB_ID(mc_virtual_rates_setpoint); _actuators_id = ORB_ID(actuator_controls_virtual_mc); + } else { _rates_sp_id = ORB_ID(vehicle_rates_setpoint); _actuators_id = ORB_ID(actuator_controls_0); @@ -699,12 +720,13 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) /* angular rates error */ math::Vector<3> rates_err = _rates_sp - rates; - _att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int + _params.rate_ff.emult(_rates_sp - _rates_sp_prev) / dt; + _att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int + + _params.rate_ff.emult(_rates_sp - _rates_sp_prev) / dt; _rates_sp_prev = _rates_sp; _rates_prev = rates; /* update integral only if not saturated on low limit and if motor commands are not saturated */ - if (_thrust_sp > MIN_TAKEOFF_THRUST && !_motor_limits.lower_limit && !_motor_limits.upper_limit ) { + if (_thrust_sp > MIN_TAKEOFF_THRUST && !_motor_limits.lower_limit && !_motor_limits.upper_limit) { for (int i = 0; i < 3; i++) { if (fabsf(_att_control(i)) < _thrust_sp) { float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; @@ -756,8 +778,9 @@ MulticopterAttitudeControl::task_main() int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit */ - if (pret == 0) + if (pret == 0) { continue; + } /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { @@ -795,38 +818,56 @@ MulticopterAttitudeControl::task_main() vehicle_motor_limits_poll(); /* Check if we are in rattitude mode and the pilot is above the threshold on pitch - * or roll (yaw can rotate 360 in normal att control). If both are true don't + * or roll (yaw can rotate 360 in normal att control). If both are true don't * even bother running the attitude controllers */ - if(_vehicle_status.main_state == vehicle_status_s::MAIN_STATE_RATTITUDE){ + if (_vehicle_status.main_state == vehicle_status_s::MAIN_STATE_RATTITUDE) { if (fabsf(_manual_control_sp.y) > _params.rattitude_thres || - fabsf(_manual_control_sp.x) > _params.rattitude_thres){ - _v_control_mode.flag_control_attitude_enabled = false; - } + fabsf(_manual_control_sp.x) > _params.rattitude_thres) { + _v_control_mode.flag_control_attitude_enabled = false; + } } if (_v_control_mode.flag_control_attitude_enabled) { - control_attitude(dt); + if (_ts_opt_recovery == nullptr) { + // the tailsitter recovery instance has not been created, thus, the vehicle + // is not a tailsitter, do normal attitude control + control_attitude(dt); - /* publish attitude rates setpoint */ - _v_rates_sp.roll = _rates_sp(0); - _v_rates_sp.pitch = _rates_sp(1); - _v_rates_sp.yaw = _rates_sp(2); - _v_rates_sp.thrust = _thrust_sp; - _v_rates_sp.timestamp = hrt_absolute_time(); + } else { + math::Quaternion q(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]); + math::Quaternion q_sp(&_v_att_sp.q_d[0]); + _ts_opt_recovery->setAttGains(_params.att_p, _params.yaw_ff); + _ts_opt_recovery->calcOptimalRates(q, q_sp, _v_att_sp.yaw_sp_move_rate, _rates_sp); - if (_v_rates_sp_pub != nullptr) { - orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); - - } else if (_rates_sp_id) { - _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); + /* limit rates */ + for (int i = 0; i < 3; i++) { + _rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i)); } + } + + /* publish attitude rates setpoint */ + _v_rates_sp.roll = _rates_sp(0); + _v_rates_sp.pitch = _rates_sp(1); + _v_rates_sp.yaw = _rates_sp(2); + _v_rates_sp.thrust = _thrust_sp; + _v_rates_sp.timestamp = hrt_absolute_time(); + + if (_v_rates_sp_pub != nullptr) { + orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); + + } else if (_rates_sp_id) { + _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); + } + //} + } else { /* attitude controller disabled, poll rates setpoint topic */ if (_v_control_mode.flag_control_manual_enabled) { /* manual rates control - ACRO mode */ - _rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max); + _rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, + _manual_control_sp.r).emult(_params.acro_rate_max); _thrust_sp = math::min(_manual_control_sp.z, MANUAL_THROTTLE_MAX_MULTICOPTER); /* publish attitude rates setpoint */ @@ -881,8 +922,9 @@ MulticopterAttitudeControl::task_main() } /* publish controller status */ - if(_controller_status_pub != nullptr) { - orb_publish(ORB_ID(mc_att_ctrl_status),_controller_status_pub, &_controller_status); + if (_controller_status_pub != nullptr) { + orb_publish(ORB_ID(mc_att_ctrl_status), _controller_status_pub, &_controller_status); + } else { _controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status); } @@ -903,11 +945,11 @@ MulticopterAttitudeControl::start() /* start the task */ _control_task = px4_task_spawn_cmd("mc_att_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 1500, - (px4_main_t)&MulticopterAttitudeControl::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 1500, + (px4_main_t)&MulticopterAttitudeControl::task_main_trampoline, + nullptr); if (_control_task < 0) { warn("task start failed"); From 7c3a67e3747e67defe05fe9ab92b607fef37ba3e Mon Sep 17 00:00:00 2001 From: Roman Date: Fri, 11 Dec 2015 10:57:41 +0100 Subject: [PATCH 061/201] use optimal recovery strategy for tailsitters --- cmake/configs/nuttx_px4fmu-v4_default.cmake | 1 + cmake/configs/posix_eagle_default.cmake | 1 + cmake/configs/qurt_eagle_hil.cmake | 1 + cmake/configs/qurt_eagle_release.cmake | 1 + cmake/configs/qurt_eagle_travis.cmake | 1 + src/modules/mc_att_control/mc_att_control_main.cpp | 5 +++-- 6 files changed, 8 insertions(+), 2 deletions(-) diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake index 9d22c71bbf..084f81d9da 100644 --- a/cmake/configs/nuttx_px4fmu-v4_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake @@ -122,6 +122,7 @@ set(config_module_list lib/launchdetection lib/terrain_estimation lib/runway_takeoff + lib/tailsitter_recovery platforms/nuttx # had to add for cmake, not sure why wasn't in original config diff --git a/cmake/configs/posix_eagle_default.cmake b/cmake/configs/posix_eagle_default.cmake index 76d9711187..b78d393ddf 100644 --- a/cmake/configs/posix_eagle_default.cmake +++ b/cmake/configs/posix_eagle_default.cmake @@ -40,6 +40,7 @@ set(config_module_list lib/conversion lib/terrain_estimation lib/runway_takeoff + lib/tailsitter_recovery platforms/common platforms/posix/px4_layer diff --git a/cmake/configs/qurt_eagle_hil.cmake b/cmake/configs/qurt_eagle_hil.cmake index 87dd29152e..0f2e92b0c0 100644 --- a/cmake/configs/qurt_eagle_hil.cmake +++ b/cmake/configs/qurt_eagle_hil.cmake @@ -49,6 +49,7 @@ set(config_module_list lib/conversion lib/terrain_estimation lib/runway_takeoff + lib/tailsitter_recovery modules/controllib # diff --git a/cmake/configs/qurt_eagle_release.cmake b/cmake/configs/qurt_eagle_release.cmake index 62a20263e5..67d84c8fa7 100644 --- a/cmake/configs/qurt_eagle_release.cmake +++ b/cmake/configs/qurt_eagle_release.cmake @@ -72,6 +72,7 @@ set(config_module_list lib/conversion lib/terrain_estimation lib/runway_takeoff + lib/tailsitter_recovery # # QuRT port diff --git a/cmake/configs/qurt_eagle_travis.cmake b/cmake/configs/qurt_eagle_travis.cmake index 75cb5f2394..85fae3373b 100644 --- a/cmake/configs/qurt_eagle_travis.cmake +++ b/cmake/configs/qurt_eagle_travis.cmake @@ -54,6 +54,7 @@ set(config_module_list lib/ecl lib/terrain_estimation lib/runway_takeoff + lib/tailsitter_recovery # # QuRT port diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 3764c95a39..91ce68b481 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -387,9 +387,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : /* fetch initial parameter values */ parameters_update(); - vehicle_status_poll(); - if (_vehicle_status.is_vtol && _params.vtol_type == 0) { + if (_params.vtol_type == 0) { // the vehicle is a tailsitter, use optimal recovery control strategy _ts_opt_recovery = new TailsitterRecovery(); } @@ -835,6 +834,8 @@ MulticopterAttitudeControl::task_main() control_attitude(dt); } else { + vehicle_attitude_setpoint_poll(); + _thrust_sp = _v_att_sp.thrust; math::Quaternion q(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]); math::Quaternion q_sp(&_v_att_sp.q_d[0]); _ts_opt_recovery->setAttGains(_params.att_p, _params.yaw_ff); From 9d130653e19b56d224d16e8b7c119cfddeacdec3 Mon Sep 17 00:00:00 2001 From: Roman Date: Sat, 12 Dec 2015 10:30:31 +0100 Subject: [PATCH 062/201] vtol: added timestamps for actuator controls message --- src/modules/vtol_att_control/standard.cpp | 2 ++ src/modules/vtol_att_control/tailsitter.cpp | 6 ++++++ src/modules/vtol_att_control/tiltrotor.cpp | 2 ++ 3 files changed, 10 insertions(+) diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index c5a015eb4d..4f0c87bdbc 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -282,6 +282,7 @@ void Standard::update_external_state() void Standard::fill_actuator_outputs() { /* multirotor controls */ + _actuators_out_0->timestamp = _actuators_mc_in->timestamp; _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight; // roll _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = @@ -292,6 +293,7 @@ void Standard::fill_actuator_outputs() _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight; // throttle /* fixed wing controls */ + _actuators_out_1->timestamp = _actuators_fw_in->timestamp; _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] * (1 - _mc_roll_weight); //roll _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index e7daa2c3a3..4089be5b83 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -446,6 +446,7 @@ void Tailsitter::fill_actuator_outputs() { switch (_vtol_mode) { case ROTARY_WING: + _actuators_out_0->timestamp = _actuators_mc_in->timestamp; _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]; _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; @@ -453,6 +454,8 @@ void Tailsitter::fill_actuator_outputs() _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; + _actuators_out_1->timestamp = _actuators_mc_in->timestamp; + if (_params->elevons_mc_lock == 1) { _actuators_out_1->control[0] = 0; _actuators_out_1->control[1] = 0; @@ -469,6 +472,7 @@ void Tailsitter::fill_actuator_outputs() case FIXED_WING: // in fixed wing mode we use engines only for providing thrust, no moments are generated + _actuators_out_0->timestamp = _actuators_fw_in->timestamp; _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = 0; _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = 0; _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = 0; @@ -487,6 +491,8 @@ void Tailsitter::fill_actuator_outputs() case TRANSITION: // in transition engines are mixed by weight (BACK TRANSITION ONLY) + _actuators_out_0->timestamp = _actuators_mc_in->timestamp; + _actuators_out_1->timestamp = _actuators_mc_in->timestamp; _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight; _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 3890a71843..17b3af75df 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -384,6 +384,7 @@ void Tiltrotor::update_external_state() */ void Tiltrotor::fill_actuator_outputs() { + _actuators_out_0->timestamp = _actuators_mc_in->timestamp; _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight; _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = @@ -400,6 +401,7 @@ void Tiltrotor::fill_actuator_outputs() _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];; } + _actuators_out_1->timestamp = _actuators_fw_in->timestamp; _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] * (1 - _mc_roll_weight); _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = From 8515326176fdcf0afe4b5fcb328539d800421952 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 24 Dec 2015 10:19:21 +0100 Subject: [PATCH 063/201] Fixed rotation code style --- src/lib/conversion/rotation.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index 16bf874303..e70d4628cf 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -233,6 +233,7 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z) y = -y; return; } + case ROTATION_PITCH_90_ROLL_90: { tmp = x; x = y; y = -z; z = -tmp; From 36a556d10726a096dbad22ee14edfbc739382779 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 8 Nov 2015 10:29:07 +0100 Subject: [PATCH 064/201] Modify uORB API to allow cleaner in-app use --- src/modules/uORB/uORB.cpp | 38 ++++++++++++++++++++++++++++++++++++++ src/modules/uORB/uORB.h | 23 +++++++++++++++++++++++ 2 files changed, 61 insertions(+) diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index 6d6f084d24..3d959b03dd 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -96,6 +96,44 @@ orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *da return uORB::Manager::get_instance()->orb_advertise_multi(meta, data, instance, priority); } +/** + * Advertise as the publisher of a topic. + * + * This performs the initial advertisement of a topic; it creates the topic + * node in /obj if required and publishes the initial data. + * + * Any number of advertisers may publish to a topic; publications are atomic + * but co-ordination between publishers is not provided by the ORB. + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @param data A pointer to the initial data to be published. + * For topics updated by interrupt handlers, the advertisement + * must be performed from non-interrupt context. + * @param instance Pointer to an integer which will yield the instance ID (0-based) + * of the publication. + * @param priority The priority of the instance. If a subscriber subscribes multiple + * instances, the priority allows the subscriber to prioritize the best + * data source as long as its available. + * @return ERROR on error, zero on success. + * If the topic in question is not known (due to an + * ORB_DEFINE with no corresponding ORB_DECLARE) + * this function will return -1 and set errno to ENOENT. + */ +int orb_publish_auto(const struct orb_metadata *meta, orb_advert_t *handle, const void *data, int *instance, + int priority) +{ + if (*handle == nullptr) { + *handle = orb_advertise_multi(meta, data, instance, priority); + + if (handle != nullptr) { + return 0; + } + + } else { + return orb_publish(meta, handle, data); + } +} /** * Publish new data to a topic. diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index cfcf39b408..2f4f7af0c9 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -181,6 +181,29 @@ extern orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *d extern orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority) __EXPORT; +/** + * Advertise and publish as the publisher of a topic. + * + * This performs the initial advertisement of a topic; it creates the topic + * node in /obj if required and publishes the initial data. + * + * Any number of advertisers may publish to a topic; publications are atomic + * but co-ordination between publishers is not provided by the ORB. + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @param data A pointer to the initial data to be published. + * For topics updated by interrupt handlers, the advertisement + * must be performed from non-interrupt context. + * @param instance Pointer to an integer which will yield the instance ID (0-based, + * limited by ORB_MULTI_MAX_INSTANCES) of the publication. + * @param priority The priority of the instance. If a subscriber subscribes multiple + * instances, the priority allows the subscriber to prioritize the best + * data source as long as its available. + * @return zero on success, error number on failure + */ +extern int orb_publish_auto(const struct orb_metadata *meta, orb_advert_t *handle, const void *data, int *instance, + int priority) /** * Publish new data to a topic. From fd01d565fec1caf012eaad9a57d74eeb207ee1df Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 8 Nov 2015 11:12:30 +0100 Subject: [PATCH 065/201] Fix code style / typos in uORB --- src/modules/uORB/uORB.cpp | 2 ++ src/modules/uORB/uORB.h | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index 3d959b03dd..bbce4e9769 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -133,6 +133,8 @@ int orb_publish_auto(const struct orb_metadata *meta, orb_advert_t *handle, cons } else { return orb_publish(meta, handle, data); } + + return -1; } /** diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index 2f4f7af0c9..afab3cd813 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -203,7 +203,7 @@ extern orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const v * @return zero on success, error number on failure */ extern int orb_publish_auto(const struct orb_metadata *meta, orb_advert_t *handle, const void *data, int *instance, - int priority) + int priority); /** * Publish new data to a topic. From eb4a56294856366bf4892aa5eb667b7805a38d3c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 8 Nov 2015 11:13:03 +0100 Subject: [PATCH 066/201] Attitude estimator Q: Move to convenience publication call --- .../attitude_estimator_q_main.cpp | 26 +++++++------------ 1 file changed, 10 insertions(+), 16 deletions(-) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index 3e44579c66..f253362dae 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -552,7 +552,7 @@ void AttitudeEstimatorQ::task_main() _vel_prev_t = 0; } - // Time from previous iteration + /* time from previous iteration */ hrt_abstime now = hrt_absolute_time(); float dt = (last_time > 0) ? ((now - last_time) / 1000000.0f) : 0.00001f; last_time = now; @@ -595,18 +595,15 @@ void AttitudeEstimatorQ::task_main() att.accel_vibration = _voter_accel.get_vibration_factor(hrt_absolute_time()); att.mag_vibration = _voter_mag.get_vibration_factor(hrt_absolute_time()); - if (_att_pub == nullptr) { - _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); - - } else { - orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att); - } + /* the instance count is not used here */ + int att_inst; + orb_publish_auto(ORB_ID(vehicle_attitude), &_att_pub, &att, &att_inst, ORB_PRIO_HIGH); struct control_state_s ctrl_state = {}; ctrl_state.timestamp = sensors.timestamp; - /* Attitude quaternions for control state */ + /* attitude quaternions for control state */ ctrl_state.q[0] = _q(0); ctrl_state.q[1] = _q(1); @@ -615,7 +612,7 @@ void AttitudeEstimatorQ::task_main() ctrl_state.q[3] = _q(3); - /* Attitude rates for control state */ + /* attitude rates for control state */ ctrl_state.roll_rate = _lp_roll_rate.apply(_rates(0)); ctrl_state.pitch_rate = _lp_pitch_rate.apply(_rates(1)); @@ -632,13 +629,10 @@ void AttitudeEstimatorQ::task_main() ctrl_state.airspeed_valid = false; } - /* Publish to control state topic */ - if (_ctrl_state_pub == nullptr) { - _ctrl_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state); - - } else { - orb_publish(ORB_ID(control_state), _ctrl_state_pub, &ctrl_state); - } + /* the instance count is not used here */ + int ctrl_inst; + /* publish to control state topic */ + orb_publish_auto(ORB_ID(control_state), &_ctrl_state_pub, &ctrl_state, &ctrl_inst, ORB_PRIO_HIGH); } } From d9ee0792b807b50e58e5cc69b2ec97092bc528d8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 24 Dec 2015 10:51:08 +0100 Subject: [PATCH 067/201] Pixhawk: Manage flash space --- .travis.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 209bcc8795..710b38e881 100644 --- a/.travis.yml +++ b/.travis.yml @@ -104,10 +104,11 @@ script: - echo 'Building UAVCAN node firmware..' && git clone https://github.com/thiemar/vectorcontrol - cd vectorcontrol - BOARD=s2740vc_1_0 make && BOARD=px4esc_1_6 make - - ../Tools/uavcan_copy.sh - cd .. - echo 'Building NuttX px4fmu-v1 Firmware..' && make px4fmu-v1_default - echo 'Building NuttX px4fmu-v2 Firmware..' && make px4fmu-v2_default +# Only provide UAVCAN firmware binaries for Pixracer and Pixhawk 3 + - ../Tools/uavcan_copy.sh - echo 'Building NuttX px4fmu-v4 Firmware..' && make px4fmu-v4_default - echo 'Running Tests..' && make px4fmu-v2_default test From 5ef99d30acfb9357bc95bd87146582d00eb4ef8b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 24 Dec 2015 11:29:56 +0100 Subject: [PATCH 068/201] Fix Travis CI build order --- .travis.yml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/.travis.yml b/.travis.yml index 710b38e881..ccd0aafbcd 100644 --- a/.travis.yml +++ b/.travis.yml @@ -101,14 +101,14 @@ script: - echo 'Running Tests..' && make posix_sitl_default test - echo 'Running Unittests..' && cd unittests && ./run_tests.sh - cd .. - - echo 'Building UAVCAN node firmware..' && git clone https://github.com/thiemar/vectorcontrol - - cd vectorcontrol - - BOARD=s2740vc_1_0 make && BOARD=px4esc_1_6 make - - cd .. - echo 'Building NuttX px4fmu-v1 Firmware..' && make px4fmu-v1_default - echo 'Building NuttX px4fmu-v2 Firmware..' && make px4fmu-v2_default # Only provide UAVCAN firmware binaries for Pixracer and Pixhawk 3 + - echo 'Building UAVCAN node firmware..' && git clone https://github.com/thiemar/vectorcontrol + - cd vectorcontrol + - BOARD=s2740vc_1_0 make && BOARD=px4esc_1_6 make - ../Tools/uavcan_copy.sh + - cd .. - echo 'Building NuttX px4fmu-v4 Firmware..' && make px4fmu-v4_default - echo 'Running Tests..' && make px4fmu-v2_default test From 2b99d42d8df0490f32a933f8aeec6fb5673bbf3a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 24 Dec 2015 13:06:07 +0100 Subject: [PATCH 069/201] jMAVSim: Enable Mag --- Tools/jMAVSim | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/jMAVSim b/Tools/jMAVSim index 4267771753..5c6216014c 160000 --- a/Tools/jMAVSim +++ b/Tools/jMAVSim @@ -1 +1 @@ -Subproject commit 4267771753c4bb5d0696a678856620229523162e +Subproject commit 5c6216014cc092ea5e889988c654b38434a47736 From 623aeac18d8c167ca32e6df4c3f39e320f17efa9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 24 Dec 2015 13:13:10 +0100 Subject: [PATCH 070/201] Fix code style in MC params --- src/modules/mc_att_control/mc_att_control_params.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 0c7e4b0134..9e0532058f 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -271,13 +271,13 @@ PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 360.0f); /** * Threshold for Rattitude mode - * + * * Manual input needed in order to override attitude control rate setpoints * and instead pass manual stick inputs as rate setpoints - * - * @unit + * + * @unit * @min 0.0 * @max 1.0 * @group Multicopter Attitude Control */ - PARAM_DEFINE_FLOAT(MC_RATT_TH, 1.0f); +PARAM_DEFINE_FLOAT(MC_RATT_TH, 1.0f); From 490ef96471d280c53409b69d7309fe60269dab11 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 24 Dec 2015 13:30:30 +0100 Subject: [PATCH 071/201] Attitude estimator Q: Also fill Quaternion field --- src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index f253362dae..d54cdc208b 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -590,6 +590,8 @@ void AttitudeEstimatorQ::task_main() /* copy rotation matrix */ memcpy(&att.R[0], R.data, sizeof(att.R)); att.R_valid = true; + memcpy(&att.q[0], _q.data, sizeof(att.q)); + att.q_valid = true; att.rate_vibration = _voter_gyro.get_vibration_factor(hrt_absolute_time()); att.accel_vibration = _voter_accel.get_vibration_factor(hrt_absolute_time()); From 73216e0497034edd041fecc02bcdb3c8ac975280 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 24 Dec 2015 14:37:18 +0100 Subject: [PATCH 072/201] Fixed geofence verbosity --- src/modules/navigator/navigator_main.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index cd6021a971..7d4736449f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -275,9 +275,8 @@ Navigator::task_main() _geofence.loadFromFile(GEOFENCE_FILENAME); } else { - mavlink_log_info(_mavlink_fd, "No geofence set"); if (_geofence.clearDm() != OK) { - warnx("Could not clear geofence"); + mavlink_log_critical(_mavlink_fd, "failed clearing geofence"); } } From d839717e4cd8f574961df06229b7ed6f51a8ee6e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 24 Dec 2015 13:51:58 +0100 Subject: [PATCH 073/201] Update ECL to not print everything in EKF --- src/lib/ecl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/ecl b/src/lib/ecl index 457a57c6ff..eda69e727f 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit 457a57c6ff224a3bfd5fc304e75995db02e1952a +Subproject commit eda69e727fab22c81c421b635cb34303b90b7968 From 73394acc6834c66d266802aa788b21ec3b603e55 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 24 Dec 2015 15:46:00 +0100 Subject: [PATCH 074/201] Fix uORB auto publication --- src/modules/uORB/uORB.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index bbce4e9769..8d8572b3be 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -126,12 +126,12 @@ int orb_publish_auto(const struct orb_metadata *meta, orb_advert_t *handle, cons if (*handle == nullptr) { *handle = orb_advertise_multi(meta, data, instance, priority); - if (handle != nullptr) { + if (*handle != nullptr) { return 0; } } else { - return orb_publish(meta, handle, data); + return orb_publish(meta, *handle, data); } return -1; From 496e7a1799fbaee6e29db3f86e90bba03941ea6a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Dec 2015 00:33:09 +0100 Subject: [PATCH 075/201] Revert jMAVSIm change --- Tools/jMAVSim | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/jMAVSim b/Tools/jMAVSim index 5c6216014c..4267771753 160000 --- a/Tools/jMAVSim +++ b/Tools/jMAVSim @@ -1 +1 @@ -Subproject commit 5c6216014cc092ea5e889988c654b38434a47736 +Subproject commit 4267771753c4bb5d0696a678856620229523162e From 0ff72b16c58afd3e74e4f5dd33434a2065af6fac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Dec 2015 14:43:08 +0100 Subject: [PATCH 076/201] FMUv1: Reduce flash use --- cmake/configs/nuttx_px4fmu-v1_default.cmake | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/cmake/configs/nuttx_px4fmu-v1_default.cmake b/cmake/configs/nuttx_px4fmu-v1_default.cmake index ef4540094a..d93e8b8b28 100644 --- a/cmake/configs/nuttx_px4fmu-v1_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v1_default.cmake @@ -27,16 +27,16 @@ set(config_module_list drivers/trone drivers/gps drivers/pwm_out_sim - drivers/hott - drivers/hott/hott_telemetry - drivers/hott/hott_sensors + #drivers/hott + #drivers/hott/hott_telemetry + #drivers/hott/hott_sensors drivers/blinkm drivers/airspeed drivers/ets_airspeed drivers/meas_airspeed drivers/frsky_telemetry modules/sensors - drivers/mkblctrl + #drivers/mkblctrl drivers/px4flow # From 60599cb9e5ac27fd13c4bbc055957866358c65e8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Dec 2015 14:43:27 +0100 Subject: [PATCH 077/201] FMUv2: Reduce flash use --- cmake/configs/nuttx_px4fmu-v2_default.cmake | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake index 1d6498ac59..caf281a982 100644 --- a/cmake/configs/nuttx_px4fmu-v2_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake @@ -30,9 +30,9 @@ set(config_module_list drivers/trone drivers/gps drivers/pwm_out_sim - drivers/hott - drivers/hott/hott_telemetry - drivers/hott/hott_sensors + #drivers/hott + #drivers/hott/hott_telemetry + #drivers/hott/hott_sensors drivers/blinkm drivers/airspeed drivers/ets_airspeed @@ -45,7 +45,7 @@ set(config_module_list drivers/gimbal drivers/pwm_input drivers/camera_trigger - drivers/bst + drivers/bst # # System commands From 03251d756b397637a2e3b06cf0bf7788ab8b3afd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Dec 2015 14:43:48 +0100 Subject: [PATCH 078/201] FMUv4: Build all apps (because we can!) --- cmake/configs/nuttx_px4fmu-v4_default.cmake | 1 + 1 file changed, 1 insertion(+) diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake index 084f81d9da..8bb4005353 100644 --- a/cmake/configs/nuttx_px4fmu-v4_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake @@ -42,6 +42,7 @@ set(config_module_list drivers/gimbal drivers/pwm_input drivers/camera_trigger + drivers/bst # # System commands From bc6b640ceb2519ec2c0b592aafaf81c53c32605a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Dec 2015 16:09:24 +0100 Subject: [PATCH 079/201] FW: Fix min / max values for L1 norm --- src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 14b73038a0..d52722c1bf 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -51,8 +51,8 @@ * A value of 25 meters works for most aircraft. Shorten * slowly during tuning until response is sharp without oscillation. * - * @min 1.0 - * @max 100.0 + * @min 12.0 + * @max 50.0 * @group L1 Control */ PARAM_DEFINE_FLOAT(FW_L1_PERIOD, 20.0f); From 4d41f1d6cece020a0d98f1bd6fbcc4269b17bcf8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Dec 2015 16:10:21 +0100 Subject: [PATCH 080/201] MC att control: Add time constant to simplify user-tuning of basic vehicle operation --- .../mc_att_control/mc_att_control_main.cpp | 22 ++++++++++++++----- .../mc_att_control/mc_att_control_params.c | 22 +++++++++++++++++++ 2 files changed, 38 insertions(+), 6 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 91ce68b481..067fa909ae 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -101,6 +101,7 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); #define MIN_TAKEOFF_THRUST 0.2f #define RATES_I_LIMIT 0.3f #define MANUAL_THROTTLE_MAX_MULTICOPTER 0.9f +#define ATTITUDE_TC_DEFAULT 0.2f class MulticopterAttitudeControl { @@ -196,6 +197,8 @@ private: param_t rattitude_thres; param_t vtol_type; + param_t roll_tc; + param_t pitch_tc; } _params_handles; /**< handles for interesting parameters */ @@ -384,6 +387,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX"); _params_handles.rattitude_thres = param_find("MC_RATT_TH"); _params_handles.vtol_type = param_find("VT_TYPE"); + _params_handles.roll_tc = param_find("MC_ROLL_TC"); + _params_handles.pitch_tc = param_find("MC_PITCH_TC"); /* fetch initial parameter values */ parameters_update(); @@ -426,27 +431,32 @@ MulticopterAttitudeControl::parameters_update() { float v; + float roll_tc, pitch_tc; + + param_get(_params_handles.roll_tc, &roll_tc); + param_get(_params_handles.pitch_tc, &pitch_tc); + /* roll gains */ param_get(_params_handles.roll_p, &v); - _params.att_p(0) = v; + _params.att_p(0) = v * (ATTITUDE_TC_DEFAULT / roll_tc); param_get(_params_handles.roll_rate_p, &v); - _params.rate_p(0) = v; + _params.rate_p(0) = v * (ATTITUDE_TC_DEFAULT / roll_tc); param_get(_params_handles.roll_rate_i, &v); _params.rate_i(0) = v; param_get(_params_handles.roll_rate_d, &v); - _params.rate_d(0) = v; + _params.rate_d(0) = v * (ATTITUDE_TC_DEFAULT / roll_tc); param_get(_params_handles.roll_rate_ff, &v); _params.rate_ff(0) = v; /* pitch gains */ param_get(_params_handles.pitch_p, &v); - _params.att_p(1) = v; + _params.att_p(1) = v * (ATTITUDE_TC_DEFAULT / pitch_tc); param_get(_params_handles.pitch_rate_p, &v); - _params.rate_p(1) = v; + _params.rate_p(1) = v * (ATTITUDE_TC_DEFAULT / pitch_tc); param_get(_params_handles.pitch_rate_i, &v); _params.rate_i(1) = v; param_get(_params_handles.pitch_rate_d, &v); - _params.rate_d(1) = v; + _params.rate_d(1) = v * (ATTITUDE_TC_DEFAULT / pitch_tc); param_get(_params_handles.pitch_rate_ff, &v); _params.rate_ff(1) = v; diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 9e0532058f..011a6755ad 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -39,6 +39,28 @@ * @author Anton Babushkin */ +/** + * Roll time constant + * + * Reduce if the system is too twitchy, increase if the response is too slow and sluggish. + * + * @min 0.15 + * @max 0.25 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_ROLL_TC, 0.2f); + +/** + * Pitch time constant + * + * Reduce if the system is too twitchy, increase if the response is too slow and sluggish. + * + * @min 0.15 + * @max 0.25 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_PITCH_TC, 0.2f); + /** * Roll P gain * From 3bb733018beb033ff067861005ee8c5bcced34a0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 27 Dec 2015 01:13:05 +0100 Subject: [PATCH 081/201] Commander: Add range check to mag cal --- src/modules/commander/mag_calibration.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index eba8feec99..02b3a9f39a 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -73,6 +73,8 @@ static constexpr unsigned int calibration_sides = 6; ///< The total number of static constexpr unsigned int calibration_total_points = 240; ///< The total points per magnetometer static constexpr unsigned int calibraton_duration_seconds = 42; ///< The total duration the routine is allowed to take +static constexpr float MAG_MAX_OFFSET_LEN = 0.6f; ///< The maximum measurement range is ~1.4 Ga, the earth field is ~0.6 Ga, so an offset larger than ~0.8-0.6 Ga means the mag will saturate in some directions. + int32_t device_ids[max_mags]; int device_prio_max = 0; int32_t device_id_primary = 0; @@ -507,6 +509,13 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: NaN in sphere fit for mag #%u", cur_mag); result = calibrate_return_error; } + + if (sqrtf(sphere_x[cur_mag] * sphere_x[cur_mag] + + sphere_y[cur_mag] * sphere_y[cur_mag] + sphere_z[cur_mag] * sphere_z[cur_mag]) + > MAG_MAX_OFFSET_LEN) { + mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: Excessive offset for mag #%u", cur_mag); + result = calibrate_return_error; + } } } } From a9b9a0530760cf2d0af34c75b8dbad325bd1e612 Mon Sep 17 00:00:00 2001 From: sander Date: Sun, 27 Dec 2015 16:41:25 +0100 Subject: [PATCH 082/201] Arm checking FW throttle --- src/modules/vtol_att_control/standard.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 4f0c87bdbc..de09359935 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -302,7 +302,7 @@ void Standard::fill_actuator_outputs() * (1 - _mc_yaw_weight); // yaw // set the fixed wing throttle control - if (_vtol_schedule.flight_mode == FW_MODE) { + if (_vtol_schedule.flight_mode == FW_MODE && _armed->armed) { // take the throttle value commanded by the fw controller _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; From af6751c55f1d22a1834b9e5de2bdbbe24aed7fa9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Dec 2015 14:26:19 +0100 Subject: [PATCH 083/201] FMUv2: Disable more unused modules --- cmake/configs/nuttx_px4fmu-v2_default.cmake | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake index caf281a982..edf597406e 100644 --- a/cmake/configs/nuttx_px4fmu-v2_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake @@ -23,7 +23,7 @@ set(config_module_list drivers/l3gd20 drivers/hmc5883 drivers/ms5611 - drivers/mb12xx + #drivers/mb12xx drivers/srf02 drivers/sf0x drivers/ll40ls @@ -134,7 +134,7 @@ set(config_module_list # # OBC challenge # - modules/bottle_drop + #modules/bottle_drop # # Rover apps From 5880cd64d638b56cffb12acab970355853dc2e84 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Dec 2015 15:12:55 +0100 Subject: [PATCH 084/201] Q estimator: Code style --- src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index d54cdc208b..fb0dce7ca5 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -607,11 +607,8 @@ void AttitudeEstimatorQ::task_main() /* attitude quaternions for control state */ ctrl_state.q[0] = _q(0); - ctrl_state.q[1] = _q(1); - ctrl_state.q[2] = _q(2); - ctrl_state.q[3] = _q(3); /* attitude rates for control state */ From 3cbf69cef572544b1d3f5bd723ff057ae0df1454 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Dec 2015 15:13:09 +0100 Subject: [PATCH 085/201] commander: print yaw for home --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index cccd2bec0c..50a4372756 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -462,7 +462,7 @@ void print_status() warnx("power: USB: %s, BRICK: %s", (status.usb_connected) ? "[OK]" : "[NO]", (status.condition_power_input_valid) ? " [OK]" : "[NO]"); warnx("avionics rail: %6.2f V", (double)status.avionics_power_rail_voltage); - warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", _home.lat, _home.lon, (double)_home.alt); + warnx("home: lat = %.7f, lon = %.7f, alt = %.2f, yaw: %.2f", _home.lat, _home.lon, (double)_home.alt, (double)_home.yaw); warnx("home: x = %.7f, y = %.7f, z = %.2f ", (double)_home.x, (double)_home.y, (double)_home.z); /* read all relevant states */ From 511c663996b56b4f3cbe7f9d92c0c20213c2499a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Dec 2015 15:14:40 +0100 Subject: [PATCH 086/201] Takeoff: Set home yaw if executed as flight mode command. --- src/modules/navigator/takeoff.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp index f669429449..5ea96f8b3f 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -81,6 +81,8 @@ Takeoff::on_activation() pos_sp_triplet->previous.valid = false; mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; + pos_sp_triplet->current.yaw = _navigator->get_home_position()->yaw; + pos_sp_triplet->current.yaw_valid = true; pos_sp_triplet->next.valid = false; _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER); From c74d61c7eadb7a42d629f97b3cc99d98de43c5ae Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Dec 2015 15:15:15 +0100 Subject: [PATCH 087/201] VTOL: Indentation fix --- src/modules/vtol_att_control/standard.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index de09359935..c0d34c0384 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -302,7 +302,7 @@ void Standard::fill_actuator_outputs() * (1 - _mc_yaw_weight); // yaw // set the fixed wing throttle control - if (_vtol_schedule.flight_mode == FW_MODE && _armed->armed) { + if (_vtol_schedule.flight_mode == FW_MODE && _armed->armed) { // take the throttle value commanded by the fw controller _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; From f41f60901db7c62c2a06104fd0a2480b85bb95d8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Dec 2015 15:16:29 +0100 Subject: [PATCH 088/201] Control state: Update topic correctly - only if it actually changed and is valid --- .../mc_pos_control/mc_pos_control_main.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index a49714ff52..2a68b44a5b 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -374,6 +374,7 @@ MulticopterPositionControl::MulticopterPositionControl() : { memset(&_vehicle_status, 0, sizeof(_vehicle_status)); memset(&_ctrl_state, 0, sizeof(_ctrl_state)); + _ctrl_state.q[0] = 1.0f; memset(&_att_sp, 0, sizeof(_att_sp)); memset(&_manual, 0, sizeof(_manual)); memset(&_control_mode, 0, sizeof(_control_mode)); @@ -573,6 +574,13 @@ MulticopterPositionControl::poll_subscriptions() if (updated) { orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); + + /* get current rotation matrix and euler angles from control state quaternions */ + math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]); + _R = q_att.to_dcm(); + math::Vector<3> euler_angles; + euler_angles = _R.to_euler(); + _yaw = euler_angles(2); } orb_check(_att_sp_sub, &updated); @@ -1120,13 +1128,6 @@ MulticopterPositionControl::task_main() poll_subscriptions(); - /* get current rotation matrix and euler angles from control state quaternions */ - math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]); - _R = q_att.to_dcm(); - math::Vector<3> euler_angles; - euler_angles = _R.to_euler(); - _yaw = euler_angles(2); - parameters_update(false); hrt_abstime t = hrt_absolute_time(); From 94bfad5057fe96a4ee616eb28498951400f78307 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 9 Dec 2015 14:05:49 +0100 Subject: [PATCH 089/201] added rotation for solos external mag --- src/lib/conversion/rotation.cpp | 10 ++++++++++ src/lib/conversion/rotation.h | 3 ++- 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index e70d4628cf..bc55454f61 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -239,5 +239,15 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z) y = -z; z = -tmp; return; } + + case ROTATION_YAW_293_PITCH_68_ROLL_90: { + float tmpx = x; + float tmpy = y; + float tmpz = z; + x = 0.143039f * tmpx + 0.368776f * tmpy + -0.918446f * tmpz; + y = -0.332133f * tmpx + -0.856289f * tmpy + -0.395546f * tmpz; + z = -0.932324f * tmpx + 0.361625f * tmpy + 0.000000f * tmpz; + return; + } } } diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index ed8e4fda38..bc6dc2e2c7 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -78,6 +78,7 @@ enum Rotation { ROTATION_ROLL_180_PITCH_270 = 27, ROTATION_PITCH_90_YAW_180 = 28, ROTATION_PITCH_90_ROLL_90 = 29, + ROTATION_YAW_293_PITCH_68_ROLL_90 = 30, ROTATION_MAX }; @@ -117,7 +118,7 @@ const rot_lookup_t rot_lookup[] = { {270, 0, 270 }, {180, 270, 0 }, { 0, 90, 180 }, - { 90, 90, 0 } + { 90, 68, 293 }, }; /** From e8e81650dcd2c5aed749a8564d6403151eb209e9 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sat, 12 Dec 2015 13:01:49 +0100 Subject: [PATCH 090/201] implemented command ACK --- msg/vehicle_command_ack.msg | 8 +++ src/modules/commander/commander.cpp | 96 ++++++++++++++++------------ src/modules/mavlink/mavlink_main.cpp | 14 ++++ src/modules/uORB/objects_common.cpp | 3 + 4 files changed, 80 insertions(+), 41 deletions(-) create mode 100644 msg/vehicle_command_ack.msg diff --git a/msg/vehicle_command_ack.msg b/msg/vehicle_command_ack.msg new file mode 100644 index 0000000000..2c90469c0e --- /dev/null +++ b/msg/vehicle_command_ack.msg @@ -0,0 +1,8 @@ +uint8 VEHICLE_RESULT_ACCEPTED = 0 +uint8 VEHICLE_RESULT_TEMPORARILY_REJECTED = 1 +uint8 VEHICLE_RESULT_DENIED = 2 +uint8 VEHICLE_RESULT_UNSUPPORTED = 3 +uint8 VEHICLE_RESULT_FAILED = 4 + +uint16 command +uint8 result diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 50a4372756..499b11b6c1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -94,6 +94,7 @@ #include #include #include +#include #include #include @@ -227,7 +228,8 @@ void usage(const char *reason); */ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, - struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub); + struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub, + orb_advert_t *command_ack_pub, struct vehicle_command_ack_s *command_ack); /** * Mainloop of commander. @@ -270,7 +272,8 @@ static void commander_set_home_position(orb_advert_t &homePub, home_position_s & */ void *commander_low_prio_loop(void *arg); -void answer_command(struct vehicle_command_s &cmd, unsigned result); +void answer_command(struct vehicle_command_s &cmd, unsigned result, + orb_advert_t &command_ack_pub, vehicle_command_ack_s &command_ack); /** * check whether autostart ID is in the reserved range for HIL setups @@ -543,7 +546,8 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char bool handle_command(struct vehicle_status_s *status_local, const struct safety_s *safety_local, struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local, struct home_position_s *home, struct vehicle_global_position_s *global_pos, - struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub) + struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub, + orb_advert_t *command_ack_pub, struct vehicle_command_ack_s *command_ack) { /* only handle commands that are meant to be handled by this system and component */ if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id) @@ -904,19 +908,13 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s default: /* Warn about unsupported commands, this makes sense because only commands * to this component ID (or all) are passed by mavlink. */ - answer_command(*cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED); + answer_command(*cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED, *command_ack_pub, *command_ack); break; } if (cmd_result != vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED) { /* already warned about unsupported commands in "default" case */ - answer_command(*cmd, cmd_result); - } - - /* send any requested ACKs */ - if (cmd->confirmation > 0 && cmd_result != vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED) { - /* send acknowledge command */ - // XXX TODO + answer_command(*cmd, cmd_result, *command_ack_pub, *command_ack); } return true; @@ -1150,6 +1148,11 @@ int commander_thread_main(int argc, char *argv[]) orb_advert_t home_pub = nullptr; memset(&_home, 0, sizeof(_home)); + /* command ack */ + orb_advert_t command_ack_pub = nullptr; + struct vehicle_command_ack_s command_ack; + memset(&command_ack, 0, sizeof(command_ack)); + /* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */ orb_advert_t mission_pub = nullptr; mission_s mission; @@ -2354,7 +2357,8 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &local_position, &attitude, &home_pub)) { + if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &local_position, + &attitude, &home_pub, &command_ack_pub, &command_ack)) { status_changed = true; } } @@ -3107,7 +3111,8 @@ print_reject_arm(const char *msg) } } -void answer_command(struct vehicle_command_s &cmd, unsigned result) +void answer_command(struct vehicle_command_s &cmd, unsigned result, + orb_advert_t &command_ack_pub, vehicle_command_ack_s &command_ack) { switch (result) { case vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED: @@ -3138,6 +3143,17 @@ void answer_command(struct vehicle_command_s &cmd, unsigned result) default: break; } + + /* publish ACK */ + command_ack.command = cmd.command; + command_ack.result = result; + + if (command_ack_pub != nullptr) { + orb_publish(ORB_ID(vehicle_command_ack), command_ack_pub, &command_ack); + + } else { + command_ack_pub = orb_advertise(ORB_ID(vehicle_command_ack), &command_ack); + } } void *commander_low_prio_loop(void *arg) @@ -3150,6 +3166,11 @@ void *commander_low_prio_loop(void *arg) struct vehicle_command_s cmd; memset(&cmd, 0, sizeof(cmd)); + /* command ack */ + orb_advert_t command_ack_pub = nullptr; + struct vehicle_command_ack_s command_ack; + memset(&command_ack, 0, sizeof(command_ack)); + /* timeout for param autosave */ hrt_abstime need_param_autosave_timeout = 0; @@ -3207,23 +3228,23 @@ void *commander_low_prio_loop(void *arg) if (is_safe(&status, &safety, &armed)) { if (((int)(cmd.param1)) == 1) { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); usleep(100000); /* reboot */ px4_systemreset(false); } else if (((int)(cmd.param1)) == 3) { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); usleep(100000); /* reboot to bootloader */ px4_systemreset(true); } else { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub, command_ack); } } else { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub, command_ack); } break; @@ -3235,7 +3256,7 @@ void *commander_low_prio_loop(void *arg) /* try to go to INIT/PREFLIGHT arming state */ if (TRANSITION_DENIED == arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_INIT, &armed, false /* fRunPreArmChecks */, mavlink_fd)) { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub, command_ack); break; } else { status.calibration_enabled = true; @@ -3243,21 +3264,21 @@ void *commander_low_prio_loop(void *arg) if ((int)(cmd.param1) == 1) { /* gyro calibration */ - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); calib_ret = do_gyro_calibration(mavlink_fd); } else if ((int)(cmd.param2) == 1) { /* magnetometer calibration */ - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); calib_ret = do_mag_calibration(mavlink_fd); } else if ((int)(cmd.param3) == 1) { /* zero-altitude pressure calibration */ - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub, command_ack); } else if ((int)(cmd.param4) == 1) { /* RC calibration */ - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); /* disable RC control input completely */ status.rc_input_blocked = true; calib_ret = OK; @@ -3265,31 +3286,31 @@ void *commander_low_prio_loop(void *arg) } else if ((int)(cmd.param4) == 2) { /* RC trim calibration */ - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); calib_ret = do_trim_calibration(mavlink_fd); } else if ((int)(cmd.param5) == 1) { /* accelerometer calibration */ - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); calib_ret = do_accel_calibration(mavlink_fd); } else if ((int)(cmd.param5) == 2) { // board offset calibration - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); calib_ret = do_level_calibration(mavlink_fd); } else if ((int)(cmd.param6) == 1) { /* airspeed calibration */ - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); calib_ret = do_airspeed_calibration(mavlink_fd); } else if ((int)(cmd.param7) == 1) { /* do esc calibration */ - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); calib_ret = do_esc_calibration(mavlink_fd, &armed); } else if ((int)(cmd.param4) == 0) { /* RC calibration ended - have we been in one worth confirming? */ if (status.rc_input_blocked) { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); /* enable RC control input */ status.rc_input_blocked = false; mavlink_log_info(mavlink_fd, "CAL: Re-enabling RC IN"); @@ -3336,7 +3357,7 @@ void *commander_low_prio_loop(void *arg) if (ret == OK) { mavlink_log_info(mavlink_fd, "settings loaded"); - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); } else { mavlink_log_critical(mavlink_fd, "settings load ERROR"); @@ -3350,7 +3371,7 @@ void *commander_low_prio_loop(void *arg) mavlink_log_critical(mavlink_fd, "ERROR: %s", strerror(ret)); } - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED, command_ack_pub, command_ack); } } else if (((int)(cmd.param1)) == 1) { @@ -3364,7 +3385,7 @@ void *commander_low_prio_loop(void *arg) } /* do not spam MAVLink, but provide the answer / green led mechanism */ - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); } else { mavlink_log_critical(mavlink_fd, "settings save error"); @@ -3378,7 +3399,7 @@ void *commander_low_prio_loop(void *arg) mavlink_log_critical(mavlink_fd, "ERROR: %s", strerror(ret)); } - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED, command_ack_pub, command_ack); } } else if (((int)(cmd.param1)) == 2) { @@ -3389,11 +3410,11 @@ void *commander_low_prio_loop(void *arg) if (ret == OK) { /* do not spam MAVLink, but provide the answer / green led mechanism */ mavlink_log_critical(mavlink_fd, "onboard parameters reset"); - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); } else { mavlink_log_critical(mavlink_fd, "param reset error"); - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED); + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED, command_ack_pub, command_ack); } } @@ -3408,13 +3429,6 @@ void *commander_low_prio_loop(void *arg) /* don't answer on unsupported commands, it will be done in main loop */ break; } - - /* send any requested ACKs */ - if (cmd.confirmation > 0 && cmd.command != vehicle_command_s::VEHICLE_CMD_DO_SET_MODE - && cmd.command != vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM) { - /* send acknowledge command */ - // XXX TODO - } } } diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 07b768354f..8e575e1b8f 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -77,6 +77,7 @@ #include #include +#include #include "mavlink_bridge_header.h" #include "mavlink_main.h" @@ -1639,9 +1640,13 @@ Mavlink::task_main(int argc, char *argv[]) uint64_t param_time = 0; MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status)); uint64_t status_time = 0; + MavlinkOrbSubscription *ack_sub = add_orb_subscription(ORB_ID(vehicle_command_ack)); + uint64_t ack_time = 0; struct vehicle_status_s status; status_sub->update(&status_time, &status); + struct vehicle_command_ack_s command_ack; + ack_sub->update(&ack_time, &command_ack); /* add default streams depending on mode */ @@ -1856,6 +1861,15 @@ Mavlink::task_main(int argc, char *argv[]) set_manual_input_mode_generation(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_GENERATED); } + /* send command ACK */ + if (ack_sub->update(&ack_time, &command_ack)) { + mavlink_command_ack_t msg; + msg.result = command_ack.result; + msg.command = command_ack.command; + + send_message(MAVLINK_MSG_ID_COMMAND_ACK, &msg); + } + /* check for requested subscriptions */ if (_subscribe_to_stream != nullptr) { if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) { diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 1b8fb18735..15dab60df4 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -270,3 +270,6 @@ ORB_DEFINE(distance_sensor, struct distance_sensor_s); #include "topics/camera_trigger.h" ORB_DEFINE(camera_trigger, struct camera_trigger_s); + +#include "topics/vehicle_command_ack.h" +ORB_DEFINE(vehicle_command_ack, struct vehicle_command_ack_s); From 3847c826ecb01b25b34ed7d015f0ad59f828c30c Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 16 Dec 2015 14:49:06 +0100 Subject: [PATCH 091/201] added initial solo config --- ROMFS/px4fmu_common/init.d/4030_solo | 29 ++++++++++++++++++++++++ ROMFS/px4fmu_common/mixers/solo.main.mix | 7 ++++++ 2 files changed, 36 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/4030_solo create mode 100644 ROMFS/px4fmu_common/mixers/solo.main.mix diff --git a/ROMFS/px4fmu_common/init.d/4030_solo b/ROMFS/px4fmu_common/init.d/4030_solo new file mode 100644 index 0000000000..185b261325 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/4030_solo @@ -0,0 +1,29 @@ +#!nsh +# +# @name Generic Quadrotor X config +# +# @type Quadrotor x +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Lorenz Meier +# + +sh /etc/init.d/rc.mc_defaults + +if [ $AUTOCNF == yes ] +then + param set MC_PITCHRATE_P 0.13 + param set MC_ROLLRATE_P 0.13 + param set MPC_MANTHR_MIN 0.08 + + # enable high bandwidth mavlink by default + param set SYS_COMPANION 921601 +fi + +set MIXER solo + +set PWM_OUT 1234 +set MIXER_AUX none diff --git a/ROMFS/px4fmu_common/mixers/solo.main.mix b/ROMFS/px4fmu_common/mixers/solo.main.mix new file mode 100644 index 0000000000..12a3bee20c --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/solo.main.mix @@ -0,0 +1,7 @@ +Multirotor mixer for PX4FMU +=========================== + +This file defines a single mixer for a quadrotor in the X configuration. All controls +are mixed 100%. + +R: 4x 10000 10000 10000 0 From fbf42c8949d23de19b12366754f07beb9a889722 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Thu, 17 Dec 2015 00:11:50 +0100 Subject: [PATCH 092/201] added auto takeoff support, updated configuration for solo and generalized landing mission items --- ROMFS/px4fmu_common/init.d/4030_solo | 6 ++ msg/vehicle_status.msg | 4 +- src/modules/commander/commander.cpp | 20 ++++ .../commander/state_machine_helper.cpp | 21 ++++ src/modules/navigator/CMakeLists.txt | 1 + src/modules/navigator/land.cpp | 96 +++++++++++++++++++ src/modules/navigator/land.h | 65 +++++++++++++ src/modules/navigator/mission_block.cpp | 48 ++++++++++ src/modules/navigator/mission_block.h | 10 ++ src/modules/navigator/navigator.h | 4 +- src/modules/navigator/navigator_main.cpp | 7 +- src/modules/navigator/rtl.cpp | 29 +----- src/modules/navigator/takeoff.cpp | 6 ++ 13 files changed, 287 insertions(+), 30 deletions(-) create mode 100644 src/modules/navigator/land.cpp create mode 100644 src/modules/navigator/land.h diff --git a/ROMFS/px4fmu_common/init.d/4030_solo b/ROMFS/px4fmu_common/init.d/4030_solo index 185b261325..e07fd9b1de 100644 --- a/ROMFS/px4fmu_common/init.d/4030_solo +++ b/ROMFS/px4fmu_common/init.d/4030_solo @@ -15,10 +15,16 @@ sh /etc/init.d/rc.mc_defaults if [ $AUTOCNF == yes ] then + # tuning param set MC_PITCHRATE_P 0.13 param set MC_ROLLRATE_P 0.13 param set MPC_MANTHR_MIN 0.08 + # takeoff. land and RTL settings + param set MIS_TAKEOFF_ALT 5.0 + param set COM_DISARM_LAND 1 + param set RTL_LAND_DELAY 1 + # enable high bandwidth mavlink by default param set SYS_COMPANION 921601 fi diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index f3a1767dde..8a67f928e5 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -10,7 +10,8 @@ uint8 MAIN_STATE_OFFBOARD = 7 uint8 MAIN_STATE_STAB = 8 uint8 MAIN_STATE_RATTITUDE = 9 uint8 MAIN_STATE_AUTO_TAKEOFF = 10 -uint8 MAIN_STATE_MAX = 11 +uint8 MAIN_STATE_AUTO_LAND = 11 +uint8 MAIN_STATE_MAX = 12 # If you change the order, add or remove arming_state_t states make sure to update the arrays # in state_machine_helper.cpp as well. @@ -104,6 +105,7 @@ uint16 counter # incremented by the writing thread everytime new data is store uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data uint8 main_state # main state machine +uint8 main_state_prev # previous main state uint8 nav_state # set navigation state machine to specified value uint8 arming_state # current arming state uint8 hil_state # current hil state diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 499b11b6c1..12fa023a0e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1083,6 +1083,7 @@ int commander_thread_main(int argc, char *argv[]) status.rc_input_blocked = false; status.rc_input_mode = vehicle_status_s::RC_IN_MODE_DEFAULT; status.main_state =vehicle_status_s::MAIN_STATE_MANUAL; + status.main_state_prev = vehicle_status_s::MAIN_STATE_MAX; status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; status.arming_state = vehicle_status_s::ARMING_STATE_INIT; @@ -2348,6 +2349,25 @@ int commander_thread_main(int argc, char *argv[]) } } + /* handle main state after takeoff and land */ + if (status.main_state == vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF + && mission_result.finished) { + + // transition back to state we had before takeoff + if (status.main_state_prev < vehicle_status_s::MAIN_STATE_MAX + && status.main_state_prev != vehicle_status_s::MAIN_STATE_AUTO_LAND) { + main_state_transition(&status, status.main_state_prev); + } + + } else if (status.main_state == vehicle_status_s::MAIN_STATE_AUTO_LAND + && status.condition_landed) { + + // transition back to state we had before takeoff + if (status.main_state_prev < vehicle_status_s::MAIN_STATE_MAX + && status.main_state_prev != vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF) { + main_state_transition(&status, status.main_state_prev); + } + } /* handle commands last, as the system needs to be updated to handle them */ orb_check(cmd_sub, &updated); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 1bde387c63..5163087830 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -352,6 +352,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta case vehicle_status_s::MAIN_STATE_AUTO_MISSION: case vehicle_status_s::MAIN_STATE_AUTO_RTL: case vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF: + case vehicle_status_s::MAIN_STATE_AUTO_LAND: /* need global position and home position */ if (status->condition_global_position_valid && status->condition_home_position_valid) { ret = TRANSITION_CHANGED; @@ -373,6 +374,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta } if (ret == TRANSITION_CHANGED) { if (status->main_state != new_main_state) { + status->main_state_prev = status->main_state; status->main_state = new_main_state; } else { ret = TRANSITION_NOT_CHANGED; @@ -766,6 +768,25 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } break; + case vehicle_status_s::MAIN_STATE_AUTO_LAND: + /* require global position and home */ + + if (status->engine_failure) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; + } else if ((!status->condition_global_position_valid || + !status->condition_home_position_valid)) { + status->failsafe = true; + + if (status->condition_local_altitude_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + } + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; + } + break; + case vehicle_status_s::MAIN_STATE_OFFBOARD: /* require offboard control, otherwise stay where you are */ if (status->offboard_control_signal_lost && !status->rc_signal_lost) { diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index 6a5c4c15d9..0ce9399a27 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -45,6 +45,7 @@ px4_add_module( loiter.cpp rtl.cpp takeoff.cpp + land.cpp mission_feasibility_checker.cpp geofence.cpp datalinkloss.cpp diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp new file mode 100644 index 0000000000..25da635622 --- /dev/null +++ b/src/modules/navigator/land.cpp @@ -0,0 +1,96 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. 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 PX4 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 land.cpp + * + * Helper class to land at the current position + * + * @author Andreas Antener + */ + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include "land.h" +#include "navigator.h" + +Land::Land(Navigator *navigator, const char *name) : + MissionBlock(navigator, name) +{ + /* load initial params */ + updateParams(); +} + +Land::~Land() +{ +} + +void +Land::on_inactive() +{ +} + +void +Land::on_activation() +{ + /* set current mission item to Land */ + set_land_item(&_mission_item, true); + + /* convert mission item to current setpoint */ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + pos_sp_triplet->previous.valid = false; + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LAND; + pos_sp_triplet->next.valid = false; + + _navigator->set_can_loiter_at_sp(false); + + _navigator->set_position_setpoint_triplet_updated(); +} + +void +Land::on_active() +{ + if (is_mission_item_reached()) { + set_idle_item(&_mission_item); + } +} diff --git a/src/modules/navigator/land.h b/src/modules/navigator/land.h new file mode 100644 index 0000000000..c97feb7046 --- /dev/null +++ b/src/modules/navigator/land.h @@ -0,0 +1,65 @@ +/*************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. 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 PX4 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 land.h + * + * Helper class to land at the current position + * + * @author Andreas Antener + */ + +#ifndef NAVIGATOR_LAND_H +#define NAVIGATOR_LAND_H + +#include +#include + +#include "navigator_mode.h" +#include "mission_block.h" + +class Land : public MissionBlock +{ +public: + Land(Navigator *navigator, const char *name); + + ~Land(); + + virtual void on_inactive(); + + virtual void on_activation(); + + virtual void on_active(); + +}; + +#endif diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index e2f454d4cf..7c55eccf57 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -323,3 +323,51 @@ MissionBlock::set_takeoff_item(struct mission_item_s *item, float min_clearance, item->autocontinue = false; item->origin = ORIGIN_ONBOARD; } + +void +MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_location) +{ + item->nav_cmd = NAV_CMD_LAND; + + /* use current position */ + if (at_current_location) { + item->lat = _navigator->get_global_position()->lat; + item->lon = _navigator->get_global_position()->lon; + + /* use home position */ + } else { + item->lat = _navigator->get_home_position()->lat; + item->lon = _navigator->get_home_position()->lon; + } + + item->altitude = 0; + item->altitude_is_relative = false; + item->yaw = NAN; + item->loiter_radius = _navigator->get_loiter_radius(); + item->loiter_direction = 1; + item->acceptance_radius = _navigator->get_acceptance_radius(); + item->time_inside = 0.0f; + item->pitch_min = 0.0f; + item->autocontinue = true; + item->origin = ORIGIN_ONBOARD; +} + +void +MissionBlock::set_idle_item(struct mission_item_s *item) +{ + item->nav_cmd = NAV_CMD_IDLE; + item->lat = _navigator->get_home_position()->lat; + item->lon = _navigator->get_home_position()->lon; + item->altitude_is_relative = false; + item->altitude = _navigator->get_home_position()->alt; + item->yaw = NAN; + item->loiter_radius = _navigator->get_loiter_radius(); + item->loiter_direction = 1; + item->acceptance_radius = _navigator->get_acceptance_radius(); + item->time_inside = 0.0f; + item->pitch_min = 0.0f; + item->autocontinue = true; + item->origin = ORIGIN_ONBOARD; +} + + diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 5490b4c6b8..36a1ecaba1 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -101,6 +101,16 @@ protected: */ void set_takeoff_item(struct mission_item_s *item, float min_clearance = -1.0f, float min_pitch = 0.0f); + /** + * Set a land mission item + */ + void set_land_item(struct mission_item_s *item, bool at_current_location); + + /** + * Set idle mission item + */ + void set_idle_item(struct mission_item_s *item); + mission_item_s _mission_item; bool _waypoint_position_reached; bool _waypoint_yaw_reached; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index b154d21769..514705a68a 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -62,6 +62,7 @@ #include "mission.h" #include "loiter.h" #include "takeoff.h" +#include "land.h" #include "rtl.h" #include "datalinkloss.h" #include "enginefailure.h" @@ -72,7 +73,7 @@ /** * Number of navigation modes that need on_active/on_inactive calls */ -#define NAVIGATOR_MODE_ARRAY_SIZE 8 +#define NAVIGATOR_MODE_ARRAY_SIZE 9 class Navigator : public control::SuperBlock { @@ -226,6 +227,7 @@ private: Mission _mission; /**< class that handles the missions */ Loiter _loiter; /**< class that handles loiter */ Takeoff _takeoff; /**< class for handling takeoff commands */ + Land _land; /**< class for handling land commands */ RTL _rtl; /**< class that handles RTL */ RCLoss _rcLoss; /**< class that handles RTL according to OBC rules (rc loss mode) */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 7d4736449f..b16f9b9b91 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -142,6 +142,7 @@ Navigator::Navigator() : _mission(this, "MIS"), _loiter(this, "LOI"), _takeoff(this, "TKF"), + _land(this, "LND"), _rtl(this, "RTL"), _rcLoss(this, "RCL"), _dataLinkLoss(this, "DLL"), @@ -161,6 +162,7 @@ Navigator::Navigator() : _navigation_mode_array[5] = &_gpsFailure; _navigation_mode_array[6] = &_rcLoss; _navigation_mode_array[7] = &_takeoff; + _navigation_mode_array[8] = &_land; updateParams(); } @@ -444,7 +446,6 @@ Navigator::task_main() case vehicle_status_s::NAVIGATION_STATE_ACRO: case vehicle_status_s::NAVIGATION_STATE_ALTCTL: case vehicle_status_s::NAVIGATION_STATE_POSCTL: - case vehicle_status_s::NAVIGATION_STATE_LAND: case vehicle_status_s::NAVIGATION_STATE_TERMINATION: case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: _navigation_mode = nullptr; @@ -481,6 +482,10 @@ Navigator::task_main() _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_takeoff; break; + case vehicle_status_s::NAVIGATION_STATE_LAND: + _pos_sp_triplet_published_invalid_once = false; + _navigation_mode = &_land; + break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: /* Use complex data link loss mode only when enabled via param * otherwise use rtl */ diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index a77f5895e1..0fc377874d 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -243,39 +243,14 @@ RTL::set_rtl_item() } case RTL_STATE_LAND: { - _mission_item.lat = _navigator->get_home_position()->lat; - _mission_item.lon = _navigator->get_home_position()->lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _navigator->get_home_position()->alt; - _mission_item.yaw = _navigator->get_home_position()->yaw; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_LAND; - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; + set_land_item(&_mission_item, false); mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: land at home"); break; } case RTL_STATE_LANDED: { - _mission_item.lat = _navigator->get_home_position()->lat; - _mission_item.lon = _navigator->get_home_position()->lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _navigator->get_home_position()->alt; - // Do not change / control yaw in landed - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_IDLE; - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; + set_idle_item(&_mission_item); mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: completed, landed"); break; diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp index 5ea96f8b3f..8d5040eac2 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -75,6 +75,8 @@ Takeoff::on_activation() { /* set current mission item to Takeoff */ set_takeoff_item(&_mission_item, _param_min_alt.get()); + _navigator->get_mission_result()->reached = false; + _navigator->get_mission_result()->finished = false; /* convert mission item to current setpoint */ struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); @@ -93,4 +95,8 @@ Takeoff::on_activation() void Takeoff::on_active() { + if (is_mission_item_reached()) { + _navigator->get_mission_result()->finished = true; + _navigator->set_mission_result_updated(); + } } From bfb862763c7eff9247a5044dae0058a16e92f9f9 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Thu, 17 Dec 2015 10:51:56 +0100 Subject: [PATCH 093/201] switching to prev main state after landing when disarmed --- src/modules/commander/commander.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 12fa023a0e..0e56c9387b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2349,20 +2349,18 @@ int commander_thread_main(int argc, char *argv[]) } } - /* handle main state after takeoff and land */ + /* reset main state after takeoff and land */ if (status.main_state == vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF && mission_result.finished) { - // transition back to state we had before takeoff if (status.main_state_prev < vehicle_status_s::MAIN_STATE_MAX && status.main_state_prev != vehicle_status_s::MAIN_STATE_AUTO_LAND) { main_state_transition(&status, status.main_state_prev); } } else if (status.main_state == vehicle_status_s::MAIN_STATE_AUTO_LAND - && status.condition_landed) { + && status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { - // transition back to state we had before takeoff if (status.main_state_prev < vehicle_status_s::MAIN_STATE_MAX && status.main_state_prev != vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF) { main_state_transition(&status, status.main_state_prev); From 44f13006ddf2b47d19140e5d098243958780496c Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Thu, 17 Dec 2015 17:03:58 +0100 Subject: [PATCH 094/201] update mission state on takeoff --- src/modules/navigator/takeoff.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp index 8d5040eac2..1cf8c95d3f 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -77,6 +77,7 @@ Takeoff::on_activation() set_takeoff_item(&_mission_item, _param_min_alt.get()); _navigator->get_mission_result()->reached = false; _navigator->get_mission_result()->finished = false; + _navigator->set_mission_result_updated(); /* convert mission item to current setpoint */ struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); From 3c4141ff5068a605f112b8b07f114b129a7fb3e4 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 18 Dec 2015 09:15:09 +0100 Subject: [PATCH 095/201] increased time for less strict takeoff detection after arming --- src/modules/land_detector/LandDetector.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index b86e431150..4b418f6523 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -100,7 +100,7 @@ protected: static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold before triggering a land */ static constexpr uint64_t LAND_DETECTOR_ARM_PHASE_TIME = - 1000000; /**< time interval in which wider acceptance thresholds are used after arming */ + 2000000; /**< time interval in which wider acceptance thresholds are used after arming */ protected: orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ From c32d44d8b44948db8a41ac06b932b4919f174d57 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 18 Dec 2015 09:16:12 +0100 Subject: [PATCH 096/201] disable position controller when landed and in manual control --- .../mc_pos_control/mc_pos_control_main.cpp | 27 +++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 2a68b44a5b..fc7861b796 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1248,6 +1248,33 @@ MulticopterPositionControl::task_main() _att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp); } + } else if (_control_mode.flag_control_manual_enabled + && _vehicle_status.condition_landed) { + /* don't run controller when landed */ + _reset_pos_sp = true; + _reset_alt_sp = true; + _mode_auto = false; + reset_int_z = true; + reset_int_xy = true; + + R.identity(); + memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); + _att_sp.R_valid = true; + + _att_sp.roll_body = 0.0f; + _att_sp.pitch_body = 0.0f; + _att_sp.yaw_body = _yaw; + _att_sp.thrust = 0.0f; + + _att_sp.timestamp = hrt_absolute_time(); + + /* publish attitude setpoint */ + if (_att_sp_pub != nullptr) { + orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp); + } else if (_attitude_setpoint_id) { + _att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp); + } + } else { /* run position & altitude controllers, if enabled (otherwise use already computed velocity setpoints) */ if (_run_pos_control) { From 64349c7a0996abf8078fe0c236264383c8e86bec Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 18 Dec 2015 09:48:47 +0100 Subject: [PATCH 097/201] ability to switch to land mode --- src/modules/commander/commander.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 0e56c9387b..91501082fc 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -615,6 +615,9 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s case PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF: main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF); break; + case PX4_CUSTOM_SUB_MODE_AUTO_LAND: + main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_AUTO_LAND); + break; default: main_ret = TRANSITION_DENIED; From f17c5d8d55dc6c39276fde20062080b0e24ed75e Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 18 Dec 2015 11:24:21 +0100 Subject: [PATCH 098/201] fixing takeoff mission and swtiching to previous flight mode after land/takeoff --- src/modules/commander/commander.cpp | 20 ++++++++------------ src/modules/navigator/mission_block.cpp | 12 ++++++++++-- src/modules/navigator/takeoff.cpp | 3 ++- 3 files changed, 20 insertions(+), 15 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 91501082fc..e8520c62d2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2352,20 +2352,16 @@ int commander_thread_main(int argc, char *argv[]) } } - /* reset main state after takeoff and land */ - if (status.main_state == vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF - && mission_result.finished) { + /* reset main state after takeoff or land has been completed */ + /* only switch back to at least altitude controlled modes */ + if (status.main_state_prev == vehicle_status_s::MAIN_STATE_POSCTL || + status.main_state_prev == vehicle_status_s::MAIN_STATE_ALTCTL) { - if (status.main_state_prev < vehicle_status_s::MAIN_STATE_MAX - && status.main_state_prev != vehicle_status_s::MAIN_STATE_AUTO_LAND) { - main_state_transition(&status, status.main_state_prev); - } + if ((status.main_state == vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF + && mission_result.finished) || + (status.main_state == vehicle_status_s::MAIN_STATE_AUTO_LAND + && status.condition_landed)) { - } else if (status.main_state == vehicle_status_s::MAIN_STATE_AUTO_LAND - && status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { - - if (status.main_state_prev < vehicle_status_s::MAIN_STATE_MAX - && status.main_state_prev != vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF) { main_state_transition(&status, status.main_state_prev); } } diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 7c55eccf57..1819337a4c 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -119,8 +119,16 @@ MissionBlock::is_mission_item_reached() if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator->get_vstatus()->is_rotary_wing) { /* require only altitude for takeoff for multicopter */ + + /* _mission_item.acceptance_radius is not always set */ + float mission_acceptance_radius = _mission_item.acceptance_radius; + /* if set to zero use the default instead */ + if (mission_acceptance_radius < NAV_EPSILON_POSITION) { + mission_acceptance_radius = _navigator->get_acceptance_radius(); + } + if (_navigator->get_global_position()->alt > - altitude_amsl - _navigator->get_acceptance_radius()) { + altitude_amsl - mission_acceptance_radius) { _waypoint_position_reached = true; } } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { @@ -301,7 +309,7 @@ MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance) void MissionBlock::set_takeoff_item(struct mission_item_s *item, float min_clearance, float min_pitch) { - item->nav_cmd = NAV_CMD_LOITER_UNLIMITED; + item->nav_cmd = NAV_CMD_TAKEOFF; /* use current position and use return altitude as clearance */ item->lat = _navigator->get_global_position()->lat; diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp index 1cf8c95d3f..4b5dc49776 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -78,6 +78,7 @@ Takeoff::on_activation() _navigator->get_mission_result()->reached = false; _navigator->get_mission_result()->finished = false; _navigator->set_mission_result_updated(); + reset_mission_item_reached(); /* convert mission item to current setpoint */ struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); @@ -96,7 +97,7 @@ Takeoff::on_activation() void Takeoff::on_active() { - if (is_mission_item_reached()) { + if (is_mission_item_reached() && !_navigator->get_mission_result()->finished) { _navigator->get_mission_result()->finished = true; _navigator->set_mission_result_updated(); } From 5a009ce4c8c7059cc03b780e7d0e1856c5a397a2 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 18 Dec 2015 14:28:05 +0100 Subject: [PATCH 099/201] don't throttle up anymore during landing --- .../mc_pos_control/mc_pos_control_main.cpp | 26 ++++++++++++++----- 1 file changed, 19 insertions(+), 7 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index fc7861b796..d6ec292f6d 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -241,6 +241,7 @@ private: math::Matrix<3, 3> _R; /**< rotation matrix from attitude quaternions */ float _yaw; /**< yaw angle (euler) */ + float _landing_thrust; /** * Update our local parameter cache. @@ -370,7 +371,8 @@ MulticopterPositionControl::MulticopterPositionControl() : _alt_hold_engaged(false), _run_pos_control(true), _run_alt_control(true), - _yaw(0.0f) + _yaw(0.0f), + _landing_thrust(1.0f) { memset(&_vehicle_status, 0, sizeof(_vehicle_status)); memset(&_ctrl_state, 0, sizeof(_ctrl_state)); @@ -1421,6 +1423,7 @@ MulticopterPositionControl::task_main() } float tilt_max = _params.tilt_max_air; + float thr_max = _params.thr_max; /* adjust limits for landing mode */ if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && @@ -1431,6 +1434,15 @@ MulticopterPositionControl::task_main() if (thr_min < 0.0f) { thr_min = 0.0f; } + + /* don't let it throttle up again during landing */ + if (thrust_sp(2) < 0.0f && thrust_sp(2) < _landing_thrust) { + _landing_thrust = thrust_sp(2); + } + thr_max = -_landing_thrust; + + } else { + _landing_thrust = _params.thr_max; } /* limit min lift */ @@ -1482,19 +1494,19 @@ MulticopterPositionControl::task_main() /* limit max thrust */ float thrust_abs = thrust_sp.length(); - if (thrust_abs > _params.thr_max) { + if (thrust_abs > thr_max) { if (thrust_sp(2) < 0.0f) { - if (-thrust_sp(2) > _params.thr_max) { + if (-thrust_sp(2) > thr_max) { /* thrust Z component is too large, limit it */ thrust_sp(0) = 0.0f; thrust_sp(1) = 0.0f; - thrust_sp(2) = -_params.thr_max; + thrust_sp(2) = -thr_max; saturation_xy = true; saturation_z = true; } else { /* preserve thrust Z component and lower XY, keeping altitude is more important than position */ - float thrust_xy_max = sqrtf(_params.thr_max * _params.thr_max - thrust_sp(2) * thrust_sp(2)); + float thrust_xy_max = sqrtf(thr_max * thr_max - thrust_sp(2) * thrust_sp(2)); float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); float k = thrust_xy_max / thrust_xy_abs; thrust_sp(0) *= k; @@ -1504,13 +1516,13 @@ MulticopterPositionControl::task_main() } else { /* Z component is negative, going down, simply limit thrust vector */ - float k = _params.thr_max / thrust_abs; + float k = thr_max / thrust_abs; thrust_sp *= k; saturation_xy = true; saturation_z = true; } - thrust_abs = _params.thr_max; + thrust_abs = thr_max; } /* update integrals */ From 09b5bdb1ee9c02db821d646e7e50d3d78cfd3354 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sat, 19 Dec 2015 07:39:48 +0100 Subject: [PATCH 100/201] in mc auto: do not reset the position sp while near the waypoint, should make switching to manual pos control smoother --- msg/position_setpoint.msg | 1 + src/modules/mc_pos_control/mc_pos_control_main.cpp | 11 +++++++++++ src/modules/navigator/land.cpp | 1 - src/modules/navigator/mission_block.cpp | 1 + src/modules/navigator/takeoff.cpp | 3 +-- 5 files changed, 14 insertions(+), 3 deletions(-) diff --git a/msg/position_setpoint.msg b/msg/position_setpoint.msg index ae6756aa8b..e46e2990c1 100644 --- a/msg/position_setpoint.msg +++ b/msg/position_setpoint.msg @@ -33,3 +33,4 @@ float32 a_y # acceleration y setpoint float32 a_z # acceleration z setpoint bool acceleration_valid # true if acceleration setpoint is valid/should be used bool acceleration_is_force # interprete acceleration as force +float32 acceptance_radius # navigation acceptance_radius if we're doint waypoint navigation diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index d6ec292f6d..3dd53bedaf 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1059,6 +1059,16 @@ void MulticopterPositionControl::control_auto(float dt) _att_sp.yaw_body = _pos_sp_triplet.current.yaw; } + /* if we're near the current pos SP don't reset it anymore, else it will make if jump back, + * especially noticable in altitude after takeoff. + */ + if (current_setpoint_valid + && _pos_sp_triplet.current.acceptance_radius > 0.0f + && (curr_sp - _pos_sp).length() < _pos_sp_triplet.current.acceptance_radius) { + _reset_pos_sp = false; + _reset_alt_sp = false; + } + } else { /* no waypoint, do nothing, setpoint was already reset */ } @@ -1439,6 +1449,7 @@ MulticopterPositionControl::task_main() if (thrust_sp(2) < 0.0f && thrust_sp(2) < _landing_thrust) { _landing_thrust = thrust_sp(2); } + // XXX: we probably need to add a margin here becaue we're limiting the complete thrust vector further down thr_max = -_landing_thrust; } else { diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp index 25da635622..b8182e2fed 100644 --- a/src/modules/navigator/land.cpp +++ b/src/modules/navigator/land.cpp @@ -79,7 +79,6 @@ Land::on_activation() struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); pos_sp_triplet->previous.valid = false; mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); - pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LAND; pos_sp_triplet->next.valid = false; _navigator->set_can_loiter_at_sp(false); diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 1819337a4c..897173d0d9 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -222,6 +222,7 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite _navigator->get_loiter_radius(); sp->loiter_direction = item->loiter_direction; sp->pitch_min = item->pitch_min; + sp->acceptance_radius = item->acceptance_radius; switch (item->nav_cmd) { case NAV_CMD_DO_SET_SERVO: diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp index 4b5dc49776..9851212320 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -84,12 +84,11 @@ Takeoff::on_activation() struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); pos_sp_triplet->previous.valid = false; mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); - pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; pos_sp_triplet->current.yaw = _navigator->get_home_position()->yaw; pos_sp_triplet->current.yaw_valid = true; pos_sp_triplet->next.valid = false; - _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER); + _navigator->set_can_loiter_at_sp(true); _navigator->set_position_setpoint_triplet_updated(); } From ea7a1a92b549b01d93f349c124594927e953aea7 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sat, 19 Dec 2015 13:09:52 +0100 Subject: [PATCH 101/201] correct thrust limiting during landing with margin, don't reset position setpoint when switching from takeoff to posctl and allow high enough z velocity for position lock --- ROMFS/px4fmu_common/init.d/4030_solo | 1 + .../mc_pos_control/mc_pos_control_main.cpp | 40 ++++++++++++------- 2 files changed, 26 insertions(+), 15 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/4030_solo b/ROMFS/px4fmu_common/init.d/4030_solo index e07fd9b1de..507c4ef0cb 100644 --- a/ROMFS/px4fmu_common/init.d/4030_solo +++ b/ROMFS/px4fmu_common/init.d/4030_solo @@ -24,6 +24,7 @@ then param set MIS_TAKEOFF_ALT 5.0 param set COM_DISARM_LAND 1 param set RTL_LAND_DELAY 1 + param set MPC_HOLD_MAX_Z 1.5 # enable high bandwidth mavlink by default param set SYS_COMPANION 921601 diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 3dd53bedaf..00e6df0655 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -242,6 +242,7 @@ private: math::Matrix<3, 3> _R; /**< rotation matrix from attitude quaternions */ float _yaw; /**< yaw angle (euler) */ float _landing_thrust; + hrt_abstime _landing_started; /** * Update our local parameter cache. @@ -372,7 +373,8 @@ MulticopterPositionControl::MulticopterPositionControl() : _run_pos_control(true), _run_alt_control(true), _yaw(0.0f), - _landing_thrust(1.0f) + _landing_thrust(1.0f), + _landing_started(0) { memset(&_vehicle_status, 0, sizeof(_vehicle_status)); memset(&_ctrl_state, 0, sizeof(_ctrl_state)); @@ -951,10 +953,6 @@ void MulticopterPositionControl::control_auto(float dt) } if (current_setpoint_valid) { - /* in case of interrupted mission don't go to waypoint but stay at current position */ - _reset_pos_sp = true; - _reset_alt_sp = true; - /* scaled space: 1 == position error resulting max allowed speed */ math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max); // TODO add mult param here @@ -1059,14 +1057,20 @@ void MulticopterPositionControl::control_auto(float dt) _att_sp.yaw_body = _pos_sp_triplet.current.yaw; } - /* if we're near the current pos SP don't reset it anymore, else it will make if jump back, - * especially noticable in altitude after takeoff. + /* + * if we're already near the current takeoff setpoint don't reset in case we switch back to posctl. + * this makes the takeoff finish smoothly. */ - if (current_setpoint_valid + if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && _pos_sp_triplet.current.acceptance_radius > 0.0f - && (curr_sp - _pos_sp).length() < _pos_sp_triplet.current.acceptance_radius) { + && (_pos - _pos_sp).length() < _pos_sp_triplet.current.acceptance_radius * 1.1f) { _reset_pos_sp = false; _reset_alt_sp = false; + + /* otherwise: in case of interrupted mission don't go to waypoint but stay at current position */ + } else { + _reset_pos_sp = true; + _reset_alt_sp = true; } } else { @@ -1432,6 +1436,7 @@ MulticopterPositionControl::task_main() thr_min = 0.0f; } + float thrust_abs = thrust_sp.length(); float tilt_max = _params.tilt_max_air; float thr_max = _params.thr_max; @@ -1440,19 +1445,26 @@ MulticopterPositionControl::task_main() _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { /* limit max tilt and min lift when landing */ tilt_max = _params.tilt_max_land; + if (_landing_started == 0) { + _landing_started = hrt_absolute_time(); + } if (thr_min < 0.0f) { thr_min = 0.0f; } /* don't let it throttle up again during landing */ - if (thrust_sp(2) < 0.0f && thrust_sp(2) < _landing_thrust) { - _landing_thrust = thrust_sp(2); + if (thrust_sp(2) < 0.0f && thrust_abs < _landing_thrust + && hrt_elapsed_time(&_landing_started) > 15e5) { + _landing_thrust = thrust_abs; } - // XXX: we probably need to add a margin here becaue we're limiting the complete thrust vector further down - thr_max = -_landing_thrust; + /* add 5% to give it some margin to compensate external influences */ + thr_max = _landing_thrust + 0.05f * _landing_thrust; + /*PX4_WARN("thrust: abs %.4f, sp(2) %.4f, _landing_thrust %.4f, vel_err(2) %.4f, vel_sp(2) %.4f, vel(2) %.4f", + (double) thrust_abs, (double) thrust_sp(2), (double) _landing_thrust, (double) vel_err(2), (double) _vel_sp(2), (double) _vel(2));*/ } else { + _landing_started = 0; _landing_thrust = _params.thr_max; } @@ -1503,8 +1515,6 @@ MulticopterPositionControl::task_main() } /* limit max thrust */ - float thrust_abs = thrust_sp.length(); - if (thrust_abs > thr_max) { if (thrust_sp(2) < 0.0f) { if (-thrust_sp(2) > thr_max) { From 05a73d2821246557e6ae44639210fbd93a73f4b8 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sun, 20 Dec 2015 17:33:12 +0100 Subject: [PATCH 102/201] added takeoff logic for position controller to get the uav off the ground fast and transition smoothly to poctl after takeoff, added landing logic to reduce thrust to zero once on the ground --- .../mc_pos_control/mc_pos_control_main.cpp | 71 +++++++++++++++++-- .../mc_pos_control/mc_pos_control_params.c | 18 +++++ 2 files changed, 83 insertions(+), 6 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 00e6df0655..e98a4fcb58 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -180,6 +180,8 @@ private: param_t xy_ff; param_t tilt_max_air; param_t land_speed; + param_t tko_jmpspd; + param_t tko_speed; param_t tilt_max_land; param_t man_roll_max; param_t man_pitch_max; @@ -197,6 +199,8 @@ private: float thr_max; float tilt_max_air; float land_speed; + float tko_jmpspd; + float tko_speed; float tilt_max_land; float man_roll_max; float man_pitch_max; @@ -243,6 +247,8 @@ private: float _yaw; /**< yaw angle (euler) */ float _landing_thrust; hrt_abstime _landing_started; + int _takeoff_jumped; + float _vel_z_lp; /** * Update our local parameter cache. @@ -374,7 +380,9 @@ MulticopterPositionControl::MulticopterPositionControl() : _run_alt_control(true), _yaw(0.0f), _landing_thrust(1.0f), - _landing_started(0) + _landing_started(0), + _takeoff_jumped(0), + _vel_z_lp(0) { memset(&_vehicle_status, 0, sizeof(_vehicle_status)); memset(&_ctrl_state, 0, sizeof(_ctrl_state)); @@ -425,6 +433,8 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_handles.xy_ff = param_find("MPC_XY_FF"); _params_handles.tilt_max_air = param_find("MPC_TILTMAX_AIR"); _params_handles.land_speed = param_find("MPC_LAND_SPEED"); + _params_handles.tko_jmpspd = param_find("MPC_TKO_JMPSPD"); + _params_handles.tko_speed = param_find("MPC_TKO_SPEED"); _params_handles.tilt_max_land = param_find("MPC_TILTMAX_LND"); _params_handles.man_roll_max = param_find("MPC_MAN_R_MAX"); _params_handles.man_pitch_max = param_find("MPC_MAN_P_MAX"); @@ -487,6 +497,8 @@ MulticopterPositionControl::parameters_update(bool force) param_get(_params_handles.tilt_max_air, &_params.tilt_max_air); _params.tilt_max_air = math::radians(_params.tilt_max_air); param_get(_params_handles.land_speed, &_params.land_speed); + param_get(_params_handles.tko_jmpspd, &_params.tko_jmpspd); + param_get(_params_handles.tko_speed, &_params.tko_speed); param_get(_params_handles.tilt_max_land, &_params.tilt_max_land); _params.tilt_max_land = math::radians(_params.tilt_max_land); @@ -1344,6 +1356,31 @@ MulticopterPositionControl::task_main() _vel_sp(2) = _params.land_speed; } + /* velocity handling during takeoff */ + if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid + && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { + + switch (_takeoff_jumped) { + case 0: + /* do a quick jump until we shoot over takeoff speed */ + _vel_sp(2) = -_params.tko_jmpspd; + + if (_vel(2) < -_params.tko_speed) { + _takeoff_jumped = 1; + } + break; + + case 1: + /* slowly reduce forced takeoff speed to set climb rate */ + float tmp = 0.3f * dt + _vel_sp_prev(2); + _vel_sp(2) = math::min(tmp, -_params.tko_speed); + break; + } + + } else { + _takeoff_jumped = 0; + } + // limit total horizontal acceleration math::Vector<2> acc_hor; acc_hor(0) = (_vel_sp(0) - _vel_sp_prev(0)) / dt; @@ -1356,6 +1393,8 @@ MulticopterPositionControl::task_main() math::Vector<2> vel_sp_hor = acc_hor * dt + vel_sp_hor_prev; _vel_sp(0) = vel_sp_hor(0); _vel_sp(1) = vel_sp_hor(1); + + /*PX4_WARN("vel limited %.4f, %.4f", (double)_vel_sp(0), (double)_vel_sp(1));*/ } _vel_sp_prev = _vel_sp; @@ -1439,29 +1478,49 @@ MulticopterPositionControl::task_main() float thrust_abs = thrust_sp.length(); float tilt_max = _params.tilt_max_air; float thr_max = _params.thr_max; + _vel_z_lp = _vel_z_lp * 0.9f + 0.1f * _vel(2); /* adjust limits for landing mode */ if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { /* limit max tilt and min lift when landing */ tilt_max = _params.tilt_max_land; - if (_landing_started == 0) { - _landing_started = hrt_absolute_time(); - } if (thr_min < 0.0f) { thr_min = 0.0f; } + if (_landing_started == 0) { + _landing_started = hrt_absolute_time(); + } + float vel_z_change = (_vel(2) - _vel_prev(2)) / dt; + /* don't let it throttle up again during landing */ if (thrust_sp(2) < 0.0f && thrust_abs < _landing_thrust + /* fix landing thrust after a certain time when velocity change is minimal */ + && (float)fabs(vel_z_change) < 0.05f + && _vel_z_lp > 0.5f * _params.land_speed && hrt_elapsed_time(&_landing_started) > 15e5) { _landing_thrust = thrust_abs; } + + /* assume ground, reduce thrust */ + if (hrt_elapsed_time(&_landing_started) > 15e5 + && _landing_thrust > FLT_EPSILON + && _vel_z_lp < 0.1f) { + _landing_thrust = 0.0f; + } + + /* if we suddenly fall, reset landing logic and remove thrust limit */ + if (hrt_elapsed_time(&_landing_started) > 15e5 + && _landing_thrust < FLT_EPSILON + && vel_z_change > 4.0f) { + _landing_thrust = _params.thr_max; + _landing_started = 0; + } + /* add 5% to give it some margin to compensate external influences */ thr_max = _landing_thrust + 0.05f * _landing_thrust; - /*PX4_WARN("thrust: abs %.4f, sp(2) %.4f, _landing_thrust %.4f, vel_err(2) %.4f, vel_sp(2) %.4f, vel(2) %.4f", - (double) thrust_abs, (double) thrust_sp(2), (double) _landing_thrust, (double) vel_err(2), (double) _vel_sp(2), (double) _vel(2));*/ } else { _landing_started = 0; diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 65673a8284..7ef530c049 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -234,6 +234,24 @@ PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 15.0f); */ PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 1.0f); +/** + * Takeoff initial speed to jump off the ground + * + * @unit m/s + * @min 0.0 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_TKO_JMPSPD, 2.5f); + +/** + * Takeoff climb rate + * + * @unit m/s + * @min 0.0 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_TKO_SPEED, 1.5f); + /** * Max manual roll * From fe90e7882bb2749f505809a7297e8b9cffcf0472 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sun, 20 Dec 2015 17:41:21 +0100 Subject: [PATCH 103/201] do mission notifications for landing, switch to idle setpoint when landed --- src/modules/navigator/land.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp index b8182e2fed..2f9ae7e57c 100644 --- a/src/modules/navigator/land.cpp +++ b/src/modules/navigator/land.cpp @@ -74,6 +74,10 @@ Land::on_activation() { /* set current mission item to Land */ set_land_item(&_mission_item, true); + _navigator->get_mission_result()->reached = false; + _navigator->get_mission_result()->finished = false; + _navigator->set_mission_result_updated(); + reset_mission_item_reached(); /* convert mission item to current setpoint */ struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); @@ -89,7 +93,13 @@ Land::on_activation() void Land::on_active() { - if (is_mission_item_reached()) { + if (is_mission_item_reached() && !_navigator->get_mission_result()->finished) { + _navigator->get_mission_result()->finished = true; + _navigator->set_mission_result_updated(); set_idle_item(&_mission_item); + + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + _navigator->set_position_setpoint_triplet_updated(); } } From c033ef959a16ce135ff1f04bd78c1a52bf39bddd Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sun, 20 Dec 2015 17:41:47 +0100 Subject: [PATCH 104/201] proper setpoint handling on takeoff, switch to loiter sp when takeoff finished --- src/modules/navigator/takeoff.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp index 9851212320..2effa1b042 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -99,5 +99,11 @@ Takeoff::on_active() if (is_mission_item_reached() && !_navigator->get_mission_result()->finished) { _navigator->get_mission_result()->finished = true; _navigator->set_mission_result_updated(); + + /* set loiter item so position controllers stop doing takeoff logic */ + set_loiter_item(&_mission_item); + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + _navigator->set_position_setpoint_triplet_updated(); } } From d9878493bdf71b697b2af1d7e640a50e9d413b9d Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 21 Dec 2015 10:54:15 +0100 Subject: [PATCH 105/201] cleaning up takeoff/landing logic, commenting on magic values --- .../mc_pos_control/mc_pos_control_main.cpp | 29 +++++++++---------- 1 file changed, 14 insertions(+), 15 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index e98a4fcb58..e8ea88a23e 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -247,7 +247,7 @@ private: float _yaw; /**< yaw angle (euler) */ float _landing_thrust; hrt_abstime _landing_started; - int _takeoff_jumped; + bool _takeoff_jumped; float _vel_z_lp; /** @@ -381,7 +381,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _yaw(0.0f), _landing_thrust(1.0f), _landing_started(0), - _takeoff_jumped(0), + _takeoff_jumped(false), _vel_z_lp(0) { memset(&_vehicle_status, 0, sizeof(_vehicle_status)); @@ -1075,7 +1075,8 @@ void MulticopterPositionControl::control_auto(float dt) */ if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && _pos_sp_triplet.current.acceptance_radius > 0.0f - && (_pos - _pos_sp).length() < _pos_sp_triplet.current.acceptance_radius * 1.1f) { + /* need to detect we're close a bit before the navigator switches from takeoff to next waypoint */ + && (_pos - _pos_sp).length() < _pos_sp_triplet.current.acceptance_radius * 1.2f) { _reset_pos_sp = false; _reset_alt_sp = false; @@ -1360,25 +1361,23 @@ MulticopterPositionControl::task_main() if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { - switch (_takeoff_jumped) { - case 0: + if (!_takeoff_jumped) { /* do a quick jump until we shoot over takeoff speed */ _vel_sp(2) = -_params.tko_jmpspd; if (_vel(2) < -_params.tko_speed) { - _takeoff_jumped = 1; + _takeoff_jumped = true; } - break; - case 1: - /* slowly reduce forced takeoff speed to set climb rate */ - float tmp = 0.3f * dt + _vel_sp_prev(2); + } else { + /* slowly reduce forced takeoff speed to set climb rate (in 3 sec) */ + float decel = _params.tko_jmpspd - _params.tko_speed; + float tmp = decel / 3.0f * dt + _vel_sp_prev(2); _vel_sp(2) = math::min(tmp, -_params.tko_speed); - break; } } else { - _takeoff_jumped = 0; + _takeoff_jumped = false; } // limit total horizontal acceleration @@ -1393,8 +1392,6 @@ MulticopterPositionControl::task_main() math::Vector<2> vel_sp_hor = acc_hor * dt + vel_sp_hor_prev; _vel_sp(0) = vel_sp_hor(0); _vel_sp(1) = vel_sp_hor(1); - - /*PX4_WARN("vel limited %.4f, %.4f", (double)_vel_sp(0), (double)_vel_sp(1));*/ } _vel_sp_prev = _vel_sp; @@ -1478,7 +1475,8 @@ MulticopterPositionControl::task_main() float thrust_abs = thrust_sp.length(); float tilt_max = _params.tilt_max_air; float thr_max = _params.thr_max; - _vel_z_lp = _vel_z_lp * 0.9f + 0.1f * _vel(2); + /* filter vel_z over 1/8sec */ + _vel_z_lp = _vel_z_lp * (1.0f - dt * 8.0f) + dt * 8.0f * _vel(2); /* adjust limits for landing mode */ if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && @@ -1514,6 +1512,7 @@ MulticopterPositionControl::task_main() /* if we suddenly fall, reset landing logic and remove thrust limit */ if (hrt_elapsed_time(&_landing_started) > 15e5 && _landing_thrust < FLT_EPSILON + /* XXX: magic value, assuming free fall above 4m/s2 acceleration */ && vel_z_change > 4.0f) { _landing_thrust = _params.thr_max; _landing_started = 0; From ee0aa7b37d9c9d8b43e908f23bfc0f5a0ce73bd3 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 21 Dec 2015 11:40:44 +0100 Subject: [PATCH 106/201] recalculate absolute thrust before limiting --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index e8ea88a23e..8cfaeafbff 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1573,6 +1573,8 @@ MulticopterPositionControl::task_main() } /* limit max thrust */ + thrust_abs = thrust_sp.length(); /* recalculate because it might have changed */ + if (thrust_abs > thr_max) { if (thrust_sp(2) < 0.0f) { if (-thrust_sp(2) > thr_max) { From e0405617ef10723781388b43725b321953535817 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 21 Dec 2015 13:52:05 +0100 Subject: [PATCH 107/201] also don't reset possp if near a loiter sp --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 8cfaeafbff..0c8cb07a22 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1073,7 +1073,8 @@ void MulticopterPositionControl::control_auto(float dt) * if we're already near the current takeoff setpoint don't reset in case we switch back to posctl. * this makes the takeoff finish smoothly. */ - if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF + if ((_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF + || _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) && _pos_sp_triplet.current.acceptance_radius > 0.0f /* need to detect we're close a bit before the navigator switches from takeoff to next waypoint */ && (_pos - _pos_sp).length() < _pos_sp_triplet.current.acceptance_radius * 1.2f) { From 98bec0e175dc55f504c9a6e7f99e826b7909761b Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Tue, 22 Dec 2015 10:41:12 +0100 Subject: [PATCH 108/201] also filter acceleration to filter out the bump on the ground on landing --- .../mc_pos_control/mc_pos_control_main.cpp | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 0c8cb07a22..8ed95e58e6 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -249,6 +249,7 @@ private: hrt_abstime _landing_started; bool _takeoff_jumped; float _vel_z_lp; + float _acc_z_lp; /** * Update our local parameter cache. @@ -382,7 +383,8 @@ MulticopterPositionControl::MulticopterPositionControl() : _landing_thrust(1.0f), _landing_started(0), _takeoff_jumped(false), - _vel_z_lp(0) + _vel_z_lp(0), + _acc_z_lp(0) { memset(&_vehicle_status, 0, sizeof(_vehicle_status)); memset(&_ctrl_state, 0, sizeof(_ctrl_state)); @@ -1478,6 +1480,9 @@ MulticopterPositionControl::task_main() float thr_max = _params.thr_max; /* filter vel_z over 1/8sec */ _vel_z_lp = _vel_z_lp * (1.0f - dt * 8.0f) + dt * 8.0f * _vel(2); + /* filter vel_z change over 1/8sec */ + float vel_z_change = (_vel(2) - _vel_prev(2)) / dt; + _acc_z_lp = _acc_z_lp * (1.0f - dt * 8.0f) + dt * 8.0f * vel_z_change; /* adjust limits for landing mode */ if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && @@ -1492,12 +1497,11 @@ MulticopterPositionControl::task_main() if (_landing_started == 0) { _landing_started = hrt_absolute_time(); } - float vel_z_change = (_vel(2) - _vel_prev(2)) / dt; /* don't let it throttle up again during landing */ if (thrust_sp(2) < 0.0f && thrust_abs < _landing_thrust /* fix landing thrust after a certain time when velocity change is minimal */ - && (float)fabs(vel_z_change) < 0.05f + && (float)fabs(_acc_z_lp) < 0.1f && _vel_z_lp > 0.5f * _params.land_speed && hrt_elapsed_time(&_landing_started) > 15e5) { _landing_thrust = thrust_abs; @@ -1506,7 +1510,8 @@ MulticopterPositionControl::task_main() /* assume ground, reduce thrust */ if (hrt_elapsed_time(&_landing_started) > 15e5 && _landing_thrust > FLT_EPSILON - && _vel_z_lp < 0.1f) { + && _vel_z_lp < 0.1f + ) { _landing_thrust = 0.0f; } @@ -1514,7 +1519,9 @@ MulticopterPositionControl::task_main() if (hrt_elapsed_time(&_landing_started) > 15e5 && _landing_thrust < FLT_EPSILON /* XXX: magic value, assuming free fall above 4m/s2 acceleration */ - && vel_z_change > 4.0f) { + && (_acc_z_lp > 4.0f + || _vel_z_lp > 2.0f * _params.land_speed) + ) { _landing_thrust = _params.thr_max; _landing_started = 0; } From b5f3c2d30d85d871222f5cf9b8ea1a67f5d3b282 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Tue, 22 Dec 2015 12:20:11 +0100 Subject: [PATCH 109/201] ramp up jump velocity instead doing a huge step --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 8ed95e58e6..2891c15cda 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1365,8 +1365,10 @@ MulticopterPositionControl::task_main() && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { if (!_takeoff_jumped) { - /* do a quick jump until we shoot over takeoff speed */ - _vel_sp(2) = -_params.tko_jmpspd; + /* do a quick jump (ramp vel sp up in 1/2sec) until we shoot over takeoff speed */ + float accel = _vel_sp_prev(2) - _params.tko_jmpspd; + float tmp = _vel_sp_prev(2) + accel * dt * 2.0f; + _vel_sp(2) = math::max(tmp, -_params.tko_jmpspd); if (_vel(2) < -_params.tko_speed) { _takeoff_jumped = true; From 65299e7aaf16e7f24fa55ad9ee63c99989ef6337 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Thu, 24 Dec 2015 10:42:44 +0100 Subject: [PATCH 110/201] updated jmavsim iris config for takeoff and landing --- posix-configs/SITL/init/rcS_jmavsim_iris | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/posix-configs/SITL/init/rcS_jmavsim_iris b/posix-configs/SITL/init/rcS_jmavsim_iris index 88a3f3978e..f7c4982ed5 100644 --- a/posix-configs/SITL/init/rcS_jmavsim_iris +++ b/posix-configs/SITL/init/rcS_jmavsim_iris @@ -32,6 +32,12 @@ param set COM_RC_IN_MODE 1 param set NAV_ACC_RAD 2.0 param set RTL_RETURN_ALT 30.0 param set RTL_DESCEND_ALT 10.0 +param set MIS_TAKEOFF_ALT 5.0 +param set MPC_HOLD_MAX_Z 2.0 +param set MPC_HOLD_XY_DZ 0.1 +param set MPC_HOLD_Z_DZ 0.1 +param set MPC_Z_VEL_MAX 2.0 +param set MPC_Z_VEL_P 0.4 rgbledsim start tone_alarm start gyrosim start @@ -59,5 +65,6 @@ mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556 mavlink stream -r 20 -s RC_CHANNELS -u 14556 mavlink stream -r 250 -s HIGHRES_IMU -u 14556 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 +mavlink stream -r 20 -s MANUAL_CONTROL -u 14556 mavlink boot_complete sdlog2 start -r 100 -e -t -a From 52951801c9ac63787b8a46e1c0679a2f65774b94 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Thu, 24 Dec 2015 10:55:23 +0100 Subject: [PATCH 111/201] updated solo config for master --- ROMFS/px4fmu_common/init.d/4030_solo | 74 +++++++++++++++++++++++----- 1 file changed, 63 insertions(+), 11 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/4030_solo b/ROMFS/px4fmu_common/init.d/4030_solo index 507c4ef0cb..5fbdcc8e48 100644 --- a/ROMFS/px4fmu_common/init.d/4030_solo +++ b/ROMFS/px4fmu_common/init.d/4030_solo @@ -4,11 +4,7 @@ # # @type Quadrotor x # -# @output AUX1 feed-through of RC AUX1 channel -# @output AUX2 feed-through of RC AUX2 channel -# @output AUX3 feed-through of RC AUX3 channel -# -# @maintainer Lorenz Meier +# @maintainer Andreas Antener # sh /etc/init.d/rc.mc_defaults @@ -16,21 +12,77 @@ sh /etc/init.d/rc.mc_defaults if [ $AUTOCNF == yes ] then # tuning - param set MC_PITCHRATE_P 0.13 - param set MC_ROLLRATE_P 0.13 + param set MC_PITCHRATE_P 0.11 + param set MC_ROLLRATE_P 0.11 param set MPC_MANTHR_MIN 0.08 + param set MPC_XY_VEL_MAX 3.0 + param set MPC_Z_VEL_MAX 2.0 - # takeoff. land and RTL settings - param set MIS_TAKEOFF_ALT 5.0 + # INAV: higher GPS weights for better altitude control + param set INAV_W_Z_BARO 0.3 + param set INAV_W_Z_GPS_P 0.8 + param set INAV_W_Z_GPS_V 0.8 + + # takeoff, land and RTL settings + param set MIS_TAKEOFF_ALT 4.0 param set COM_DISARM_LAND 1 param set RTL_LAND_DELAY 1 + param set RTL_DESCEND_ALT 5.0 + param set RTL_RETURN_ALT 15.0 + param set MPC_TILTMAX_LND 8.0 + param set MPC_LAND_SPEED 0.4 param set MPC_HOLD_MAX_Z 1.5 + param set MPC_TKO_JMPSPD 2.0 + param set MPC_TKO_SPEED 1.5 - # enable high bandwidth mavlink by default - param set SYS_COMPANION 921601 + # setup + # main board rotation: pitch 180 + param set SENS_BOARD_ROT 12 + # solo external mag rotation + param set CAL_MAG0_ROT 30 + # no safety switch + param set CBRK_IO_SAFETY 22027 + # battery voltage not available yet + param set CBRK_SUPPLY_CHK 894281 + + # RC configuration + param set RC_MAP_MODE_SW 5 + param set RC_MAP_PITCH 2 + param set RC_MAP_ROLL 1 + param set RC_MAP_THROTTLE 3 + param set RC_MAP_YAW 4 + + param set RC1_DZ 10 + param set RC1_MAX 1988 + param set RC1_MIN 1003 + param set RC1_REV 1 + param set RC1_TRIM 1499 + param set RC2_DZ 10 + param set RC2_MAX 1987 + param set RC2_MIN 1023 + param set RC2_REV -1 + param set RC2_TRIM 1499 + param set RC3_DZ 10 + param set RC3_MAX 1877 + param set RC3_MIN 1023 + param set RC3_REV 1 + param set RC3_TRIM 1023 + param set RC4_DZ 10 + param set RC4_MAX 1998 + param set RC4_MIN 1012 + param set RC4_REV 1 + param set RC4_TRIM 1500 + param set RC5_DZ 10 + param set RC5_MAX 2000 + param set RC5_MIN 1000 + param set RC5_REV 1 + param set RC5_TRIM 1500 fi set MIXER solo set PWM_OUT 1234 set MIXER_AUX none + +# enable high-speed link on telem 1 +set MAVLINK_F "-d /dev/ttyS1 -b 921600 -r 80000 -m onboard -x" From 68c9c4ae0f4e6a8639fa8fda5bd05830c29a54fc Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 25 Dec 2015 13:42:23 +0100 Subject: [PATCH 112/201] better defaults for relevant landing/takeoff parameters --- src/modules/mc_pos_control/mc_pos_control_params.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 7ef530c049..3e00c5f58c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -223,7 +223,7 @@ PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR, 45.0f); * @max 90.0 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 15.0f); +PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 12.0f); /** * Landing descend rate @@ -232,7 +232,7 @@ PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 15.0f); * @min 0.0 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 1.0f); +PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 0.5f); /** * Takeoff initial speed to jump off the ground @@ -241,7 +241,7 @@ PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 1.0f); * @min 0.0 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_TKO_JMPSPD, 2.5f); +PARAM_DEFINE_FLOAT(MPC_TKO_JMPSPD, 2.0f); /** * Takeoff climb rate From bd3d53902aaf7a450ac8ef5e92b2a2bf5e719a3b Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 25 Dec 2015 13:47:47 +0100 Subject: [PATCH 113/201] readded missing rotation --- src/lib/conversion/rotation.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index bc6dc2e2c7..09336c15e0 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -118,6 +118,7 @@ const rot_lookup_t rot_lookup[] = { {270, 0, 270 }, {180, 270, 0 }, { 0, 90, 180 }, + { 90, 90, 0 }, { 90, 68, 293 }, }; From 6c04ab970b92c4bff4bdcf384b1d88c573c04c01 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 25 Dec 2015 20:21:45 +0100 Subject: [PATCH 114/201] fixed formatting --- src/lib/conversion/rotation.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index bc55454f61..872f22ec4a 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -242,12 +242,12 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z) case ROTATION_YAW_293_PITCH_68_ROLL_90: { float tmpx = x; - float tmpy = y; - float tmpz = z; - x = 0.143039f * tmpx + 0.368776f * tmpy + -0.918446f * tmpz; - y = -0.332133f * tmpx + -0.856289f * tmpy + -0.395546f * tmpz; - z = -0.932324f * tmpx + 0.361625f * tmpy + 0.000000f * tmpz; - return; + float tmpy = y; + float tmpz = z; + x = 0.143039f * tmpx + 0.368776f * tmpy + -0.918446f * tmpz; + y = -0.332133f * tmpx + -0.856289f * tmpy + -0.395546f * tmpz; + z = -0.932324f * tmpx + 0.361625f * tmpy + 0.000000f * tmpz; + return; } } } From ea1439c6273e8e4cbc0ef05ea05a1d6cb325e625 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Dec 2015 16:00:32 +0100 Subject: [PATCH 115/201] Commander: Support landing through commandline --- src/modules/commander/commander.cpp | 33 +++++++++++++++++++++++++++++ 1 file changed, 33 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e8520c62d2..7c745ed91b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -446,6 +446,28 @@ int commander_main(int argc, char *argv[]) return 0; } + if (!strcmp(argv[1], "land")) { + int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0); + + vehicle_command_s cmd = {}; + cmd.target_system = status.system_id; + cmd.target_component = status.component_id; + + cmd.command = vehicle_command_s::VEHICLE_CMD_NAV_LAND; + // cmd.param1 = 0.25f; /* minimum pitch */ + // /* param 2-3 unused */ + // cmd.param4 = home_position.yaw; + // cmd.param5 = home_position.lat; + // cmd.param6 = home_position.lon; + // cmd.param7 = home_position.alt; + + // XXX inspect use of publication handle + (void)orb_advertise(ORB_ID(vehicle_command), &cmd); + + px4_close(mavlink_fd_local); + return 0; + } + usage("unrecognized command"); return 1; } @@ -890,6 +912,17 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } break; + case vehicle_command_s::VEHICLE_CMD_NAV_LAND: { + /* ok, home set, use it to take off */ + if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_LAND)) { + warnx("landing!"); + } else { + warnx("landing denied"); + } + + } + break; + case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION: case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: From 2af066bc6aa5c3fe5b4e9a676c31be6462c995fd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 28 Dec 2015 11:14:03 +0100 Subject: [PATCH 116/201] guard against invalid yawmode values --- src/modules/navigator/mission.cpp | 3 ++- src/modules/navigator/mission.h | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 299b69b3d0..ab8e43add4 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -189,8 +189,9 @@ Mission::on_active() } /* see if we need to update the current yaw heading for rotary wing types */ - if (_navigator->get_vstatus()->is_rotary_wing + if (_navigator->get_vstatus()->is_rotary_wing && _param_yawmode.get() != MISSION_YAWMODE_NONE + && _param_yawmode.get() < MISSION_YAWMODE_MAX && _mission_type != MISSION_TYPE_NONE) { heading_sp_update(); } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index b0117cf1fe..c8a34132de 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -88,7 +88,8 @@ public: MISSION_YAWMODE_NONE = 0, MISSION_YAWMODE_FRONT_TO_WAYPOINT = 1, MISSION_YAWMODE_FRONT_TO_HOME = 2, - MISSION_YAWMODE_BACK_TO_HOME = 3 + MISSION_YAWMODE_BACK_TO_HOME = 3, + MISSION_YAWMODE_MAX = 4 }; private: From 7c52e8b96f132db3dabed275517f17225e3ea1e4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 28 Dec 2015 11:15:03 +0100 Subject: [PATCH 117/201] Improve landing for special yawmode settings --- src/modules/navigator/mission.cpp | 64 +++++++++++++++++++------------ 1 file changed, 40 insertions(+), 24 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index ab8e43add4..55628f3438 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -526,36 +526,52 @@ Mission::heading_sp_update() return; } - /* Don't do FOH for landing and takeoff waypoints, the ground may be near - * and the FW controller has a custom landing logic */ - if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) { + /* Don't change heading for takeoff waypoints, the ground may be near */ + if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { return; } /* set yaw angle for the waypoint iff a loiter time has been specified */ if (_waypoint_position_reached && _mission_item.time_inside > 0.0f) { _mission_item.yaw = _on_arrival_yaw; - /* always keep the front of the rotary wing pointing to the next waypoint */ - } else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_WAYPOINT) { - _mission_item.yaw = get_bearing_to_next_waypoint( - _navigator->get_global_position()->lat, - _navigator->get_global_position()->lon, - _mission_item.lat, - _mission_item.lon); - /* always keep the back of the rotary wing pointing towards home */ - } else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_HOME) { - _mission_item.yaw = get_bearing_to_next_waypoint( - _navigator->get_global_position()->lat, - _navigator->get_global_position()->lon, - _navigator->get_home_position()->lat, - _navigator->get_home_position()->lon); - /* always keep the back of the rotary wing pointing towards home */ - } else if (_param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME) { - _mission_item.yaw = _wrap_pi(get_bearing_to_next_waypoint( - _navigator->get_global_position()->lat, - _navigator->get_global_position()->lon, - _navigator->get_home_position()->lat, - _navigator->get_home_position()->lon) + M_PI_F); + } else { + + /* calculate direction the vehicle should point to: + * normal waypoint: current position to {waypoint or home or home + 180deg} + * landing waypoint: last waypoint to {waypoint or home or home + 180deg} + * For landing the last waypoint (= constant) is used to avoid excessive yawing near the ground + */ + double point_from_latlon[2]; + if (_mission_item.nav_cmd == NAV_CMD_LAND) { + point_from_latlon[0] = pos_sp_triplet->previous.lat; + point_from_latlon[1] = pos_sp_triplet->previous.lon; + } else { + point_from_latlon[0] = _navigator->get_global_position()->lat; + point_from_latlon[1] = _navigator->get_global_position()->lon; + } + + /* always keep the front of the rotary wing pointing to the next waypoint */ + if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_WAYPOINT) { + _mission_item.yaw = get_bearing_to_next_waypoint( + point_from_latlon[0], + point_from_latlon[1], + _mission_item.lat, + _mission_item.lon); + /* always keep the back of the rotary wing pointing towards home */ + } else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_HOME) { + _mission_item.yaw = get_bearing_to_next_waypoint( + point_from_latlon[0], + point_from_latlon[1], + _navigator->get_home_position()->lat, + _navigator->get_home_position()->lon); + /* always keep the back of the rotary wing pointing towards home */ + } else if (_param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME) { + _mission_item.yaw = _wrap_pi(get_bearing_to_next_waypoint( + point_from_latlon[0], + point_from_latlon[1], + _navigator->get_home_position()->lat, + _navigator->get_home_position()->lon) + M_PI_F); + } } mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); From 76cb67ae356e016fa7e315d0d99e8071e88e1a8b Mon Sep 17 00:00:00 2001 From: dogmaphobic Date: Sun, 27 Dec 2015 16:55:23 -0500 Subject: [PATCH 118/201] Add mavlink log messages handler --- .gitignore | 1 + src/modules/mavlink/CMakeLists.txt | 1 + src/modules/mavlink/mavlink_log_handler.cpp | 529 ++++++++++++++++++++ src/modules/mavlink/mavlink_log_handler.h | 117 +++++ src/modules/mavlink/mavlink_main.cpp | 9 + src/modules/mavlink/mavlink_main.h | 2 + 6 files changed, 659 insertions(+) create mode 100644 src/modules/mavlink/mavlink_log_handler.cpp create mode 100644 src/modules/mavlink/mavlink_log_handler.h diff --git a/.gitignore b/.gitignore index 07848d7a90..f936568899 100644 --- a/.gitignore +++ b/.gitignore @@ -23,6 +23,7 @@ build_*/ core cscope.out Firmware.sublime-workspace +user.sublime* Images/*.bin Images/*.px4 mavlink/include/mavlink/v0.9/ diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt index c8a0e7d870..3ae2e570ec 100644 --- a/src/modules/mavlink/CMakeLists.txt +++ b/src/modules/mavlink/CMakeLists.txt @@ -56,6 +56,7 @@ px4_add_module( mavlink_rate_limiter.cpp mavlink_receiver.cpp mavlink_ftp.cpp + mavlink_log_handler.cpp DEPENDS platforms__common ) diff --git a/src/modules/mavlink/mavlink_log_handler.cpp b/src/modules/mavlink/mavlink_log_handler.cpp new file mode 100644 index 0000000000..350973a5e5 --- /dev/null +++ b/src/modules/mavlink/mavlink_log_handler.cpp @@ -0,0 +1,529 @@ +/**************************************************************************** + * + * Copyright (c) 2014, 2015 PX4 Development Team. 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 PX4 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 mavlink_log_handler.h +/// @author px4dev, Gus Grubba + +#include "mavlink_log_handler.h" +#include "mavlink_main.h" +#include +#include + +#define MOUNTPOINT PX4_ROOTFSDIR "/fs/microsd" +static const char *kLogRoot = MOUNTPOINT "/log"; +static const char *kLogData = MOUNTPOINT "/logdata.txt"; +static const char *kTmpData = MOUNTPOINT "/$log$.txt"; + +#ifdef __PX4_NUTTX + #define PX4LOG_REGULAR_FILE DTYPE_FILE + #define PX4LOG_DIRECTORY DTYPE_DIRECTORY +#else + #define PX4LOG_REGULAR_FILE DT_REG + #define PX4LOG_DIRECTORY DT_DIR +#endif + +// #define MAVLINK_LOG_HANDLER_VERBOSE + +#ifdef MAVLINK_LOG_HANDLER_VERBOSE +#define PX4LOG_WARN(fmt, ...) warnx(fmt, ##__VA_ARGS__) +#else +#define PX4LOG_WARN(fmt, ...) +#endif + +//------------------------------------------------------------------- +static bool +stat_file(const char* file, uint32_t* date = 0, uint32_t* size = 0) { + struct stat st; + if (stat(file, &st) == 0) { + if (date) *date = st.st_mtime; + if (size) *size = st.st_size; + return true; + } + return false; +} + +//------------------------------------------------------------------- +MavlinkLogHandler * +MavlinkLogHandler::new_instance(Mavlink *mavlink) +{ + return new MavlinkLogHandler(mavlink); +} + +//------------------------------------------------------------------- +MavlinkLogHandler::MavlinkLogHandler(Mavlink *mavlink) + : MavlinkStream(mavlink) + , _pLogHandlerHelper(0) +{ + +} + +//------------------------------------------------------------------- +void +MavlinkLogHandler::handle_message(const mavlink_message_t *msg) +{ + switch (msg->msgid) { + case MAVLINK_MSG_ID_LOG_REQUEST_LIST: + _log_request_list(msg); + break; + case MAVLINK_MSG_ID_LOG_REQUEST_DATA: + _log_request_data(msg); + break; + case MAVLINK_MSG_ID_LOG_ERASE: + _log_request_erase(msg); + break; + case MAVLINK_MSG_ID_LOG_REQUEST_END: + _log_request_end(msg); + break; + } +} + +//------------------------------------------------------------------- +const char* +MavlinkLogHandler::get_name(void) const +{ + return "MAVLINK_LOG_HANDLER"; +} + +//------------------------------------------------------------------- +uint8_t +MavlinkLogHandler::get_id(void) +{ + return MAVLINK_MSG_ID_LOG_ENTRY; +} + +//------------------------------------------------------------------- +unsigned +MavlinkLogHandler::get_size(void) +{ + //-- Sending Log Entries + if (_pLogHandlerHelper && _pLogHandlerHelper->current_status == LogListHelper::LOG_HANDLER_LISTING) { + return MAVLINK_MSG_ID_LOG_ENTRY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + //-- Sending Log Data (contents of one of the log files) + } else if (_pLogHandlerHelper && _pLogHandlerHelper->current_status == LogListHelper::LOG_HANDLER_SENDING_DATA) { + return MAVLINK_MSG_ID_LOG_DATA_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + //-- Idle + } else { + return 0; + } +} + +//------------------------------------------------------------------- +void +MavlinkLogHandler::send(const hrt_abstime /*t*/) +{ + //-- Send log entry stream packets until buffer is full + while (_pLogHandlerHelper && _pLogHandlerHelper->current_status == LogListHelper::LOG_HANDLER_LISTING && _mavlink->get_free_tx_buf() > get_size()) { + _log_send_listing(); + }; + //-- An arbitrary count of max log data packets in one go + int count = 100; + while (_pLogHandlerHelper && _pLogHandlerHelper->current_status == LogListHelper::LOG_HANDLER_SENDING_DATA && _mavlink->get_free_tx_buf() > get_size() && --count) { + _log_send_data(); + }; +} + +//------------------------------------------------------------------- +void +MavlinkLogHandler::_log_request_list(const mavlink_message_t *msg) +{ + mavlink_log_request_list_t request; + mavlink_msg_log_request_list_decode(msg, &request); + //-- Clear any existing request + if(_pLogHandlerHelper) + delete _pLogHandlerHelper; + //-- Prepare new request + _pLogHandlerHelper = new LogListHelper; + if (_pLogHandlerHelper->log_count) + { + //-- Define (and clamp) range + _pLogHandlerHelper->next_entry = request.start < _pLogHandlerHelper->log_count ? request.start : _pLogHandlerHelper->log_count - 1; + _pLogHandlerHelper->last_entry = request.end < _pLogHandlerHelper->log_count ? request.end : _pLogHandlerHelper->log_count - 1; + } + //-- Send first response (the rest, if any, is sent by the stream system) + if(_mavlink->get_free_tx_buf() > get_size()) + _log_send_listing(); +} + +//------------------------------------------------------------------- +void +MavlinkLogHandler::_log_request_data(const mavlink_message_t *msg) +{ + //-- If we haven't listed, we can't do much + if(!_pLogHandlerHelper) { + PX4LOG_WARN("MavlinkLogHandler::_log_request_data Log request with no list requested.\n"); + return; + } + mavlink_log_request_data_t request; + mavlink_msg_log_request_data_decode(msg, &request); + //-- Does the requested log exist? + if (request.id >= _pLogHandlerHelper->log_count) { + PX4LOG_WARN("MavlinkLogHandler::_log_request_data Requested log %u but we only have %u.\n", request.id, _pLogHandlerHelper->log_count); + return; + } + //-- If we were sending log entries, stop it + _pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE; + //-- Init send log dataset + _pLogHandlerHelper->current_log_filename[0] = 0; + _pLogHandlerHelper->current_log_index = request.id; + uint32_t time_utc = 0; + _pLogHandlerHelper->get_entry(_pLogHandlerHelper->current_log_index, _pLogHandlerHelper->current_log_size, time_utc, _pLogHandlerHelper->current_log_filename); + _pLogHandlerHelper->current_log_data_offset = request.ofs; + if (_pLogHandlerHelper->current_log_data_offset >= _pLogHandlerHelper->current_log_size) { + _pLogHandlerHelper->current_log_data_remaining = 0; + } else { + _pLogHandlerHelper->current_log_data_remaining = _pLogHandlerHelper->current_log_size - request.ofs; + } + if (_pLogHandlerHelper->current_log_data_remaining > request.count) { + _pLogHandlerHelper->current_log_data_remaining = request.count; + } + _pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_SENDING_DATA; + //-- Send first chunck. Rest is sent by stream system. + if(_mavlink->get_free_tx_buf() > get_size()) + _log_send_data(); +} + +//------------------------------------------------------------------- +void +MavlinkLogHandler::_log_request_erase(const mavlink_message_t *msg) +{ + /* + mavlink_log_erase_t request; + mavlink_msg_log_erase_decode(msg, &request); + */ + if (_pLogHandlerHelper) { + delete _pLogHandlerHelper; + _pLogHandlerHelper = 0; + } + LogListHelper::delete_all(kLogRoot); +} + +//------------------------------------------------------------------- +void +MavlinkLogHandler::_log_request_end(const mavlink_message_t *msg) +{ + if (_pLogHandlerHelper) { + delete _pLogHandlerHelper; + _pLogHandlerHelper = 0; + } +} + +//------------------------------------------------------------------- +void +MavlinkLogHandler::_log_send_listing() +{ + mavlink_log_entry_t response; + _pLogHandlerHelper->get_entry(_pLogHandlerHelper->next_entry, response.size, response.time_utc); + response.id = _pLogHandlerHelper->next_entry; + response.num_logs = _pLogHandlerHelper->log_count; + response.last_log_num = _pLogHandlerHelper->last_entry; + _mavlink->send_message(MAVLINK_MSG_ID_LOG_ENTRY, &response); + //-- If we're done listing, flag it. + if (_pLogHandlerHelper->next_entry == _pLogHandlerHelper->last_entry) { + _pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE; + } else { + _pLogHandlerHelper->next_entry++; + } +} + +//------------------------------------------------------------------- +void +MavlinkLogHandler::_log_send_data() +{ + mavlink_log_data_t response; + uint32_t len = _pLogHandlerHelper->current_log_data_remaining; + if (len > sizeof(response.data)) { + len = sizeof(response.data); + } + size_t read_size = _pLogHandlerHelper->get_log_data(len, response.data); + if (read_size < sizeof(response.data)) { + memset(&response.data[read_size], 0, sizeof(response.data) - read_size); + } + response.ofs = _pLogHandlerHelper->current_log_data_offset; + response.id = _pLogHandlerHelper->current_log_index; + response.count = read_size; + _mavlink->send_message(MAVLINK_MSG_ID_LOG_DATA, &response); + _pLogHandlerHelper->current_log_data_offset += read_size; + _pLogHandlerHelper->current_log_data_remaining -= read_size; + if (read_size < sizeof(response.data) || _pLogHandlerHelper->current_log_data_remaining == 0) { + _pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE; + } +} + +//------------------------------------------------------------------- +LogListHelper::LogListHelper() + : next_entry(0) + , last_entry(0) + , log_count(0) + , current_status(LOG_HANDLER_LISTING) + , current_log_index(0) + , current_log_size(0) + , current_log_data_offset(0) + , current_log_data_remaining(0) +{ + _init(); +} + +//------------------------------------------------------------------- +LogListHelper::~LogListHelper() +{ + // Remove log data files (if any) + unlink(kLogData); + unlink(kTmpData); +} + +//------------------------------------------------------------------- +bool +LogListHelper::get_entry(int idx, uint32_t& size, uint32_t& date, char* filename) +{ + //-- Find log file in log list file created during init() + size = 0; + date = 0; + bool result = false; + //-- Open list of log files + FILE* f = ::fopen(kLogData, "r"); + if (f) { + //--- Find requested entry + char line[160]; + int count = 0; + while (fgets(line, sizeof(line), f)) { + //-- Found our "index" + if(count++ == idx) { + char file[128]; + if(sscanf(line, "%u %u %s", &date, &size, file) == 3) { + if(filename) { + strcpy(filename, file); + } + result = true; + break; + } + } + } + fclose(f); + } + return result; +} + +//------------------------------------------------------------------- +size_t +LogListHelper::get_log_data(uint8_t len, uint8_t* buffer) +{ + if(!current_log_filename[0]) + return 0; + FILE* f = fopen(current_log_filename, "r"); + if (!f) { + PX4LOG_WARN("MavlinkLogHandler::get_log_data Could not open %s\n", current_log_filename); + return 0; + } + if(fseek(f, current_log_data_offset, SEEK_SET)) { + fclose(f); + PX4LOG_WARN("MavlinkLogHandler::get_log_data Seek error in %s\n", current_log_filename); + return 0; + } + size_t result = fread(buffer, 1, len, f); + fclose(f); + return result; +} + +//------------------------------------------------------------------- +void +LogListHelper::_init() +{ + /* + + When this helper is created, it scans the log directory + and collects all log files found into one file for easy, + subsequent access. + */ + + current_log_filename[0] = 0; + // Remove old log data file (if any) + unlink(kLogData); + // Open log directory + DIR *dp = opendir(kLogRoot); + if (dp == nullptr) { + // No log directory. Nothing to do. + return; + } + // Create work file + FILE* f = ::fopen(kTmpData, "w"); + if (!f) { + PX4LOG_WARN("MavlinkLogHandler::init Error creating %s\n", kTmpData); + closedir(dp); + return; + } + // Scan directory and collect log files + struct dirent entry, *result = nullptr; + while (readdir_r(dp, &entry, &result) == 0) { + // no more entries? + if (result == nullptr) { + break; + } + if (entry.d_type == PX4LOG_DIRECTORY) + { + time_t tt; + char log_path[128]; + snprintf(log_path, sizeof(log_path), "%s/%s", kLogRoot, entry.d_name); + if (_get_session_date(log_path, entry.d_name, tt)) { + _scan_logs(f, log_path, tt); + } + } + } + closedir(dp); + fclose(f); + // Rename temp file to data file + if (rename(kTmpData, kLogData)) { + PX4LOG_WARN("MavlinkLogHandler::init Error renaming %s\n", kTmpData); + log_count = 0; + } +} + +//------------------------------------------------------------------- +bool +LogListHelper::_get_session_date(const char* path, const char* dir, time_t& date) +{ + if(strlen(dir) > 4) { + // Convert "sess000" to 00:00 Jan 1 1970 (day per session) + if (strncmp(dir, "sess", 4) == 0) { + unsigned u; + if(sscanf(&dir[4], "%u", &u) == 1) { + date = u * 60 * 60 * 24; + return true; + } + } else { + if (stat_file(path, &date)) { + return true; + } + /* strptime not available for some reason + // Get date from directory name + struct tm tt; + if(strptime(dir, "%Y-%m-%d", &tt)) { + date = mktime(&tt); + return true; + } + */ + } + } + return false; +} + +//------------------------------------------------------------------- +void +LogListHelper::_scan_logs(FILE* f, const char* dir, time_t& date) +{ + DIR *dp = opendir(dir); + if (dp) { + struct dirent entry, *result = nullptr; + while (readdir_r(dp, &entry, &result) == 0) { + // no more entries? + if (result == nullptr) { + break; + } + if (entry.d_type == PX4LOG_REGULAR_FILE) { + time_t ldate = date; + uint32_t size = 0; + char log_file_path[128]; + snprintf(log_file_path, sizeof(log_file_path), "%s/%s", dir, entry.d_name); + if(_get_log_time_size(log_file_path, entry.d_name, ldate, size)) { + //-- Write entry out to list file + fprintf(f, "%u %u %s\n", ldate, size, log_file_path); + log_count++; + } + } + } + } +} + +//------------------------------------------------------------------- +bool +LogListHelper::_get_log_time_size(const char* path, const char* file, time_t& date, uint32_t& size) +{ + if(file && file[0] && strstr(file, ".px4log")) { + // Convert "log000" to 00:00 (minute per flight in session) + if (strncmp(file, "log", 3) == 0) { + unsigned u; + if(sscanf(&file[3], "%u", &u) == 1) { + date += (u * 60); + if (stat_file(path, 0, &size)) { + return true; + } + } + } else { + if (stat_file(path, &date, &size)) { + return true; + } + /* strptime not available for some reason + // Get time from file name + struct tm tt; + if(strptime(file, "%H_%M_%S", &tt)) { + date += mktime(&tt); + return true; + } + */ + } + } + return false; +} + +//------------------------------------------------------------------- +void +LogListHelper::delete_all(const char* dir) +{ + //-- Open log directory + DIR *dp = opendir(dir); + if (dp == nullptr) { + return; + } + struct dirent entry, *result = nullptr; + while (readdir_r(dp, &entry, &result) == 0) { + // no more entries? + if (result == nullptr) { + break; + } + if (entry.d_type == PX4LOG_DIRECTORY && entry.d_name[0] != '.') { + char log_path[128]; + snprintf(log_path, sizeof(log_path), "%s/%s", dir, entry.d_name); + LogListHelper::delete_all(log_path); + if(rmdir(log_path)) { + PX4LOG_WARN("MavlinkLogHandler::delete_all Error removing %s\n", log_path); + } + } + if (entry.d_type == PX4LOG_REGULAR_FILE) { + char log_path[128]; + snprintf(log_path, sizeof(log_path), "%s/%s", dir, entry.d_name); + if(unlink(log_path)) { + PX4LOG_WARN("MavlinkLogHandler::delete_all Error deleting %s\n", log_path); + } + } + } + closedir(dp); +} diff --git a/src/modules/mavlink/mavlink_log_handler.h b/src/modules/mavlink/mavlink_log_handler.h new file mode 100644 index 0000000000..92b1bec4bc --- /dev/null +++ b/src/modules/mavlink/mavlink_log_handler.h @@ -0,0 +1,117 @@ +/**************************************************************************** + * + * Copyright (c) 2014, 2015 PX4 Development Team. 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 PX4 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. + * + ****************************************************************************/ + +#pragma once + +/// @file mavlink_log_handler.h +/// @author px4dev, Gus Grubba + +#include +#include +#include +#include +#include +#include "mavlink_stream.h" + +class Mavlink; + +// Log Listing Helper +class LogListHelper +{ +public: + LogListHelper(); + ~LogListHelper(); + +public: + static void delete_all(const char* dir); + +public: + + bool get_entry (int idx, uint32_t& size, uint32_t& date, char* filename = 0); + size_t get_log_data (uint8_t len, uint8_t* buffer); + + enum { + LOG_HANDLER_IDLE, + LOG_HANDLER_LISTING, + LOG_HANDLER_SENDING_DATA + }; + + int next_entry; + int last_entry; + int log_count; + + int current_status; + uint16_t current_log_index; + uint32_t current_log_size; + uint32_t current_log_data_offset; + uint32_t current_log_data_remaining; + char current_log_filename[128]; + +private: + void _init (); + bool _get_session_date (const char* path, const char* dir, time_t& date); + void _scan_logs (FILE* f, const char* dir, time_t& date); + bool _get_log_time_size (const char* path, const char* file, time_t& date, uint32_t& size); +}; + +// MAVLink LOG_* Message Handler +class MavlinkLogHandler : public MavlinkStream +{ +public: + /// @brief Contructor is only public so unit test code can new objects. + MavlinkLogHandler(Mavlink *mavlink); + + static MavlinkLogHandler *new_instance(Mavlink *mavlink); + + // Handle possible LOG message + void handle_message (const mavlink_message_t *msg); + + // Overrides from MavlinkStream + const char* get_name (void) const; + uint8_t get_id (void); + unsigned get_size (void); + void send (const hrt_abstime t); + +private: + void _log_message (const mavlink_message_t *msg); + void _log_request_list (const mavlink_message_t *msg); + void _log_request_data (const mavlink_message_t *msg); + void _log_request_erase (const mavlink_message_t *msg); + void _log_request_end (const mavlink_message_t *msg); + void _log_send_listing (); + void _log_send_data (); + +private: + LogListHelper *_pLogHandlerHelper; + +}; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 8e575e1b8f..9d76fa8bf3 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -147,6 +147,7 @@ Mavlink::Mavlink() : _mission_manager(nullptr), _parameters_manager(nullptr), _mavlink_ftp(nullptr), + _mavlink_log_handler(nullptr), _mode(MAVLINK_MODE_NORMAL), _channel(MAVLINK_COMM_0), _radio_id(0), @@ -1032,6 +1033,9 @@ Mavlink::handle_message(const mavlink_message_t *msg) /* handle packet with ftp component */ _mavlink_ftp->handle_message(msg); + /* handle packet with log component */ + _mavlink_log_handler->handle_message(msg); + if (get_forwarding_on()) { /* forward any messages to other mavlink instances */ Mavlink::forward_message(msg, this); @@ -1669,6 +1673,11 @@ Mavlink::task_main(int argc, char *argv[]) _mavlink_ftp->set_interval(interval_from_rate(80.0f)); LL_APPEND(_streams, _mavlink_ftp); + /* MAVLINK_Log_Handler */ + _mavlink_log_handler = (MavlinkLogHandler *) MavlinkLogHandler::new_instance(this); + _mavlink_log_handler->set_interval(interval_from_rate(80.0f)); + LL_APPEND(_streams, _mavlink_log_handler); + /* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on * remote requests rate. Rate specified here controls how much bandwidth we will reserve for * mission messages. */ diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 37c66b5963..c610bc1b74 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -67,6 +67,7 @@ #include "mavlink_mission.h" #include "mavlink_parameters.h" #include "mavlink_ftp.h" +#include "mavlink_log_handler.h" enum Protocol { SERIAL = 0, @@ -369,6 +370,7 @@ private: MavlinkMissionManager *_mission_manager; MavlinkParametersManager *_parameters_manager; MavlinkFTP *_mavlink_ftp; + MavlinkLogHandler *_mavlink_log_handler; MAVLINK_MODE _mode; From 1869b6ff97b15790c965d107e025fe4efa80f465 Mon Sep 17 00:00:00 2001 From: dogmaphobic Date: Mon, 28 Dec 2015 16:05:16 -0500 Subject: [PATCH 119/201] Fix build error. --- src/modules/mavlink/mavlink_log_handler.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_log_handler.cpp b/src/modules/mavlink/mavlink_log_handler.cpp index 350973a5e5..7fb78448d4 100644 --- a/src/modules/mavlink/mavlink_log_handler.cpp +++ b/src/modules/mavlink/mavlink_log_handler.cpp @@ -62,7 +62,7 @@ static const char *kTmpData = MOUNTPOINT "/$log$.txt"; //------------------------------------------------------------------- static bool -stat_file(const char* file, uint32_t* date = 0, uint32_t* size = 0) { +stat_file(const char* file, time_t* date = 0, uint32_t* size = 0) { struct stat st; if (stat(file, &st) == 0) { if (date) *date = st.st_mtime; From b4e69ccd4b68627762c7e2a3aa67d3d83ed53844 Mon Sep 17 00:00:00 2001 From: dogmaphobic Date: Mon, 28 Dec 2015 17:02:32 -0500 Subject: [PATCH 120/201] One day I will get this right. I had not tested the build under POSIX --- src/modules/mavlink/mavlink_log_handler.cpp | 2 +- src/modules/mavlink/mavlink_log_handler.h | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_log_handler.cpp b/src/modules/mavlink/mavlink_log_handler.cpp index 7fb78448d4..853047030a 100644 --- a/src/modules/mavlink/mavlink_log_handler.cpp +++ b/src/modules/mavlink/mavlink_log_handler.cpp @@ -455,7 +455,7 @@ LogListHelper::_scan_logs(FILE* f, const char* dir, time_t& date) snprintf(log_file_path, sizeof(log_file_path), "%s/%s", dir, entry.d_name); if(_get_log_time_size(log_file_path, entry.d_name, ldate, size)) { //-- Write entry out to list file - fprintf(f, "%u %u %s\n", ldate, size, log_file_path); + fprintf(f, "%u %u %s\n", (unsigned)ldate, (unsigned)size, log_file_path); log_count++; } } diff --git a/src/modules/mavlink/mavlink_log_handler.h b/src/modules/mavlink/mavlink_log_handler.h index 92b1bec4bc..ebbbe116a8 100644 --- a/src/modules/mavlink/mavlink_log_handler.h +++ b/src/modules/mavlink/mavlink_log_handler.h @@ -88,7 +88,6 @@ private: class MavlinkLogHandler : public MavlinkStream { public: - /// @brief Contructor is only public so unit test code can new objects. MavlinkLogHandler(Mavlink *mavlink); static MavlinkLogHandler *new_instance(Mavlink *mavlink); From 6e1f54e2ff62d9cf7d5f2d7044d976f0308d7d2d Mon Sep 17 00:00:00 2001 From: lchish Date: Thu, 22 Oct 2015 09:22:55 +1300 Subject: [PATCH 121/201] Add missing stm32f4discovery make targets --- .travis.yml | 6 +- Makefile | 3 + .../nuttx_px4-stm32f4discovery_default.cmake | 87 +++++++++++++++++++ cmake/nuttx/px4_impl_nuttx.cmake | 8 ++ .../px4-stm32f4discovery/px4discovery_init.c | 12 --- src/firmware/nuttx/CMakeLists.txt | 2 +- 6 files changed, 103 insertions(+), 15 deletions(-) create mode 100644 cmake/configs/nuttx_px4-stm32f4discovery_default.cmake diff --git a/.travis.yml b/.travis.yml index ccd0aafbcd..d984d2a83e 100644 --- a/.travis.yml +++ b/.travis.yml @@ -110,6 +110,7 @@ script: - ../Tools/uavcan_copy.sh - cd .. - echo 'Building NuttX px4fmu-v4 Firmware..' && make px4fmu-v4_default + - echo 'Building NuttX px4-stm32f4discovery Firmware..' && make px4-stm32f4discovery_default - echo 'Running Tests..' && make px4fmu-v2_default test after_success: @@ -117,8 +118,9 @@ after_success: cp build_px4fmu-v1_default/src/firmware/nuttx/nuttx-px4fmu-v1-default.px4 px4fmu-v1_default.px4 && cp build_px4fmu-v2_default/src/firmware/nuttx/nuttx-px4fmu-v2-default.px4 px4fmu-v2_default.px4 && cp build_px4fmu-v4_default/src/firmware/nuttx/nuttx-px4fmu-v4-default.px4 px4fmu-v4_default.px4 - && zip Firmware.zip px4fmu-v1_default.px4 px4fmu-v2_default.px4 px4fmu-v4_default.px4 - && ./CI-Tools/s3cmd-put px4fmu-v1_default.px4 px4fmu-v2_default.px4 px4fmu-v4_default.px4 build_px4fmu-v2_default/parameters.xml build_px4fmu-v2_default/airframes.xml CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/ + && cp build_px4-stm32f4discovery_default/src/firmware/nuttx/nuttx-px4-stm32f4discovery-default.px4 px4-stm32f4discovery-default.px4 + && zip Firmware.zip px4fmu-v1_default.px4 px4fmu-v2_default.px4 px4fmu-v4_default.px4 px4-stm32f4discovery-default.px4 + && ./CI-Tools/s3cmd-put px4fmu-v1_default.px4 px4fmu-v2_default.px4 px4fmu-v4_default.px4 px4-stm32f4discovery-default.px4 build_px4fmu-v2_default/parameters.xml build_px4fmu-v2_default/airframes.xml CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/ && ./CI-Tools/s3cmd-put Firmware.zip archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/ && ./CI-Tools/s3cmd-put CI-Tools/directory/index.html archives/Firmware/$TRAVIS_BRANCH/ && ./CI-Tools/s3cmd-put CI-Tools/index.html index.html diff --git a/Makefile b/Makefile index ee2670244d..77abd708a6 100644 --- a/Makefile +++ b/Makefile @@ -133,6 +133,9 @@ px4fmu-v2_default: px4fmu-v4_default: $(call cmake-build,nuttx_px4fmu-v4_default) +px4-stm32f4discovery_default: + $(call cmake-build,nuttx_px4-stm32f4discovery_default) + px4fmu-v2_simple: $(call cmake-build,nuttx_px4fmu-v2_simple) diff --git a/cmake/configs/nuttx_px4-stm32f4discovery_default.cmake b/cmake/configs/nuttx_px4-stm32f4discovery_default.cmake new file mode 100644 index 0000000000..8815992e40 --- /dev/null +++ b/cmake/configs/nuttx_px4-stm32f4discovery_default.cmake @@ -0,0 +1,87 @@ +include(nuttx/px4_impl_nuttx) + +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake) + +set(config_module_list + # + # Board support modules + # + drivers/device + drivers/stm32 + drivers/led + drivers/boards/px4-stm32f4discovery + + # + # System commands + # + systemcmds/bl_update + systemcmds/mixer + systemcmds/param + systemcmds/perf + systemcmds/reboot + systemcmds/top + systemcmds/config + systemcmds/nshterm + systemcmds/ver + + # + # Library modules + # + modules/param + modules/systemlib + modules/systemlib/mixer + modules/controllib + modules/uORB + + # + # Libraries + # + #lib/mathlib/CMSIS + lib/mathlib + lib/mathlib/math/filter + lib/ecl + lib/external_lgpl + lib/geo + lib/conversion + platforms/nuttx + + # had to add for cmake, not sure why wasn't in original config + platforms/common + platforms/nuttx/px4_layer + + # + # Demo apps + # + #examples/math_demo + # Tutorial code from + # https://px4.io/dev/px4_simple_app + examples/px4_simple_app + + # Tutorial code from + # https://px4.io/dev/daemon + #examples/px4_daemon_app + + # Tutorial code from + # https://px4.io/dev/debug_values + #examples/px4_mavlink_debug + + # Tutorial code from + # https://px4.io/dev/example_fixedwing_control + #examples/fixedwing_control + + # Hardware test + #examples/hwtest +) + +set(config_extra_builtin_cmds + serdis + sercon + ) + +add_custom_target(sercon) +set_target_properties(sercon PROPERTIES + MAIN "sercon" STACK "2048") + +add_custom_target(serdis) +set_target_properties(serdis PROPERTIES + MAIN "serdis" STACK "2048") diff --git a/cmake/nuttx/px4_impl_nuttx.cmake b/cmake/nuttx/px4_impl_nuttx.cmake index ae3d95eaf5..783886e373 100644 --- a/cmake/nuttx/px4_impl_nuttx.cmake +++ b/cmake/nuttx/px4_impl_nuttx.cmake @@ -467,6 +467,14 @@ function(px4_os_add_flags) -mfpu=fpv4-sp-d16 -mfloat-abi=hard ) + elseif (${BOARD} STREQUAL "px4-stm32f4discovery") + set(cpu_flags + -mcpu=cortex-m4 + -mthumb + -march=armv7e-m + -mfpu=fpv4-sp-d16 + -mfloat-abi=hard + ) elseif (${BOARD} STREQUAL "aerocore") set(cpu_flags -mcpu=cortex-m4 diff --git a/src/drivers/boards/px4-stm32f4discovery/px4discovery_init.c b/src/drivers/boards/px4-stm32f4discovery/px4discovery_init.c index fb1b282364..1c74be9058 100644 --- a/src/drivers/boards/px4-stm32f4discovery/px4discovery_init.c +++ b/src/drivers/boards/px4-stm32f4discovery/px4discovery_init.c @@ -129,18 +129,6 @@ stm32_boardinitialize(void) #include -#ifdef __cplusplus -__EXPORT int matherr(struct __exception *e) -{ - return 1; -} -#else -__EXPORT int matherr(struct exception *e) -{ - return 1; -} -#endif - __EXPORT int nsh_archinitialize(void) { diff --git a/src/firmware/nuttx/CMakeLists.txt b/src/firmware/nuttx/CMakeLists.txt index 895eec044a..2b24b477b5 100644 --- a/src/firmware/nuttx/CMakeLists.txt +++ b/src/firmware/nuttx/CMakeLists.txt @@ -51,7 +51,7 @@ if(NOT ${BOARD} STREQUAL "sim") ) if (config_io_board) add_dependencies(romfs fw_io) - endif() + endif() set(fw_file ${CMAKE_CURRENT_BINARY_DIR}/${OS}-${BOARD}-${LABEL}.px4) From 5d6f63af1901536e1a41690c8ea95e2fe0c59975 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Dec 2015 14:15:33 +0100 Subject: [PATCH 122/201] update jMAVSim --- Tools/jMAVSim | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/jMAVSim b/Tools/jMAVSim index 4267771753..eabdbee894 160000 --- a/Tools/jMAVSim +++ b/Tools/jMAVSim @@ -1 +1 @@ -Subproject commit 4267771753c4bb5d0696a678856620229523162e +Subproject commit eabdbee894be672e8913df619ed725b341a4e0cf From 5dc4ea8146d591e01897856a6cbd312130b137c1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Dec 2015 14:17:59 +0100 Subject: [PATCH 123/201] Sim: minor cleanups --- src/modules/simulator/simulator_mavlink.cpp | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 507f220a43..2b993c31f9 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -426,6 +426,13 @@ void Simulator::initializeSensorData() void Simulator::pollForMAVLinkMessages(bool publish) { + // set the threads name +#ifdef __PX4_DARWIN + pthread_setname_np("sim_rcv"); +#else + pthread_setname_np(pthread_self(), "sim_rcv"); +#endif + // udp socket data struct sockaddr_in _myaddr; const int _port = UDP_PORT; @@ -504,14 +511,13 @@ void Simulator::pollForMAVLinkMessages(bool publish) pret = ::poll(&fds[0], fd_count, 100); } - PX4_INFO("Found initial message, pret = %d", pret); _initialized = true; // reset system time (void)hrt_reset(); if (fds[0].revents & POLLIN) { len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen); - PX4_INFO("Sending initial controls message to jMAVSim."); + PX4_INFO("Sending initial controls message to simulator"); send_controls(); } @@ -523,14 +529,6 @@ void Simulator::pollForMAVLinkMessages(bool publish) pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, NULL); pthread_attr_destroy(&sender_thread_attr); - - // set the threads name -#ifdef __PX4_DARWIN - pthread_setname_np("sim_rcv"); -#else - pthread_setname_np(pthread_self(), "sim_rcv"); -#endif - // wait for new mavlink messages to arrive while (true) { From 947aa183f677bc62fc049445b8212f28d932334c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Dec 2015 14:22:06 +0100 Subject: [PATCH 124/201] Use 900 us as default motor off PWM since some ESCs have lower limits --- src/drivers/drv_pwm_output.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 06d1f10d47..5d9c6901a1 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -81,7 +81,7 @@ __BEGIN_DECLS /** * Default value for a shutdown motor */ -#define PWM_MOTOR_OFF 800 +#define PWM_MOTOR_OFF 900 /** * Default minimum PWM in us From fb3fade653ceefb3f682380fb8be444b363f7b74 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Dec 2015 14:22:37 +0100 Subject: [PATCH 125/201] VTOL: Use correct motor off define --- src/modules/vtol_att_control/tiltrotor.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 17b3af75df..41da1d31c2 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -428,7 +428,7 @@ void Tiltrotor::set_rear_motor_state(rear_motor_state state) break; case DISABLED: - pwm_value = PWM_LOWEST_MAX; + pwm_value = PWM_MOTOR_OFF; _rear_motors = DISABLED; break; @@ -446,21 +446,21 @@ void Tiltrotor::set_rear_motor_state(rear_motor_state state) if (fd < 0) {PX4_WARN("can't open %s", dev);} ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); - struct pwm_output_values pwm_values; - memset(&pwm_values, 0, sizeof(pwm_values)); + struct pwm_output_values pwm_max_values; + memset(&pwm_max_values, 0, sizeof(pwm_max_values)); for (int i = 0; i < _params->vtol_motor_count; i++) { if (is_motor_off_channel(i)) { - pwm_values.values[i] = pwm_value; + pwm_max_values.values[i] = pwm_value; } else { - pwm_values.values[i] = PWM_DEFAULT_MAX; + pwm_max_values.values[i] = PWM_DEFAULT_MAX; } - pwm_values.channel_count = _params->vtol_motor_count; + pwm_max_values.channel_count = _params->vtol_motor_count; } - ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); + ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_max_values); if (ret != OK) {PX4_WARN("failed setting max values");} From 62763904f24db62ca8aafaac16495ebc48ac0cb3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Dec 2015 17:12:29 +0100 Subject: [PATCH 126/201] Simulator: Add performance counter for incoming packet interval --- src/modules/simulator/simulator.h | 2 ++ src/modules/simulator/simulator_mavlink.cpp | 1 + 2 files changed, 3 insertions(+) diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 182be7916b..5da68d002a 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -230,6 +230,7 @@ private: _perf_gps(perf_alloc_once(PC_ELAPSED, "sim_gps_delay")), _perf_airspeed(perf_alloc_once(PC_ELAPSED, "sim_airspeed_delay")), _perf_sim_delay(perf_alloc_once(PC_ELAPSED, "sim_network_delay")), + _perf_sim_interval(perf_alloc(PC_INTERVAL, "sim_network_interval")), _accel_pub(nullptr), _baro_pub(nullptr), _gyro_pub(nullptr), @@ -270,6 +271,7 @@ private: perf_counter_t _perf_gps; perf_counter_t _perf_airspeed; perf_counter_t _perf_sim_delay; + perf_counter_t _perf_sim_interval; // uORB publisher handlers orb_advert_t _accel_pub; diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 2b993c31f9..01899bf0ba 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -248,6 +248,7 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish) uint64_t timestamp = ts.tv_sec * 1000 * 1000 + ts.tv_nsec / 1000; perf_set(_perf_sim_delay, timestamp - sim_timestamp); + perf_count(_perf_sim_interval); if (publish) { publish_sensor_topics(&imu); From a4018bcbc1d554a2f06a1a0bc2a9872e40bf3f11 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Dec 2015 17:13:22 +0100 Subject: [PATCH 127/201] Update to higher jMAVSim update rate --- Tools/jMAVSim | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/jMAVSim b/Tools/jMAVSim index eabdbee894..0876a46d53 160000 --- a/Tools/jMAVSim +++ b/Tools/jMAVSim @@ -1 +1 @@ -Subproject commit eabdbee894be672e8913df619ed725b341a4e0cf +Subproject commit 0876a46d53954db34775151f73db024bcfc656a0 From 4f548f328aa69f6c54336b34b9a86da9a18578ba Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Dec 2015 11:54:52 +0100 Subject: [PATCH 128/201] Set range to 0.9 Ga max since full scale range is 1.3 Ga --- src/modules/commander/mag_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 02b3a9f39a..ef0689bec2 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -73,7 +73,7 @@ static constexpr unsigned int calibration_sides = 6; ///< The total number of static constexpr unsigned int calibration_total_points = 240; ///< The total points per magnetometer static constexpr unsigned int calibraton_duration_seconds = 42; ///< The total duration the routine is allowed to take -static constexpr float MAG_MAX_OFFSET_LEN = 0.6f; ///< The maximum measurement range is ~1.4 Ga, the earth field is ~0.6 Ga, so an offset larger than ~0.8-0.6 Ga means the mag will saturate in some directions. +static constexpr float MAG_MAX_OFFSET_LEN = 0.9f; ///< The maximum measurement range is ~1.4 Ga, the earth field is ~0.6 Ga, so an offset larger than ~0.8-0.6 Ga means the mag will saturate in some directions. int32_t device_ids[max_mags]; int device_prio_max = 0; From 38fe768421924cdc149ae94baef451ec4a4be2f0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Dec 2015 17:24:01 +0100 Subject: [PATCH 129/201] Start 2nd MAVLink instance in Gazebo SITL --- posix-configs/SITL/init/rcS_gazebo_iris | 1 + 1 file changed, 1 insertion(+) diff --git a/posix-configs/SITL/init/rcS_gazebo_iris b/posix-configs/SITL/init/rcS_gazebo_iris index 95be984fb4..a85419a9e4 100644 --- a/posix-configs/SITL/init/rcS_gazebo_iris +++ b/posix-configs/SITL/init/rcS_gazebo_iris @@ -50,6 +50,7 @@ mc_pos_control start mc_att_control start mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_w.main.mix mavlink start -u 14556 -r 2000000 +mavlink start -u 14557 -r 2000000 -m onboard mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556 mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556 mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556 From 85b3de0b0058b6b4639d501dda466b81de9a99d7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Dec 2015 17:25:27 +0100 Subject: [PATCH 130/201] Start 2nd MAVLink instance in jMAVSim SITL --- posix-configs/SITL/init/rcS_jmavsim_iris | 1 + 1 file changed, 1 insertion(+) diff --git a/posix-configs/SITL/init/rcS_jmavsim_iris b/posix-configs/SITL/init/rcS_jmavsim_iris index f7c4982ed5..6fd1d59db1 100644 --- a/posix-configs/SITL/init/rcS_jmavsim_iris +++ b/posix-configs/SITL/init/rcS_jmavsim_iris @@ -57,6 +57,7 @@ mc_pos_control start mc_att_control start mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix mavlink start -u 14556 -r 2000000 +mavlink start -u 14557 -r 2000000 -m onboard mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556 mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556 mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556 From eff94677a47382146a3437cef49539eaee0bebb0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Dec 2015 17:26:09 +0100 Subject: [PATCH 131/201] MAVLink: Only broadcast heartbeat on local network if not in onboard mode --- src/modules/mavlink/mavlink_main.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 8e575e1b8f..c3004add29 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -896,9 +896,10 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID struct telemetry_status_s &tstatus = get_rx_status(); /* resend heartbeat via broadcast */ - if (((hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000) || + if ((_mode != MAVLINK_MODE_ONBOARD) + && (((hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000) || (tstatus.heartbeat_time == 0)) && - msgid == MAVLINK_MSG_ID_HEARTBEAT) { + msgid == MAVLINK_MSG_ID_HEARTBEAT)) { int bret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_bcast_addr, sizeof(_bcast_addr)); From d39e3137688c0f236bc9a482f166661f072750e9 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Thu, 31 Dec 2015 11:50:52 +0100 Subject: [PATCH 132/201] use the proper check to prevent multiple mavlink instances on the same udp port, added warning when different remote than localhost connects to udp --- src/modules/mavlink/mavlink_main.cpp | 18 ++++++++++++------ src/modules/mavlink/mavlink_receiver.cpp | 1 + 2 files changed, 13 insertions(+), 6 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c3004add29..10e0a3ac22 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1570,14 +1570,20 @@ Mavlink::task_main(int argc, char *argv[]) _datarate = MAX_DATA_RATE; } - if (Mavlink::instance_exists(_device_name, this)) { - warnx("%s already running", _device_name); - return ERROR; - } - if (get_protocol() == SERIAL) { - warnx("mode: %u, data rate: %d B/s on %s @ %dB", _mode, _datarate, _device_name, _baudrate); + if (Mavlink::instance_exists(_device_name, this)) { + warnx("%s already running", _device_name); + return ERROR; + } + + warnx("mode: %u, data rate: %d B/s on %s @ %dB", _mode, _datarate, _device_name, _baudrate); + } else if (get_protocol() == UDP) { + if (Mavlink::get_instance_for_network_port(_network_port) != nullptr) { + warnx("port %d already occupied", _network_port); + return ERROR; + } + warnx("mode: %u, data rate: %d B/s on udp port %hu", _mode, _datarate, _network_port); } /* flush stdout in case MAVLink is about to take it over */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 51f08bb01c..e73d706c19 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1817,6 +1817,7 @@ MavlinkReceiver::receive_thread(void *arg) if (srcaddr_last->sin_addr.s_addr == htonl(localhost) && srcaddr.sin_addr.s_addr != htonl(localhost)) { // if we were sending to localhost before but have a new host then accept him memcpy(srcaddr_last, &srcaddr, sizeof(srcaddr)); + PX4_WARN("UDP source addr changed: %s", inet_ntoa(srcaddr.sin_addr)); } #endif /* if read failed, this loop won't execute */ From 3d971e214ada0e2436b590f74dbd21db0ba756a8 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 30 Nov 2015 21:44:28 +0100 Subject: [PATCH 133/201] don't update local position reference if home position changes --- .../position_estimator_inav_main.cpp | 45 ------------------- 1 file changed, 45 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp index 1da3ff305c..3244392829 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -63,7 +63,6 @@ #include #include #include -#include #include #include #include @@ -273,7 +272,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) const hrt_abstime ref_init_delay = 1000000; // wait for 1s after 3D fix struct map_projection_reference_s ref; memset(&ref, 0, sizeof(ref)); - hrt_abstime home_timestamp = 0; uint16_t accel_updates = 0; uint16_t baro_updates = 0; @@ -346,8 +344,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memset(&sensor, 0, sizeof(sensor)); struct vehicle_gps_position_s gps; memset(&gps, 0, sizeof(gps)); - struct home_position_s home; - memset(&home, 0, sizeof(home)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_local_position_s local_pos; @@ -373,7 +369,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); int vision_position_estimate_sub = orb_subscribe(ORB_ID(vision_position_estimate)); int att_pos_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap)); - int home_position_sub = orb_subscribe(ORB_ID(home_position)); int distance_sensor_sub = orb_subscribe(ORB_ID(distance_sensor)); /* advertise */ @@ -677,46 +672,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) flow_updates++; } - /* home position */ - orb_check(home_position_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(home_position), home_position_sub, &home); - - if (home.timestamp != home_timestamp) { - home_timestamp = home.timestamp; - - double est_lat, est_lon; - float est_alt; - - if (ref_inited) { - /* calculate current estimated position in global frame */ - est_alt = local_pos.ref_alt - local_pos.z; - map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon); - } - - /* update reference */ - map_projection_init(&ref, home.lat, home.lon); - - /* update baro offset */ - baro_offset += home.alt - local_pos.ref_alt; - - local_pos.ref_lat = home.lat; - local_pos.ref_lon = home.lon; - local_pos.ref_alt = home.alt; - local_pos.ref_timestamp = home.timestamp; - - if (ref_inited) { - /* reproject position estimate with new reference */ - map_projection_project(&ref, est_lat, est_lon, &x_est[0], &y_est[0]); - z_est[0] = -(est_alt - local_pos.ref_alt); - } - - ref_inited = true; - } - } - - /* check no vision circuit breaker is set */ if (params.no_vision != CBRK_NO_VISION_KEY) { /* vehicle vision position */ From bdaaca3d7889af2b05f77a40fef4a447f0231da3 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 31 Dec 2015 14:40:56 +1100 Subject: [PATCH 134/201] make: Add missing ekf2 module to PX4-v1 build --- cmake/configs/nuttx_px4fmu-v1_default.cmake | 1 + 1 file changed, 1 insertion(+) diff --git a/cmake/configs/nuttx_px4fmu-v1_default.cmake b/cmake/configs/nuttx_px4fmu-v1_default.cmake index d93e8b8b28..de8753341c 100644 --- a/cmake/configs/nuttx_px4fmu-v1_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v1_default.cmake @@ -73,6 +73,7 @@ set(config_module_list modules/attitude_estimator_q modules/ekf_att_pos_estimator modules/position_estimator_inav + modules/ekf2 # # Vehicle Control From 28f5cb8fe6c3111398b7fe0114ca2a0f966eebc3 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 31 Dec 2015 18:37:16 +1100 Subject: [PATCH 135/201] ekf2: Changes required to enter POSCTL mode Adds missing local position and global position data --- src/lib/ecl | 2 +- src/modules/ekf2/ekf2_main.cpp | 121 ++++++++++++++++++++++++++++----- 2 files changed, 104 insertions(+), 19 deletions(-) diff --git a/src/lib/ecl b/src/lib/ecl index eda69e727f..0cb210d045 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit eda69e727fab22c81c421b635cb34303b90b7968 +Subproject commit 0cb210d045d005d0775bc7b4c1631addb128c8e3 diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 13f615c4e2..913b83227d 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -70,6 +70,7 @@ #include #include #include +#include #include @@ -111,6 +112,8 @@ public: void print(); + void print_status(); + private: static constexpr float _dt_max = 0.02; bool _task_should_exit = false; /**< if true, task should exit */ @@ -123,6 +126,7 @@ private: orb_advert_t _att_pub; orb_advert_t _lpos_pub; orb_advert_t _control_state_pub; + orb_advert_t _vehicle_global_position_pub; /* Low pass filter for attitude rates */ math::LowPassFilter2p _lp_roll_rate; @@ -162,6 +166,11 @@ void Ekf2::print() _ekf->printStoredIMU(); } +void Ekf2::print_status() +{ + warnx("position OK %s", (_ekf->position_is_valid()) ? "[YES]" : "[NO]"); +} + void Ekf2::task_main() { // subscribe to relevant topics @@ -244,12 +253,13 @@ void Ekf2::task_main() _ekf->setAirspeedData(airspeed.timestamp, &airspeed.indicated_airspeed_m_s); } - struct vehicle_attitude_s att; - struct vehicle_local_position_s lpos; - att.timestamp = hrt_absolute_time(); - lpos.timestamp = hrt_absolute_time(); + // run the EKF update _ekf->update(); + // generate vehicle attitude data + struct vehicle_attitude_s att; + att.timestamp = hrt_absolute_time(); + _ekf->copy_quaternion(att.q); matrix::Quaternion q(att.q[0], att.q[1], att.q[2], att.q[3]); matrix::Euler euler(q); @@ -257,25 +267,65 @@ void Ekf2::task_main() att.pitch = euler(1); att.yaw = euler(2); + // generate vehicle local position data + struct vehicle_local_position_s lpos; float pos[3] = {}; float vel[3] = {}; + lpos.timestamp = hrt_absolute_time(); + + // Position in local NED frame _ekf->copy_position(pos); lpos.x = pos[0]; lpos.y = pos[1]; lpos.z = pos[2]; + // Velocity in NED frame (m/s) _ekf->copy_velocity(vel); lpos.vx = vel[0]; lpos.vy = vel[1]; lpos.vz = vel[2]; + // TODO: better status reporting + lpos.xy_valid = _ekf->position_is_valid(); + lpos.z_valid = true; + lpos.v_xy_valid = _ekf->position_is_valid(); + lpos.v_z_valid = true; + + // Position of local NED origin in GPS / WGS84 frame + lpos.ref_timestamp = _ekf->_last_gps_origin_time_us; // Time when reference position was set + lpos.xy_global = _ekf->position_is_valid();// true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) + lpos.z_global = true;// true if z is valid and has valid global reference (ref_alt) + lpos.ref_lat = _ekf->_posRef.lat_rad * (double)180.0 * M_PI; // Reference point latitude in degrees + lpos.ref_lon = _ekf->_posRef.lon_rad * (double)180.0 * M_PI; // Reference point longitude in degrees + lpos.ref_alt = _ekf->_gps_alt_ref; // Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level + + // TODO: uORB definition does not define what this variable is. We have assumed it to be heading angle in radians lying on the range from +-pi + lpos.yaw = euler(2); + + // TODO: Distance to surface. Units not defined. + lpos.dist_bottom = 0.0f; // Distance to bottom surface (ground) + lpos.dist_bottom_rate = 0.0f; // Distance to bottom surface (ground) change rate + lpos.surface_bottom_timestamp = 0; // Time when new bottom surface found + lpos.dist_bottom_valid = false; // true if distance to bottom surface is valid + + // TODO: uORB definition does not define what thes variables are. We have assumed them to be horizontal and vertical 1-std dev accuracy in metres + // TODO: Should use sqrt of filter position variances + lpos.eph = gps.eph; + lpos.epv = gps.epv; + + // publish vehicle local position data + if (_lpos_pub == nullptr) { + _lpos_pub = orb_advertise(ORB_ID(vehicle_local_position), &lpos); + } else { + orb_publish(ORB_ID(vehicle_local_position), _lpos_pub, &lpos); + } + + // generate control state data control_state_s ctrl_state = {}; ctrl_state.timestamp = hrt_absolute_time(); ctrl_state.roll_rate = _lp_roll_rate.apply(sensors.gyro_rad_s[0]); - ctrl_state.pitch_rate = _lp_pitch_rate.apply(sensors.gyro_rad_s[1]); - ctrl_state.yaw_rate = _lp_yaw_rate.apply(sensors.gyro_rad_s[2]); ctrl_state.q[0] = q(0); @@ -283,6 +333,14 @@ void Ekf2::task_main() ctrl_state.q[2] = q(2); ctrl_state.q[3] = q(3); + // publish control state data + if (_control_state_pub == nullptr) { + _control_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state); + } else { + orb_publish(ORB_ID(control_state), _control_state_pub, &ctrl_state); + } + + // generate vehicle attitude data att.q[0] = q(0); att.q[1] = q(1); att.q[2] = q(2); @@ -293,25 +351,51 @@ void Ekf2::task_main() att.pitchspeed = sensors.gyro_rad_s[1]; att.yawspeed = sensors.gyro_rad_s[2]; - if (_control_state_pub == nullptr) { - _control_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state); - } else { - orb_publish(ORB_ID(control_state), _control_state_pub, &ctrl_state); - } - + // publish vehicle attitude data if (_att_pub == nullptr) { _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); } else { orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att); } - if (_lpos_pub == nullptr) { - _lpos_pub = orb_advertise(ORB_ID(vehicle_local_position), &lpos); - } else { - orb_publish(ORB_ID(vehicle_local_position), _lpos_pub, &lpos); + // generate and publish global position data + struct vehicle_global_position_s global_pos; + if (_ekf->position_is_valid()) { + // TODO: local origin is currenlty at GPS height origin - this is different to ekf_att_pos_estimator + + global_pos.timestamp = hrt_absolute_time(); // Time of this estimate, in microseconds since system start + global_pos.time_utc_usec = gps.time_utc_usec; // GPS UTC timestamp in microseconds + + double est_lat, est_lon; + map_projection_reproject(&_ekf->_posRef, lpos.x, lpos.y, &est_lat, &est_lon); + global_pos.lat = est_lat; // Latitude in degrees + global_pos.lon = est_lon; // Longitude in degrees + + global_pos.alt = -pos[2]; // Altitude AMSL in meters + + global_pos.vel_n = vel[0]; // Ground north velocity, m/s + global_pos.vel_e = vel[1]; // Ground east velocity, m/s + global_pos.vel_d = vel[2]; // Ground downside velocity, m/s + + global_pos.yaw = euler(2); // Yaw in radians -PI..+PI. + + global_pos.eph = gps.eph; // Standard deviation of position estimate horizontally + global_pos.epv = gps.epv; // Standard deviation of position vertically + + // TODO: implement terrain estimator + global_pos.terrain_alt = 0.0f; // Terrain altitude in m, WGS84 + global_pos.terrain_alt_valid = false; // Terrain altitude estimate is valid + // TODO use innovatun consistency check timouts to set this + global_pos.dead_reckoning = false; // True if this position is estimated through dead-reckoning + + global_pos.pressure_alt = sensors.baro_alt_meter[0]; // Pressure altitude AMSL (m) + + if (_vehicle_global_position_pub == nullptr) { + _vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); + } else { + orb_publish(ORB_ID(vehicle_global_position), _vehicle_global_position_pub, &global_pos); + } } - - } } @@ -394,6 +478,7 @@ int ekf2_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (ekf2::instance) { PX4_WARN("running"); + ekf2::instance->print_status(); return 0; } else { From 635d9ea760c2a6410a441085187de6124cca5cd2 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 1 Jan 2016 13:38:02 +1100 Subject: [PATCH 136/201] msg: Improve vehicle_local_position documentation Clean up formatting, improve consistency of descriptions and ensure units are defined --- msg/vehicle_local_position.msg | 50 +++++++++++++++++----------------- 1 file changed, 25 insertions(+), 25 deletions(-) diff --git a/msg/vehicle_local_position.msg b/msg/vehicle_local_position.msg index 4da027ae77..6547f6defd 100644 --- a/msg/vehicle_local_position.msg +++ b/msg/vehicle_local_position.msg @@ -1,36 +1,36 @@ # Fused local position in NED. -uint64 timestamp # Time of this estimate, in microseconds since system start -bool xy_valid # true if x and y are valid -bool z_valid # true if z is valid -bool v_xy_valid # true if vy and vy are valid -bool v_z_valid # true if vz is valid +uint64 timestamp # Time of this estimate since system start, (microseconds) +bool xy_valid # true if x and y are valid +bool z_valid # true if z is valid +bool v_xy_valid # true if vy and vy are valid +bool v_z_valid # true if vz is valid # Position in local NED frame -float32 x # X position in meters in NED earth-fixed frame -float32 y # X position in meters in NED earth-fixed frame -float32 z # Z position in meters in NED earth-fixed frame (negative altitude) +float32 x # North position in NED earth-fixed frame, (metres) +float32 y # East position in NED earth-fixed frame, (metres) +float32 z # Down position (negative altitude) in NED earth-fixed frame, (metres) # Velocity in NED frame -float32 vx # Ground X Speed (Latitude), m/s in NED -float32 vy # Ground Y Speed (Longitude), m/s in NED -float32 vz # Ground Z Speed (Altitude), m/s in NED +float32 vx # North velocity in NED earth-fixed frame, (metres/sec) +float32 vy # East velocity in NED earth-fixed frame, (metres/sec) +float32 vz # Down velocity in NED earth-fixed frame, (metres/sec) # Heading -float32 yaw +float32 yaw # Euler yaw angle relative to NED earth-fixed frame, -PI..+PI, (radians) -# Reference position in GPS / WGS84 frame -bool xy_global # true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) -bool z_global # true if z is valid and has valid global reference (ref_alt) -uint64 ref_timestamp # Time when reference position was set -float64 ref_lat # Reference point latitude in degrees -float64 ref_lon # Reference point longitude in degrees -float32 ref_alt # Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level +# Position of reference point (local NED frame origin) in global (GPS / WGS84) frame +bool xy_global # true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) +bool z_global # true if z is valid and has valid global reference (ref_alt) +uint64 ref_timestamp # Time when reference position was set since system start, (microseconds) +float64 ref_lat # Reference point latitude, (degrees) +float64 ref_lon # Reference point longitude, (degrees) +float32 ref_alt # Reference altitude AMSL, MUST be set to current (not at reference point!) ground level, (metres) # Distance to surface -float32 dist_bottom # Distance to bottom surface (ground) -float32 dist_bottom_rate # Distance to bottom surface (ground) change rate -uint64 surface_bottom_timestamp # Time when new bottom surface found -bool dist_bottom_valid # true if distance to bottom surface is valid -float32 eph -float32 epv +float32 dist_bottom # Distance from from bottom surface to ground, (metres) +float32 dist_bottom_rate # Rate of change of distance from bottom surface to ground, (metres/sec) +uint64 surface_bottom_timestamp # Time when new bottom surface found since system start, (microseconds) +bool dist_bottom_valid # true if distance to bottom surface is valid +float32 eph # Standard deviation of horizontal position error, (metres) +float32 epv # Standard deviation of vertical position error, (metres) \ No newline at end of file From 092b0d5dfba7932030250ee302b500c4b7900028 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 1 Jan 2016 13:38:21 +1100 Subject: [PATCH 137/201] msg: Improve vehicle_global_position documentation Clean up formatting, improve consistency of descriptions and ensure units are defined --- msg/vehicle_global_position.msg | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/msg/vehicle_global_position.msg b/msg/vehicle_global_position.msg index cee381e871..8aa7917df0 100644 --- a/msg/vehicle_global_position.msg +++ b/msg/vehicle_global_position.msg @@ -4,18 +4,18 @@ # estimator, which will take more sources of information into account than just GPS, # e.g. control inputs of the vehicle in a Kalman-filter implementation. # -uint64 timestamp # Time of this estimate, in microseconds since system start -uint64 time_utc_usec # GPS UTC timestamp in microseconds -float64 lat # Latitude in degrees -float64 lon # Longitude in degrees -float32 alt # Altitude AMSL in meters -float32 vel_n # Ground north velocity, m/s -float32 vel_e # Ground east velocity, m/s -float32 vel_d # Ground downside velocity, m/s -float32 yaw # Yaw in radians -PI..+PI. -float32 eph # Standard deviation of position estimate horizontally -float32 epv # Standard deviation of position vertically -float32 terrain_alt # Terrain altitude in m, WGS84 +uint64 timestamp # Time of this estimate since system start, (microseconds) +uint64 time_utc_usec # GPS UTC timestamp, (microseconds) +float64 lat # Latitude, (degrees) +float64 lon # Longitude, (degrees) +float32 alt # Altitude AMSL, (meters) +float32 vel_n # North velocity in NED earth-fixed frame, (metres/sec) +float32 vel_e # East velocity in NED earth-fixed frame, (metres/sec) +float32 vel_d # Down velocity in NED earth-fixed frame, (metres/sec) +float32 yaw # Euler yaw angle relative to NED earth-fixed frame, -PI..+PI, (radians) +float32 eph # Standard deviation of horizontal position error, (metres) +float32 epv # Standard deviation of vertical position error, (metres) +float32 terrain_alt # Terrain altitude WGS84, (metres) bool terrain_alt_valid # Terrain altitude estimate is valid bool dead_reckoning # True if this position is estimated through dead-reckoning -float32 pressure_alt # Pressure altitude +float32 pressure_alt # Pressure altitude AMSL, (metres) From 9264cec8070eefbfdf66d4af412b0ad74bc7dea5 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 1 Jan 2016 13:39:16 +1100 Subject: [PATCH 138/201] msg: Improve vehicle_gps_position documentation Clean up formatting, improve consistency of descriptions and ensure units are defined. --- msg/vehicle_gps_position.msg | 34 +++++++++++++++++----------------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/msg/vehicle_gps_position.msg b/msg/vehicle_gps_position.msg index 718144f327..697bfb51d5 100644 --- a/msg/vehicle_gps_position.msg +++ b/msg/vehicle_gps_position.msg @@ -1,30 +1,30 @@ # GPS position in WGS84 coordinates. -uint64 timestamp_position # Timestamp for position information -int32 lat # Latitude in 1E-7 degrees +uint64 timestamp_position # Time of the position estimates since system start, (microseconds) +int32 lat # Latitude in 1E-7 degrees int32 lon # Longitude in 1E-7 degrees -int32 alt # Altitude in 1E-3 meters (millimeters) above MSL -int32 alt_ellipsoid # Altitude in 1E-3 meters (millimeters) above Ellipsoid +int32 alt # Altitude in 1E-3 meters above MSL, (millimetres) +int32 alt_ellipsoid # Altitude in 1E-3 meters bove Ellipsoid, (millimetres) -uint64 timestamp_variance -float32 s_variance_m_s # speed accuracy estimate m/s -float32 c_variance_rad # course accuracy estimate rad +uint64 timestamp_variance # Time of the accuracy estimates since system start, (microseconds) +float32 s_variance_m_s # GPS speed accuracy estimate, (metres/sec) +float32 c_variance_rad # GPS course accuracy estimate, (radians) uint8 fix_type # 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic, float, 6: Real-Time Kinematic, fixed, 8: Extrapolated. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. -float32 eph # GPS HDOP horizontal dilution of position in m -float32 epv # GPS VDOP horizontal dilution of position in m +float32 eph # GPS horizontal position accuracy (metres) +float32 epv # GPS vertical position accuracy (metres) int32 noise_per_ms # GPS noise per millisecond int32 jamming_indicator # indicates jamming is occurring -uint64 timestamp_velocity # Timestamp for velocity informations -float32 vel_m_s # GPS ground speed (m/s) -float32 vel_n_m_s # GPS ground speed in m/s -float32 vel_e_m_s # GPS ground speed in m/s -float32 vel_d_m_s # GPS ground speed in m/s -float32 cog_rad # Course over ground (NOT heading, but direction of movement) in rad, -PI..PI -bool vel_ned_valid # Flag to indicate if NED speed is valid +uint64 timestamp_velocity # Time of the velocity estimates since system start, (microseconds) +float32 vel_m_s # GPS ground speed, (metres/sec) +float32 vel_n_m_s # GPS North velocity, (metres/sec) +float32 vel_e_m_s # GPS East velocity, (metres/sec) +float32 vel_d_m_s # GPS Down velocity, (metres/sec) +float32 cog_rad # Course over ground (NOT heading, but direction of movement), -PI..PI, (radians) +bool vel_ned_valid # True if NED velocity is valid -uint64 timestamp_time # Timestamp for time information +uint64 timestamp_time # Time of the UTC timestamp since system start, (microseconds) uint64 time_utc_usec # Timestamp (microseconds, UTC), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0 uint8 satellites_used # Number of satellites used From 61ff954d26b75e4de949f479fce0c55d963019a6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Jan 2016 11:38:56 +0100 Subject: [PATCH 139/201] uORB msg spec: Clarify local position yaw --- msg/vehicle_local_position.msg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/msg/vehicle_local_position.msg b/msg/vehicle_local_position.msg index 6547f6defd..89c141c417 100644 --- a/msg/vehicle_local_position.msg +++ b/msg/vehicle_local_position.msg @@ -17,7 +17,7 @@ float32 vy # East velocity in NED earth-fixed frame, (metres/sec) float32 vz # Down velocity in NED earth-fixed frame, (metres/sec) # Heading -float32 yaw # Euler yaw angle relative to NED earth-fixed frame, -PI..+PI, (radians) +float32 yaw # Euler yaw angle transforming the tangent plane relative to NED earth-fixed frame, -PI..+PI, (radians) # Position of reference point (local NED frame origin) in global (GPS / WGS84) frame bool xy_global # true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) From 276855acd297ce8abd3ff7af8875ef441b4a0e2c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Jan 2016 11:39:11 +0100 Subject: [PATCH 140/201] EKF2: Remove todo --- src/modules/ekf2/ekf2_main.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 913b83227d..2bd5d25fa0 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -300,11 +300,10 @@ void Ekf2::task_main() lpos.ref_lon = _ekf->_posRef.lon_rad * (double)180.0 * M_PI; // Reference point longitude in degrees lpos.ref_alt = _ekf->_gps_alt_ref; // Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level - // TODO: uORB definition does not define what this variable is. We have assumed it to be heading angle in radians lying on the range from +-pi - lpos.yaw = euler(2); + // The rotation of the tangent plane vs. geographical north + lpos.yaw = 0.0f; - // TODO: Distance to surface. Units not defined. - lpos.dist_bottom = 0.0f; // Distance to bottom surface (ground) + lpos.dist_bottom = 0.0f; // Distance to bottom surface (ground) in meters lpos.dist_bottom_rate = 0.0f; // Distance to bottom surface (ground) change rate lpos.surface_bottom_timestamp = 0; // Time when new bottom surface found lpos.dist_bottom_valid = false; // true if distance to bottom surface is valid From ce43c79a5ae9b039eb1f48a03ed403b67626015d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Jan 2016 11:42:25 +0100 Subject: [PATCH 141/201] Update ECL use --- src/lib/ecl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/ecl b/src/lib/ecl index 0cb210d045..7537c966f9 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit 0cb210d045d005d0775bc7b4c1631addb128c8e3 +Subproject commit 7537c966f95707f90d343441e4216de91334db5c From dc11b8a7efa8a18de2b2c8a68f70c6849775bcf5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Jan 2016 12:30:52 +0100 Subject: [PATCH 142/201] EKF2 sitl target: start normal app selection --- cmake/configs/posix_sitl_ekf2.cmake | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/cmake/configs/posix_sitl_ekf2.cmake b/cmake/configs/posix_sitl_ekf2.cmake index aefe3255d9..20c2d82ff9 100644 --- a/cmake/configs/posix_sitl_ekf2.cmake +++ b/cmake/configs/posix_sitl_ekf2.cmake @@ -32,18 +32,24 @@ set(config_module_list modules/sensors modules/simulator modules/mavlink - modules/dataman modules/attitude_estimator_ekf - #modules/attitude_estimator_q + modules/attitude_estimator_q modules/ekf_att_pos_estimator + modules/ekf2 modules/position_estimator_inav + modules/navigator + modules/vtol_att_control modules/mc_pos_control modules/mc_att_control - modules/navigator + modules/mc_pos_control_multiplatform + modules/mc_att_control_multiplatform + modules/land_detector + modules/fw_att_control + modules/fw_pos_control_l1 + modules/dataman + modules/sdlog2 modules/commander - modules/vtol_att_control modules/controllib - modules/ekf2 lib/mathlib lib/mathlib/math/filter lib/conversion @@ -54,6 +60,8 @@ set(config_module_list lib/launchdetection lib/terrain_estimation lib/runway_takeoff + lib/tailsitter_recovery + examples/px4_simple_app ) set(config_extra_builtin_cmds From e50fdbe32775edb6ee907eb76811da5879526423 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Jan 2016 12:31:56 +0100 Subject: [PATCH 143/201] Update ECL --- src/lib/ecl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/ecl b/src/lib/ecl index 7537c966f9..52cb9d02c7 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit 7537c966f95707f90d343441e4216de91334db5c +Subproject commit 52cb9d02c7ed46c06d34ce7edb1d24d4cebdecba From 56e5c5070370ae4be07fd40ab37f89432b8ece12 Mon Sep 17 00:00:00 2001 From: DroneBuster Date: Tue, 29 Dec 2015 19:48:48 +0200 Subject: [PATCH 144/201] VTOL: Fix MOT_OFF bug --- src/modules/vtol_att_control/tiltrotor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 41da1d31c2..c5e3ba7465 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -145,7 +145,7 @@ int Tiltrotor::get_motor_off_channels(int channels) break; } - channel_bitmap |= 1 << channel; + channel_bitmap |= 1 << (channel - 1); channels = channels / 10; } From 3b2e82cd62266555fd417c7de5331266f464b99c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Jan 2016 14:44:42 +0100 Subject: [PATCH 145/201] VTOL: Fix motor index use in VT_FW_MOT_OFF. Create new param to re-default all deployed vehicles to not shut down motors. --- ROMFS/px4fmu_common/init.d/13002_firefly6 | 2 +- src/modules/vtol_att_control/tiltrotor.cpp | 2 +- src/modules/vtol_att_control/tiltrotor_params.c | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/13002_firefly6 b/ROMFS/px4fmu_common/init.d/13002_firefly6 index d840f6c7e0..96569ddc94 100644 --- a/ROMFS/px4fmu_common/init.d/13002_firefly6 +++ b/ROMFS/px4fmu_common/init.d/13002_firefly6 @@ -46,6 +46,6 @@ set PWM_AUX_MAX 2000 set MAV_TYPE 21 param set VT_MOT_COUNT 6 -param set VT_FW_MOT_OFF 23 +param set VT_FW_MOT_OFFID 34 param set VT_IDLE_PWM_MC 1080 param set VT_TYPE 1 diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index c5e3ba7465..81cb1cb710 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -67,7 +67,7 @@ Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) : _params_handles_tiltrotor.airspeed_blend_start = param_find("VT_ARSP_BLEND"); _params_handles_tiltrotor.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK"); _params_handles_tiltrotor.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR"); - _params_handles_tiltrotor.fw_motors_off = param_find("VT_FW_MOT_OFF"); + _params_handles_tiltrotor.fw_motors_off = param_find("VT_FW_MOT_OFFID"); } Tiltrotor::~Tiltrotor() diff --git a/src/modules/vtol_att_control/tiltrotor_params.c b/src/modules/vtol_att_control/tiltrotor_params.c index 58062c40b0..e37b520473 100644 --- a/src/modules/vtol_att_control/tiltrotor_params.c +++ b/src/modules/vtol_att_control/tiltrotor_params.c @@ -90,7 +90,7 @@ PARAM_DEFINE_FLOAT(VT_TRANS_P2_DUR, 0.5f); * * * @min 0 - * @max 123456 + * @max 12345678 * @group VTOL Attitude Control */ -PARAM_DEFINE_INT32(VT_FW_MOT_OFF, 0); +PARAM_DEFINE_INT32(VT_FW_MOT_OFFID, 0); From 4ba7408ba0e47a92783086cc1bf4df5035c76235 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Jan 2016 21:21:49 +0100 Subject: [PATCH 146/201] Speed up Vagrant --- Vagrantfile | 29 +++++++++++++++++++++++++++-- 1 file changed, 27 insertions(+), 2 deletions(-) diff --git a/Vagrantfile b/Vagrantfile index dfa4da4d8b..685d346e59 100644 --- a/Vagrantfile +++ b/Vagrantfile @@ -37,17 +37,42 @@ Vagrant.configure(2) do |config| # the path on the host to the actual folder. The second argument is # the path on the guest to mount the folder. And the optional third # argument is a set of non-required options. + # NFS should be faster: https://stefanwrobel.com/how-to-make-vagrant-performance-not-suck config.vm.synced_folder ".", "/Firmware", type: "nfs" # Provider-specific configuration so you can fine-tune various # backing providers for Vagrant. These expose provider-specific options. # Example for VirtualBox: # + + # This is to configure the machine to be as fast as possible + # Alternative: https://github.com/rdsubhas/vagrant-faster config.vm.provider "virtualbox" do |vb| # Display the VirtualBox GUI when booting the machine vb.gui = false vb.customize ["modifyvm", :id, "--ioapic", "on"] - vb.customize ["modifyvm", :id, "--cpus", "2"] + #vb.customize ["modifyvm", :id, "--cpus", "2"] + + config.vm.provider "virtualbox" do |v| + host = RbConfig::CONFIG['host_os'] + + # Give VM 1/4 system memory & access to all cpu cores on the host + if host =~ /darwin/ + cpus = `sysctl -n hw.ncpu`.to_i + # sysctl returns Bytes and we need to convert to MB + mem = `sysctl -n hw.memsize`.to_i / 1024 / 1024 / 4 + elsif host =~ /linux/ + cpus = `nproc`.to_i + # meminfo shows KB and we need to convert to MB + mem = `grep 'MemTotal' /proc/meminfo | sed -e 's/MemTotal://' -e 's/ kB//'`.to_i / 1024 / 4 + else # sorry Windows folks, I can't help you + cpus = 2 + mem = 1024 + end + + v.customize ["modifyvm", :id, "--memory", mem] + v.customize ["modifyvm", :id, "--cpus", cpus] + end # Since make and other tools freak out if they see timestamps # from the future and we share directories, tightly lock the host and guest clocks together (clock sync if more than 2 seconds off) @@ -57,7 +82,7 @@ Vagrant.configure(2) do |config| vb.customize ["guestproperty", "set", :id, "/VirtualBox/GuestAdd/VBoxService/--timesync-set-on-restore", "1"] # Customize the amount of memory on the VM: - vb.memory = "2048" + #vb.memory = "2048" end # # View the documentation for the provider you are using for more From bbe69fbe45f33f86e499e5ee6c5d49713ac0beb3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 2 Jan 2016 01:06:22 +0100 Subject: [PATCH 147/201] Fix FMUv4 USB PID --- nuttx-configs/px4fmu-v4/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4fmu-v4/nsh/defconfig b/nuttx-configs/px4fmu-v4/nsh/defconfig index 5e9c276494..90aaf6261f 100644 --- a/nuttx-configs/px4fmu-v4/nsh/defconfig +++ b/nuttx-configs/px4fmu-v4/nsh/defconfig @@ -669,7 +669,7 @@ CONFIG_CDCACM_BULKIN_REQLEN=96 CONFIG_CDCACM_RXBUFSIZE=600 CONFIG_CDCACM_TXBUFSIZE=4000 CONFIG_CDCACM_VENDORID=0x26ac -CONFIG_CDCACM_PRODUCTID=0x0011 +CONFIG_CDCACM_PRODUCTID=0x0012 CONFIG_CDCACM_VENDORSTR="3D Robotics" CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v4.x" # CONFIG_USBMSC is not set From 55ce8c0c42a0e7ca00f95a7473848c2f6c8bb8e3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 2 Jan 2016 01:27:29 +0100 Subject: [PATCH 148/201] Fix QuRT build error --- src/drivers/device/vdev_posix.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index d99c7cfc74..43c55bcc53 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -245,11 +245,14 @@ extern "C" { const unsigned NAMELEN = 32; char thread_name[NAMELEN] = {}; + + #ifndef __PX4_QURT int nret = pthread_getname_np(pthread_self(), thread_name, NAMELEN); if (nret || thread_name[0] == 0) { PX4_WARN("failed getting thread name"); } + #endif PX4_DEBUG("Called px4_poll timeout = %d", timeout); px4_sem_init(&sem, 0, 0); From 2d72c95edaa0ca2353942b128409d75bb94d1645 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 2 Jan 2016 09:30:51 +0100 Subject: [PATCH 149/201] Add new posix_eagle_default and qurt_eagle_default targets --- Makefile | 6 ++ cmake/configs/qurt_eagle_default.cmake | 90 +++++++++++++++++++ .../qurt/px4_layer/commands_default.c | 55 ++++++++++++ 3 files changed, 151 insertions(+) create mode 100644 cmake/configs/qurt_eagle_default.cmake create mode 100644 src/platforms/qurt/px4_layer/commands_default.c diff --git a/Makefile b/Makefile index 77abd708a6..0bd5797512 100644 --- a/Makefile +++ b/Makefile @@ -166,6 +166,12 @@ qurt_eagle_release: posix_eagle_release: $(call cmake-build,$@) +qurt_eagle_default: + $(call cmake-build,$@) + +posix_eagle_default: + $(call cmake-build,$@) + posix: posix_sitl_default sitl_deprecation: diff --git a/cmake/configs/qurt_eagle_default.cmake b/cmake/configs/qurt_eagle_default.cmake new file mode 100644 index 0000000000..6d0e886cb7 --- /dev/null +++ b/cmake/configs/qurt_eagle_default.cmake @@ -0,0 +1,90 @@ +include(qurt/px4_impl_qurt) + +if ("${HEXAGON_DRIVERS_ROOT}" STREQUAL "") + message(FATAL_ERROR "HEXAGON_DRIVERS_ROOT is not set") +endif() + +if ("${EAGLE_DRIVERS_SRC}" STREQUAL "") + message(FATAL_ERROR "EAGLE_DRIVERS_SRC is not set") +endif() + +include_directories(${HEXAGON_DRIVERS_ROOT}/inc) + +# For Actual flight we need to link against the driver dynamic libraries +set(target_libraries + -L${HEXAGON_DRIVERS_ROOT}/libs +# The plan is to replace these with our drivers +# mpu9x50 +# uart_esc +# csr_gps +# rc_receiver + ) + + +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake) + +set(config_module_list + # + # Board support modules + # + drivers/device + modules/sensors +# The plan is to replace these with our drivers +# $(EAGLE_DRIVERS_SRC)/mpu9x50 +# $(EAGLE_DRIVERS_SRC)/uart_esc +# $(EAGLE_DRIVERS_SRC)/rc_receiver +# $(EAGLE_DRIVERS_SRC)/csr_gps + + # + # System commands + # + systemcmds/param + + # + # Estimation modules (EKF/ SO3 / other filters) + # + #modules/attitude_estimator_ekf + modules/ekf_att_pos_estimator + modules/attitude_estimator_q + modules/position_estimator_inav + + # + # Vehicle Control + # + modules/mc_att_control + modules/mc_pos_control + + # + # Library modules + # + modules/param + modules/systemlib + modules/systemlib/mixer + modules/uORB + modules/commander + modules/controllib + + # + # Libraries + # + lib/mathlib + lib/mathlib/math/filter + lib/geo + lib/geo_lookup + lib/conversion + lib/terrain_estimation + lib/runway_takeoff + lib/tailsitter_recovery + + # + # QuRT port + # + platforms/common + platforms/qurt/px4_layer + platforms/posix/work_queue + + # + # sources for muorb over fastrpc + # + modules/muorb/adsp + ) diff --git a/src/platforms/qurt/px4_layer/commands_default.c b/src/platforms/qurt/px4_layer/commands_default.c new file mode 100644 index 0000000000..80022147f0 --- /dev/null +++ b/src/platforms/qurt/px4_layer/commands_default.c @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * Copyright (C) 2015 Mark Charlebois. 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 PX4 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 commands_default.c + * Commands to run for the "qurt_eagle_default" config + * + * @author Mark Charlebois + */ + +const char *get_commands() +{ + + static const char *commands = + "uorb start\n" + "param set CAL_GYRO0_ID 2293760\n" + "param set CAL_ACC0_ID 1310720\n" + "param set CAL_ACC1_ID 1376256\n" + "param set CAL_MAG0_ID 196608\n" + "commander start\n" + + ; + + return commands; + +} From c91df50d7ead9fcbe4ff4852bcdb649a10326f4c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 2 Jan 2016 09:59:06 +0100 Subject: [PATCH 150/201] VDev: fix code style --- src/drivers/device/vdev_posix.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index 43c55bcc53..220862d1ad 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -246,13 +246,14 @@ extern "C" { const unsigned NAMELEN = 32; char thread_name[NAMELEN] = {}; - #ifndef __PX4_QURT +#ifndef __PX4_QURT int nret = pthread_getname_np(pthread_self(), thread_name, NAMELEN); if (nret || thread_name[0] == 0) { PX4_WARN("failed getting thread name"); } - #endif + +#endif PX4_DEBUG("Called px4_poll timeout = %d", timeout); px4_sem_init(&sem, 0, 0); From 735c823b5767a8cb2427afdbb6d357351d11952b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 2 Jan 2016 03:13:49 -0800 Subject: [PATCH 151/201] Remove unmaintained NuttX config --- Makefile | 3 --- cmake/configs/nuttx_sim_simple.cmake | 21 --------------------- 2 files changed, 24 deletions(-) delete mode 100644 cmake/configs/nuttx_sim_simple.cmake diff --git a/Makefile b/Makefile index 0bd5797512..ab1b2681cb 100644 --- a/Makefile +++ b/Makefile @@ -142,9 +142,6 @@ px4fmu-v2_simple: px4fmu-v2_lpe: $(call cmake-build,nuttx_px4fmu-v2_lpe) -nuttx_sim_simple: - $(call cmake-build,$@) - posix_sitl_default: $(call cmake-build,$@) diff --git a/cmake/configs/nuttx_sim_simple.cmake b/cmake/configs/nuttx_sim_simple.cmake deleted file mode 100644 index 068c6409f2..0000000000 --- a/cmake/configs/nuttx_sim_simple.cmake +++ /dev/null @@ -1,21 +0,0 @@ -include(nuttx/px4_impl_nuttx) - -message(WARNING "this is a work in progress and doesn't build yet") - -set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake) - -set(config_module_list - #platforms/nuttx - #platforms/nuttx/px4_layer - platforms/common - #drivers/led - drivers/device - #modules/systemlib - #modules/uORB - #examples/px4_simple_app - #lib/mathlib/math/filter - #lib/conversion - ) - -set(config_extra_builtin_cmds - ) From 6f1e53c810d19a30af4c5cab30df451e10b67e53 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 2 Jan 2016 03:14:06 -0800 Subject: [PATCH 152/201] Fix POSIX eagle config --- cmake/configs/posix_eagle_default.cmake | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/cmake/configs/posix_eagle_default.cmake b/cmake/configs/posix_eagle_default.cmake index b78d393ddf..9de00fb694 100644 --- a/cmake/configs/posix_eagle_default.cmake +++ b/cmake/configs/posix_eagle_default.cmake @@ -35,9 +35,10 @@ set(config_module_list lib/mathlib lib/mathlib/math/filter + lib/conversion + lib/ecl lib/geo lib/geo_lookup - lib/conversion lib/terrain_estimation lib/runway_takeoff lib/tailsitter_recovery From 0ed13a911ebd849e6591fcdcb049787aa7fb802a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 2 Jan 2016 03:51:26 -0800 Subject: [PATCH 153/201] Get QuRT drivers out of the way, as we are using our own --- cmake/configs/qurt_eagle_default.cmake | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/cmake/configs/qurt_eagle_default.cmake b/cmake/configs/qurt_eagle_default.cmake index 6d0e886cb7..e39a96fe3e 100644 --- a/cmake/configs/qurt_eagle_default.cmake +++ b/cmake/configs/qurt_eagle_default.cmake @@ -1,14 +1,14 @@ include(qurt/px4_impl_qurt) -if ("${HEXAGON_DRIVERS_ROOT}" STREQUAL "") - message(FATAL_ERROR "HEXAGON_DRIVERS_ROOT is not set") -endif() +#if ("${HEXAGON_DRIVERS_ROOT}" #STREQUAL "") +# message(FATAL_ERROR "HEXAGON_DRIVERS_ROOT is not set") +#endif() -if ("${EAGLE_DRIVERS_SRC}" STREQUAL "") - message(FATAL_ERROR "EAGLE_DRIVERS_SRC is not set") -endif() +#if ("${EAGLE_DRIVERS_SRC}" STREQUAL "") +# message(FATAL_ERROR "EAGLE_DRIVERS_SRC is not set") +#endif() -include_directories(${HEXAGON_DRIVERS_ROOT}/inc) +#include_directories(${HEXAGON_DRIVERS_ROOT}/inc) # For Actual flight we need to link against the driver dynamic libraries set(target_libraries From ba90ba14b0dcbcf4de425db4254662ce93e3e965 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 2 Jan 2016 17:17:27 +0100 Subject: [PATCH 154/201] NuttX configs: added px4fmu-v2_ekf2 target for EKF2 development on Pixhawk --- Makefile | 4 +- cmake/configs/nuttx_px4fmu-v2_default.cmake | 1 - ...imple.cmake => nuttx_px4fmu-v2_ekf2.cmake} | 69 +++++++++++++++---- 3 files changed, 57 insertions(+), 17 deletions(-) rename cmake/configs/{nuttx_px4fmu-v2_simple.cmake => nuttx_px4fmu-v2_ekf2.cmake} (69%) diff --git a/Makefile b/Makefile index ab1b2681cb..2a5ab238f2 100644 --- a/Makefile +++ b/Makefile @@ -136,8 +136,8 @@ px4fmu-v4_default: px4-stm32f4discovery_default: $(call cmake-build,nuttx_px4-stm32f4discovery_default) -px4fmu-v2_simple: - $(call cmake-build,nuttx_px4fmu-v2_simple) +px4fmu-v2_ekf2: + $(call cmake-build,nuttx_px4fmu-v2_ekf2) px4fmu-v2_lpe: $(call cmake-build,nuttx_px4fmu-v2_lpe) diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake index edf597406e..9b2c674167 100644 --- a/cmake/configs/nuttx_px4fmu-v2_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake @@ -83,7 +83,6 @@ set(config_module_list modules/attitude_estimator_q modules/ekf_att_pos_estimator modules/position_estimator_inav - modules/ekf2 # # Vehicle Control diff --git a/cmake/configs/nuttx_px4fmu-v2_simple.cmake b/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake similarity index 69% rename from cmake/configs/nuttx_px4fmu-v2_simple.cmake rename to cmake/configs/nuttx_px4fmu-v2_ekf2.cmake index 070fbd703f..570082e97c 100644 --- a/cmake/configs/nuttx_px4fmu-v2_simple.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake @@ -2,6 +2,8 @@ include(nuttx/px4_impl_nuttx) set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake) +set(config_uavcan_num_ifaces 1) + set(config_module_list # # Board support modules @@ -12,16 +14,14 @@ set(config_module_list drivers/stm32/tone_alarm drivers/led drivers/px4fmu - drivers/px4io - drivers/boards/px4fmu-v2 + drivers/boards/px4fmu-v4 drivers/rgbled drivers/mpu6000 drivers/mpu9250 - drivers/lsm303d - drivers/l3gd20 drivers/hmc5883 drivers/ms5611 drivers/mb12xx + drivers/srf02 drivers/sf0x drivers/ll40ls drivers/trone @@ -42,6 +42,7 @@ set(config_module_list drivers/gimbal drivers/pwm_input drivers/camera_trigger + drivers/bst # # System commands @@ -53,12 +54,40 @@ set(config_module_list systemcmds/pwm systemcmds/esc_calib systemcmds/reboot + systemcmds/topic_listener systemcmds/top systemcmds/config systemcmds/nshterm systemcmds/mtd systemcmds/dumpfile systemcmds/ver + systemcmds/tests + + # + # General system control + # + modules/commander + modules/navigator + modules/mavlink + modules/gpio_led + modules/uavcan + modules/land_detector + + # + # Estimation modules (EKF/ SO3 / other filters) + # + modules/position_estimator_inav + modules/ekf2 + + # + # Vehicle Control + # + # modules/segway # XXX Needs GCC 4.7 fix + modules/fw_pos_control_l1 + modules/fw_att_control + modules/mc_att_control + modules/mc_pos_control + modules/vtol_att_control # # Logging @@ -81,18 +110,32 @@ set(config_module_list #lib/mathlib/CMSIS lib/mathlib lib/mathlib/math/filter + lib/rc lib/ecl lib/external_lgpl lib/geo lib/geo_lookup lib/conversion lib/launchdetection + lib/terrain_estimation + lib/runway_takeoff + lib/tailsitter_recovery platforms/nuttx # had to add for cmake, not sure why wasn't in original config - platforms/common + platforms/common platforms/nuttx/px4_layer + # + # OBC challenge + # + modules/bottle_drop + + # + # Rover apps + # + examples/rover_steering_control + # # Demo apps # @@ -103,18 +146,18 @@ set(config_module_list # Tutorial code from # https://px4.io/dev/daemon - examples/px4_daemon_app + #examples/px4_daemon_app # Tutorial code from # https://px4.io/dev/debug_values - examples/px4_mavlink_debug + #examples/px4_mavlink_debug # Tutorial code from # https://px4.io/dev/example_fixedwing_control - examples/fixedwing_control + #examples/fixedwing_control # Hardware test - examples/hwtest + #examples/hwtest ) set(config_extra_builtin_cmds @@ -122,16 +165,14 @@ set(config_extra_builtin_cmds sercon ) -set(config_io_board - px4io-v2 - ) - set(config_extra_libs ${CMAKE_SOURCE_DIR}/src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a + uavcan + uavcan_stm32_driver ) set(config_io_extra_libs - ${CMAKE_SOURCE_DIR}/src/lib/mathlib/CMSIS/libarm_cortexM3l_math.a + #${CMAKE_SOURCE_DIR}/src/lib/mathlib/CMSIS/libarm_cortexM3l_math.a ) add_custom_target(sercon) From 6e0f96c095762ec2cb7a9955c54fa874e63168c5 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 3 Jan 2016 18:43:03 +1100 Subject: [PATCH 155/201] Update ECL --- src/lib/ecl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/ecl b/src/lib/ecl index 52cb9d02c7..b457709bd9 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit 52cb9d02c7ed46c06d34ce7edb1d24d4cebdecba +Subproject commit b457709bd9d7c215e4f245f217e5fadcb390fbdd From 2794ff2ddadb00c92d1be4d0e260b4e16bab2ec5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 3 Jan 2016 15:27:45 +0100 Subject: [PATCH 156/201] Start shell only if SD card not present --- ROMFS/px4fmu_common/init.d/rcS | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index a2a726bd7c..5e859ae222 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -60,9 +60,6 @@ unset FRC if [ $MODE == autostart ] then - # Try to get an USB console - # REBOOTWORK this needs to start after the flight control loop - nshterm /dev/ttyACM0 & # # Start the ORB (first app to start) @@ -844,6 +841,15 @@ then px4flow start & fi +# Start USB shell if no microSD present, MAVLink else +if [ $LOG_FILE == /dev/null ] +then + # Try to get an USB console + nshterm /dev/ttyACM0 & +else + mavlink start -r 800000 -d /dev/ttyACM0 -m config -x +fi + if [ $EXIT_ON_END == yes ] then echo "Exit from nsh" From a8a9c9b8ece94e8fedfd89dbdad2e94ac62f845c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 3 Jan 2016 15:28:04 +0100 Subject: [PATCH 157/201] MAVLink: Start slightly differently on USB --- src/modules/mavlink/mavlink_main.cpp | 44 +++++++++++++++++++++++++++- src/modules/mavlink/mavlink_main.h | 2 ++ 2 files changed, 45 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 10e0a3ac22..fc383fa996 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -661,9 +661,48 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * return -EINVAL; } + /* back off 1800 ms to avoid running into the USB setup timing */ + while (_mode == MAVLINK_MODE_CONFIG && + hrt_absolute_time() < 1800U * 1000U) { + usleep(50000); + } + /* open uart */ _uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY); + /* if this is a config link, stay here and wait for it to open */ + if (_uart_fd < 0 && _mode == MAVLINK_MODE_CONFIG) { + + int armed_fd = orb_subscribe(ORB_ID(actuator_armed)); + struct actuator_armed_s armed; + + /* get the system arming state and abort on arming */ + while (_uart_fd < 0) { + + /* abort if an arming topic is published and system is armed */ + bool updated = false; + orb_check(armed_fd, &updated); + + if (updated) { + /* the system is now providing arming status feedback. + * instead of timing out, we resort to abort bringing + * up the terminal. + */ + orb_copy(ORB_ID(actuator_armed), armed_fd, &armed); + + if (armed.armed) { + /* this is not an error, but we are done */ + return -1; + } + } + + usleep(100000); + _uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY); + }; + + close(armed_fd); + } + if (_uart_fd < 0) { return _uart_fd; } @@ -1595,9 +1634,12 @@ Mavlink::task_main(int argc, char *argv[]) /* default values for arguments */ _uart_fd = mavlink_open_uart(_baudrate, _device_name, &uart_config_original); - if (_uart_fd < 0) { + if (_uart_fd < 0 && _mode != MAVLINK_MODE_CONFIG) { warn("could not open %s", _device_name); return ERROR; + } else if (_uart_fd < 0 && _mode == MAVLINK_MODE_CONFIG) { + /* the config link is optional */ + return OK; } #endif diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 37c66b5963..4f2967382c 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -342,6 +342,8 @@ public: #endif static bool boot_complete() { return _boot_complete; } + bool is_usb_uart() { return _is_usb_uart; } + protected: Mavlink *next; From 77f869680aa60b79215afb4d32e7434dfb3b2f44 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 3 Jan 2016 17:16:47 +0100 Subject: [PATCH 158/201] Updated MAVLink protocol version --- mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index db5cf66d1a..21d36c9e23 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit db5cf66d1a7f549ccbaa92b5476e78754e008297 +Subproject commit 21d36c9e23867b09145dc61b11941fb606726cc2 From ae2c46ac4c759ddcf8a57909f92a207d9d84521b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 3 Jan 2016 19:03:17 +0100 Subject: [PATCH 159/201] Fix px4fmu-v2_ekf2 config --- cmake/configs/nuttx_px4fmu-v2_ekf2.cmake | 27 ++++++++++++------------ 1 file changed, 14 insertions(+), 13 deletions(-) diff --git a/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake b/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake index 570082e97c..0e456c738e 100644 --- a/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake @@ -14,29 +14,32 @@ set(config_module_list drivers/stm32/tone_alarm drivers/led drivers/px4fmu - drivers/boards/px4fmu-v4 + drivers/px4io + drivers/boards/px4fmu-v2 drivers/rgbled drivers/mpu6000 drivers/mpu9250 + drivers/lsm303d + drivers/l3gd20 drivers/hmc5883 drivers/ms5611 - drivers/mb12xx + #drivers/mb12xx drivers/srf02 drivers/sf0x drivers/ll40ls drivers/trone drivers/gps drivers/pwm_out_sim - drivers/hott - drivers/hott/hott_telemetry - drivers/hott/hott_sensors + #drivers/hott + #drivers/hott/hott_telemetry + #drivers/hott/hott_sensors drivers/blinkm drivers/airspeed drivers/ets_airspeed drivers/meas_airspeed drivers/frsky_telemetry modules/sensors - drivers/mkblctrl + #drivers/mkblctrl drivers/px4flow drivers/oreoled drivers/gimbal @@ -54,14 +57,13 @@ set(config_module_list systemcmds/pwm systemcmds/esc_calib systemcmds/reboot - systemcmds/topic_listener + #systemcmds/topic_listener systemcmds/top systemcmds/config systemcmds/nshterm systemcmds/mtd systemcmds/dumpfile systemcmds/ver - systemcmds/tests # # General system control @@ -76,7 +78,7 @@ set(config_module_list # # Estimation modules (EKF/ SO3 / other filters) # - modules/position_estimator_inav + # Too high RAM usage due to static allocations modules/ekf2 # @@ -110,7 +112,6 @@ set(config_module_list #lib/mathlib/CMSIS lib/mathlib lib/mathlib/math/filter - lib/rc lib/ecl lib/external_lgpl lib/geo @@ -123,13 +124,13 @@ set(config_module_list platforms/nuttx # had to add for cmake, not sure why wasn't in original config - platforms/common + platforms/common platforms/nuttx/px4_layer # # OBC challenge # - modules/bottle_drop + #modules/bottle_drop # # Rover apps @@ -142,7 +143,7 @@ set(config_module_list #examples/math_demo # Tutorial code from # https://px4.io/dev/px4_simple_app - examples/px4_simple_app + #examples/px4_simple_app # Tutorial code from # https://px4.io/dev/daemon From 0a5a5f4619ba0816cb128aec9243f9345a4f9577 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 3 Jan 2016 22:44:19 +0100 Subject: [PATCH 160/201] Fix incomplete boot on new EKF config --- cmake/configs/nuttx_px4fmu-v2_ekf2.cmake | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake b/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake index 0e456c738e..77526b4afd 100644 --- a/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_ekf2.cmake @@ -2,7 +2,7 @@ include(nuttx/px4_impl_nuttx) set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake) -set(config_uavcan_num_ifaces 1) +set(config_uavcan_num_ifaces 2) set(config_module_list # @@ -79,7 +79,10 @@ set(config_module_list # Estimation modules (EKF/ SO3 / other filters) # # Too high RAM usage due to static allocations + # modules/attitude_estimator_ekf + modules/attitude_estimator_q modules/ekf2 + modules/position_estimator_inav # # Vehicle Control @@ -166,6 +169,10 @@ set(config_extra_builtin_cmds sercon ) +set(config_io_board + px4io-v2 + ) + set(config_extra_libs ${CMAKE_SOURCE_DIR}/src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a uavcan From 3fb54e66b27efed56912fc71d6b6cfdaacb40a8d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 3 Jan 2016 23:57:06 +0100 Subject: [PATCH 161/201] ROMFS: Set 3DR quad tuning to more realistic default values --- ROMFS/px4fmu_common/init.d/10020_3dr_quad | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10020_3dr_quad b/ROMFS/px4fmu_common/init.d/10020_3dr_quad index 26509d128d..a1b2e90270 100644 --- a/ROMFS/px4fmu_common/init.d/10020_3dr_quad +++ b/ROMFS/px4fmu_common/init.d/10020_3dr_quad @@ -12,11 +12,11 @@ sh /etc/init.d/rc.mc_defaults if [ $AUTOCNF == yes ] then param set MC_ROLL_P 6.5 - param set MC_ROLLRATE_P 0.08 + param set MC_ROLLRATE_P 0.14 param set MC_ROLLRATE_I 0.1 param set MC_ROLLRATE_D 0.004 param set MC_PITCH_P 6.0 - param set MC_PITCHRATE_P 0.16 + param set MC_PITCHRATE_P 0.14 param set MC_PITCHRATE_I 0.09 param set MC_PITCHRATE_D 0.004 param set MC_YAW_P 4 From c343926b51e69240db6a91aa9abaaf13692a5299 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 4 Jan 2016 12:25:35 +0100 Subject: [PATCH 162/201] MAVLink app: Enable network and serial on POSIX --- src/modules/mavlink/mavlink_main.cpp | 95 +++++++++++++--------------- 1 file changed, 45 insertions(+), 50 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index fc383fa996..eecfbaf0f2 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -51,9 +51,7 @@ #include #include #include -#ifndef __PX4_POSIX #include -#endif #include #include /* isinf / isnan checks */ @@ -156,9 +154,7 @@ Mavlink::Mavlink() : _verbose(false), _forwarding_on(false), _ftp_on(false), -#ifndef __PX4_POSIX _uart_fd(-1), -#endif _baudrate(57600), _datarate(1000), _datarate_events(500), @@ -606,9 +602,16 @@ int Mavlink::get_component_id() return mavlink_system.compid; } -#ifndef __PX4_POSIX int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original) { +#ifndef B460800 + #define B460800 460800 +#endif + +#ifndef B921600 + #define B921600 921600 +#endif + /* process baud rate */ int speed; @@ -700,7 +703,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * _uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY); }; - close(armed_fd); + ::close(armed_fd); } if (_uart_fd < 0) { @@ -807,8 +810,6 @@ Mavlink::enable_flow_control(bool enabled) return ret; } -#endif - int Mavlink::set_hil_enabled(bool hil_enabled) { @@ -841,8 +842,6 @@ Mavlink::get_free_tx_buf() */ int buf_free = 0; -#ifndef __PX4_POSIX - // No FIONWRITE on Linux #if !defined(__PX4_LINUX) && !defined(__PX4_DARWIN) (void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free); @@ -861,8 +860,6 @@ Mavlink::get_free_tx_buf() } } -#endif - // if we are using network sockets, return max lenght of one packet if (get_protocol() == UDP || get_protocol() == TCP ) { return 1500; @@ -923,12 +920,13 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8); size_t ret = -1; -#ifndef __PX4_POSIX + /* send message to UART */ if (get_protocol() == SERIAL) { ret = ::write(_uart_fd, buf, packet_len); } -#else + +#ifdef __PX4_POSIX if (get_protocol() == UDP) { ret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr)); @@ -1000,21 +998,20 @@ Mavlink::resend_message(mavlink_message_t *msg) buf[MAVLINK_NUM_HEADER_BYTES + msg->len] = (uint8_t)(msg->checksum & 0xFF); buf[MAVLINK_NUM_HEADER_BYTES + msg->len + 1] = (uint8_t)(msg->checksum >> 8); -#ifndef __PX4_POSIX - /* send message to UART */ - ssize_t ret = ::write(_uart_fd, buf, packet_len); + if (_uart_fd >= 0) { + /* send message to UART */ + ssize_t ret = ::write(_uart_fd, buf, packet_len); - if (ret != (int) packet_len) { - count_txerr(); - count_txerrbytes(packet_len); + if (ret != (int) packet_len) { + count_txerr(); + count_txerrbytes(packet_len); - } else { - _last_write_success_time = _last_write_try_time; - count_txbytes(packet_len); + } else { + _last_write_success_time = _last_write_try_time; + count_txbytes(packet_len); + } } -#endif - pthread_mutex_unlock(&_send_mutex); } @@ -1609,6 +1606,8 @@ Mavlink::task_main(int argc, char *argv[]) _datarate = MAX_DATA_RATE; } + struct termios uart_config_original = {}; + if (get_protocol() == SERIAL) { if (Mavlink::instance_exists(_device_name, this)) { warnx("%s already running", _device_name); @@ -1617,6 +1616,20 @@ Mavlink::task_main(int argc, char *argv[]) warnx("mode: %u, data rate: %d B/s on %s @ %dB", _mode, _datarate, _device_name, _baudrate); + /* flush stdout in case MAVLink is about to take it over */ + fflush(stdout); + + /* default values for arguments */ + _uart_fd = mavlink_open_uart(_baudrate, _device_name, &uart_config_original); + + if (_uart_fd < 0 && _mode != MAVLINK_MODE_CONFIG) { + warn("could not open %s", _device_name); + return ERROR; + } else if (_uart_fd < 0 && _mode == MAVLINK_MODE_CONFIG) { + /* the config link is optional */ + return OK; + } + } else if (get_protocol() == UDP) { if (Mavlink::get_instance_for_network_port(_network_port) != nullptr) { warnx("port %d already occupied", _network_port); @@ -1625,24 +1638,6 @@ Mavlink::task_main(int argc, char *argv[]) warnx("mode: %u, data rate: %d B/s on udp port %hu", _mode, _datarate, _network_port); } - /* flush stdout in case MAVLink is about to take it over */ - fflush(stdout); - -#ifndef __PX4_POSIX - struct termios uart_config_original; - - /* default values for arguments */ - _uart_fd = mavlink_open_uart(_baudrate, _device_name, &uart_config_original); - - if (_uart_fd < 0 && _mode != MAVLINK_MODE_CONFIG) { - warn("could not open %s", _device_name); - return ERROR; - } else if (_uart_fd < 0 && _mode == MAVLINK_MODE_CONFIG) { - /* the config link is optional */ - return OK; - } - -#endif /* initialize send mutex */ pthread_mutex_init(&_send_mutex, NULL); @@ -1860,7 +1855,7 @@ Mavlink::task_main(int argc, char *argv[]) } /* radio config check */ - if (_radio_id != 0 && _rstatus.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO) { + if (_uart_fd >= 0 && _radio_id != 0 && _rstatus.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO) { /* request to configure radio and radio is present */ FILE *fs = fdopen(_uart_fd, "w"); @@ -2055,13 +2050,13 @@ Mavlink::task_main(int argc, char *argv[]) /* wait for threads to complete */ pthread_join(_receive_thread, NULL); -#ifndef __PX4_POSIX - /* reset the UART flags to original state */ - tcsetattr(_uart_fd, TCSANOW, &uart_config_original); + if (_uart_fd >= 0) { + /* reset the UART flags to original state */ + tcsetattr(_uart_fd, TCSANOW, &uart_config_original); - /* close UART */ - ::close(_uart_fd); -#endif + /* close UART */ + ::close(_uart_fd); + } /* close mavlink logging device */ px4_close(_mavlink_fd); From 5227dbe26a2f995d182689b7124e8952d1885509 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 4 Jan 2016 12:35:23 +0100 Subject: [PATCH 163/201] MAVLink: Send RC_CHANNELS_OVERRIDE --- src/modules/mavlink/mavlink_messages.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index ea8d2fae84..a196b6eb8e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -2170,6 +2170,22 @@ protected: msg.rssi = rc.rssi; _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS, &msg); + + /* send override message - harmless if connected to GCS, allows to connect a board to a Linux system */ + /* http://mavlink.org/messages/common#RC_CHANNELS_OVERRIDE */ + mavlink_rc_channels_override_t over; + over.target_system = mavlink_system.sysid; + over.target_component = 0; + over.chan1_raw = msg.chan1_raw; + over.chan2_raw = msg.chan2_raw; + over.chan3_raw = msg.chan3_raw; + over.chan4_raw = msg.chan4_raw; + over.chan5_raw = msg.chan5_raw; + over.chan6_raw = msg.chan6_raw; + over.chan7_raw = msg.chan7_raw; + over.chan8_raw = msg.chan8_raw; + + _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, &over); } } }; From 9415a6f8904268123ce686462135785e3eb9c8d1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 4 Jan 2016 15:10:22 +0100 Subject: [PATCH 164/201] Configure Easystar HIL setup to do Runway takeoff --- ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil index 7938c47bae..e37fc4389a 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -39,5 +39,7 @@ then param set FW_RR_P 0.3 fi +param set RWTO_TKOFF 1 + set HIL yes set MIXER AERT From 723f4bf3a1336bc6a92e4f0fa94deb2c4ef7cae6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 4 Jan 2016 15:52:56 +0100 Subject: [PATCH 165/201] Build lift drag plugin for OS X --- Tools/sitl_gazebo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index 03a749a3b3..f8d13b75ae 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit 03a749a3b3238c014329e9e8da57d102ede6f4f0 +Subproject commit f8d13b75aeb305c02a9741b4e5566cd03f07e1ba From 2be8d1a58aa7392f83da0a312593de44dc3af025 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 4 Jan 2016 18:17:56 +0100 Subject: [PATCH 166/201] Disable sdlog2 for tailsitter --- posix-configs/SITL/init/rcS_gazebo_tailsitter | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/posix-configs/SITL/init/rcS_gazebo_tailsitter b/posix-configs/SITL/init/rcS_gazebo_tailsitter index c88948cf55..2e3b01084e 100644 --- a/posix-configs/SITL/init/rcS_gazebo_tailsitter +++ b/posix-configs/SITL/init/rcS_gazebo_tailsitter @@ -52,6 +52,7 @@ fw_pos_control_l1 start fw_att_control start mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix mavlink start -u 14556 -r 2000000 +mavlink start -u 14557 -r 2000000 -m onboard mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556 mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556 mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556 @@ -61,4 +62,3 @@ mavlink stream -r 20 -s RC_CHANNELS -u 14556 mavlink stream -r 250 -s HIGHRES_IMU -u 14556 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 mavlink boot_complete -sdlog2 start -r 100 -e -t -a From a79c6be39b43a69d1f7f352cfc8cc8c263042f24 Mon Sep 17 00:00:00 2001 From: Roman Date: Mon, 4 Jan 2016 15:30:42 +0100 Subject: [PATCH 167/201] tailsitter: go into mc mode it transition switch not assigned --- src/modules/vtol_att_control/tailsitter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 4089be5b83..a1680cc09c 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -129,7 +129,7 @@ void Tailsitter::update_vtol_state() * For the backtransition the pitch is controlled in MC mode again and switches to full MC control reaching the sufficient pitch angle. */ - if (_manual_control_sp->aux1 < 0.0f) { + if (!_attc->is_fixed_wing_requested()) { switch (_vtol_schedule.flight_mode) { // user switchig to MC mode From e6155d64aeda4d4c1bc9284f4134878446518b94 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Mon, 4 Jan 2016 09:26:11 -0700 Subject: [PATCH 168/201] fix fmu arming bug --- src/drivers/px4fmu/fmu.cpp | 14 +++++--------- src/modules/systemlib/pwm_limit/pwm_limit.h | 2 +- 2 files changed, 6 insertions(+), 10 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 00b9f81f25..996743bbd8 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -138,7 +138,7 @@ private: int _dsm_fd; volatile bool _initialized; - bool _servo_armed; + bool _throttle_armed; bool _pwm_on; MixerGroup *_mixers; @@ -283,7 +283,7 @@ PX4FMU::PX4FMU() : _sbus_fd(-1), _dsm_fd(-1), _initialized(false), - _servo_armed(false), + _throttle_armed(false), _pwm_on(false), _mixers(nullptr), _groups_required(0), @@ -769,7 +769,7 @@ PX4FMU::cycle() uint16_t pwm_limited[_max_actuators]; /* the PWM limit call takes care of out of band errors, NaN and constrains */ - pwm_limit_calc(_servo_armed, arm_nothrottle(), num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm, + pwm_limit_calc(_throttle_armed, arm_nothrottle(), num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm, outputs, pwm_limited, &_pwm_limit); /* output to the servos */ @@ -789,14 +789,10 @@ PX4FMU::cycle() orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); /* update the armed status and check that we're not locked down */ - bool set_armed = (_armed.armed || _armed.prearmed) && !_armed.lockdown; - - if (_servo_armed != set_armed) { - _servo_armed = set_armed; - } + _throttle_armed = _armed.armed && !_armed.lockdown; /* update PWM status if armed or if disarmed PWM values are set */ - bool pwm_on = (set_armed || _num_disarmed_set > 0); + bool pwm_on = (_armed.armed || _num_disarmed_set > 0); if (_pwm_on != pwm_on) { _pwm_on = pwm_on; diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.h b/src/modules/systemlib/pwm_limit/pwm_limit.h index 23adc9ee3a..662fcbb196 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.h +++ b/src/modules/systemlib/pwm_limit/pwm_limit.h @@ -55,7 +55,7 @@ __BEGIN_DECLS /* * time to slowly ramp up the ESCs */ -#define RAMP_TIME_US 2500000 +#define RAMP_TIME_US 250000 enum pwm_limit_state { PWM_LIMIT_STATE_OFF = 0, From 1b860f0b31eaade21fc9a5c3f5a76116b68a675c Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Mon, 4 Jan 2016 10:51:45 -0700 Subject: [PATCH 169/201] fix code style --- src/drivers/px4fmu/fmu.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 996743bbd8..17dfb452b2 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -138,7 +138,7 @@ private: int _dsm_fd; volatile bool _initialized; - bool _throttle_armed; + bool _throttle_armed; bool _pwm_on; MixerGroup *_mixers; @@ -283,7 +283,7 @@ PX4FMU::PX4FMU() : _sbus_fd(-1), _dsm_fd(-1), _initialized(false), - _throttle_armed(false), + _throttle_armed(false), _pwm_on(false), _mixers(nullptr), _groups_required(0), @@ -769,7 +769,7 @@ PX4FMU::cycle() uint16_t pwm_limited[_max_actuators]; /* the PWM limit call takes care of out of band errors, NaN and constrains */ - pwm_limit_calc(_throttle_armed, arm_nothrottle(), num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm, + pwm_limit_calc(_throttle_armed, arm_nothrottle(), num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm, outputs, pwm_limited, &_pwm_limit); /* output to the servos */ @@ -789,10 +789,10 @@ PX4FMU::cycle() orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); /* update the armed status and check that we're not locked down */ - _throttle_armed = _armed.armed && !_armed.lockdown; + _throttle_armed = _armed.armed && !_armed.lockdown; /* update PWM status if armed or if disarmed PWM values are set */ - bool pwm_on = (_armed.armed || _num_disarmed_set > 0); + bool pwm_on = (_armed.armed || _num_disarmed_set > 0); if (_pwm_on != pwm_on) { _pwm_on = pwm_on; From f18afc47cb305938b3232c76b2fa979386e1b344 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Mon, 4 Jan 2016 10:54:38 -0700 Subject: [PATCH 170/201] revert short PWM ramp interval --- src/modules/systemlib/pwm_limit/pwm_limit.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.h b/src/modules/systemlib/pwm_limit/pwm_limit.h index 662fcbb196..23adc9ee3a 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.h +++ b/src/modules/systemlib/pwm_limit/pwm_limit.h @@ -55,7 +55,7 @@ __BEGIN_DECLS /* * time to slowly ramp up the ESCs */ -#define RAMP_TIME_US 250000 +#define RAMP_TIME_US 2500000 enum pwm_limit_state { PWM_LIMIT_STATE_OFF = 0, From 68329fe152843944fca0b2a7eefe95beace95a18 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 4 Jan 2016 22:33:02 +0100 Subject: [PATCH 171/201] SITL Gazebo: Link against protobuf lib --- Tools/sitl_gazebo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index f8d13b75ae..d5e14db741 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit f8d13b75aeb305c02a9741b4e5566cd03f07e1ba +Subproject commit d5e14db741e2cf26b406ca12071f8bb91150d536 From 11ed5169cc3ce40fdd3389e938617394f3867eed Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 4 Jan 2016 23:53:34 +0100 Subject: [PATCH 172/201] don't use default source address for onboard udp link, wait on remote --- src/modules/mavlink/mavlink_main.cpp | 15 ++++++++++----- src/modules/mavlink/mavlink_main.h | 5 +++++ src/modules/mavlink/mavlink_receiver.cpp | 4 +++- 3 files changed, 18 insertions(+), 6 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index fc383fa996..a9c76322ba 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -930,7 +930,9 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID } #else if (get_protocol() == UDP) { - ret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr)); + if (get_client_source_initialized()) { + ret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr)); + } struct telemetry_status_s &tstatus = get_rx_status(); @@ -1045,11 +1047,14 @@ Mavlink::init_udp() return; } - /* set default target address */ + /* set default target address, but not for onboard mode (will be set on first recieved packet) */ memset((char *)&_src_addr, 0, sizeof(_src_addr)); - _src_addr.sin_family = AF_INET; - inet_aton("127.0.0.1", &_src_addr.sin_addr); - _src_addr.sin_port = htons(DEFAULT_REMOTE_PORT_UDP); + if (_mode != MAVLINK_MODE_ONBOARD) { + set_client_source_initialized(); + _src_addr.sin_family = AF_INET; + inet_aton("127.0.0.1", &_src_addr.sin_addr); + _src_addr.sin_port = htons(DEFAULT_REMOTE_PORT_UDP); + } /* default broadcast address */ memset((char *)&_bcast_addr, 0, sizeof(_bcast_addr)); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 4f2967382c..1688398bb4 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -339,6 +339,10 @@ public: int get_socket_fd () { return _socket_fd; }; #ifdef __PX4_POSIX struct sockaddr_in * get_client_source_address() {return &_src_addr;}; + + void set_client_source_initialized() { _src_addr_initialized = true; }; + + bool get_client_source_initialized() { return _src_addr_initialized; }; #endif static bool boot_complete() { return _boot_complete; } @@ -423,6 +427,7 @@ private: struct sockaddr_in _myaddr; struct sockaddr_in _src_addr; struct sockaddr_in _bcast_addr; + bool _src_addr_initialized; #endif int _socket_fd; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e73d706c19..231ea21c1d 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1814,10 +1814,12 @@ MavlinkReceiver::receive_thread(void *arg) struct sockaddr_in * srcaddr_last = _mavlink->get_client_source_address(); int localhost = (127 << 24) + 1; - if (srcaddr_last->sin_addr.s_addr == htonl(localhost) && srcaddr.sin_addr.s_addr != htonl(localhost)) { + if ((srcaddr_last->sin_addr.s_addr == htonl(localhost) && srcaddr.sin_addr.s_addr != htonl(localhost)) + || (_mavlink->get_mode() == Mavlink::MAVLINK_MODE_ONBOARD && !_mavlink->get_client_source_initialized())) { // if we were sending to localhost before but have a new host then accept him memcpy(srcaddr_last, &srcaddr, sizeof(srcaddr)); PX4_WARN("UDP source addr changed: %s", inet_ntoa(srcaddr.sin_addr)); + _mavlink->set_client_source_initialized(); } #endif /* if read failed, this loop won't execute */ From c0773c01573b3e15dd5990e8de84757c805370e7 Mon Sep 17 00:00:00 2001 From: dogmaphobic Date: Tue, 5 Jan 2016 01:00:48 -0500 Subject: [PATCH 173/201] Handling missing data requests without clearing session. Letting stream handle all transfers. Tested and fully working with new QGC PR. --- src/modules/mavlink/mavlink_log_handler.cpp | 48 ++++++++++++++------- 1 file changed, 33 insertions(+), 15 deletions(-) diff --git a/src/modules/mavlink/mavlink_log_handler.cpp b/src/modules/mavlink/mavlink_log_handler.cpp index 853047030a..08197f51fe 100644 --- a/src/modules/mavlink/mavlink_log_handler.cpp +++ b/src/modules/mavlink/mavlink_log_handler.cpp @@ -40,6 +40,7 @@ #include #define MOUNTPOINT PX4_ROOTFSDIR "/fs/microsd" + static const char *kLogRoot = MOUNTPOINT "/log"; static const char *kLogData = MOUNTPOINT "/logdata.txt"; static const char *kTmpData = MOUNTPOINT "/$log$.txt"; @@ -52,7 +53,7 @@ static const char *kTmpData = MOUNTPOINT "/$log$.txt"; #define PX4LOG_DIRECTORY DT_DIR #endif -// #define MAVLINK_LOG_HANDLER_VERBOSE +//#define MAVLINK_LOG_HANDLER_VERBOSE #ifdef MAVLINK_LOG_HANDLER_VERBOSE #define PX4LOG_WARN(fmt, ...) warnx(fmt, ##__VA_ARGS__) @@ -158,20 +159,31 @@ MavlinkLogHandler::_log_request_list(const mavlink_message_t *msg) { mavlink_log_request_list_t request; mavlink_msg_log_request_list_decode(msg, &request); - //-- Clear any existing request - if(_pLogHandlerHelper) - delete _pLogHandlerHelper; - //-- Prepare new request - _pLogHandlerHelper = new LogListHelper; + //-- Check for re-requests (data loss) or new request + if(_pLogHandlerHelper) { + _pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_IDLE; + //-- Is this a new request? + if((request.end - request.start) > _pLogHandlerHelper->log_count) { + delete _pLogHandlerHelper; + _pLogHandlerHelper = NULL; + } + } + if(!_pLogHandlerHelper) { + //-- Prepare new request + _pLogHandlerHelper = new LogListHelper; + } if (_pLogHandlerHelper->log_count) { //-- Define (and clamp) range _pLogHandlerHelper->next_entry = request.start < _pLogHandlerHelper->log_count ? request.start : _pLogHandlerHelper->log_count - 1; _pLogHandlerHelper->last_entry = request.end < _pLogHandlerHelper->log_count ? request.end : _pLogHandlerHelper->log_count - 1; } - //-- Send first response (the rest, if any, is sent by the stream system) - if(_mavlink->get_free_tx_buf() > get_size()) - _log_send_listing(); + PX4LOG_WARN("\nMavlinkLogHandler::_log_request_list: start: %u last: %u count: %u\n", + _pLogHandlerHelper->next_entry, + _pLogHandlerHelper->last_entry, + _pLogHandlerHelper->log_count); + //-- Enable streaming + _pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_LISTING; } //------------------------------------------------------------------- @@ -206,15 +218,13 @@ MavlinkLogHandler::_log_request_data(const mavlink_message_t *msg) if (_pLogHandlerHelper->current_log_data_remaining > request.count) { _pLogHandlerHelper->current_log_data_remaining = request.count; } + //-- Enable streaming _pLogHandlerHelper->current_status = LogListHelper::LOG_HANDLER_SENDING_DATA; - //-- Send first chunck. Rest is sent by stream system. - if(_mavlink->get_free_tx_buf() > get_size()) - _log_send_data(); } //------------------------------------------------------------------- void -MavlinkLogHandler::_log_request_erase(const mavlink_message_t *msg) +MavlinkLogHandler::_log_request_erase(const mavlink_message_t* /*msg*/) { /* mavlink_log_erase_t request; @@ -229,8 +239,9 @@ MavlinkLogHandler::_log_request_erase(const mavlink_message_t *msg) //------------------------------------------------------------------- void -MavlinkLogHandler::_log_request_end(const mavlink_message_t *msg) +MavlinkLogHandler::_log_request_end(const mavlink_message_t* /*msg*/) { + PX4LOG_WARN("MavlinkLogHandler::_log_request_end\n"); if (_pLogHandlerHelper) { delete _pLogHandlerHelper; _pLogHandlerHelper = 0; @@ -253,6 +264,13 @@ MavlinkLogHandler::_log_send_listing() } else { _pLogHandlerHelper->next_entry++; } + PX4LOG_WARN("MavlinkLogHandler::_log_send_listing id: %u count: %u last: %u size: %u date: %u status: %d\n", + response.id, + response.num_logs, + response.last_log_num, + response.size, + response.time_utc, + _pLogHandlerHelper->current_status); } //------------------------------------------------------------------- @@ -284,7 +302,7 @@ LogListHelper::LogListHelper() : next_entry(0) , last_entry(0) , log_count(0) - , current_status(LOG_HANDLER_LISTING) + , current_status(LOG_HANDLER_IDLE) , current_log_index(0) , current_log_size(0) , current_log_data_offset(0) From 7c8a2a1e185d4bfcc302e31df3e2c27dbd989491 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 5 Jan 2016 19:00:07 +0100 Subject: [PATCH 174/201] HW test example: Use enough stack --- src/examples/hwtest/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/src/examples/hwtest/CMakeLists.txt b/src/examples/hwtest/CMakeLists.txt index 31548df813..ff256f21be 100644 --- a/src/examples/hwtest/CMakeLists.txt +++ b/src/examples/hwtest/CMakeLists.txt @@ -33,6 +33,7 @@ px4_add_module( MODULE examples__hwtest MAIN ex_hwtest + STACK 2000 SRCS hwtest.c DEPENDS From 669f8bf098a49f851756f246b641aabef43a7e0e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 5 Jan 2016 19:00:21 +0100 Subject: [PATCH 175/201] Matlab example: Use enough stack --- src/examples/matlab_csv_serial/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/src/examples/matlab_csv_serial/CMakeLists.txt b/src/examples/matlab_csv_serial/CMakeLists.txt index 516acda092..648d03dbbc 100644 --- a/src/examples/matlab_csv_serial/CMakeLists.txt +++ b/src/examples/matlab_csv_serial/CMakeLists.txt @@ -33,6 +33,7 @@ px4_add_module( MODULE examples__matlab_csv_serial MAIN matlab_csv_serial + STACK 2000 SRCS matlab_csv_serial.c DEPENDS From 2e9cec5b06566011833ccc8a08e8d21e760b3649 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 5 Jan 2016 19:00:36 +0100 Subject: [PATCH 176/201] Simple app: Use enough stack --- src/examples/px4_simple_app/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/src/examples/px4_simple_app/CMakeLists.txt b/src/examples/px4_simple_app/CMakeLists.txt index 5ba6781c60..a41f6997b3 100644 --- a/src/examples/px4_simple_app/CMakeLists.txt +++ b/src/examples/px4_simple_app/CMakeLists.txt @@ -33,6 +33,7 @@ px4_add_module( MODULE examples__px4_simple_app MAIN px4_simple_app + STACK 2000 SRCS px4_simple_app.c DEPENDS From adad9271dfd360057dc25dc371e4454af8b7462c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 5 Jan 2016 19:02:07 +0100 Subject: [PATCH 177/201] POSIX: Warn about file descriptor exceed with associated thread name --- src/drivers/device/vdev_posix.cpp | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index 220862d1ad..957b60afe1 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -124,7 +124,20 @@ extern "C" { ret = dev->open(filemap[i]); } else { - PX4_WARN("exceeded maximum number of file descriptors!"); + + const unsigned NAMELEN = 32; + char thread_name[NAMELEN] = {}; + +#ifndef __PX4_QURT + int nret = pthread_getname_np(pthread_self(), thread_name, NAMELEN); + + if (nret || thread_name[0] == 0) { + PX4_WARN("failed getting thread name"); + } + +#endif + + PX4_WARN("%s: exceeded maximum number of file descriptors!", thread_name); ret = -ENOENT; } From fc2b9a5031c8cc7146ce3b18d71162b8952fe945 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 5 Jan 2016 19:03:29 +0100 Subject: [PATCH 178/201] Print backtrace on file descriptor exceed error --- src/drivers/device/vdev_posix.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index 957b60afe1..e58c858719 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -135,6 +135,7 @@ extern "C" { PX4_WARN("failed getting thread name"); } + PX4_BACKTRACE(); #endif PX4_WARN("%s: exceeded maximum number of file descriptors!", thread_name); From f01df766a9e11d93d401db0c4f0454f8a11b53a8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 5 Jan 2016 19:02:07 +0100 Subject: [PATCH 179/201] POSIX: Warn about file descriptor exceed with associated thread name --- src/drivers/device/vdev_posix.cpp | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index 220862d1ad..957b60afe1 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -124,7 +124,20 @@ extern "C" { ret = dev->open(filemap[i]); } else { - PX4_WARN("exceeded maximum number of file descriptors!"); + + const unsigned NAMELEN = 32; + char thread_name[NAMELEN] = {}; + +#ifndef __PX4_QURT + int nret = pthread_getname_np(pthread_self(), thread_name, NAMELEN); + + if (nret || thread_name[0] == 0) { + PX4_WARN("failed getting thread name"); + } + +#endif + + PX4_WARN("%s: exceeded maximum number of file descriptors!", thread_name); ret = -ENOENT; } From 30c620a426569c594a1b61e0c96840ac9a616f23 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 5 Jan 2016 19:03:29 +0100 Subject: [PATCH 180/201] Print backtrace on file descriptor exceed error --- src/drivers/device/vdev_posix.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index 957b60afe1..e58c858719 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -135,6 +135,7 @@ extern "C" { PX4_WARN("failed getting thread name"); } + PX4_BACKTRACE(); #endif PX4_WARN("%s: exceeded maximum number of file descriptors!", thread_name); From 2ce3ef1caa06a647e01bdbf8c004c688a28bbc43 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 5 Jan 2016 22:10:32 +0100 Subject: [PATCH 181/201] VTOL: initialize fds struct --- src/modules/vtol_att_control/vtol_att_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index f21ed3fe52..65ab5f6233 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -559,7 +559,7 @@ void VtolAttitudeControl::task_main() _vtol_type->set_idle_mc(); /* wakeup source*/ - px4_pollfd_struct_t fds[3]; /*input_mc, input_fw, parameters*/ + px4_pollfd_struct_t fds[3] = {}; /*input_mc, input_fw, parameters*/ fds[0].fd = _actuator_inputs_mc; fds[0].events = POLLIN; From cb52a7eeec6403160f6418b3c7b78b6678cdd4b6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 5 Jan 2016 22:53:27 +0100 Subject: [PATCH 182/201] Raise number of max FDs for VDev --- src/drivers/device/vdev_posix.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index e58c858719..d9e11b50d7 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -56,7 +56,7 @@ pthread_mutex_t filemutex = PTHREAD_MUTEX_INITIALIZER; extern "C" { -#define PX4_MAX_FD 200 +#define PX4_MAX_FD 300 static device::file_t *filemap[PX4_MAX_FD] = {}; int px4_errno; @@ -138,7 +138,8 @@ extern "C" { PX4_BACKTRACE(); #endif - PX4_WARN("%s: exceeded maximum number of file descriptors!", thread_name); + PX4_WARN("%s: exceeded maximum number of file descriptors, accessing %s", + thread_name, path); ret = -ENOENT; } From 7d5a12f9979cc61577dd372493e9d8df0fb88f17 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 5 Jan 2016 22:53:44 +0100 Subject: [PATCH 183/201] Re-instate logging for tailsitter --- posix-configs/SITL/init/rcS_gazebo_tailsitter | 1 + 1 file changed, 1 insertion(+) diff --git a/posix-configs/SITL/init/rcS_gazebo_tailsitter b/posix-configs/SITL/init/rcS_gazebo_tailsitter index 2e3b01084e..1f4fa0ad60 100644 --- a/posix-configs/SITL/init/rcS_gazebo_tailsitter +++ b/posix-configs/SITL/init/rcS_gazebo_tailsitter @@ -62,3 +62,4 @@ mavlink stream -r 20 -s RC_CHANNELS -u 14556 mavlink stream -r 250 -s HIGHRES_IMU -u 14556 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 mavlink boot_complete +sdlog2 start -r 200 -e -t -a From 99f4487cc3d8ef8355bdef8a3b5e5acfadf6f38c Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Tue, 5 Jan 2016 18:14:36 +0100 Subject: [PATCH 184/201] allow yawing with 0 throttle, changed condition to not being landed --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 2891c15cda..dad5173cd5 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1765,8 +1765,8 @@ MulticopterPositionControl::task_main() _att_sp.yaw_body = _yaw; } - /* do not move yaw while arming */ - else if (_manual.z > 0.1f) { + /* do not move yaw while sitting on the ground */ + else if (!_vehicle_status.condition_landed) { const float yaw_offset_max = _params.man_yaw_max / _params.mc_att_yaw_p; _att_sp.yaw_sp_move_rate = _manual.r * _params.man_yaw_max; From b2aa25448b3961a653c47013df3133f2918cf188 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 7 Jan 2016 00:26:22 +0100 Subject: [PATCH 185/201] Pixracer: Boost buffer and FTP transfer capability --- nuttx-configs/px4fmu-v4/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4fmu-v4/nsh/defconfig b/nuttx-configs/px4fmu-v4/nsh/defconfig index 90aaf6261f..f4533279d4 100644 --- a/nuttx-configs/px4fmu-v4/nsh/defconfig +++ b/nuttx-configs/px4fmu-v4/nsh/defconfig @@ -552,7 +552,7 @@ CONFIG_UART7_SERIAL_CONSOLE=y # USART1 Configuration # CONFIG_USART1_RXBUFSIZE=600 -CONFIG_USART1_TXBUFSIZE=1100 +CONFIG_USART1_TXBUFSIZE=2000 CONFIG_USART1_BAUD=115200 CONFIG_USART1_BITS=8 CONFIG_USART1_PARITY=0 From 20c8c6a2fad266e55ec3d928931a79422eeb8164 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 7 Jan 2016 22:24:10 +0100 Subject: [PATCH 186/201] Fix VDev formatting --- src/drivers/device/vdev_posix.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index d9e11b50d7..6f2d749e9b 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -139,7 +139,7 @@ extern "C" { #endif PX4_WARN("%s: exceeded maximum number of file descriptors, accessing %s", - thread_name, path); + thread_name, path); ret = -ENOENT; } From 4b9a793c8f86099310dc0cfd075835ed64196bb8 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Thu, 7 Jan 2016 16:21:23 +0100 Subject: [PATCH 187/201] properly handle takeoff waypoint when not currently at takeoff location --- src/modules/navigator/mission.cpp | 10 ++++++++++ src/modules/navigator/mission.h | 1 + 2 files changed, 11 insertions(+) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 55628f3438..992a426bfe 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -286,6 +286,7 @@ Mission::advance_mission() { if (_takeoff) { _takeoff = false; + _takeoff_finished = true; } else { switch (_mission_type) { @@ -430,6 +431,8 @@ Mission::set_mission_items() /* do takeoff before going to setpoint */ /* set mission item as next position setpoint */ mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->next); + /* next SP is not takeoff anymore */ + pos_sp_triplet->next.type = position_setpoint_s::SETPOINT_TYPE_POSITION; /* calculate takeoff altitude */ float takeoff_alt = get_absolute_altitude_for_item(_mission_item); @@ -466,6 +469,13 @@ Mission::set_mission_items() } } + if (_takeoff_finished) { + /* we just finished takeoff */ + /* in case we still have to move to the takeoff waypoint we need a waypoint mission item */ + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _takeoff_finished = false; + } + /* set current position setpoint from mission item */ mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index c8a34132de..d54d91244b 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -185,6 +185,7 @@ private: int _current_offboard_mission_index; bool _need_takeoff; /**< if true, then takeoff must be performed before going to the first waypoint (if needed) */ bool _takeoff; /**< takeoff state flag */ + bool _takeoff_finished; /**< set if takeoff was requested before and is now done */ enum { MISSION_TYPE_NONE, From 72dc4d3290ba895c9cc12d60d424549aed3c84e0 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 6 Jan 2016 21:28:41 -0800 Subject: [PATCH 188/201] simulator: fix mavlink parsing of sim+RC stream The mavlink stream from the simulator and the mavlink stream over serial (used for RC input) both had the same MAVLINK_COMM channel and were therefore sharing an internal buffer in mavlink_parse_char. This meant that most (low rate) serial messages were getting swallowed by the high rate simulator stream. This change fixes the issue where RC input was not properly arriving in Gazebo, however it breaks the `commander takeoff` command in SITL which needs to be investigated. --- src/modules/simulator/simulator_mavlink.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 01899bf0ba..2dcfe4c9b9 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -575,7 +575,7 @@ void Simulator::pollForMAVLinkMessages(bool publish) mavlink_status_t status; for (int i = 0; i < len; ++i) { - if (mavlink_parse_char(MAVLINK_COMM_0, serial_buf[i], &msg, &status)) { + if (mavlink_parse_char(MAVLINK_COMM_1, serial_buf[i], &msg, &status)) { // have a message, handle it handle_message(&msg, true); } From 24116b26726b021e9f11774e280df154a7bf8ce9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 7 Jan 2016 23:19:00 +0100 Subject: [PATCH 189/201] Only try to disable HW flow control when not being UDP --- src/modules/mavlink/mavlink_main.cpp | 36 ++++++++++++++-------------- 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 51fd0d1062..d58fd0c93e 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -843,27 +843,27 @@ Mavlink::get_free_tx_buf() */ int buf_free = 0; -// No FIONWRITE on Linux -#if !defined(__PX4_LINUX) && !defined(__PX4_DARWIN) - (void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free); -#endif - - if (get_flow_control_enabled() && buf_free < FLOW_CONTROL_DISABLE_THRESHOLD) { - /* Disable hardware flow control: - * if no successful write since a defined time - * and if the last try was not the last successful write - */ - if (_last_write_try_time != 0 && - hrt_elapsed_time(&_last_write_success_time) > 500 * 1000UL && - _last_write_success_time != _last_write_try_time) { - warnx("Disabling hardware flow control"); - enable_flow_control(false); - } - } - // if we are using network sockets, return max lenght of one packet if (get_protocol() == UDP || get_protocol() == TCP ) { return 1500; + } else { + // No FIONWRITE on Linux +#if !defined(__PX4_LINUX) && !defined(__PX4_DARWIN) + (void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free); +#endif + + if (get_flow_control_enabled() && buf_free < FLOW_CONTROL_DISABLE_THRESHOLD) { + /* Disable hardware flow control: + * if no successful write since a defined time + * and if the last try was not the last successful write + */ + if (_last_write_try_time != 0 && + hrt_elapsed_time(&_last_write_success_time) > 500 * 1000UL && + _last_write_success_time != _last_write_try_time) { + warnx("Disabling hardware flow control"); + enable_flow_control(false); + } + } } return buf_free; From 4907bf3374d3b1e7b059f2eb1e6db48c849cdbd5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 7 Jan 2016 23:19:20 +0100 Subject: [PATCH 190/201] Sim: Keep MAVLink parse status persistent, because that is the frickin state machine --- src/modules/simulator/simulator_mavlink.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 2dcfe4c9b9..2c807fca05 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -530,6 +530,9 @@ void Simulator::pollForMAVLinkMessages(bool publish) pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, NULL); pthread_attr_destroy(&sender_thread_attr); + mavlink_status_t udp_status = {}; + mavlink_status_t serial_status = {}; + // wait for new mavlink messages to arrive while (true) { @@ -555,10 +558,9 @@ void Simulator::pollForMAVLinkMessages(bool publish) if (len > 0) { mavlink_message_t msg; - mavlink_status_t status; for (int i = 0; i < len; i++) { - if (mavlink_parse_char(MAVLINK_COMM_0, _buf[i], &msg, &status)) { + if (mavlink_parse_char(MAVLINK_COMM_0, _buf[i], &msg, &udp_status)) { // have a message, handle it handle_message(&msg, publish); } @@ -572,10 +574,9 @@ void Simulator::pollForMAVLinkMessages(bool publish) if (len > 0) { mavlink_message_t msg; - mavlink_status_t status; for (int i = 0; i < len; ++i) { - if (mavlink_parse_char(MAVLINK_COMM_1, serial_buf[i], &msg, &status)) { + if (mavlink_parse_char(MAVLINK_COMM_1, serial_buf[i], &msg, &serial_status)) { // have a message, handle it handle_message(&msg, true); } From 2b04f5acb96dafcc33bb137a7a0a824ce3be340a Mon Sep 17 00:00:00 2001 From: Roman Date: Fri, 8 Jan 2016 07:07:45 +0100 Subject: [PATCH 191/201] updated ecl ekf2 --- src/lib/ecl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/ecl b/src/lib/ecl index b457709bd9..2af5856361 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit b457709bd9d7c215e4f245f217e5fadcb390fbdd +Subproject commit 2af5856361adf7ede444cc7f3fd6cf725dfa0213 From bf1a51061120257a1b662ad2d6d67d74baea0f17 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Thu, 7 Jan 2016 19:04:28 -0700 Subject: [PATCH 192/201] init sbus uart in full duplex mode --- src/drivers/px4fmu/fmu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 17dfb452b2..05d7047c97 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -632,7 +632,7 @@ PX4FMU::cycle() update_pwm_rev_mask(); #ifdef SBUS_SERIAL_PORT - _sbus_fd = sbus_init(SBUS_SERIAL_PORT, true); + _sbus_fd = sbus_init(SBUS_SERIAL_PORT, false); #endif #ifdef DSM_SERIAL_PORT From 94aaa28d886329a4c5a9fb6e661e0d875a9994e4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 8 Jan 2016 08:01:29 +0100 Subject: [PATCH 193/201] Travis CI: Move to C++ 4.9 --- .travis.yml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/.travis.yml b/.travis.yml index d984d2a83e..b03ad62a77 100644 --- a/.travis.yml +++ b/.travis.yml @@ -27,8 +27,8 @@ addons: - ccache - clang-3.5 - cmake - - g++-4.8 - - gcc-4.8 + - g++-4.9 + - gcc-4.9 - genromfs - libc6-i386 - libncurses5-dev @@ -81,8 +81,8 @@ before_script: - ln -s /usr/bin/ccache ~/bin/clang - ln -s /usr/bin/ccache ~/bin/clang-3.4 - ln -s /usr/bin/ccache ~/bin/clang-3.5 - - ln -s /usr/bin/ccache ~/bin/g++-4.8 - - ln -s /usr/bin/ccache ~/bin/gcc-4.8 + - ln -s /usr/bin/ccache ~/bin/g++-4.9 + - ln -s /usr/bin/ccache ~/bin/gcc-4.9 - export PATH=~/bin:$PATH env: From 1edd9d5a9a10b22c939b051e3cc28c334b9b951f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 8 Jan 2016 08:37:56 +0100 Subject: [PATCH 194/201] Travis CI tweaks --- .travis.yml | 4 ---- 1 file changed, 4 deletions(-) diff --git a/.travis.yml b/.travis.yml index b03ad62a77..5c3c63d40e 100644 --- a/.travis.yml +++ b/.travis.yml @@ -27,8 +27,6 @@ addons: - ccache - clang-3.5 - cmake - - g++-4.9 - - gcc-4.9 - genromfs - libc6-i386 - libncurses5-dev @@ -81,8 +79,6 @@ before_script: - ln -s /usr/bin/ccache ~/bin/clang - ln -s /usr/bin/ccache ~/bin/clang-3.4 - ln -s /usr/bin/ccache ~/bin/clang-3.5 - - ln -s /usr/bin/ccache ~/bin/g++-4.9 - - ln -s /usr/bin/ccache ~/bin/gcc-4.9 - export PATH=~/bin:$PATH env: From 5bc2019fd583d61cd6cc329da771079f887ab58a Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 7 Jan 2016 15:09:38 -0800 Subject: [PATCH 195/201] Fixes for qurt build Added missing functions that were added for other targets but not for qurt. Added workaround for missing sem_timedwait(). This may have a performance impact until a sem_timedwait is supported. std::to_string is not supported by the hexagon compiler Signed-off-by: Mark Charlebois --- src/lib/matrix | 2 +- src/modules/commander/commander.cpp | 2 +- src/modules/systemlib/rc_check.h | 1 + src/platforms/posix/px4_layer/drv_hrt.c | 2 +- src/platforms/px4_defines.h | 3 +- src/platforms/px4_posix.h | 7 ++- src/platforms/qurt/include/qurt_log.h | 3 ++ src/platforms/qurt/px4_layer/drv_hrt.c | 32 +++++++++++- .../qurt/px4_layer/px4_qurt_tasks.cpp | 50 +++++++++++++++++-- 9 files changed, 90 insertions(+), 12 deletions(-) diff --git a/src/lib/matrix b/src/lib/matrix index cc1658db15..b9d6cac199 160000 --- a/src/lib/matrix +++ b/src/lib/matrix @@ -1 +1 @@ -Subproject commit cc1658db15d720e05d577787363823be0f2bd161 +Subproject commit b9d6cac199bd4476c6268f7eba76c01bf29f0295 diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7c745ed91b..03684847e6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -3209,7 +3209,7 @@ void answer_command(struct vehicle_command_s &cmd, unsigned result, void *commander_low_prio_loop(void *arg) { /* Set thread name */ - px4_prctl(PR_SET_NAME, "commander_low_prio", getpid()); + px4_prctl(PR_SET_NAME, "commander_low_prio", px4_getpid()); /* Subscribe to command topic */ int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); diff --git a/src/modules/systemlib/rc_check.h b/src/modules/systemlib/rc_check.h index 239c8a6a13..bf869bda33 100644 --- a/src/modules/systemlib/rc_check.h +++ b/src/modules/systemlib/rc_check.h @@ -36,6 +36,7 @@ * * RC calibration check */ +#include #pragma once diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c index 3bb8fa129f..69dbb90609 100644 --- a/src/platforms/posix/px4_layer/drv_hrt.c +++ b/src/platforms/posix/px4_layer/drv_hrt.c @@ -81,7 +81,7 @@ static void hrt_unlock(void) px4_sem_post(&_hrt_lock); } -#if defined(__APPLE__) && defined(__MACH__) +#if (defined(__APPLE__) && defined(__MACH__)) || defined(__PX4_QURT) #include #include #define CLOCK_REALTIME 0 diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 6f46bb2421..38ea682943 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -177,8 +177,7 @@ __END_DECLS #define MAX_RAND 32767 #if defined(__PX4_QURT) -#define M_PI 3.14159265358979323846 -#define M_PI_2 1.57079632679489661923 +#include "dspal_math.h" __BEGIN_DECLS #include __END_DECLS diff --git a/src/platforms/px4_posix.h b/src/platforms/px4_posix.h index fa1c60adee..9f48524b45 100644 --- a/src/platforms/px4_posix.h +++ b/src/platforms/px4_posix.h @@ -77,11 +77,16 @@ typedef sem_t px4_sem_t; #define px4_sem_init sem_init #define px4_sem_wait sem_wait -#define px4_sem_timedwait sem_timedwait #define px4_sem_post sem_post #define px4_sem_getvalue sem_getvalue #define px4_sem_destroy sem_destroy +#ifdef __PX4_QURT +__EXPORT int px4_sem_timedwait(px4_sem_t *sem, const struct timespec *abstime); +#else +#define px4_sem_timedwait sem_timedwait +#endif + __END_DECLS #endif diff --git a/src/platforms/qurt/include/qurt_log.h b/src/platforms/qurt/include/qurt_log.h index 29ef6302e1..7a9a0eba20 100644 --- a/src/platforms/qurt/include/qurt_log.h +++ b/src/platforms/qurt/include/qurt_log.h @@ -35,11 +35,14 @@ #include #include +#include #ifdef __cplusplus extern "C" { #endif +__EXPORT extern uint64_t hrt_absolute_time(void); + //void qurt_log(int level, const char *file, int line, const char *format, ...); // declaration to make the compiler happy. This symbol is part of the adsp static image. diff --git a/src/platforms/qurt/px4_layer/drv_hrt.c b/src/platforms/qurt/px4_layer/drv_hrt.c index c15c818172..bdc49da74a 100644 --- a/src/platforms/qurt/px4_layer/drv_hrt.c +++ b/src/platforms/qurt/px4_layer/drv_hrt.c @@ -63,8 +63,36 @@ static void hrt_call_reschedule(void); static sem_t _hrt_lock; static struct work_s _hrt_work; -static void -hrt_call_invoke(void); +#include +#include +#define CLOCK_REALTIME 0 + +#ifndef CLOCK_MONOTONIC +#define CLOCK_MONOTONIC 1 +#endif + +int px4_clock_gettime(clockid_t clk_id, struct timespec *tp) +{ + struct timeval now; + int rv = gettimeofday(&now, NULL); + + if (rv) { + return rv; + } + + tp->tv_sec = now.tv_sec; + tp->tv_nsec = now.tv_usec * 1000; + + return 0; +} + +int px4_clock_settime(clockid_t clk_id, struct timespec *tp) +{ + /* do nothing right now */ + return 0; +} + +static void hrt_call_invoke(void); static void hrt_lock(void) { diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp index 19319d1736..bb49fde864 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -37,7 +37,10 @@ * Implementation of existing task API for Linux */ -#include +#include "px4_log.h" +#include "px4_posix.h" +#include "px4_workqueue.h" +#include "hrt_work.h" #include #include #include @@ -318,7 +321,7 @@ void px4_show_tasks() __BEGIN_DECLS -int px4_getpid() +unsigned long px4_getpid() { pthread_t pid = pthread_self(); // @@ -333,7 +336,7 @@ int px4_getpid() } PX4_ERR("px4_getpid() called from unknown thread context!"); - return -EINVAL; + return ~0; } @@ -350,5 +353,44 @@ const char *getprogname() return "Unknown App"; } -__END_DECLS +static void timer_cb(void *data) +{ + px4_sem_t *sem = reinterpret_cast(data); + + sem_post(sem); +} + +int px4_sem_timedwait(px4_sem_t *sem, const struct timespec *ts) +{ + void *result; + work_s _hpwork = {}; + + // Create a timer to unblock + uint32_t timeout = ts->tv_sec*1000000+ (ts->tv_nsec/1000); + hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)sem, timeout); + sem_wait(sem); + hrt_work_cancel(&_hpwork); + return 0; +} + +int px4_prctl(int option, const char *arg2, unsigned pid) +{ + int rv; + + switch (option) { + case PR_SET_NAME: + // set the threads name - Not supported + // rv = pthread_setname_np(pthread_self(), arg2); + rv = -1; + break; + + default: + rv = -1; + PX4_WARN("FAILED SETTING TASK NAME"); + break; + } + + return rv; +} +__END_DECLS From 44cf30e0399c3947860a8ef84e3bd150e0abdd70 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 8 Jan 2016 16:43:50 +0100 Subject: [PATCH 196/201] SITL: Update tailsitter model --- Tools/sitl_gazebo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index d5e14db741..9acd71a3ff 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit d5e14db741e2cf26b406ca12071f8bb91150d536 +Subproject commit 9acd71a3ffced3be36f5ab0140747dc9163f80dd From 0adb6d0104714b7090ad09a16a17a1f0a44a39e8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 9 Jan 2016 01:42:30 +0100 Subject: [PATCH 197/201] Fix QuRT formatting --- src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp index bb49fde864..b73e296b0c 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -367,7 +367,7 @@ int px4_sem_timedwait(px4_sem_t *sem, const struct timespec *ts) work_s _hpwork = {}; // Create a timer to unblock - uint32_t timeout = ts->tv_sec*1000000+ (ts->tv_nsec/1000); + uint32_t timeout = ts->tv_sec * 1000000 + (ts->tv_nsec / 1000); hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)sem, timeout); sem_wait(sem); hrt_work_cancel(&_hpwork); From d98024a3003d71a542b01f1fe3540f2dce55e3ee Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Fri, 8 Jan 2016 19:23:38 +0100 Subject: [PATCH 198/201] updated sitl_gazebo --- Tools/sitl_gazebo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index 9acd71a3ff..6af47249ca 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit 9acd71a3ffced3be36f5ab0140747dc9163f80dd +Subproject commit 6af47249caefa3ddb6de2c31ae061aff1858fd0e From e12e029659a95fcf2a14f933a79abe1add0ad799 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 8 Jan 2016 09:44:00 -0800 Subject: [PATCH 199/201] Updated dspal version Added dspal_math.h Signed-off-by: Mark Charlebois --- src/lib/dspal | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/dspal b/src/lib/dspal index 94188fb83a..135d402c42 160000 --- a/src/lib/dspal +++ b/src/lib/dspal @@ -1 +1 @@ -Subproject commit 94188fb83ab6b2da59eb9a8c8596ab49b67c42f6 +Subproject commit 135d402c428ade4045cb3d0c404fbabcbad432fd From c17466ffee318b99e9594ac3bc171d69204d864c Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 8 Jan 2016 12:07:15 -0800 Subject: [PATCH 200/201] Added fix for missing declaration of get_commands for qurt Signed-off-by: Mark Charlebois --- .../qurt/px4_layer/commands_default.c | 2 + src/platforms/qurt/px4_layer/get_commands.h | 46 +++++++++++++++++++ src/platforms/qurt/px4_layer/main.cpp | 1 + 3 files changed, 49 insertions(+) create mode 100644 src/platforms/qurt/px4_layer/get_commands.h diff --git a/src/platforms/qurt/px4_layer/commands_default.c b/src/platforms/qurt/px4_layer/commands_default.c index 80022147f0..ce7ac90768 100644 --- a/src/platforms/qurt/px4_layer/commands_default.c +++ b/src/platforms/qurt/px4_layer/commands_default.c @@ -37,6 +37,8 @@ * @author Mark Charlebois */ +#include "get_commands.h" + const char *get_commands() { diff --git a/src/platforms/qurt/px4_layer/get_commands.h b/src/platforms/qurt/px4_layer/get_commands.h new file mode 100644 index 0000000000..df0fbd50c6 --- /dev/null +++ b/src/platforms/qurt/px4_layer/get_commands.h @@ -0,0 +1,46 @@ +/**************************************************************************** + * Copyright (C) 2015 Mark Charlebois. 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 PX4 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 get_commands.cpp + * functions to call to run the set of startup commands + * + * @author Mark Charlebois + */ + +__BEGIN_DECLS +// The commands to run are specified in a target file: commands_.c +extern const char *get_commands(void); + +// Enable external library hook +void qurt_external_hook(void) __attribute__((weak)); +__END_DECLS + diff --git a/src/platforms/qurt/px4_layer/main.cpp b/src/platforms/qurt/px4_layer/main.cpp index fc8801b60e..d8caf4659b 100644 --- a/src/platforms/qurt/px4_layer/main.cpp +++ b/src/platforms/qurt/px4_layer/main.cpp @@ -47,6 +47,7 @@ #include #include #include +#include "get_commands.h" using namespace std; From 501ad87dd2b07b3a512f4700cc6793431130bf33 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 9 Jan 2016 02:07:27 +0100 Subject: [PATCH 201/201] Travis CI: Next attempt at GCC fix --- .travis.yml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 5c3c63d40e..92ac917fa0 100644 --- a/.travis.yml +++ b/.travis.yml @@ -27,6 +27,8 @@ addons: - ccache - clang-3.5 - cmake + - g++-4.9 + - gcc-4.9 - genromfs - libc6-i386 - libncurses5-dev @@ -53,7 +55,7 @@ before_install: && mkdir -p ~/bin && wget -O ~/bin/astyle https://github.com/PX4/astyle/releases/download/2.05.1/astyle-linux && chmod +x ~/bin/astyle && astyle --version - && if [ "$CXX" = "g++" ]; then export CXX="g++-4.8" CC="gcc-4.8"; fi + && if [ "$CXX" = "g++" ]; then export CXX="g++-4.9" CC="gcc-4.9"; fi ; elif [ "${TRAVIS_OS_NAME}" = "osx" ]; then brew tap PX4/homebrew-px4